robot.inoの解説

#include <Servo.h>      //サーボモーターのライブラリをインクルード
#include <NewPing.h>    //超音波センサー用のNewPingライブラリをインクルード（要インストール）

//L298N モーターコントローラーのピン設定
const int LeftMotorForward = 5;
const int LeftMotorBackward = 4;
const int RightMotorForward = 3;
const int RightMotorBackward = 2;

//超音波センサーのピン設定
#define trig_pin A3   //定数trig_pinをA3に設定
#define echo_pin A2   //定数echo_pinをA2に設定

#define maximum_distance 200   //maximum_distanceを200に設定
boolean goesForward = false;
int distance = 100;

NewPing sonar(trig_pin, echo_pin, maximum_distance);    //超音波センサーの値の初期設定
Servo servo_motor;    //Servo型の変数servo_motorを宣言


void setup(){

  //各ピンを出力に設定
  pinMode(RightMotorForward, OUTPUT);
  pinMode(LeftMotorForward, OUTPUT);
  pinMode(LeftMotorBackward, OUTPUT);
  pinMode(RightMotorBackward, OUTPUT);
  
  servo_motor.attach(11);   //11番ピンをサーボモーターの出力に設定

  servo_motor.write(90);    //90度の位置（正面）まで動かす
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
}

void loop(){

  int distanceRight = 0;
  int distanceLeft = 0;
  delay(50);

  if (distance <= 20){    //distanceが20以下の時
    moveStop();     //停止
    delay(300);
    moveBackward();     //後退
    delay(400);
    moveStop();     //停止
    delay(300);
    distanceRight = lookRight();      //右距離測定
    delay(300);
    distanceLeft = lookLeft();      //左距離測定
    delay(300);

    if (distance >= distanceLeft){    //左距離がdistanceより大きい時
      turnRight();    //右回転
      moveStop();
    }
    else{
      turnLeft();    //左回転
      moveStop();
    }
  }
  else{
    moveForward(); 
  }
    distance = readPing();    //readPing()で前方距離測定
}

//右距離測定
int lookRight(){  
  servo_motor.write(10);    //10度の位置まで動かす
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(90);    //正面に戻す
  return distance;
}

//左距離測定
int lookLeft(){
  servo_motor.write(170);    //170度の位置まで動かす
  delay(500);
  int distance = readPing();
  delay(100);
  servo_motor.write(90);    //正面に戻す
  return distance;
  delay(100);
}

//距離測定処理
int readPing(){
  delay(70);
  int cm = sonar.ping_cm();   //距離を測定してセンチメートルで値を読み込み
  if (cm==0){
    cm=250;
  }
  return cm;
}

//停止処理
void moveStop(){
  
  digitalWrite(RightMotorForward, LOW);
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);
  digitalWrite(LeftMotorBackward, LOW);
}

//前進処理
void moveForward(){

  if(!goesForward){

    goesForward=true;
    
    digitalWrite(LeftMotorForward, HIGH);
    digitalWrite(RightMotorForward, HIGH);
  
    digitalWrite(LeftMotorBackward, LOW);
    digitalWrite(RightMotorBackward, LOW); 
  }
}

//後退処理
void moveBackward(){

  goesForward=false;

  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorForward, LOW);
  
}

//右回転処理
void turnRight(){

  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorBackward, HIGH);
  
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorForward, LOW);
  
  delay(500);
  
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward, LOW);
 
  
  
}


//左回転処理
void turnLeft(){

  digitalWrite(LeftMotorBackward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorForward, LOW);
  digitalWrite(RightMotorBackward, LOW);

  delay(500);
  
  digitalWrite(LeftMotorForward, HIGH);
  digitalWrite(RightMotorForward, HIGH);
  
  digitalWrite(LeftMotorBackward, LOW);
  digitalWrite(RightMotorBackward, LOW);
}