#include <Ultrasonic.h>

int leftenable=3;
int leftforward=2;
int leftbackward=4;

int rightenable=6;
int rightforward=5;
int rightbackward=7;

int trig1=8;
int echo1=9;
Ultrasonic  leftultrasonic(trig1,echo1,5000UL);

int trig2=10;
int echo2=11;
Ultrasonic  frontultrasonic(trig1,echo1,5000UL);

int trig3=12;
int echo3=13;
Ultrasonic  rightultrasonic(trig1,echo1,5000UL);

int objectdistance=20;
int runtime=500;
int ptolarance=5;
int ntolarance=-5;
int maxleftspeed=255;
int maxrightspeed=255;

int leftdistance;
int frontdistance;
int rightdistance;
int totaldistance;
int difference;
int leftspeed;
int rightspeed;

void setup() {
  // put your setup code here, to run once:
  pinMode(leftforward, OUTPUT);
  pinMode(rightforward, OUTPUT);
  pinMode(leftbackward, OUTPUT);
  pinMode(rightbackward, OUTPUT);
  frontdistance=frontultrasonic.read();  
}

void forward()
{
  analogWrite(leftenable,maxleftspeed);
  analogWrite(rightenable,maxrightspeed);
  digitalWrite(leftforward,HIGH);
  digitalWrite(leftbackward,LOW);
  digitalWrite(rightforward,HIGH);
  digitalWrite(rightbackward,LOW);
}

void left()
{
  analogWrite(leftenable,maxleftspeed);
  analogWrite(rightenable,maxrightspeed);
  digitalWrite(leftforward,LOW);
  digitalWrite(leftbackward,HIGH);
  digitalWrite(rightforward,HIGH);
  digitalWrite(rightbackward,LOW);
  delay(runtime);
  digitalWrite(leftforward,HIGH);
  digitalWrite(leftbackward,LOW);
  digitalWrite(rightforward,HIGH);
  digitalWrite(rightbackward,LOW);
}

void right()
{
  analogWrite(leftenable,maxleftspeed);
  analogWrite(rightenable,maxrightspeed);
  digitalWrite(leftforward,HIGH);
  digitalWrite(leftbackward,LOW);
  digitalWrite(rightforward,LOW);
  digitalWrite(rightbackward,HIGH);
  delay(runtime);
  digitalWrite(leftforward,HIGH);
  digitalWrite(leftbackward,LOW);
  digitalWrite(rightforward,HIGH);
  digitalWrite(rightbackward,LOW);
}

void stop()
{
  digitalWrite(leftforward,LOW);
  digitalWrite(leftbackward,LOW);
  digitalWrite(rightforward,LOW);
  digitalWrite(rightbackward,LOW);
}

void loop() {
  // put your main code here, to run repeatedly:
  if(frontdistance<=objectdistance)
  {
    stop();
    leftdistance=leftultrasonic.read();
    rightdistance=rightultrasonic.read();
    if(leftdistance>frontdistance)
    {
      left();
    }
    else if(rightdistance>frontdistance)
    {
      right();
    }
    else
    {
      stop();
    }
  }
  else
  {
    forward();
    delay(runtime);
    leftdistance=leftultrasonic.read();
    rightdistance=rightultrasonic.read();
    totaldistance=leftdistance+rightdistance;
    difference=leftdistance-rightdistance;
    if(difference<=ptolarance&&difference>=ntolarance)
    {
      forward();
    }
    else if(leftdistance>rightdistance)
    {
      leftspeed=255-map(leftdistance,0,totaldistance,0,255);
      analogWrite(leftenable,leftspeed);
  	  analogWrite(rightenable,maxrightspeed);
  	  digitalWrite(leftforward,HIGH);
  	  digitalWrite(leftbackward,LOW);
  	  digitalWrite(rightforward,HIGH);
  	  digitalWrite(rightbackward,LOW);
    }
    else if(rightdistance>leftdistance)
    {
      rightspeed=255-map(rightdistance,0,totaldistance,0,255);
      analogWrite(leftenable,maxleftspeed);
  	  analogWrite(rightenable,rightspeed);
  	  digitalWrite(leftforward,HIGH);
  	  digitalWrite(leftbackward,LOW);
  	  digitalWrite(rightforward,HIGH);
  	  digitalWrite(rightbackward,LOW);
    }
  }
  frontdistance=frontultrasonic.read();
}