Obstacle detecting Robot

/**
*
* This code is for driving a two motor robot. Components:
* 1. Arduino
* 2. HC-SR04 Ultrasonic Sensor
* 3. Dual motor driver L298N or similar.
* 
* Important NoteS: 
* 1. DO NOT POWER YOUR MOTORS FROM ARDUINO
* 2. DO NOT POWER YOUR ARDUINO FROM MOTOR DRIVER
* 3. You will most likely need dual DC power source for 
*    3.1 Arduino (Most models should work, upto 12 V)
*    3.2 Motor Driver
* 4. DUAL POWER IS ONLY NEEDED IN CASE SINGLE POWER
*    SOURCE CANNOT POWER THE MOTORS.
*
* THIS IS A FATHER DAUGHTER PROJECT AND IS TESTED AS MUCH AS WE COULD. IF
* YOU FIND ANY BUGS WE WILL TRY TO FIX IT BUT NO GURANTEES.
*
* The code works on a simple logic, the robot tries to find a way when
* it encounters an obstacle. It turns counter clockwise by 90 degrees, if it finds an obstacle again
* it turns clockwise by 180 degrees and if it finds an obstacle again it will turn around. The turn around
* depends on the spatial geometry. To achieve the turn by degrees the macro degreesToMillis
* is used. Basically what this does is it calculates the time in milliseconds that the wheels
* must spin in order to make the turn. 
* The Robot will always turn on it's central axis, so both wheels would turn in opposite
* directions. This makes the turning radius much smaller.
* 
* The RPM of the two motors we had were not exactly same, so MOTOR_SPEED_ADJUSTMENT_DELAY is used to adjust that. 
* You may set that to 0 if the RPM of the two motors are same.
* The code tries to keep track of 2D spatial geometry.
*/


#define MAX_OBSTACLE_DISTANCE 25    // MAX distance in mm for obstacle
#define MOTOR_RPM 60.0              // Actual RPM of the motor
#define PI 3.141
#define ROTATION_RADIUS_MM 70.0     // Radius of rotation of the entire Robot
#define WHEEL_RADIUS_MM 32.5        // Radius of each wheel
#define WHEEL_TURNS_PER_DEGREE ROTATION_RADIUS_MM / (WHEEL_RADIUS_MM * 360)   // Turns / degree
#define MIN_TO_MILLIS 60000.0
#define degreesToMillis(x) (x * WHEEL_TURNS_PER_DEGREE * MIN_TO_MILLIS) / MOTOR_RPM
#define revsToMillis(x) (MIN_TO_MILLIS / MOTOR_RPM) * x
#define MOTOR_SPEED_ADJUSTMENT_DELAY 900

// Change the values below as per your need
uint8_t trigPin = 3;      // Trig pin of HC-SR04
uint8_t echoPin = 2;      // Echo pin of HC-SR04

uint8_t revright6 = 5;    //Reverse motion of Right motor
uint8_t fwdright7 = 6;    //Forward motion of Right motor
uint8_t revleft8 = 10;    //Reverse motion of Left motor
uint8_t fwdleft9 = 11;    //Forward motion of Left motor

// DO NOT MODIFY UNLESS YOU ARE ABSOLUTELY SURE

int8_t pos2d = 0;

int findDistance() {
  int distance = 0;
  int duration = 0;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);   
  digitalWrite(trigPin, HIGH);     // send waves for 10 us
  delayMicroseconds(10);
  duration = pulseIn(echoPin, HIGH); // receive reflected waves
  distance = duration / 58.2;   // convert to distance  
  Serial.write("Distance\r\n");
  Serial.println(distance, DEC);
  return distance;
}

void forward() {
  // Reset the 2d coordinate space
  pos2d = 0;
  Serial.write("Forward\r\n");  
  digitalWrite(revleft8, LOW);          
  digitalWrite(revright6, LOW);
  digitalWrite(fwdright7, HIGH); 
  delayMicroseconds(MOTOR_SPEED_ADJUSTMENT_DELAY);  
  digitalWrite(fwdleft9, HIGH);    
}

void reverse() {
  // Reset the 2d coordinate space
  pos2d = 0;
  Serial.write("Reverse\r\n");
  digitalWrite(fwdright7, LOW);  
  digitalWrite(fwdleft9, LOW);
  digitalWrite(revright6, HIGH);  
  digitalWrite(revleft8, HIGH);  
  delay(revsToMillis(1));
}

void stop() {
  Serial.write("Stop\r\n");
  digitalWrite(revright6, LOW);
  digitalWrite(fwdright7, LOW);                          
  digitalWrite(revleft8, LOW);  
  digitalWrite(fwdleft9, LOW); 
  delay(500);
}

void turnLeft(int degrees) {
  Serial.write("Left\r\n");
  pos2d += degrees;
  digitalWrite(revright6, LOW);   
  digitalWrite(fwdleft9, LOW); 
  digitalWrite(fwdright7, HIGH);  
  digitalWrite(revleft8, HIGH);       
  delay(degreesToMillis(degrees));
  digitalWrite(fwdright7, LOW);  
  digitalWrite(revleft8, LOW);   
}

void turnRight(int degrees) {
  Serial.write("Right\r\n");
  pos2d += (degrees * -1);  
  digitalWrite(revleft8, LOW);
  digitalWrite(fwdright7, LOW);       
  digitalWrite(revright6, HIGH);
  digitalWrite(fwdleft9, HIGH);       
  delay(degreesToMillis(degrees));
  digitalWrite(revright6, LOW);
  digitalWrite(fwdleft9, LOW); 
}

void turnAround() {
  Serial.write("Turn around\r\n");
  if (pos2d >= 0) {
    digitalWrite(revright6, LOW);       
    digitalWrite(fwdleft9, LOW);  
    digitalWrite(revleft8, HIGH);    
    digitalWrite(fwdright7, HIGH);
    delay(degreesToMillis(180 + pos2d));
    digitalWrite(revleft8, LOW);    
    digitalWrite(fwdright7, LOW);
  } else {    
    digitalWrite(revleft8, LOW);    
    digitalWrite(fwdright7, LOW);
    digitalWrite(revright6, HIGH);       
    digitalWrite(fwdleft9, HIGH);  
    delay(degreesToMillis(180 + pos2d));
    digitalWrite(revright6, LOW);       
    digitalWrite(fwdleft9, LOW);  
  }
}

void findPath() {  
  Serial.write("Find path\r\n");
  int dist = findDistance();
  // First try to find a path on the right
  turnRight(90);
  stop();
  dist = findDistance();  
  if (dist <= MAX_OBSTACLE_DISTANCE) {
    turnLeft(180);
    stop();
  }    
  dist = findDistance();
  if (dist <= MAX_OBSTACLE_DISTANCE) {
    turnAround();
  }
}

void setup() {
  Serial.begin(9600);
  pinMode(revright6, OUTPUT);
  pinMode(fwdright7, OUTPUT);
  pinMode(revleft8, OUTPUT);      // set Motor pins as output
  pinMode(fwdleft9, OUTPUT); 
  pinMode(trigPin, OUTPUT);         // set trig pin as output
  pinMode(echoPin, INPUT);          //set echo pin as input to capture reflected waves*/
}

void loop() {
  int distance = findDistance();
  if (distance > 0 && distance <= MAX_OBSTACLE_DISTANCE) {
    Serial.write("Obstacle\r\n");
    //Stop
    stop();
    reverse();
    stop();
    findPath();
  } else {    
    forward();    
  }
}