#include <Servo.h>

Servo myservo1,myservo2;
int sensor_pin[]={2,3,4,5,6,7};
int sensor_read;
int inp,u;
int outval[]={0,0,0,0,0,0};
int base_speed=90;
int lockStart=0,lockStop=0;
int mode=0;
int rightPWMValue = 0;
int leftPWMValue = 0;
//====================== PID Parameter Configuration ==============
int err;
int sumerr;
int deverr;
int lasterr = 0;
int kp, ki, kd;
int Setp= 59;
//=================================================================
int Start,Stop,Runstate=0;
int PID(int Acuann, int OutSense)
{
  kp = 1;ki = 0;kd = 0.5;
  err = Acuann - OutSense;
  sumerr += err;
  if(sumerr>1000 || sumerr > -1000)
  {
    sumerr=0;
  }
  deverr = err - lasterr;
  lasterr = err;

  return (kp*err)+(ki*sumerr)+(kd*deverr);
}
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(38, INPUT_PULLUP); 
  pinMode(40, INPUT_PULLUP); 
  myservo1.attach(9);
  myservo2.attach(10);
  myservo1.write(90);
  myservo2.write(90);
  

  int a;
  //setting pin sensor sebagai INPUT
   while(a<6){
    pinMode(sensor_pin[a],INPUT_PULLUP);
    a=a+1;
   } 

}

void loop() {
  // put your main code here, to run repeatedly:
  Start=   digitalRead(40);
  Stop=   digitalRead(38);

  sensor_read=outSense(sensor_pin);
  if(sensor_read==62)
  {
    sensor_read=87;
  }
  
  if(sensor_read==61)
  {
    sensor_read=71;
  }

  if (Start==0 && lockStart==0){lockStart=1;}
  if (Start!=0 && lockStart==1){lockStart=0; mode++;}
  if (Stop==0 && lockStop==0){lockStop=1;}
  if (Stop!=0 && lockStop==1){lockStop=0; mode--;}

  if(mode>1)
  {
    mode=1;
  }
  if(mode<0)
  {
    mode=0;

  }

  switch (mode)
  {
    case 0 : myservo1.write(90);
             myservo2.write(90);Runstate=0;break;
    case 1 : Runstate=1;break; 
  }  

  if(Runstate==1)
  {
    if(sensor_read==0 || sensor_read==63)
    {
      mode=0;
    }
    u=PID(Setp, sensor_read);
    rightPWMValue = base_speed + u;
    leftPWMValue = base_speed - u;
    myservo1.write(leftPWMValue);
    myservo2.write(rightPWMValue);
  }

  Serial.print(outval[0]);
  Serial.print(outval[1]);
  Serial.print(outval[2]);
  Serial.print(outval[3]);
  Serial.print(outval[4]);
  Serial.print(outval[5]);
  Serial.print(";  ");
  Serial.print(sensor_read);
  Serial.print(";");
  Serial.print(err);
  Serial.print(";");
  Serial.print(sumerr);
  Serial.print(";");
  Serial.print(deverr);
  Serial.print(";");
  Serial.print(rightPWMValue);
  Serial.print(";");
  Serial.println(leftPWMValue);

}

int outSense(int sensor_pin[]){
   int a,b,c=0;
   inp=0;
   while(c<6){
    outval[c]=0;
    c=c+1;
   }
   for(a=0;a<6;a++){
     outval[a]=digitalRead(sensor_pin[a]);
     
   }
   inp=(outval[5]*pow(2,5))+(outval[4]*pow(2,4))+(outval[3]*pow(2,3))+(outval[2]*pow(2,2))+(outval[1]*pow(2,1))+(outval[0]*pow(2,0));
  //  while(b<6){
  //    inp=(inp*10)+hasil_baca[b];
  //    b=b+1;
  //  }
   
  return inp;
}