/*
  Robot Obstacle Avoidance

 */

#define ENA1 2   /* turn on/off left motor */
#define IN1 3    /* control for left motor */
#define IN2 4    /* control for left motor */

#define ENA2 7 /* turn on/off right motor */
#define IN3 5  /* control for right motor */
#define IN4 6  /* control for right motor */

#define sonarPower 13 /* pin to provide _5 to power sonar device */
#define trigPin 11    /* trigger for sonar */
#define echoPin 10    /* data for sonar */

#define threshhold 100 /* how close is to close */
#define maxIterations 222

int i;

// the setup routine runs once when you press reset:
void setup()
 {
  Serial.begin(9600);

/* right motor */
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(ENA1,OUTPUT);

/* let motor */
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);
  pinMode(ENA2,OUTPUT);

/* sonar */
  pinMode(trigPin, OUTPUT);


  i = 0;

/* turn a pin on to provide power for sonar */
  pinMode(sonarPower, OUTPUT);
  digitalWrite(sonarPower, HIGH);

}

// the loop routine runs over and over again forever:
void loop() {
/* direction:
   forward: low/high
   reverse: high/low
   stop:    low/low
*/


  i = i + 1;
  /* find out how farA away obstacle is */
  if (threshhold > howFarAway())
   { /* obstruction -- must turn */
     turnLeft(255, 15);  /* 128 */
   } /* obstruction -- must turn */
  else
   { /* all clear -- forward ho! */
     moveForward(255, 10); /* 96 */
   } /* all clear -- forward ho! */
  if (maxIterations < i)
   { /* turn other way in case we are hung up */
     i = 0;
     turnRight(255, 20); /* 128 */
     Serial.println("turn right");
   } /* turn other way in case we are hung up */
}

int howFarAway()
 {
  int duration;
  int distance;

  digitalWrite(trigPin, LOW);  // Added this line
  delayMicroseconds(2); // Added this line
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); // Added this line
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  if (0 > duration)
   { duration = 99999; } /* hack because over long returns negative */
  distance = (duration/2) / 29.1;
  Serial.print("Duration: ");
  Serial.print(duration);
  Serial.print("  Distance: ");
  Serial.println(distance);

  return distance;
 }

void moveForward(int speed, int tSec)
{
   digitalWrite(IN1,LOW);
   digitalWrite(IN2,HIGH);
   digitalWrite(IN3,LOW);
   digitalWrite(IN4,HIGH);
   moveRobot(speed, tSec);
}

void moveBackward(int speed, int tSec)
{
   digitalWrite(IN1,HIGH);
   digitalWrite(IN2,LOW);
   digitalWrite(IN3,HIGH);
   digitalWrite(IN4,LOW);
   moveRobot(speed, tSec);
}

void turnRight(int speed, int tSec)
{
   digitalWrite(IN1,LOW);
   digitalWrite(IN2,HIGH);
   digitalWrite(IN3,HIGH);
   digitalWrite(IN4,LOW);
   moveRobot(speed, tSec);
}

void turnLeft(int speed, int tSec)
{
   digitalWrite(IN1,HIGH);
   digitalWrite(IN2,LOW);
   digitalWrite(IN3,LOW);
   digitalWrite(IN4,HIGH);
   Serial.println("Turn left");
   moveRobot(speed, tSec);
}

void moveRobot(int motorSpeed, int tSec)
{
   analogWrite(ENA1, motorSpeed - 0);// motor speed
   analogWrite(ENA2, motorSpeed);// motor speed
   Serial.println("Moving");
   delay(tSec * 11); /* 10 */
   Serial.println("Stopping");
   analogWrite(ENA1, 0);
   analogWrite(ENA2, 0);
}