Grover - A simple autonomous arduino bot - Work in progress

grover.c

#include <Servo.h>
#define HEAD_SERVO_PIN 13

#define ULTRASONIC_TRIG_PIN 2
#define ULTRASONIC_ECHO_PIN 5
#define ULTRASONIC_POWER_PIN 3
#define OBSTACLE_MIN_DISTANCE 25 //in centimeter

#define MOVE_LEFT_PIN 12 //left
#define MOVE_RIGHT_PIN 11 //right
#define MOVE_FORWARD_PIN 10 //forward
#define MOVE_BACKWARD_PIN 9  // reverse

Servo groverHeadServo;

void setup()
{
  pinMode(HEAD_SERVO_PIN, OUTPUT);
  groverHeadServo.attach(HEAD_SERVO_PIN);
  
  // Motor pins
  pinMode(MOVE_LEFT_PIN, OUTPUT);     
  pinMode(MOVE_RIGHT_PIN, OUTPUT);     
  pinMode(MOVE_FORWARD_PIN, OUTPUT);     
  pinMode(MOVE_BACKWARD_PIN, OUTPUT);
  
  groverHeadServo.write(30);
  delay(1000);
  groverHeadServo.write(130);
  delay(1000);
  groverHeadServo.write(90);
  delay(500);
  Serial.begin (9600);
  
  pinMode(ULTRASONIC_TRIG_PIN, OUTPUT);
  pinMode(ULTRASONIC_POWER_PIN, OUTPUT);
  pinMode(ULTRASONIC_ECHO_PIN, INPUT);
  digitalWrite(ULTRASONIC_POWER_PIN, HIGH);
}

void loop()
{
  int objectInFrontStatus = 0;
  while(objectInFrontStatus == 0){
    Serial.println("Inside while loop");
	//Keep moving forward
    objectInFrontStatus = objectInFront();
    Serial.print("Status: ");
    Serial.println(objectInFrontStatus);
    digitalWrite(MOVE_FORWARD_PIN, HIGH);
    //delay(3000);
  }
  
  Serial.println("Outside while loop");
  digitalWrite(MOVE_FORWARD_PIN, LOW);
  //delay(5000);
  digitalWrite(MOVE_RIGHT_PIN, HIGH);
  //delay(1000);
  digitalWrite(MOVE_BACKWARD_PIN, HIGH);
  delay(1000);
  digitalWrite(MOVE_RIGHT_PIN, LOW);
  digitalWrite(MOVE_BACKWARD_PIN, LOW);
  digitalWrite(MOVE_LEFT_PIN, HIGH);
  digitalWrite(MOVE_FORWARD_PIN, HIGH);
  delay(1000);
  digitalWrite(MOVE_LEFT_PIN, LOW);
  
  //delay(1000);
  Serial.println("End of loop");
  /* static int val = 90;
  int angleMoveVal = 20;
  int delayVal = 4000;
  groverHeadServo.write(val);
  delay(delayVal);
  while(val <=159){
  groverHeadServo.write(val+=angleMoveVal);
  delay(delayVal);
  }
  val = 90;
  groverHeadServo.write(val);
  while(val > 0){
  groverHeadServo.write(val-=angleMoveVal);
  delay(delayVal); 
  }*/
}

int objectInFront(){
  Serial.println("In object in front::: ");
  long duration, distance;
  digitalWrite(ULTRASONIC_TRIG_PIN, LOW);
  delayMicroseconds(2); 
  
  digitalWrite(ULTRASONIC_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  
  digitalWrite(ULTRASONIC_TRIG_PIN, LOW);
  duration = pulseIn(ULTRASONIC_ECHO_PIN, HIGH);
  
  distance = (duration/2) / 29.1;
  if (distance >= 200 || distance <= 0){
    Serial.println("Returning false - dist >=200 or <=0");
    return 0;
  }
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");
 // delay(3000);
  if(distance < OBSTACLE_MIN_DISTANCE) {
    Serial.println("Returning true - object in front");
      return 1;
  }
  else {
    Serial.println("Returning false - object in front");
      return 0;
  }
}