I am using the Ardumoto board on a Magician chassis with the HC-SR04 ultrasonic sensor to check for obstacles.
I am using standard code to check for obstacles (using the NewPing.h library). However, though the obstacle checking code and the code to run the motors work fine by themselves, when I use the two in conjunction, the ultrasonic sensor always returns the wrong distance (always about 4 - 5 cm).
Here is the code - any help appreciated!
#include <NewPing.h>
#define pingPin 8
#define trigPin 7
#define MAX_DISTANCE 200
// pins for motor speed and direction
int speed1 = 3, speed2 = 11, direction1 = 12, direction2 = 13;
int startSpeed = 150;
long duration; //time it takes to receive bounced back signal
const int dangerThresh = 15; //threshold for obstacles (in cm)
NewPing sonar(trigPin, pingPin, MAX_DISTANCE); // NewPing setup of pin and maximum distance.
// initial speeds of left and right motors
int left = startSpeed, right = startSpeed;
void setup() {
pinMode(trigPin, OUTPUT);
pinMode(pingPin, INPUT);
pinMode(9, OUTPUT); // led
// set the motor pins to outputs
pinMode(speed1, OUTPUT);
pinMode(speed2, OUTPUT);
pinMode(direction1, OUTPUT);
pinMode(direction2, OUTPUT);
delay(1000);
// make both motors same speed
left = startSpeed;
right = startSpeed;
Serial.begin(115200);
}
void loop() {
// read the sensor
delay(50);
int distanceFwd = sonar.ping();
distanceFwd = distanceFwd/US_ROUNDTRIP_CM;
while ((distanceFwd<dangerThresh) && (distanceFwd!=0)) { //if path is blocked
Serial.print("dist: ");
Serial.println(distanceFwd);
goBeep();
distanceFwd = sonar.ping();
distanceFwd = distanceFwd/US_ROUNDTRIP_CM;
// no logic yet to stop the motors... just testing...
}
// no obstacle, so move ahead
// if I comment out the moveAhead() line below, I get the correct distance,
// otherwise I always get a distance of about 4 - 5 cm
moveAhead();}
// was using a buzzer first to beep when obstacle detected.
// but just have an LED on Pin 9 now...
void goBeep() {
analogWrite(9, 20);
delay(200);
analogWrite(9, 0);
delay(200);
}
void moveAhead() {
// set motor direction to forward
digitalWrite(direction1, HIGH);
digitalWrite(direction2, HIGH);
// set speed of both motors
analogWrite(speed1,left);
analogWrite(speed2,right);
}
