自動走行車に測距センサーを使って衝突回避機能をつけたかった Arduino+HC-SR04

衝突回避機能

作ったやつは感知しない2秒間の間に衝突してしまうので、delayの2秒をなくし、衝突判定を0.1秒毎かつ2秒間にすることで期待したように動くようにした。

スケッチ

#define PIN_IN_A1 9
#define PIN_IN_A2 10
#define PIN_IN_B1 1
#define PIN_IN_B2 11
#define TRG_PIN 2
#define ECHO_PIN 3
long randomNum = 0;
double Duration = 0;
double Distance = 0;

void setup() {
  pinMode(PIN_IN_A1, OUTPUT);
  pinMode(PIN_IN_A2, OUTPUT);
  pinMode(PIN_IN_B1, OUTPUT);
  pinMode(PIN_IN_B2, OUTPUT);
  pinMode(TRG_PIN, OUTPUT);
  digitalWrite(TRG_PIN, LOW);
  pinMode(ECHO_PIN, INPUT);
  randomSeed(analogRead(0));
}

void forward() {
  digitalWrite(PIN_IN_A1, HIGH);
  digitalWrite(PIN_IN_A2, LOW);
  digitalWrite(PIN_IN_B1, HIGH);
  digitalWrite(PIN_IN_B2, LOW);
}

void brake() {
  digitalWrite(PIN_IN_A1, LOW);
  digitalWrite(PIN_IN_A2, LOW);
  digitalWrite(PIN_IN_B1, LOW);
  digitalWrite(PIN_IN_B2, LOW);
}

void reverse() {
  digitalWrite(PIN_IN_A1, LOW);
  digitalWrite(PIN_IN_A2, HIGH);
  digitalWrite(PIN_IN_B1, LOW);
  digitalWrite(PIN_IN_B2, HIGH);
}

void coast() {
  digitalWrite(PIN_IN_A1, HIGH);
  digitalWrite(PIN_IN_A2, HIGH);
  digitalWrite(PIN_IN_B1, HIGH);
  digitalWrite(PIN_IN_B2, HIGH);
}

void turnLeft() {
  digitalWrite(PIN_IN_A1, LOW);
  digitalWrite(PIN_IN_A2, LOW);
  digitalWrite(PIN_IN_B1, HIGH);
  digitalWrite(PIN_IN_B2, LOW);
}

void turnRight() {
  digitalWrite(PIN_IN_A1, HIGH);
  digitalWrite(PIN_IN_A2, LOW);
  digitalWrite(PIN_IN_B1, LOW);
  digitalWrite(PIN_IN_B2, LOW);
}

void rotateLeft() {
  digitalWrite(PIN_IN_A1, LOW);
  digitalWrite(PIN_IN_A2, HIGH);
  digitalWrite(PIN_IN_B1, HIGH);
  digitalWrite(PIN_IN_B2, LOW);
}

void rotateRight() {
  digitalWrite(PIN_IN_A1, HIGH);
  digitalWrite(PIN_IN_A2, LOW);
  digitalWrite(PIN_IN_B1, LOW);
  digitalWrite(PIN_IN_B2, HIGH);
}

double measureDistance() {
  digitalWrite(TRG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRG_PIN, LOW);
  Duration = pulseIn(ECHO_PIN, HIGH);
  if (Duration > 0) {
    Duration = Duration / 2; //往復距離を半分にする
    Distance = Duration * 340 * 100 / 1000000; // 音速を340m/sに設定
  }
  return Distance;
}

void avoid() {
  reverse();
  delay(500);
  coast();
  rotateRight();
  delay(500);
  coast();
}

void basicMotion() {
  randomNum = random(0, 7);

  switch (randomNum) {
    case 0:
      forward();
      break;
    case 1:
      reverse();
      break;
    case 2:
      turnLeft();
      break;
    case 3:
      turnRight();
      break;
    case 4:
      rotateLeft();
      break;
    case 5:
      rotateRight();
      break;
    case 6:
      coast();
      break;
    case 7:
      brake();
      break;
  }
}

boolean withinDistance() {
  double d = 0;
  d = measureDistance();
  if (d < 10) {
    return true;
  } else {
    return false;
  }
}

void collisionAvoid(int msec,int reptime) {
  for (int i = 1; i <= reptime; i++) {
    if (withinDistance()) {
      avoid();
    }
    delay(msec);
  }
}

void loop() {
  basicMotion();
  collisionAvoid(100,20);
  brake();
}

動画

いい感じ。後退時はセンサーがないからしょうがないけど、それ以外はちゃんと回避行動が取れてる。