子供の夏休みの自由研究（障害物回避ロボットカー編）

前回は、ソナーセンサーで距離を測るところまで出来たので、今回は距離に基づいて障害物回避をするところまで進める。hirotaka-hachiya.hatenablog.com

以下のように、ブレッドボードを、ソナーセンサがちょうどロボットの先端にくるように配置した。

以下のように、10cm未満に障害物がある場合、右に回転し、ない場合前進するプログラムを作成した。youtu.be

//ソナーセンサTrig、Echoのピン番号設定
int Trig = 4;
int Echo = 5;
int Duration;
float Distance;


//左側のモータのピン番号設定
const int motorL_PWM = 11;
const int motorL_1 = 9;
const int motorL_2 = 7;


//右側のモータのピン番号設定
const int motorR_PWM = 10;
const int motorR_1 = 8;
const int motorR_2 = 6;


void setup() {
  //シリアル通信のボーレートを9600Hzに設定
  Serial.begin(9600);

  //TrigとEchoを出力モードに設定
  pinMode(Trig,OUTPUT);
  pinMode(Echo,INPUT);
}

void loop() {
  //TrigにLow, High, Lowを出力
  digitalWrite(Trig,LOW);
  delayMicroseconds(1);
  digitalWrite(Trig,HIGH);
  delayMicroseconds(1);
  digitalWrite(Trig,LOW);

  //EchoがLOWからHighになってから再びLOWに戻るまでの時間（us）を計測
  Duration = pulseIn(Echo,HIGH);
  
  if (Duration>0) {
    //行と帰りがあるので、２で割る    
    Distance = Duration/2;

    //超音波の速度は、340m/sなので、0.034cm/usを、Durationにかけて、距離(cm)に変換
    Distance = Distance*340*100/1000000;

    //DurationとDistanceをシリアル通信で出力
    Serial.print(Duration);
    Serial.print(" us ");
    Serial.print(Distance);
    Serial.println(" cm");

    //Distanceが10cm未満の場合
    if (Distance < 10) {
      //--------------------------
      // 右に回転
      digitalWrite(motorL_1,LOW);
      digitalWrite(motorL_2,HIGH);
      analogWrite(motorL_PWM,100);

      digitalWrite(motorR_1,LOW);
      digitalWrite(motorR_2,HIGH);
      analogWrite(motorR_PWM,100);  
      //--------------------------
    }else{
      //--------------------------
      // 前進する
      digitalWrite(motorL_1,LOW);
      digitalWrite(motorL_2,HIGH);
      analogWrite(motorL_PWM,100);

      digitalWrite(motorR_1,HIGH);
      digitalWrite(motorR_2,LOW);
      analogWrite(motorR_PWM,100);  
      //--------------------------      
    }    
  }
  delay(500);
}