Simple obstacle avoiding sketch using an ultrasonic sensor

Found a new library for ultrasonic sensors on the Arduino forums, NewPing.h. You can download it from the Arduino playground. So I wanted to try it out with my Dagu Rover 5. I modified an older sketch that used the ultrasonic.h library. It was pretty easy to change and I can confirm that the new library works. The example that comes with the library uses the millis function to trigger the “pinging”. I had a bit of trouble integrating it to my sketch so I replaced the millis code with a simple delay. Not the best solution so I will try to make a new sketch without delays in the future 🙂
The sketch is far from perfect, my Rover easily gets stuck in corners.

Here is the simple sketch:

// http://www.bajdi.com
// Dagu Rover 5 chassis with 4 motors
// Autonomous mode using an SRF06 ultrasonic sensor mounted on a 9G servo on the front
// µcontroller = Dagu Red back spider, Arduino Mega 1280 compatible
// Motor controller = Dagu 4 channel motor controller

#include <NewPing.h>
#include <Servo.h>

const int DirM1 = 30; // direction motor 1
const int DirM2 = 31; // direction motor 2
const int DirM3 = 32; // direction motor 3
const int DirM4 = 33; // direction motor 4
const int PWMM1 = 4;  // pwm motor 1
const int PWMM2 = 5;  // pwm motor 2
const int PWMM3 = 6;  // pwm motor 3
const int PWMM4 = 7;  // pwm motor 4

const int TRIGGER_PIN = 36;  // Arduino pin tied to trigger pin on ping sensor.
const int ECHO_PIN = 37;  // Arduino pin tied to echo pin on ping sensor.
const int MAX_DISTANCE = 200; // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing srf06(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

int leftDistance, rightDistance, fwdDistance; 

Servo SRF6AServo;        // servo objects

void setup() {
  Serial.begin(57600);
  pinMode(DirM1, OUTPUT);  // direction motor 1, left front
  pinMode(DirM2, OUTPUT);  // direction motor 2, left rear
  pinMode(DirM3, OUTPUT);  // direction motor 3, right front
  pinMode(DirM4, OUTPUT);  // direction motor 4, right rear
  pinMode(PWMM1, OUTPUT);  // PWM motor 1
  pinMode(PWMM2, OUTPUT);  // PWM motor 2
  pinMode(PWMM3, OUTPUT);  // PWM motor 3
  pinMode(PWMM4, OUTPUT);  // PWM motor 4

  SRF6AServo.attach(8);       
  SRF6AServo.write(55) ; // set servo to center pois
  delay(150);
}

void loop() {

  fwdDistance =  srf06.ping_cm();
  delay(100); 
  if (fwdDistance > 30)
  {
    forward();
  }
  else
  {
    stop();
    SRF6AServo.write(110);  // move servo left
    delay(500);
    leftDistance = srf06.ping_cm();
    delay(100);
    SRF6AServo.write(10);    // move servo right
    delay(500);
    rightDistance = srf06.ping_cm();
    delay(100);
    SRF6AServo.write(55);    // move servo to center
    delay(50);
    compareDistance();

  }      

}

void compareDistance()
{
  if (leftDistance>rightDistance) //if left is less obstructed 
  {
    turnleft();
    delay(800); 
  }
  else if (rightDistance>leftDistance) //if right is less obstructed
  {
    turnright;
    delay(800);
  }
  else if (rightDistance == leftDistance)
  {
    turnleft();
    delay(1500);
  }
}

void turnleft()
{
  digitalWrite(DirM1, HIGH);
  digitalWrite(DirM2, HIGH);
  digitalWrite(DirM3, LOW);
  digitalWrite(DirM4, LOW);
  analogWrite(PWMM1, 130);
  analogWrite(PWMM2, 130);
  analogWrite(PWMM3, 130);
  analogWrite(PWMM4, 130);
}

void turnright()
{
  digitalWrite(DirM1, LOW);
  digitalWrite(DirM2, LOW);
  digitalWrite(DirM3, HIGH);
  digitalWrite(DirM4, HIGH);
  analogWrite(PWMM1, 130);
  analogWrite(PWMM2, 130);
  analogWrite(PWMM3, 130);
  analogWrite(PWMM4, 130); 
}

void forward()
{
  digitalWrite(DirM1, LOW);
  digitalWrite(DirM2, LOW);
  digitalWrite(DirM3, LOW);
  digitalWrite(DirM4, LOW);
  analogWrite(PWMM1, 70);
  analogWrite(PWMM2, 70);
  analogWrite(PWMM3, 70);
  analogWrite(PWMM4, 70);
}

void stop()
{
  analogWrite(PWMM1, 0);
  analogWrite(PWMM2, 0);
  analogWrite(PWMM3, 0);
  analogWrite(PWMM4, 0);
}