Rover Obstacle Avoidance

This Rover avoids objects within 50cm.

Parts used:

1. Arduino Uno
2. Adafruit Motor Shield
3. Parallax PING Ultrasound Sensor
4. Sparkfun Magician Chassis
5. 220ohm Resistor
6. Red LED

Avoidance.ino

#include <Adafruit_MotorShield.h>
#include <Wire.h>
/*

Nathan Carlson

http://nathancarlson-projects.blogspot.com/2015/01/rover-obstacle-avoidance.html

January 2015

*/



Adafruit_MotorShield AFMS = Adafruit_MotorShield(); //Declare Motorshield
Adafruit_DCMotor *RightMotor = AFMS.getMotor(1); //Motor 1
Adafruit_DCMotor *LeftMotor = AFMS.getMotor(2); //Motor 2

int trigPin = 5; //Blue
int echoPin = 6; //Green
int led =  7; // Status LED and Strobe



void setup() {
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(led, OUTPUT); //led detects obstacle
  AFMS.begin();  // create with the default frequency 1.6KHz
  
  // Set the speed to start, from 0 (off) to 255 (max speed)
  RightMotor->setSpeed(150);
  LeftMotor->setSpeed(150);
  
  RightMotor->run(FORWARD);
  LeftMotor->run(FORWARD);
  
  // turn on motor
  RightMotor->run(RELEASE);
  LeftMotor->run(RELEASE);

}

void loop() {
  long duration, distance;
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2); 
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
  
  RobotForward(); //Move Forward

 
  if (distance < 50 ) {  // Within 50 CM of Line of Sight 
    RobotStop();
    blinkLed(10, 100);
    delay(500);
    RobotBack();   
    delay(700);
    RobotStop(); 
    delay(500);
    RobotPivot();
    delay(700);
    RobotStop();
    delay(600);
    
    
  }
  
 else{
    RobotForward();
   digitalWrite(led, LOW);
 }
 
 
} // End LOOP

 /* ---------------------- Functions ----------------------- */
 
 void blinkLed(int countBlinks, int time){ //Number of Blinks, Delay time in Milliseconds
    for (int x = 0; x < countBlinks; x++){
       digitalWrite(led, HIGH);
       delay(time);
       digitalWrite(led, LOW);
       delay(time);     
    }
  }
 
 void RobotForward(){
  RightMotor->run(FORWARD);
  LeftMotor->run(FORWARD);
  RightMotor->setSpeed(175); 
  LeftMotor->setSpeed(175);
 

 }
   
 void RobotBack(){
  RightMotor->run(BACKWARD);
  LeftMotor->run(BACKWARD);
  RightMotor->setSpeed(100); 
  LeftMotor->setSpeed(100);
 }
 
 void RobotPivot(){
  
  LeftMotor->run(FORWARD);
  LeftMotor->setSpeed(90); //TURN LEFT MOTOR
  RightMotor->run(BACKWARD); 
  RightMotor->setSpeed(90);
  
  
 }
 
 
 void RobotStop(){ //Stop the Motors
   
  RightMotor->run(RELEASE);
  LeftMotor->run(RELEASE);
  delay(500); //Wait 20 Milliseconds
 }