//匯入

#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <MeMCore.h>
#include <Timer.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;                     //設定一個參數為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;          //設定計算次數
byte S3;                        //callback使用判斷值
byte S3Output = 0;              //循線使用回傳值
Timer t;                        //設定Timer為t
int add = 0;                    //callback使用計算值
bool Buttomif =true;            //設定按下按鈕後的判斷條件


void setup()
{
      buzzer.tone(262,200);
      buzzer.tone(294,200);
      buzzer.tone(330,200);        //輸出已(重)開機訊號
      t.every(80,callback,6);      //設定timer參數
}
void callback()               //timer使用的callback函數
{
      if(S3 == 0)                 //S3感測為黑時
      {
          add++;                    //add計算值+1
      }
      if(add>=3)              //如果add大於等於3時
      {
          S3Output = 0;         //輸入訊號為黑
          add = 0;              //add計算時歸零
      }
      else{
          S3Output = 1;         //輸入訊號為白
          add = 0;              //add計算時歸零
          }
}
void loop()
{
  buttom=analogRead(A7);         //設定buttom為板載按鈕
  ReturnIF1=IF1.readSensors();
  ReturnIF2=IF2.readSensors();
  ReturnIF3=IF3.readSensors();   //設定五路的感測資料位置
  distance=UTS.distanceCm();     //令超音波資料為distance
  S3=IF2.readSensors();          //callback判對值為中間感測器資料
  S3Output=IF2.readSensors();    //循線輸入值(S3)
  int Atimes = 0;
  int Atimes2 = 0;
  int Atimes3 = 0;
  t.update();

if(buttom<10)                //如果按鈕按下去
      {
          Buttomif = false;
      }
      if(Buttomif == false)
      {                         //按下板載按鈕後執行所有loop程式
          if(distance <= 5)     //距離小於5cm  程式避障
                {
                  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{                              //無感測到障礙物時

              if(ReturnIF1!=0 && S3Output!=0 && ReturnIF3!=0)
              {
                     while(Atimes < Ttimes)     //計算次數(次數(0~3)<目標次數)
                     {
                            if (ReturnIF1!=0 && S3Output!=0 && ReturnIF3!=0) // 全感白
                             {
                                motor_left.run(-255);   //直走
                                motor_right.run(255);
                                delay(20);              //0.02秒(未測試)
                                Atimes++;               //次數+1

                             }
                         else{
                                Atimes = 0;
                                break;                  //就跳出
                             }
                     }

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

              if(ReturnIF1==0 && S3Output==0 && ReturnIF3==0)
              {
                   while(Atimes2 < Ttimes2)                 //計算次數(次數(0~3)<目標次數)
                   {
                         if (ReturnIF1==0 && S3Output==0 && ReturnIF3==0) // 全感黑
                             {
                                  motor_left.run(-255);   //直走
                                  motor_right.run(255);
                                  delay(20);              //0.02秒(未測試)
                                  Atimes2++;              //次數+1

                             }
                         else{
                                  Atimes2 = 0;
                                  break;                  //就跳出
                             }
                   }

                   if(Atimes2 == Ttimes2)                //次數=目標次數
                   {
                        while(ReturnIF1==0 && S3Output==0 && ReturnIF3==0) //假如是黑
                        {
                             Atimes2 = 0;
                             motor_left.run(-255);
                             motor_right.run(255);
                            //直走
                        }
                   }
               }



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