/*このスケッチを丸ごとコピーするだけでは再現できません。機体に合わせた設定値の変更が必要です。
------------------------------------------------------------------------------------*/
#include "Servo.h" //サーボ用ライブラリー
#include "IRremote.h" //赤外線リモコン用ライブラリー
Servo leftservo;//左サーボを定義
Servo rightservo;//右サーボを定義
Servo USservo;//超音波サーボを定義
const int trigger = 7;//トリガーを7番ピンに定義
const int echo = 6;//エコーを6番ピンに定義
const int modeLED = 9;//modeLEDを7番ピンに定義
const int PowerLED = 10;//PowerLEDを6番ピンに定義
const int distancelimit = 12;//超音波センサー前方距離リミット値
const int sidedistancelimit = 10;//超音波センサー横側距離リミット値
const int turntime = 200;//左右旋回時間の設定
int leftscanval, centerscanval, rightscanval;//超音波距離測定の定義
int receiver = 4;//赤外線受信センサーを4番ピンに定義
int modecontrol = 0;//モードコントロールの初期値
int power = 0;//パワー初期値
int distance;
int numcycles = 0;
int thereis;
IRrecv irrecv(receiver);//赤外線受信の定義
decode_results results;
char choice;
char contcommand;
char turndirection;

void setup() {
  rightservo.attach(11);//右サーボを11番ピンに設定
  rightservo.write(90);//初期サーボ角度を設定
  USservo.attach(12);//超音波センサーサーボを12番ピンに設定
  USservo.write(90);//初期サーボ角度を設定
  leftservo.attach(13);//左サーボを13番ピンに設定
  leftservo.write(90);//初期サーボ角度を設定
  irrecv.enableIRIn();//赤外線受信センサーを設定
  pinMode(trigger, OUTPUT);//トリガーを出力ピンに設定
  pinMode(echo, INPUT);//エコーを入力ピンに設定
  pinMode(PowerLED, OUTPUT);//PowerLEDを入力ピンに設定
  pinMode(modeLED, OUTPUT);//modeLEDを入力ピンに設定
  digitalWrite(trigger, LOW);//トリガーをLOWに設定
  digitalWrite(modeLED, LOW);//PowerLEDをLOWに設定
  digitalWrite(PowerLED, LOW);//PowerLEDをLOWに設定
}

int watch() {//以下、超音波センサーの設定
  long howfar;
  digitalWrite(trigger, LOW);
  delayMicroseconds(5);
  digitalWrite(trigger, HIGH);
  delayMicroseconds(15);
  digitalWrite(trigger, LOW);
  howfar = pulseIn(echo, HIGH);
  howfar = howfar * 0.01657;
  return round(howfar);
}

void forwards() {//前進時のサーボ角度設定
  leftservo.write (150);
  rightservo.write (25);
}

void backwards() {//後進時のサーボ角度設定
  leftservo.write (30);
  rightservo.write (150);
}

void turnleft(int t) {//左旋回時のサーボ角度設定
  leftservo.write (85);
  rightservo.write (85);
  delay(t);
}

void turnright(int t) {//右旋回時のサーボ角度設定
  leftservo.write (95);
  rightservo.write (95);
  delay(t);
}

void stopmove() {//ストップ時のサーボ角度設定
  leftservo.write (90);
  rightservo.write (90);
}

void watchsurrounding() {
  USservo.write(90);//ソナーサーボを90度（前方）に向ける
  delay(200);//200ms状態を保持
  centerscanval = watch();//前方を監視
  if (centerscanval < distancelimit) {//前方の距離がリミットより短い場合
    stopmove();//停止
  }
  USservo.write(140);//ソナーサーボを140度（斜め左前方）に向ける
  delay(100);//100ms状態を保持
  leftscanval = watch();//左側を監視
  if (leftscanval < sidedistancelimit) {//左側の距離が横側リミットより短い場合
    stopmove();//停止
  }
  USservo.write(90);//ソナーサーボを90度（前方）に向ける
  delay(200);//200ms状態を保持
  centerscanval = watch();//前方を監視
  if (centerscanval < distancelimit) {//前方の距離がリミットより短い場合
    stopmove();//停止
  }
  USservo.write(40);//ソナーサーボを40度（右斜め前方）に向ける
  delay(100);//100ms状態を保持
  rightscanval = watch();//右側を監視
  if (rightscanval < sidedistancelimit) {//右側の距離が横側リミットより短い場合
    stopmove();//停止
  }
}

