//匯入

#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <MeMCore.h>

//自訂參數
MBotDCMotor motor_left(9);
MBotDCMotor motor_right(10);
MeLineFollower IF1(2);    // ss1 ss2
MeLineFollower IF2(1);    // ss3
MeLineFollower IF3(4);    // ss4 ss5
MeUltrasonicSensor UTS(3);//超音波
MeBuzzer buzzer; //嗡鳴器
byte ReturnIF1;
byte ReturnIF2;
byte ReturnIF3;
int buttom;
int distance;   //設定一個參數為distance
bool times = false;       //設定一個是否值
const int Ttimes = 3;      //目標計算次數
bool times2 = false;       //設定一個是否值
const int Ttimes2 = 3;      //設定計算次數
bool times3 = false;       //設定一個是否值
const int Ttimes3 = 3;      //設定計算次數
void setup()
{
  Serial.begin(115200);     //串列阜(之後要刪除)
  buzzer.tone(262,200);
  buzzer.tone(294,200);
  buzzer.tone(330,200);    //輸出已開機訊號
  }
void loop()
{
  buttom=analogRead(A7);         //設定buttom為板載按鈕
  ReturnIF1=IF1.readSensors();
  ReturnIF2=IF2.readSensors();
  ReturnIF3=IF3.readSensors();   //設定五路的感測資料位置
  distance=UTS.distanceCm();     //令超音波資料為distance
  int Atimes = 0;
  int Atimes2 = 0;
  int Atimes3 = 0;
  Serial.println(distance);       //串列阜(之後要刪除)

   if(buttom<10){                //按鈕按下去後執行

   if(distance >= 5){          //距離小於5cm
    Serial.println("closer");     //輸出距離太小(之後要刪除)
    motor_left.run(255);
    motor_right.run(-255);
    delay(200);   //後退
    motor_left.run(-55);
    motor_right.run(255);
    delay(500);    //左轉
    motor_left.run(-255);
    motor_right.run(255);
    delay(200);    //前進
    motor_left.run(-255);
    motor_right.run(55);
    delay(700);    //右轉
    motor_left.run(-255);
    motor_right.run(255);
    delay(150);    //接線時請調150，鋰電池時請調250(前進)
    motor_left.run(255);
    motor_right.run(0);
    delay(680);    //回復路線
    }else{
    Serial.println("far");                 //輸出距離太遠(之後要刪除)


    if(times){
    times = true;       //開始計算
    while(Atimes <Ttimes){        //計算次數(次數(0~3)<目標次數)
    if (ReturnIF1==0 && ReturnIF2==0 && ReturnIF3==0) // 全感黑

     {
       break;       //就跳出
     }
     else
     {
        motor_left.run(-255);   //直走
        motor_right.run(255);
        delay(20);              //0.02毫秒(未測試)
        Atimes++;               //次數+1
      }
      }

      if(Atimes == Ttimes){     //次數=目標次數
        while(ReturnIF1!=0 && ReturnIF2!=0 && ReturnIF3!=0) //假如是白
        {
          motor_left.run(-255);
          motor_right.run(255);
         //直走
          }
      }
    times = false;       //結束計算
    }



    if(times2){
    times2 = true;       //開始計算
    while(Atimes2 <Ttimes2){        //計算次數(次數(0~3)<目標次數)
    if (ReturnIF1!=0 && ReturnIF2!=0 && ReturnIF3!=0) // 全感白

     {
       break;       //就跳出
     }
     else
     {
        motor_left.run(-255);   //直走
        motor_right.run(255);
        delay(20);              //0.02毫秒(未測試)
        Atimes2++;               //次數+1
      }
      }

      if(Atimes2 == Ttimes2){     //次數=目標次數
        while(ReturnIF1==0 && ReturnIF2==0 && ReturnIF3==0) //假如是黑
        {
          motor_left.run(-255);
          motor_right.run(255);
         //直走
          }
      }
    times2 = false;       //結束計算
    }




    //以下為新型循線
    // ==0 = 黑  !=0 = 白
    if(ReturnIF1!=0 && ReturnIF2==0 && ReturnIF3!=0)        //ss3感黑 其他感白
      {
        //白黑白
         motor_left.run(-255);
         motor_right.run(255);
         //直走
      }
     else if (ReturnIF1==0 && ReturnIF2!=0 && ReturnIF3!=0) // 一二路感黑 其他感白
     {
         //黑白白
         motor_left.run(0);
         motor_right.run(255);
         //往左 (未測試)
      }
      else if (ReturnIF1==0 && ReturnIF2==0 && ReturnIF3!=0) // 四五路感白 其他感黑
     {
         //黑黑白
         motor_left.run(-255);
         motor_right.run(0);
         //往右(未測試)
      }
      else if (ReturnIF1!=0 && ReturnIF2!=0 && ReturnIF3==0) // 四五路感黑 其他感白
     {
         //白白黑
         motor_left.run(0);
         motor_right.run(255);
         //往左 (未測試)
      }
      else if (ReturnIF1!=0 && ReturnIF2==0 && ReturnIF3==0) // 一二路感白 其他感黑
     {
         //白黑黑
         motor_left.run(-255);
         motor_right.run(0);
         //往右(未測試)
      };
      }
    }
    };
  //以上為超音波感測器