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

MBotDCMotor motor_left(9);     //左邊輪子
MBotDCMotor motor_right(10);   //右邊輪子
MeLineFollower lfer(2);        //循線(插槽2)
MeUltrasonicSensor UTS(3);     //超音波(插槽3)
MeBuzzer buzzer;               //蜂鳴器
unsigned long StartTime;       //設定開機時間(按下板載按鈕)
unsigned long Ftime;           //設定目標時間
bool time1 = true;             //設定開關函式

void setup()
    {
    // 開機音效
    buzzer.tone(262, 200);
    buzzer.tone(294, 200);
    buzzer.tone(330, 200);

    while(analogRead(A7)>10);  //按下板載按鈕(執行所有程式)
    StartTime = millis();      //設定起始時間
    }

void loop()
{
     int distance = UTS.distanceCm();    //設定避障回傳值
     Ftime = StartTime + 20000;          //設定Ftime為起始時間加20秒
     if(time1 && (millis()-StartTime >= Ftime))
     //如果time1是true，現在時間>=最終時間
     {
           motor_left.run(-230);
           motor_right.run(45);
          while(lfer.readSensors() == 0)
          {
                motor_left.run(-255);
                motor_right.run(255);
                delay(5000);
          }
          time1 = false;
      }



       if (distance <= 5)
       {
            // 避障
            motor_left.run(0);
            motor_right.run(0);
            delay(500);

            motor_left.run(0);
            motor_right.run(-100);
            delay(1000);

            motor_left.run(-200);
            motor_right.run(200);
            delay(800);

            motor_left.run(0);
            motor_right.run(200);
            delay(800);

            motor_left.run(-200);
            motor_right.run(200);
            delay(500);

            motor_left.run(-200);
            motor_right.run(-100);
            delay(1000);
    }
    else {
            byte ReturnValue = lfer.readSensors();
            switch (ReturnValue)
            {
                case 0:
                    motor_left.run(-255);
                    motor_right.run(255);

                    break;

                case 1:
                    // 左轉
                    motor_left.run(-40);
                    motor_right.run(235);

                    while (lfer.readSensors() == 1);
                    break;

                case 2:
                    // 右轉
                    motor_left.run(-235);
                    motor_right.run(40);

                    while (lfer.readSensors() == 2);
                    break;

                case 3:
                    // 遇到白白
                    motor_left.run(150);
                    motor_right.run(-150);
//                delay(300);
//
//                // 若無則左轉直到感測到黑
//                motor_left.run(150);
//                motor_right.run(-150);

                while (lfer.readSensors() == 3);
                break;
        }
    }
}