int RPWM_L=5;
int LPWM_L=6;
int EN_L=7;
int EN_R=8;
int RPWM_R=9;
int LPWM_R=10;

int recurrence = 10;
long errI_L = 0;
int err_L = 0, errPre_L = 0;
long errI_R = 0;
int err_R = 0, errPre_R = 0;

int target_L[4] = {0,0,0,0};
int target_R[4] = {0,0,0,0};
int center_L = 530;
int center_R = 560;

int count = 0;
int targetCnt = 0;

float targetPre_L = center_L;
float targetPre_R = center_R;


void setup() {
  for(int i=5;i<=10;i++){
    pinMode(i,OUTPUT);
    digitalWrite(i,LOW);
  }
  Serial.begin(115200);

  for(int i=0;i<4;i++){
    target_L[i] += center_L;
    target_R[i] += center_R;
  }
  
  targetPre_L = target_L[0];
  targetPre_R = target_R[0];
  
  TCCR0B = TCCR0B & 0b11111000 | 0x02;
  TCCR1B = TCCR1B & 0b11111000 | 0x02;
  TCCR2B = TCCR2B & 0b11111000 | 0x02;
}



void loop() {
  int sig_L,sig_R;
  int duty_L, duty_R;
  long errP_L = 0, errD_L = 0;
  long errP_R = 0, errD_R = 0;
  float Kp = 8 , Ki = 0.008, Kd = 2;
  float targetFlt_L,targetFlt_R;
  float a=0.02; // target filtering


  sig_L = analogRead(A3); //L
  sig_R = analogRead(A0); //R  

  targetFlt_L = a*float(target_L[targetCnt]) + (1-a)*float(targetPre_L);
  targetFlt_R = a*float(target_R[targetCnt]) + (1-a)*float(targetPre_R);
  targetPre_L = targetFlt_L;
  targetPre_R = targetFlt_R;
  
  err_L = targetFlt_L - sig_L;
  err_R = targetFlt_R - sig_R;
  //err = 512 - sig;
  
  errP_L = err_L;
  errP_R = err_R;
  errI_L += err_L;
  errI_R += err_R;
  errD_L = err_L - errPre_L;
  errD_R = err_R - errPre_R;

  errPre_L = err_L;
  errPre_R = err_R;

  if(errI_L>500000) errI_L = 500000;
  if(errI_L<-500000) errI_L = -500000;

  if(errI_R>500000) errI_R = 500000;
  if(errI_R<-500000) errI_R = -500000;

  
  duty_L = int(Kp*float(errP_L) + Ki*float(errI_L)+Kd*float(errD_L));
  duty_R = int(Kp*float(errP_R) + Ki*float(errI_R)+Kd*float(errD_R));

  if(duty_L>255) duty_L = 255;
  else if(duty_L <-255) duty_L = -255;

  if(duty_R>255) duty_R = 255;
  else if(duty_R <-255) duty_R = -255;

  digitalWrite(EN_L,HIGH);
  digitalWrite(EN_R,HIGH);  

  
  if(duty_L<0){
    analogWrite(LPWM_L,-1*duty_L);
    analogWrite(RPWM_L,0);
  }else{
    analogWrite(RPWM_L,duty_L);
    analogWrite(LPWM_L,0);
  }

  if(duty_R<0){
    analogWrite(LPWM_R,-1*duty_R);
    analogWrite(RPWM_R,0);
  }else{
    analogWrite(RPWM_R,duty_R);
    analogWrite(LPWM_R,0);
  }
  
  count++;
  if(count%500 == 0){
    count = 0;
    targetCnt++;
    if(targetCnt>3){
      targetCnt = 0;
    }
  }
  
  Serial.print(targetFlt_L);
  Serial.print('\t');
  Serial.print(sig_L);
  Serial.print('\t');
  Serial.print(duty_L);
  Serial.print('\t');
  Serial.print(targetFlt_R);
  Serial.print('\t');
  Serial.print(sig_R);
  Serial.print('\t');
  Serial.print(duty_R);
  Serial.print('\n');
  
  delay(6);
}