#include <Ultrasonic.h>

#define DIR_L_A 8
#define DIR_L_B 7
#define ENB_L 5

#define DIR_R_A 11
#define DIR_R_B 9
#define ENB_R 6

Ultrasonic frontSensor(A5, A4);
Ultrasonic leftSensor(A3, A2, 40000UL);

int distance;

void setup() {
  pinMode(DIR_R_A, OUTPUT);
  pinMode(DIR_R_B, OUTPUT);
  pinMode(ENB_R, OUTPUT);
  pinMode(DIR_L_A, OUTPUT);
  pinMode(DIR_L_B, OUTPUT);
  pinMode(ENB_L, OUTPUT);
  Serial.begin(9600);
  analogWrite(ENB_L, 255);
  analogWrite(ENB_R, 255);
}

void loop() {
  // Pass INC as a parameter to get the distance in inches





  //Serial.print("Front Distance in CM: ");
  //Serial.println(distance);

  distance = leftSensor.read();
  Serial.print("Left Distance in CM: ");
  Serial.println(distance);

  forward();
  if(distance > 250){
    analogWrite(ENB_L, 200);
    analogWrite(ENB_R, 255);
  }
  if(distance < 250){
    analogWrite(ENB_L, 255);
    analogWrite(ENB_R, 200);
  }
}


void forward(){
  digitalWrite(DIR_L_A, LOW);
  digitalWrite(DIR_L_B, HIGH);
  digitalWrite(DIR_R_A, LOW);
  digitalWrite(DIR_R_B, HIGH);
}

void stop(){
  digitalWrite(DIR_L_A, LOW);
  digitalWrite(DIR_L_B, LOW);
  digitalWrite(DIR_R_A, LOW);
  digitalWrite(DIR_R_B, LOW);
}

void turnRight(){
  digitalWrite(DIR_L_A, LOW);
  digitalWrite(DIR_L_B, HIGH);
  digitalWrite(DIR_R_A, HIGH);
  digitalWrite(DIR_R_B, LOW);
  delay(220);
  stop();
}

void turnLeft(){
  digitalWrite(DIR_R_A, LOW);
  digitalWrite(DIR_R_B, HIGH);
  digitalWrite(DIR_L_A, HIGH);
  digitalWrite(DIR_L_B, LOW);
  delay(200);
  stop();
}