<h1>參考 4/30</h1>

<ul>
<li>那個 tracking 的直行條件有改</li>
<li>改一下轉彎方式(轉一點再轉到黑線)</li>
<li>刪掉一些沒用的變數??</li>
<li>盡量用 define，scope 盡量縮小</li>
<li>括號沒佔一行，括號表示難過</li>
<li>其實最後不用 '\0' ??</li>
</ul>

#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 0.9;
#define kI 0.0;
#define kD 1.0;
#define vB 75;


//-----------------------------------------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 = "11143432115";
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;
      
    if(sum == 0)
        MotorWriting(wheel_L,wheel_R,-75,-75);
    else if(sum >= 4 && string[i] != Forward)
        manual();
    else
    {
         if(sum >= 4 && string[i] == Forward && (t == 0 || millis()-t > 500))
         {
             t = millis();
             ++i;
         }
    
         static double error =0;
         static double sumError =0;
         double dError =-error;
    
         error =l3*-2 + l2*-1 + r2*1 + r3*2;
         error *=100/(sum*2);
         dError +=error;
         sumError +=error;
                 
         int vL =vB + kP*error + kD*dError + kI*sumError;
         int vR =2*vB-vL;
         
         MotorWriting(wheel_L,wheel_R,vL,vR);
    }
}






void manual()
{
    if(string[i] == '\0');
    else if(string[i] != Forward)
    {
        MotorWriting(wheel_L,wheel_R,0,0);      //先停一下
        delay(300);
    }
    else if(string[i] == Right)
    {
        delay(200);
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,200,0);
        delay(100);
        
        while(!digitalRead(A0));
    }
    else if(string[i] == Left)
    {
        delay(200);
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,0,200);
        delay(100);
        
        while(!digitalRead(A0));
    }
    else if(string[i] == GoBack)
    {
        while(!RFID())
        {
            MotorWriting(wheel_L,wheel_R,0,0);           //在偵測到RFID前都要前進
            MotorWriting(wheel_L,wheel_R,70,70);
        }
        
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,0,200);
        delay(300);
        
        while(!digitalRead(A0));
    }
    else if(string[i] == Halt)
    {
        MotorWriting(wheel_L,wheel_R,0,0);
        
        while(1);
    }

    //BT.write(i);
    ++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(string[length-1] != Halt)    ////////////racing??
    {
        length =sizeof(string.size);
        
        if (BT.available())
        {
            char nxt =BT.read();
            string +=nxt;
        }
    }
    */
}

void loop()
{
    static bool start =false;
    
    if(start == true)
        double current =Tracking();
    else if (BT.available() && BT.read() == 's')
        start = true;
}