#include <AFMotor.h>  //ライブラリからAFMoterを読み込みます

#include <NewPing.h>  //ライブラリからNewPingを読み込みます



#define TRIG_PIN A4  //超音波センサーのTRIGとA4を結線しました

#define ECHO_PIN A5  //超音波センサーのECOHとA5を結線しました

#define MAX_DISTANCE_POSSIBLE 800  //超音波センサーの最大距離を800cmに設定しました

#define MAX_SPEED 120  //モーターの速度は120％

#define MOTORS_CALIBRATION_OFFSET 3  //左右のモーターの許容誤差を設定しました

#define COLL_DIST 10  //ロボットが停止してバックする距離は10cm

#define TURN_DIST COLL_DIST+20  //遠ざかる方向に＋20cmを設定




NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE_POSSIBLE);  //超音波センサーライブラリを設定します。



AF_DCMotor leftMotor(1, MOTOR12_1KHZ);  //左のモーターは１番に接続しました

AF_DCMotor rightMotor(2, MOTOR12_1KHZ);  //右のモーターは２番に接続しました



int pos = 0;            //変数を設定　位置

int maxDist = 0;        //変数を設定　最大距離

int maxAngle = 0;       //変数を設定　最大角度

int maxRight = 0;       //変数を設定　面舵いっぱ～い

int maxLeft = 0;	//変数を設定　取舵いっぱ～い

int maxFront = 0;	//変数を設定　全速前進～ん

int course = 0;		//変数を設定　コース

int curDist = 0;	//変数を設定　

String motorSet = "";	//文字を設定

int speedSet = 0;	//変数を設定　速度設定





void setup() {

  delay(1000);	//1秒待ちます

  checkPath();

  motorSet = "FORWARD";

  moveForward();	//前進します

}



void loop() {

  checkForward();  //前進するように設定 

  checkPath();

}



void checkPath() {

  int curLeft = 0;	//変数を設定

  int curFront = 0;	//変数を設定

  int curRight = 0;	//変数を設定

  int curDist = 0;	//変数を設定

  {



    checkForward();	 //前方を確認する

    curDist = readPing();  //障害物までの距離を取得する

    if (curDist < COLL_DIST) {  //距離が衝突距離よりも小さい場合

      checkCourse();  //checkCourse関数を実行します

    }

    if (curDist < TURN_DIST) {  //距離がターン距離よりも小さい場合

      changePath();

    }

    if (curDist > curDist) {maxAngle = pos;}

    if (pos > 90 && curDist > curLeft) { curLeft = curDist;}

    if (pos == 90 && curDist > curFront) {curFront = curDist;}

    if (pos < 90 && curDist > curRight) {curRight = curDist;}

  }

  maxLeft = curLeft;

  maxRight = curRight;

  maxFront = curFront;

}



void setCourse() {      //どの方向が最も距離が離れているのか？ 右または左に曲がるか？選択する

    if (maxAngle < 90) {turnRight();}

    if (maxAngle > 90) {turnLeft();}

    maxLeft = 0;

    maxRight = 0;

    maxFront = 0;

}



void checkCourse() {    //バックしてストップして、空いたルートを見つけます

  moveBackward();

  delay(1000);

  moveStop();

  setCourse();

}



void changePath() {

  if (pos < 90) {lookLeft();}   //センサー位置が90度未満の場合、左を確認

  if (pos > 90) {lookRight();}  //センサー位置が90度以上の場合、右を確認

}



int readPing() {

  delay(90);

  unsigned int uS = sonar.ping();   //超音波センサーの距離遅延を読み取ります

  int cm = uS/US_ROUNDTRIP_CM;

  return cm;

}



void checkForward() { if (motorSet=="FORWARD") {leftMotor.run(FORWARD); rightMotor.run(BACKWARD); } }

//前進していることを確認する



void checkBackward() 

{ if (motorSet=="BACKWARD") {leftMotor.run(BACKWARD); rightMotor.run(FORWARD); } }

//後退していることを確認する



void moveStop() {leftMotor.run(RELEASE); rightMotor.run(RELEASE);}    //ストップします

void moveForward() {

    motorSet = "FORWARD";  //前進

    leftMotor.run(FORWARD);  //左モーターを正転します

    rightMotor.run(BACKWARD);  //右モーターを逆転します

  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)     //速度をゆっくり上げます

  {

    leftMotor.setSpeed(speedSet+MOTORS_CALIBRATION_OFFSET);    //速度をゆっくり上げます

    rightMotor.setSpeed(speedSet);

    delay(5); //ほんのちょっとだけ遅れます

  }

}

void moveBackward() {            //バックしますご注意ください

    motorSet = "BACKWARD";   //モーター設定「バックワード」

    leftMotor.run(BACKWARD);  //左モーターを逆転します

    rightMotor.run(FORWARD);  //右モーターを正転します

  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=10)     //速度をゆっくり上げます

  {

    leftMotor.setSpeed(speedSet+MOTORS_CALIBRATION_OFFSET);

    rightMotor.setSpeed(speedSet);

    delay(10); //ほんのちょっとだけ遅れます

  }

}

void turnRight() {              //右に曲がりますご注意ください

  motorSet = "RIGHT";        //モーター設定「ライト」

  leftMotor.run(FORWARD);    //左モーターを正転します

  rightMotor.run(FORWARD);   //右モーターを正転します

  delay(1500);    //1.5秒続けます

  motorSet = "FORWARD";     //モーター設定「フォワード」

  leftMotor.run(FORWARD);    //左モーターを正転します

  rightMotor.run(BACKWARD);  //右モーターを逆転します

}



void turnLeft() {                //左に曲がりますご注意ください

  motorSet = "LEFT";          //モーター設定「レフト」

  leftMotor.run(BACKWARD);   //左モーターを逆転します

  rightMotor.run(BACKWARD);  //右モーターを逆転します

  delay(1500);    //1.5秒続けます
  motorSet = "FORWARD";     //モーター設定「フォワード」

  leftMotor.run(FORWARD);    //左モーターを正転します

  rightMotor.run(BACKWARD);  //右モーターを逆転します

}



void lookRight() {rightMotor.run(FORWARD); delay(1000); rightMotor.run(FORWARD);}

void lookLeft() {leftMotor.run(BACKWARD); delay(1000); leftMotor.run(BACKWARD);}