const int AIN1 = 13;
const int AIN2 = 12;
const int PWMA = 11;


const int PWMB = 8;
const int BIN2 = 9;
const int BIN1 = 10;

const int trigPinLeft=4;
const int echoPinLeft=3;
const int trigPinRight=2;
const int echoPinRight=1;
const int trigPin = 6;
const int echoPin = 5;
int switchPin = 7;

float distance = 0;
float distanceRight=0;
float distanceLeft;
/********************************************************************************/
void setup()
{
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);

  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  Serial.begin(9600);
}

/********************************************************************************/
void loop()
{
  distance = getDistance();
    if(distance < 13.1){
    	stop();
    	delay(100);
    	goLeft();
    	delay(100);
    	distanceLeft=getDistanceLeft();
  		distanceRight=getDistanceRight();
    	while(distanceLeft<1||distanceRight<1){
    		if(distanceRight<1&&distanceLeft<1){
    			goForwards(255);
    			break;
    		}
    		else if(distanceLeft<1){
    			goRight();
    		}
    		else if(distanceRight<1){
    			goLeft();
    		}
    		distanceLeft=getDistanceLeft();
    		distanceRight=getDistanceRight();
    		delay(50);
    	}
    }
    else{
      goForwards(255);
    }
  delay(50);
}

/********************************************************************************/
void rightMotor(int motorSpeed)
{
  if (motorSpeed > 0)
  	{
    	digitalWrite(AIN1, HIGH);
    	digitalWrite(AIN2, LOW);
  	}
  else if (motorSpeed < 0)
  	{
    	digitalWrite(AIN1, LOW);
    	digitalWrite(AIN2, HIGH);
  	}
  else
  	{
    	digitalWrite(AIN1, LOW);
    	digitalWrite(AIN2, LOW);
  	}
  analogWrite(PWMA, abs(motorSpeed));
}

/********************************************************************************/
void leftMotor(int motorSpeed)
{
  if (motorSpeed > 0)
  	{
    	digitalWrite(BIN1, HIGH);
    	digitalWrite(BIN2, LOW);
  	}
  else if (motorSpeed < 0)
  	{
  		digitalWrite(BIN1, LOW);
  		digitalWrite(BIN2, HIGH);
  	}
  else
  	{
    	digitalWrite(BIN1, LOW);
    	digitalWrite(BIN2, LOW);
  	}
  analogWrite(PWMB, abs(motorSpeed));
}
/********************************************************************************/
void goForwards(int speed){
	rightMotor(speed);
	leftMotor(speed);
}
/********************************************************************************/
void goBackwards(int speed){
	rightMotor(speed*-1);
	leftMotor(speed*-1);
}
/********************************************************************************/
void goRight(){
	rightMotor(-255);
	leftMotor(255);
}
/********************************************************************************/
void goLeft(){
	rightMotor(255);
	leftMotor(-255);
}
/********************************************************************************/
void stop(){
	rightMotor(0);
	leftMotor(0);
}
/********************************************************************************/
float getDistance()
{
  float echoTime;
  float calculatedDistance;
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  echoTime = pulseIn(echoPin, HIGH);
  calculatedDistance = echoTime / 148.0;
  return calculatedDistance;
}
/********************************************************************************/
float getDistanceRight()
{
  float echoTimeRight;
  float calculatedDistanceRight;

  digitalWrite(trigPinRight, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinRight, LOW);

  echoTimeRight = pulseIn(echoPinRight, HIGH);
  calculatedDistanceRight = echoTimeRight / 148.0;

  return calculatedDistanceRight;
}
/********************************************************************************/
float getDistanceLeft()
{
  float echoTimeLeft;
  float calculatedDistanceLeft;

  digitalWrite(trigPinLeft, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinLeft, LOW);

  echoTimeLeft = pulseIn(echoPinLeft, HIGH);
  calculatedDistanceLeft = echoTimeLeft/ 148.0;

  return calculatedDistanceLeft;
}