Source code of my obstacle avoidance robot for Tiva C Launchpad

ObstacleAvoidanceRobot

#include <Servo.h>

// Obstacle avoiding program - Tiva C Launchpad
// Cuong T. Nguyen - 4/15/2014
/* Code for controling an obstacle avoidance robot, which has two DC drivers 
   interface with a L298N modul and a HC-SR04 range sensor mounted on a servo motor.
   
   Range sensor 
     VCC: VBus
     GND: GND
     TrigPin: PC4 37
     EchoPin: PC5 36
  
   L298N:
     In1: PE1 27
     In2: PE2 28
     In3: PE3 29
     In4: PE4 5
    
   Servo: 
     VCC: VBus
     GND: GND
     Signal: PB3 38
 
*/

//========================================
#define TrigPin 37     
#define EchoPin 36
#define ServoSignal 38
#define DCIn1 27
#define DCIn2 28
#define DCIn3 29
#define DCIn4 5

const int MAX_DISTANCE = 35; // cm
const int TURN_LEFT_DELAY = 320; // ms
const int TURN_RIGHT_DELAY = 320; // ms
const int RED = 0;
const int GREEN = 1;
const int BLUE = 2;
const int YELLOW = 3;
const int NOCOLOR = 4;

Servo ServoController; 

//============LED Driver=================
void turnOnOne(int x) {
  digitalWrite(RED_LED, LOW);
  digitalWrite(GREEN_LED, LOW);
  digitalWrite(BLUE_LED, LOW);
  if (x == RED) digitalWrite(RED_LED, HIGH);
  if (x == GREEN) digitalWrite(GREEN_LED, HIGH);
  if (x == BLUE) digitalWrite(BLUE_LED, HIGH);
  if (x == YELLOW) {
     digitalWrite(RED_LED, HIGH);
     digitalWrite(GREEN_LED, HIGH);
  }
}
//=======================================

//============Motor Driver===============

void stop() {
  turnOnOne(RED);
  digitalWrite(DCIn1, LOW); 
  digitalWrite(DCIn2, LOW); 
  digitalWrite(DCIn3, LOW); 
  digitalWrite(DCIn4, LOW); 
}

void goStraight() {
  turnOnOne(GREEN);
  digitalWrite(DCIn1, LOW); 
  digitalWrite(DCIn2, HIGH); 
  digitalWrite(DCIn3, LOW); 
  digitalWrite(DCIn4, HIGH);    
}

void turnLeft() {
  turnOnOne(BLUE);
  digitalWrite(DCIn1, HIGH); 
  digitalWrite(DCIn2, LOW); 
  digitalWrite(DCIn3, LOW); 
  digitalWrite(DCIn4, HIGH);      
}

void turnRight() {
  turnOnOne(YELLOW);
  digitalWrite(DCIn1, LOW); 
  digitalWrite(DCIn2, HIGH); 
  digitalWrite(DCIn3, HIGH); 
  digitalWrite(DCIn4, LOW);    
}

//=======================================

//=============Servo Driver==============

void lookFront() {
  ServoController.write(70);
  delay(500);
}

void lookLeft() {
  ServoController.write(180);
  delay(500);
}

void lookRight() {
  ServoController.write(0);
  delay(500);
}

//=======================================

//===========Range Sensor================

double getDistance() {  // in centimeter
  double distance;
  int duration;
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(1000);
  digitalWrite(TrigPin, LOW);
  duration = pulseIn(EchoPin, HIGH);
  distance = (duration/2) / 29.1;
  return distance;
}
//=======================================



void setup()
{
  pinMode(EchoPin, INPUT);
  
  pinMode(TrigPin, OUTPUT);
  pinMode(DCIn1, OUTPUT);
  pinMode(DCIn2, OUTPUT);
  pinMode(DCIn3, OUTPUT);
  pinMode(DCIn4, OUTPUT);
  pinMode(RED_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
  pinMode(BLUE_LED, OUTPUT);
  ServoController.attach(ServoSignal);
  
  tooClose();
}

int tooClose() {
  lookFront();
  int d = getDistance();
  return d <= MAX_DISTANCE; 
}

void decideAndTurn() {
  double dLeft, dRight, dFront;
  
  lookLeft();
  dLeft = getDistance();
  
  lookRight();
  dRight = getDistance(); 
  
  lookFront();
  dFront = getDistance();
  
  if (dLeft > dRight && dLeft > dFront) {
    turnLeft();
    delay(TURN_LEFT_DELAY);
    stop();
  }
  else if (dRight > dFront) {
      turnRight();
      delay(TURN_RIGHT_DELAY);
      stop();
  }
  else {
      goStraight();      
  }
}

void loop()
{
  delay(5);
  if (tooClose()) {
    digitalWrite(RED_LED, HIGH);
    stop();
    decideAndTurn();
  }
  else {
    digitalWrite(RED_LED, LOW);
    goStraight(); 
  }
}