//匯入

#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 distance;   //設定一個參數為distance
void setup()
{
  Serial.begin(115200);     //串列阜(之後要刪除)
  buzzer.tone(262,200);
  buzzer.tone(294,200);
  buzzer.tone(330,200);    //輸出已開機訊號
  }
void loop()
{
  ReturnIF1=IF1.readSensors();
  ReturnIF2=IF2.readSensors();
  ReturnIF3=IF3.readSensors();   //設定五路的感測資料位置
  distance=UTS.distanceCm();     //令超音波資料為distance
  Serial.println(distance);       //串列阜(之後要刪除)
   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");                                   //輸出距離太遠(之後要刪除)
    //以下為新型循線
    // ==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);
         //往右(未測試)
      }
      else if (ReturnIF1!=0 && ReturnIF2!=0 && ReturnIF3!=0) // 全感白
     {
         //白白白
         motor_left.run(0);
         motor_right.run(0);
         //(未測試)
      };
      }

    };
  //以上為超音波感測器