int valL = 0;       // variable to store the value coming from the sensor
int valR = 0;
int valC = 0;

int forward = 103; // Set forward speed
int reverse = 82; // Set reverse speed

int sens0 =0;//Used by the Sharp

#include <Servo.h>

Servo servo3; //Sensor Servo
Servo servo2; //Turning
Servo servo1; //ESC



void setup() {

  pinMode(1,OUTPUT);
  servo3.attach(11); //Sensor Servo
  servo2.attach(10); //Turning
  servo1.attach(12); //ESC
  Serial.begin(115200);
  Serial.print("Ready");
  servo1.write(90); // Center servo
  servo3.write(90); // Center servo
  delay(4000); // wait for ESC


}

void loop() {

  //Move servos and take two readings each in right, left and center 

  servo3.write(150); 
  //Serial.println("move right!");
  delay(200); //wait for servo do arrive
  float Rvolts = analogRead(sens0)*0.0048828125;
  float valR = 65*pow(Rvolts, -1.10);          // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
  float R2volts = analogRead(sens0)*0.0048828125;
  float valR2 = 65*pow(Rvolts, -1.10);          // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
  //  Serial.print("ValR=");  // Print "ValR=" to serial
  //  Serial.println(valR);   // Print  the value of valR to serial and line feed
  //  Serial.print("ValR2="); // Print "ValR2=" to serial
  //  Serial.println(valR2);  // Print the value of valR2 to serial and line feed


  servo3.write(90);
  //Serial.println("move center!");  
  delay(200); //wait for servo do arrive
  float Cvolts = analogRead(sens0)*0.0048828125;   // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
  float valC = 65*pow(Cvolts, -1.10);          // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
  float C2volts = analogRead(sens0)*0.0048828125;   // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
  float valC2 = 65*pow(C2volts, -1.10);          // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
  //  Serial.print("ValC=");   // Print "ValC=" to serial
  //  Serial.println(valC);    // Print the value of valC to serial and line feed
  //  Serial.print("ValC2=");  // Print "Val2=" to serial
  //  Serial.println(valC2);   // Print the value of valC2 to serial and line feed


  servo3.write(40);
  // Serial.println("move left!");
  delay(200); //wait for servo do arrive
  float Lvolts = analogRead(sens0)*0.0048828125;
  float valL = 65*pow(Lvolts, -1.10);          // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk
  float L2volts = analogRead(sens0)*0.0048828125;
  float valL2 = 65*pow(L2volts, -1.10);          // worked out from graph 65 = theretical distance / (1/Volts)S - luckylarry.co.uk


  //  Serial.print("ValL=");   // Print "ValL=" to serial
  //  Serial.println(valL);    // Print the value of valL to serial and line feed
  //  Serial.print("ValL2=");  // Print "ValL=" to serial
  //  Serial.println(valL2);   // Print the value of valL2 to serial and line feed  



  // Sum the val and val2 up 

  valC += valC2;
  valR += valR2;
  valL += valL2;

  //Serial.println(""); 
  //Serial.println("*****Totals*****");  
  //Serial.print("Total valC=");  
  //Serial.println(valC);                       
  //Serial.print("Total valR=");  
  //Serial.println(valR);                 
  //Serial.print("Total valL=");  
  //Serial.println(valL);    
  //Serial.println("*****Totals*****");   
  //Serial.println("");

  //Devide the new val:s by two 

  valC /= 2;
  valR /= 2;
  valL /= 2;

  // Print calcutated distance to serial.

  Serial.println(""); 
  Serial.println("*****Divided by two*****");  
  Serial.print("Divided by two valC=");  
  Serial.println(valC);                       
  Serial.print("Divided by two valR=");  
  Serial.println(valR);                       
  Serial.print("Divided by two valL=");  
  Serial.println(valL);    
  Serial.println("*****Divided by two*****");   
  Serial.println("");




  if (valC < 55 || valL < 55 || valR < 55){  // If wall detected on right, left or center do stuff below

    //stop!!! 
    Serial.println("problem ahead. Car stop and right-turn" );
    servo1.write(90); // Stop car
    delay(500);
    servo2.write(30); //Turn wheels right
    servo1.write(reverse); //Reverse
    delay(1200);
    servo2.write(87); //straight wheels
    servo1.write(90); //stop reversing
    delay(500);

  }
  else{
    //continue forward
    Serial.println("Car forward" );
    servo1.write(forward);
  }
}