# 4/29-30

arduino=
#include <SoftwareSerial.h>
#include <SPI.h>
#include <MFRC522.h>
#include <time.h>
#include "wheel.h"

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

SoftwareSerial BT(2, 8); // 接收腳, 傳送腳

#define RST_PIN    9 
#define SS_PIN    10

MFRC522 *mfrc522;

double kP = 0.9;
double kI = 0.0;
double kD = 1.0;
int vB = 70;
int counter = 0;

String string = "11143432115\0";
char val;
char Forward = '1';
char GoBack = '2';
char Right = '3';
char Left = '4';
char Halt = '5';

bool flag =true;
unsigned long t =0;
static int i=0;

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

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

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]);
    }
    Serial.println();

    mfrc522->PICC_HaltA();
    mfrc522->PCD_StopCrypto1();
    return true;
    }
  return false;
}

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

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

    if (BT.available()) {
      val = BT.read();
      string += val;
    }
    
   
    if(error >= 150){
    sumError = 0;
    counter = 0;
    }
    else{
      counter += 1;
    }
    
    MotorWriting(wheel_L,wheel_R,vL,vR);
  }

  last =error;
  
  return error;
}

void manual()
{
        
    if(string[i] == '\0'){
      while(1)
      {MotorWriting(wheel_L,wheel_R,0,0);
      delay(10000);}
      
    }
    
      if(string[i] != Forward){

       MotorWriting(wheel_L,wheel_R,0,0);
       delay(300);
      }
      
       if(string[i] == Right){
   /*      MotorWriting(wheel_L,wheel_R,0,0);
   delay(500);
   MotorWriting(wheel_L,wheel_R,-80,-80);
   delay(350);
   MotorWriting(wheel_L,wheel_R,0,0);
   delay(100);*/
   
  MotorWriting(wheel_L,wheel_R,0,0);
   delay(500);
   MotorWriting(wheel_L,wheel_R,200,0);
   delay(280);
   MotorWriting(wheel_L,wheel_R,200,0);
   delay(100);

   /*MotorWriting(wheel_L,wheel_R,120,-180);
   delay(600);
   MotorWriting(wheel_L,wheel_R,-180,120);
   delay(50);
    MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,70,70);
         delay(500);*/
        /*MotorWriting(wheel_L,wheel_R,80,80);
        delay(100);*/
        
      }
      else if(string[i] == Left){
   /*MotorWriting(wheel_L,wheel_R,0,0);
   delay(500);
   MotorWriting(wheel_L,wheel_R,-80,-80);
   delay(80);
   MotorWriting(wheel_L,wheel_R,0,0);
   delay(100);*/
   MotorWriting(wheel_L,wheel_R,0,0);
   delay(500);
   MotorWriting(wheel_L,wheel_R,0,200);
   delay(280);
   MotorWriting(wheel_L,wheel_R,0,200);
   delay(100);

   /*MotorWriting(wheel_L,wheel_R,-180,120);
   delay(600);
   MotorWriting(wheel_L,wheel_R,120,-180);
   delay(50);
   MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,70,70);
         delay(500);*/
        /*MotorWriting(wheel_L,wheel_R,80,80);
        delay(100);*/
        
      }
      else if(string[i] == GoBack){
        while(!RFID()){
          MotorWriting(wheel_L,wheel_R,0,0);
          
          MotorWriting(wheel_L,wheel_R,70,70);
          
        }
        
        MotorWriting(wheel_L,wheel_R,-180,120);
        delay(680);
        MotorWriting(wheel_L,wheel_R,120,-180);
        delay(50);
   
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(500);
        MotorWriting(wheel_L,wheel_R,70,70);
         delay(500);
        /*MotorWriting(wheel_L,wheel_R,80,80);
        delay(100);*/
        
      }
      else if(string[i] == Halt){
        MotorWriting(wheel_L,wheel_R,0,0);
        delay(800);
      }
      
      ///////
      BT.write(i);
      
      //lastTime =millis();
      i++;
}

void setup() {
   Serial.begin(9600);
   BT.begin(9600);  
   Serial.println("BT is ready!");
    
   SPI.begin();
   mfrc522 = new MFRC522(SS_PIN, RST_PIN);
   mfrc522->PCD_Init();
   Serial.println(F("Read UID on a MIFARE PICC:"));
}

double last;
bool start = false;

void loop() {
  
    double current =Tracking();
    last =current;
   /*MotorWriting(wheel_L,wheel_R,-180,120);
   delay(680);
   MotorWriting(wheel_L,wheel_R,120,-180);
   delay(50);
   
   MotorWriting(wheel_L,wheel_R,0,0);
   delay(1000);*/
}