/*
  Robot Obstacle Avoidance

 */

#include <Servo.h>

#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 RIGHT -1
#define CENTER 0
#define LEFT 1

Servo myservo;  // create servo object to control a servo
                // a maximum of eight servo objects can be created

// 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);

/* setup the servo */
  myservo.attach(14);  // attaches the servo on pin 14 (a0) to the servo object

/* 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
*/
#define DLY 1000     /* how long to pause after moving servo */
#define ANGLE 135    /* center angle */
#define OFFSET 25    /* amunt right and left for servo to turn */
#define DURATION 40  /* how long to move */
#define THRESHOLD 150 /* if nothing closer than this -- go forward */

  int left;
  int center;
  int right;
  int way;

  /* right */
   Serial.println("servo right");
   myservo.write(ANGLE - OFFSET);
   right = howFarAway();
   delay(DLY);
  /* center */
   Serial.println("servo center");
   myservo.write(ANGLE);
   center = howFarAway();
   delay(DLY);
  /* left */
   Serial.println("servo left");
   myservo.write(ANGLE + OFFSET);
   left = howFarAway();
   delay(DLY);

   way = direct(right, center, left);
   switch (way)
    { /* switch */
      case RIGHT: turnRight(255, (DURATION/2)); Serial.println("turn right"); break;
      case CENTER: moveForward(255, DURATION); Serial.println("go straight"); break;
      case LEFT: turnLeft(255, (DURATION/2)); Serial.println("turn left"); break;
    } /* switch */
}

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);
}

int direct(int r, int c, int l)
 { /* FUNCTION direct */
   int ret;

   if ((THRESHOLD < r) && (THRESHOLD < c) && (THRESHOLD < l))
      ret = CENTER;
   else
      if (r > c)
         if (r > l)
            ret = RIGHT;
         else
            ret = LEFT;
      else
         if (l < c)
           ret = CENTER;
         else
           ret = LEFT;
  return ret;
 } /* FUNCTION direct */

int mx(int a, int b, int c)
 { /* FUNCTION mx */
   int ret;

   if (a > b)
      if (a > c)
         ret = a;
      else
         ret = c;
   else
      if (b > c)
        ret = b;
      else
        ret = c;
  return ret;
 } /* FUNCTION mx */