# 參考4/30之指定題版本

#include <SoftwareSerial.h>
#include <SPI.h>
#include <MFRC522.h>
#include "wheel.h"


//------------------------------------------L298N-------------------------------------------
#define L_ENA 9
#define L_ENB 3  
#define L_IN1 7
#define L_IN2 6
#define L_IN3 5
#define L_IN4 4

//-----------------------------------------mfrc522------------------------------------------
#define RST_PIN    9 
#define SS_PIN    10

//----------------------------------------bluetooth-----------------------------------------
#define rx 2  // 接收腳
#define tx 8  // 傳送腳
#define communicateRate 9600

//---------------------------------------PID control-----------------------------------------
#define kP 1.0
#define kI 0
#define kD 1.0
#define vB 75
#define vT 95


//-----------------------------------------globals------------------------------------------
// 讀卡的
MFRC522 *mfrc522;
SoftwareSerial BT(rx, tx);

// 引擎
wheel wheel_L(L_ENA, L_IN1, L_IN2);
wheel wheel_R(L_ENB, L_IN3, L_IN4);

// 循跡
String string = "";
char Forward  = '1';
char GoBack   = '2';
char Right    = '3';
char Left     = '4';
char Halt     = '5';

unsigned long t =0;
int i=0;






void MotorWriting(wheel wheel_L, wheel wheel_R, int vL, int vR)
{
    wheel_L.power(vL);
    wheel_R.power(vR);
}






bool RFID()
{
    if (mfrc522->PICC_IsNewCardPresent() && mfrc522->PICC_ReadCardSerial())
    {
        Serial.println(F("**Card Detected:**"));
        
        byte *id = mfrc522->uid.uidByte;   // 取得卡片的UID
        byte idSize = mfrc522->uid.size;
        
        for (byte i = 0; i < idSize; i++)  // 逐一顯示UID碼
        {
            Serial.print(F("id["));
            Serial.print(i);
            Serial.print(F("]: "));
            Serial.println(id[i], HEX);       // 以16進位顯示UID值
            BT.write(id[i]);                  // 把ID傳給python
        }
            
        Serial.println();
        
        mfrc522->PICC_HaltA();
        mfrc522->PCD_StopCrypto1();
        
        return true;
    }
    
    return false;
}






void Tracking()
{
    int l3 = digitalRead(A0);
    int l2 = digitalRead(A1);
    int  m = digitalRead(A2);
    int r2 = digitalRead(A3);
    int r3 = digitalRead(A4);
    
    int sum =l3 + l2 + m + r2 + r3;

    static double error =0;
    static double sumError =0;
    static double dError =0;
    static int vL;
    static int vR;
    static int k=0;
      
    if(sum == 0){
          MotorWriting(wheel_L,wheel_R,-vB,-vB);

          if(vL>0 && vR>0)
            k++;
          vL=-vB;
          vR=-vB;
          if(k>=5){
            
            MotorWriting(wheel_L,wheel_R,vB,vB);
            delay(150);
            
            for(int i =0, d =50; !sum; ++i)
            {
                int time =d*i;
    
                while(time > 0 && !(digitalRead(A2) || digitalRead(A1) || digitalRead(A3) ) )
                {
                  int l3 = digitalRead(A0);
                  int l2 = digitalRead(A1);
                  int  m = digitalRead(A2);
                  int r2 = digitalRead(A3);
                  int r3 = digitalRead(A4);
    
                  sum =l3 + l2 + m + r2 + r3;
    
                  i%2 ? MotorWriting(wheel_L, wheel_R, -95, 80) : MotorWriting(wheel_L, wheel_R, 80, -85);
                  delay(10);
    
                  time -=10;
                }
                k=0;
          }
       }
    }
    else if(sum >= 4 && (t == 0 || millis()-t > 950))
    {
         if(string[i] == Forward)
              t = millis();
              
          manual();
    }
    else
    {
         dError =-error;
         error =l3*-2 + l2*-1 + r2*1 + r3*2;
         error *=100/(sum*2);
         dError +=error;
         sumError +=error;

         if(error*dError>0)
         {
              vL =vB + kP*error + kD*dError + kI*sumError;
              vR =2*vB-vL;
         }
         else
         {
              vL =vB + kP*error + kI*sumError;
              vR =2*vB-vL;
         }
         
         
         MotorWriting(wheel_L,wheel_R,vL,vR);
    }
}






void manual()
{
    if(string[i] == Forward)
    {
    }
    else if(string[i] == Right)
    {
        delay(75);
        
        MotorWriting(wheel_L, wheel_R, -vB, -vB);
        /*if(i != 0 && string[i-1] == Forward)
            delay(200);
        else*/
            delay(40);
        
        MotorWriting(wheel_L, wheel_R, vT, -0.8*vT);
        delay(400);

      
        MotorWriting(wheel_L, wheel_R, vT, -0.8*vT);
        while(!(digitalRead(A4) || digitalRead(A3) || digitalRead(A2)));
    }
    else if(string[i] == Left)
    {
        delay(150);
        
          MotorWriting(wheel_L, wheel_R, -vB, -vB);
        /*if(i != 0 && string[i-1] == Forward)
            delay(200);
        else*/
            delay(40);
        
        MotorWriting(wheel_L, wheel_R, -1.2*vT, 1.4*vT);
        delay(350);

        while(!(digitalRead(A0) || digitalRead(A1) || digitalRead(A2)));
    }
    else if(string[i] == GoBack)
    {
        MotorWriting(wheel_L,wheel_R,0,0);           //在偵測到RFID前都要前進
        MotorWriting(wheel_L,wheel_R,-vB,-vB);

         
        if(i != 0 && string[i-1] == Forward){
            MotorWriting(wheel_L, wheel_R, -vB, -vB);
            delay(200);
        }
        int k=0;
        while(!RFID())
        {
            if(k<10){
              MotorWriting(wheel_L,wheel_R,0,0);           //在偵測到RFID前都要前進
              delay(50);
              MotorWriting(wheel_L,wheel_R,vB,vB);
              delay(50);
            }
            else{
              MotorWriting(wheel_L,wheel_R,0,0);           //在偵測到RFID前都要前進
              delay(50);
              MotorWriting(wheel_L,wheel_R,-1.3*vB,-1.3*vB);
              delay(50);
            }
            k++;
        }
        
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,-1.5*vT, vT);
        delay(500);
        
        while(!digitalRead(A2));
    }
    else if(string[i] == Halt)
    {
        MotorWriting(wheel_L,wheel_R,0,0);
        
        while(1);
    }
   
    ++i;
}






void setup()
{
    //藍芽
    BT.begin(communicateRate);
    Serial.begin(communicateRate);
    Serial.println(F("BT is ready!"));

    //讀卡
    SPI.begin();
    mfrc522 =new MFRC522(SS_PIN, RST_PIN);
    mfrc522->PCD_Init();
    Serial.println(F("Read UID on a MIFARE PICC:"));
    
    
    int length =0;
    
    while(length == 0 || string[length-1] != 's')    
    { 
        if (BT.available())
        {
            char nxt =BT.read();
            string +=nxt;
            length++;
        }
    }

    Serial.print(string);

    
}

void loop()
{
    
    Tracking();
    
}