# PID control

#include <SoftwareSerial.h>
class wheel
{
  const int EN;
  const int IN1;
  const int IN2;

 public:
  wheel(int, int, int);
  wheel power(int);
};

wheel wheel::power(int P)
{
  if(P > 255)
    P =255;
   else if(P < -255)
    P =-255;

  if(P >= 0){//go straight Or Stop
    analogWrite(EN, P);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  }
  else{//go back
    analogWrite(EN, -P);
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  }
    
  return *this;
}

wheel::wheel(int a, int b, int c)
  :EN(a), IN1(b), IN2(c)
{
   pinMode(EN, OUTPUT);
   pinMode(IN1, OUTPUT);
   pinMode(IN2, OUTPUT);
}

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


#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); // 接收腳, 傳送腳
char val;  // 儲存接收資料的變數

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


void MotorCheck(){
  //forward
  MotorWriting(wheel_L, wheel_R, 100, 100);
  delay(1500);
  //stop
  MotorWriting(wheel_L, wheel_R, 0, 0);
  delay(1500);
  //back
  MotorWriting(wheel_L, wheel_R, -100, -100);
  delay(1500);
  //turn right
  MotorWriting(wheel_L, wheel_R, 100, -60);
  delay(1500);
  //turn left
  MotorWriting(wheel_L, wheel_R, -60, 100);
  delay(1500);
}

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 error=l3*-2 + l2*-1 + r2*1 + r3*2;
  int sum=l3 + l2 + m + r2 + r3;
  
  if(sum!=0){
    error *= 100/(sum*2);
    double dError=3*(error-last);
    sumError+=0.1*error;
    int vL = 80 + 1.2*error + dError + 0.1*sumError;
    int vR = 80 - 1.2*error - dError - 0.1*sumError;

 /*   if (Serial.available()) {
      val = Serial.read();
    BT.write(val);
    BT.write(error);
    delay(50);
    BT.write(dError);
    delay(50);
    BT.write(sumError);
    delay(50);
    BT.write(" ");
    }*/
    
    Serial.println(error);
    Serial.println(dError);
    Serial.println(sumError);
    Serial.println(" ");
    MotorWriting(wheel_L,wheel_R,vL,vR);
  }
  else
  {
    MotorWriting(wheel_L,wheel_R,-150,-150
    );
  }

  last =error;
  
  return error;
  
}

void setup() {
   Serial.begin(9600);
   BT.begin(9600);  
   Serial.println("BT is ready!");
}

double last;

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

  last =current;

  delay(10);
}