<h1>PD Control</h1>

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

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;
  
  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=150*(error-last);
    int vL = 100 + error + dError;
    int vR = 100 - error - dError;

  MotorWriting(wheel_L,wheel_R,vL,vR);
  }
  else
  {
    MotorWriting(wheel_L,wheel_R,-150,-150
    );
  }

  last =error;
  
  return error;
  
}

void setup() {
  //MotorCheck();

}

double last;

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

  
  
  last =current;

  delay(10);
}