#include <Servo.h>



#define echoPin 9 // Echo Pin
#define trigPin 10 // Trigger Pin
#define echoPinleft 12 // Echo Pin
#define trigPinleft 13 // Trigger Pin
#define echoPinright 7 // Echo Pin
#define trigPinright 8 // Trigger Pin
#define L2 5
#define L1 11
#define L3 3
#define L4 6
#define M 30
#define minDist 3

Servo myservo;

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPinleft, OUTPUT);
  pinMode(echoPinleft, INPUT);
  pinMode(trigPinright, OUTPUT);
  pinMode(echoPinright, INPUT);
  pinMode (L1,OUTPUT);
  pinMode (L2,OUTPUT);
  pinMode (L3,OUTPUT);
  pinMode (L4,OUTPUT);

  myservo.attach(4);
  myservo.write(90);
}

double get_f(){
 digitalWrite(trigPin, LOW);
 delayMicroseconds(2);

 digitalWrite(trigPin, HIGH);
 delayMicroseconds(10);

 digitalWrite(trigPin, LOW);
 int duration = pulseIn(echoPin, HIGH);

 //Calculate the distance (in cm) based on the speed of sound.
 int distance = duration/58.2;

 return distance;
}

double get_r(){
 digitalWrite(trigPinright, LOW);
 delayMicroseconds(2);

 digitalWrite(trigPinright, HIGH);
 delayMicroseconds(10);

 digitalWrite(trigPinright, LOW);
 int durationr = pulseIn(echoPinright, HIGH);

 //Calculate the distance (in cm) based on the speed of sound.
 int distancer = durationr/58.2;

 return distancer;
}

double get_l(){
 digitalWrite(trigPinleft, LOW);
 delayMicroseconds(2);

 digitalWrite(trigPinleft, HIGH);
 delayMicroseconds(10);

 digitalWrite(trigPinleft, LOW);
 int durationl = pulseIn(echoPinleft, HIGH);

 //Calculate the distance (in cm) based on the speed of sound.
 int distancel = durationl/58.2;

 return distancel;
}

int generate(int row1 , int col1){
   if(row1>0x08){
      int nr = row1 - 0x09;
      row1 = 0x08 - nr;
   }

   if(col1>0x08){
      int ncc = col1 - 0x09;
      col1 = 0x08 - ncc;
   }



   return (0x0f - (col1-0x01) - (row1 - 0x01));
}

void robMove(int l1, int l2, int r1, int r2)
{
digitalWrite(L1, l1);
digitalWrite(L3, r1);
digitalWrite(L2, l2);
digitalWrite(L4, r2);
}

void reverse(){
  robMove(LOW , LOW , LOW , LOW);
  delay(1000);

  robMove(HIGH , LOW , LOW , HIGH);
  delay(800);

  robMove(HIGH , LOW , HIGH , LOW);
}

void lturn(){
  robMove(LOW , LOW , LOW , LOW);
  delay(1000);

  robMove(LOW , LOW , HIGH , LOW);
  delay(700);
}

void rturn(){
  robMove(LOW , LOW , LOW , LOW);
  delay(1000);

  robMove(HIGH , LOW , LOW , LOW);
  delay(700);
}

void loop() {


  Serial.print("right--> ");Serial.println(get_r());
 Serial.print("left--> "); Serial.println(get_l());
 Serial.print("front--> "); Serial.println(get_f());

  //robMove(HIGH, LOW, HIGH, LOW);
  /*robMove(HIGH, LOW, HIGH, LOW);
  while(get_l() - get_r() > (double)minDist) robMove(LOW, LOW, HIGH, LOW);
  robMove(HIGH, LOW, HIGH, LOW);
  while(get_r() - get_l() > (double)minDist) robMove(HIGH, LOW, LOW, LOW);
  */

analogWrite(L3 , 50);
analogWrite(L1 , 55);
delay(2000);
analogWrite(L1 , 0);
analogWrite(L3 , 0);
delay(2000);
//return;
}