#include <Servo.h> 

int SensR(A4),SensL(A2),SensF(A3),White(A5);
int sensR(1), sensL(1), sensF(1), error(0);   // Store the value of the reading
Servo myservoL, myservoR;
const float CALIB(0.08),CALIBFRONT(0.04);    // Measuringtime
const int Ts(100), TRESH(20);    // Samplingtime
int KpLeft(5), KpRight(2);
const int velocity(-30),ZEROLEFT(93),ZERORIGHT(96); // We should calibrate the zeros of the servos each time with ServoControl sketch
float Phot(0);

void setup()
{
        Serial.begin(9600);  //set the Baud Rate
        myservoR.attach(11);
        myservoL.attach(10);

}

void loop()
{
            Phot=analogRead(White)/1023.0*5;
            
            Serial.println(Phot);
            if((Phot)>2.50){
            myservoR.write(ZERORIGHT);
            myservoL.write(ZEROLEFT);        
            }
            else{
                            
              
                // Reading the IR sensors
                sensR=1023.0/(5*CALIB*analogRead(SensR))-0.42;// cm
                sensL=1023.0/(5*CALIB*analogRead(SensL))-0.42;// cm
                sensF=1023.0/(5*CALIBFRONT*analogRead(SensF))-0.42;// cm
                  
                  // To avoid crazy behaviour we limit the measurements to a maximum value
                  if(sensL>TRESH){
                  sensL=TRESH;
                  }
                  else if(sensR>TRESH){
                  sensR=TRESH;
                  }
                  
                
                // Calculating the error
                error=sensL-sensR;
                
                
                // Responsible for turning behaviour if there's an obstacle in front
                    if(sensF<=15){
                      if(error<0){
                        myservoR.write(150);
                        myservoL.write(40);
                        delay(50);
                      }
                      else
                      {
                        myservoR.write(40);
                        myservoL.write(150);
                        delay(50);              
                      }
                    }
                
                
                 
              
                  // Driving forward with feedback. Note that the proportional gains are changed in the case the error is small.
                  if(abs(error)>10){
                  KpLeft=2;
                  KpRight=2;
                  }
                  else{
                  KpLeft=1;
                  KpRight=2;
                  }
                  
                  if(abs(error)>5){
                    myservoR.write(ZERORIGHT+velocity-KpRight*error);
                    myservoL.write(ZEROLEFT+velocity+KpLeft*error);
          
                  }
                  else{
                    myservoR.write(ZERORIGHT+velocity);
                    myservoL.write(ZEROLEFT+velocity);
                  }
                    
                    

                    

            }
            
            
            
            
    
    
        //delay(500);      // Take this as sampling time and assume the lines above are much smaller//  
        
        //myservoR.write(75);
        //myservoL.write(65);
        //delay(2000);
        
  
//delay(5000); // 5s time to regulate the right position


// almost forward driving forward 1 65, 2 75
//delay(5000);
//myservo.write(0);
//delay(5000);
}