char decide() {
  watchsurrounding();//周囲の監視
  if (leftscanval > rightscanval && leftscanval > centerscanval) {
    choice = 'l';//左より右の距離が近く、且つ左より前方の距離が近い場合は'l'
  }
  else if (rightscanval > leftscanval && rightscanval > centerscanval) {
    choice = 'r';//右より左の距離が近く、且つ右より前方の距離が近い場合は'r'
  }
  else {
    choice = 'f';//それ以外の場合は'f'
  }
  return choice;
}

void translateIR() {//赤外線リモコンの手動運転モード（シャープ製テレビリ用モコンの場合）
  switch (results.value) {
    case 0xF148EA81: // ↑ボタン
      forwards();// 前進
      break;
    case 0xF148EB80: // ←ボタン
      turnleft(turntime);// 左旋回
      stopmove();//turntime設定時間後停止
      break;
    case 0xF1484A8B: // 決定ボタン
      stopmove();//停止
      break;
    case 0xF1481B8F: // →ボタン
      turnright(turntime);// 右旋回
      stopmove();//turntime設定時間後停止
      break;
    case 0xF1480481: // ↓ボタン
      backwards();// 後進
      break;
    case 0xF148C144: // 黄色ボタン（手動モード終了）
      modecontrol = 0;//モードを0に戻す
      stopmove();//停止
      digitalWrite (modeLED, LOW);//modeLEDを消灯
      break;
    default:
      ;
  }
  delay(500);
}

void loop() {
  if (irrecv.decode(&results)) { //赤外線リモコンによるモード切り替え
    if (results.value == 0xF1480148) { // 青色ボタン（スタート）
      power = 1;//自動運転ON
      digitalWrite (PowerLED, HIGH);//PowerLEDを点灯
    }
    if (results.value == 0xF1488140) { // 赤ボタン（ストップ）
      stopmove();//停止
      power = 0;//自動運転OFF
      digitalWrite (PowerLED, LOW);//PowerLEDを消灯
    }
    if (results.value == 0xF148C144) { // 黄ボタン(手動モード開始）
      modecontrol = 1;//手動運転モード
      stopmove();//停止
      digitalWrite (modeLED, HIGH);//modeLEDを点灯
    }
    irrecv.resume();//受信待機
  }
  
  while (modecontrol == 1) { //手動運転が選択された場合
    if (irrecv.decode(&results)) { //赤外線受信デコード
      translateIR();//関数実行
      irrecv.resume();//受信待機
    }
  }

  if (power == 1) {  //赤外線リモコンの青色ボタンが押された場合
    forwards();//前進
    ++numcycles;//numcyclesを加算
    if (numcycles > 100) { //numcyclesカウンターが100以上になれば
      watchsurrounding();//watchsurrounding()を実行
      if (leftscanval < sidedistancelimit) { //左横リミット内に入った場合
        turnright(turntime);//右旋回
      }
      if (rightscanval < sidedistancelimit) { //横リミット内に入った場合
        turnleft(turntime);//左旋回
      }
      numcycles = 0;//カウンターを戻す
    }

    distance = watch();//リミット内に入った場合の処理
    if (distance < distancelimit) {//リミット内に入った場合
      ++thereis;//thereisカウンターを加算
    }
    if (distance > distancelimit) {//リミット外に出た場合
      thereis = 0;//thereisカウンターを戻す
    }
    if (thereis > 15) {//thereisカウンター値が15以上になった場合
      stopmove();//停止
      turndirection = decide();
      switch (turndirection) {
        case 'l':
          turnleft(turntime);//左旋回
          break;
        case 'r':
          turnright(turntime);//右旋回
          break;
        case 'f':
          ;
          break;
      }
      thereis = 0;//thereisカウンター値が15以下の場合は0
    }
  }
}