# 節點控制 RFID

#include <SoftwareSerial.h>
#include <SPI.h>
#include <MFRC522.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 = 1.4;
double kI = 0.02;
double kD = 5;
int vB = 80;
int counter = 0;

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

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:**"));
    mfrc522->PICC_DumpDetailsToSerial(&(mfrc522->uid));
    mfrc522->PICC_HaltA();
    mfrc522->PCD_StopCrypto1();
    return true;
    }
  return false;
}

/*void translate(double target)
{
int output =target < 0 ? -1*target : target;
int output2 =target < 0 ? int(-100*target)%100 : int(100*target)%100;

Serial.println(target);

if(target < 0)
BT.write('-');

int temp[10] ={};

int i =0;
for(; output > 0; ++i)
{
  temp[i] +=output%10;
  output /=10;
}

for(;i >= 0; --i)
  BT.write(temp[i]+48);
  
  BT.write('.');
  BT.write(output2/10+48); 
  BT.write(output2%10+48);
  BT.write("\n");
}*/

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;
  
  if(sum!=0){
    error *= 100/(sum*2);
    dError=(error-last);
    sumError += error;
    int vL=0,vR=0;
    /*if(error<15 || error>-15){
      vL = vB + 1*error ;//+ 3*dError;
      vR = vB - 1*error ;//- 3*dError;
    }
    else{
    vL = vB + kP*error + kD*dError + kI*sumError;
    vR = vB - kP*error - kD*dError - kI*sumError;
    }*/
    vL = vB + kP*error + kD*dError + kI*sumError;
    vR = vB - kP*error - kD*dError - kI*sumError;

    if (BT.available()) {
      val = BT.read();
      string += val;
    }
    
    static int i=0;
    if(sum == 5){
      if(string[i] == Forward){
        MotorWriting(wheel_L,wheel_R,80,80);
        delay(800);
      }
      if(string[i] == Right){
        MotorWriting(wheel_L,wheel_R,125,-75);
        delay(800);
      }
      if(string[i] == Left){
        MotorWriting(wheel_L,wheel_R,-75,125);
        delay(800);
      }
      if(string[i] == GoBack){
        while(!RFID()){
          MotorWriting(wheel_L,wheel_R,50,50);
        }
        
        MotorWriting(wheel_L,wheel_R,125,-125);
        delay(800);
      }

      i++;
    }


    
    if(error >= 150){
    //sumError = 0;
    counter = 0;
    }
    else{
      counter += 1;
    }
    
    MotorWriting(wheel_L,wheel_R,vL,vR);
  }
  else
  {
    MotorWriting(wheel_L,wheel_R,-75,-75);
    sumError = 0;
    
  }

  /*translate(error);
  translate(dError);
  translate(0.2*sumError);
  
  BT.write("\n\n\n");*/

    
  last =error;
  
  return error;
  
}

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;

void loop() {
  
  double current =Tracking();
  
  last =current;

}