# 看有無轉彎

#include <SoftwareSerial.h>
#include <math.h>
double kP = 1.3;
double kI = 0.03;
double kD = 2.5;
int vB = 80;
int counter = 0;

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); // 接收腳, 傳送腳

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

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;
    }
    if(counter >= 150){
    sumError = 0;
    counter = 0;
  }
  else{
    counter += 1;
  }
    
    MotorWriting(wheel_L,wheel_R,vL,vR*0.9);
  }
  else
  {
    MotorWriting(wheel_L,wheel_R,-150,-150*0.9);
  }

translate(error);
translate(dError);
translate(sumError);

BT.write("\n\n\n");
    
  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(20);
}