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);
}
