Arduino Obstacle Avoidance and Cliff Avoidance Robot

Hello everyone in the Arduino community. Im asking for some help with programing a 3 wheel robot with 2 ultrasonic sensors. The 1st ultrasonic sensor (Parallax) is used as an obstacle avoidance sensor. The second ultrasonic sensor (SR04) is used to check the ground distance so the robot won't fall into a hole in the ground. The problem I'm having is that the Center Distance Reading from the ultrasonic sensor is always reading zero (0) even if there's nothing in front of the ultrasonic sensors. I have used both of these ultrasonic sensors in another project and they both work perfectly so I now there's nothing wrong with the ultrasonic sensors. I will attach the code I'm using for this project. Thank you in advance for your help.

#include <AFMotor.h> // Enables the Motor library
#include <Servo.h> // Enables the Servo library


Servo PingServo;
AF_DCMotor motor1(1); // Motor 1 is connected to the port 1 on the motor shield
AF_DCMotor motor2(2); // Motor 2 is connected to the port 2 on the motor shield
int minSafeObstacleDist = 11 ; // Minimum distance for ping sensor to know when to turn
int pingPin = A0; // Parallax Ping sensor is connected to port A5
int minSafeGroundDist = 2 ; // Minimum distance for ping sensor to know when to turn
int TrigPin = A5; // SR04 Ping sensor TrigPin is connected to port A5
int EchoPin = A4; // SR04 Ping sensor EchoPin is connected to port A4
int centerDist, leftDist, rightDist, backDist; // Define variables center, left, right and back distance
long duration, inches, cm; // Define variables for Ping sensor

void setup() {
PingServo.attach(10); // Servo is attached to pin 10 in the motor shield
PingServo.write(90); // Center the Ping sensor (puts it at 90 degrees)
motor1.setSpeed(250); // Sets the speed of the first motor.
motor2.setSpeed(250); // Sets the speed of the second motor.

Serial.begin(9600); // Enables Serial monitor for debugging purposes
Serial.println("Serial test!"); // Test the Serial communication

}

void AllStop() {
motor1.run(RELEASE); // Turns off motor 1
motor2.run(RELEASE); // Turns off motor 2
}
void AllForward() { // Makes the robot go forward
motor1.run(FORWARD); // Motor 1 goes forward
motor2.run(FORWARD); // Motor 2 goes forward
Serial.println("Going forward"); // Prints a line in the serial monitor
}
void turnRight() { // Makes the robot go right
motor2.run(BACKWARD); // Turns off motor 2
motor1.run(FORWARD); // Motor 1 goes forward
delay(1600); // Time required to turn right (1.6 seconds)
Serial.println("Motors going Right"); // Prints a line in the serial monitor
}
void GoBack(){ // Makes the robot go back
motor2.run(BACKWARD); // Motor 2 goes back
motor1.run(BACKWARD); // Motor 1 goes back
delay(1600); // Time Required to go back (1.6 seconds)
Serial.println("Backward"); // Prints a line in the serial monitor
}
void turnLeft() { // Makes the robot go Left
motor2.run(FORWARD); // Motor 2 goes forward
motor1.run(BACKWARD); // turns off motor 1
delay(1600); //Time Required to turn left (1.6)Seconds
Serial.println("Motors going Left");// Prints a line in the serial monitor
}
// Starts the loop to decide what to do
void loop()
{
LookAhead();
Serial.print(inches);
Serial.println(" inches"); // Prints a line in the serial monitor
if((inches >= minSafeObstacleDist) && (inches <= minSafeGroundDist))/* If the inches in front of an object is greater than or equal to the minimum safe distance A (11 inches) and 
floor distance is less than or equal to the minimum safe distance B, react*/
{
AllForward(); // All wheels forward
delay(110); // Wait 0.11 seconds
}else // If not:

{
AllStop(); // Stop all motors
LookAround(); // Check your surroundings for best route
if(rightDist > leftDist) // If the right distance is greater than the left distance , turn right
{
turnRight();
}else if (leftDist > rightDist) // If the left distance is greater than the right distance , turn left
{
turnLeft();
}else if (leftDist&&rightDist<minSafeObstacleDist) // If the left and right distance is smaller than the min safe distance (11 inch) go back
{
GoBack();
}
}
}

unsigned long ping() {
pinMode(pingPin, OUTPUT); // Make the Pingpin to output
digitalWrite(pingPin, LOW); //Send a low pulse
delayMicroseconds(2); // wait for two microseconds
digitalWrite(pingPin, HIGH); // Send a high pulse
delayMicroseconds(5); // wait for 5 micro seconds
digitalWrite(pingPin, LOW); // send a low pulse
pinMode(pingPin,INPUT); // switch the Pingpin to input
duration = pulseIn(pingPin, HIGH); //listen for echo
unsigned long ping();
pinMode(TrigPin, OUTPUT); // Make the Pingpin to output
digitalWrite(TrigPin, LOW); //Send a low pulse
delayMicroseconds(2); // wait for two microseconds
digitalWrite(TrigPin, HIGH); // Send a high pulse
delayMicroseconds(5); // wait for 5 micro seconds
digitalWrite(TrigPin, LOW); // send a low pulse
pinMode(EchoPin,INPUT); // switch the Pingpin to input
duration = pulseIn(EchoPin, HIGH); //listen for echo

/*Convert micro seconds to Inches
-------------------------------------*/

inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
}

long microsecondsToInches(long microseconds) // converts time to a distance
{
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds) // converts time to a distance
{
return microseconds / 29 / 2;
}

void LookAhead() {
PingServo.write(90);// angle to look forward
delay(175); // wait 0.175 seconds
ping();
}

void LookAround(){
PingServo.write(180); // 180° angle
delay(320); // wait 0.32 seconds
ping();
rightDist = inches; //get the right distance
PingServo.write(0); // look to the other side
delay(620); // wait 0.62 seconds
ping();
leftDist = inches; // get the left distance
PingServo.write(90); // 90° angle
delay(275); // wait 0.275 seconds

// Prints a line in the serial monitor
Serial.print("RightDist: ");
Serial.println(rightDist);
Serial.print("LeftDist: ");
Serial.println(leftDist);
Serial.print("CenterDist: ");
Serial.println(centerDist);
}

That is not the way to post code. Modify your post. Select all the code. Select the icon with the # symbol, to add code tags. Save your changes. Then, we'll look at the code.

Deleting the code, opening the IDE, opening the sketch, using Tools + Auto Format, and then posting the fixed up code would be an even better idea.

There will be someone along any minute asking you to highlight your code and put code tags around it by clicking on the # above the smileys to make it easier to read.

Oh, I just did.....

Told you !

unsigned long ping() {
pinMode(pingPin, OUTPUT); // Make the Pingpin to output
digitalWrite(pingPin, LOW); //Send a low pulse
delayMicroseconds(2); // wait for two microseconds
digitalWrite(pingPin, HIGH); // Send a high pulse
delayMicroseconds(5); // wait for 5 micro seconds
digitalWrite(pingPin, LOW); // send a low pulse
pinMode(pingPin,INPUT); // switch the Pingpin to input
duration = pulseIn(pingPin, HIGH); //listen for echo
unsigned long ping();    <<<<HERE
pinMode(TrigPin, OUTPUT); // Make the Pingpin to output
digitalWrite(TrigPin, LOW); //Send a low pulse
delayMicroseconds(2); // wait for two microseconds
digitalWrite(TrigPin, HIGH); // Send a high pulse
delayMicroseconds(5); // wait for 5 micro seconds
digitalWrite(TrigPin, LOW); // send a low pulse
pinMode(EchoPin,INPUT); // switch the Pingpin to input
duration = pulseIn(EchoPin, HIGH); //listen for echo

that's an unusual place to put a function prototype.
Difficult (for me) to see what you're trying to achieve.
If you've got two sensors, on different pins, make a single function with the pin number as parameter.

Just wondering, how do you test a cliff avoidance robot ? :wink:

With a very small cliff?

Bob

With a very small cliff?

LOL
(so not by playing Cliff Richard mp3's :wink:

robtillaart:
Just wondering, how do you test a cliff avoidance robot ? :wink:

You walk backwards in front of it so that can be see it working.

No, wait a minute. I have just thought of a snag in my QA plan.

CaliforniAduino:
Hello everyone in the Arduino community. Im asking for some help with programing a 3 wheel robot with 2 ultrasonic sensors. The 1st ultrasonic sensor (Parallax) is used as an obstacle avoidance sensor. The second ultrasonic sensor (SR04) is used to check the ground distance so the robot won't fall into a hole in the ground. The problem I'm having is that the Center Distance Reading from the ultrasonic sensor is always reading zero (0) even if there's nothing in front of the ultrasonic sensors. I have used both of these ultrasonic sensors in another project and they both work perfectly so I now there's nothing wrong with the ultrasonic sensors. I will attach the code I'm using for this project. Thank you in advance for your help.

#include <AFMotor.h> // Enables the Motor library

#include <Servo.h> // Enables the Servo library

Servo PingServo;
AF_DCMotor motor1(1); // Motor 1 is connected to the port 1 on the motor shield
AF_DCMotor motor2(2); // Motor 2 is connected to the port 2 on the motor shield
int minSafeObstacleDist = 11 ; // Minimum distance for ping sensor to know when to turn
int pingPin = A0; // Parallax Ping sensor is connected to port A5
int minSafeGroundDist = 2 ; // Minimum distance for ping sensor to know when to turn
int TrigPin = A5; // SR04 Ping sensor TrigPin is connected to port A5
int EchoPin = A4; // SR04 Ping sensor EchoPin is connected to port A4
int centerDist, leftDist, rightDist, backDist; // Define variables center, left, right and back distance
long duration, inches, cm; // Define variables for Ping sensor

void setup() {
PingServo.attach(10); // Servo is attached to pin 10 in the motor shield
PingServo.write(90); // Center the Ping sensor (puts it at 90 degrees)
motor1.setSpeed(250); // Sets the speed of the first motor.
motor2.setSpeed(250); // Sets the speed of the second motor.

Serial.begin(9600); // Enables Serial monitor for debugging purposes
Serial.println("Serial test!"); // Test the Serial communication

}

void AllStop() {
motor1.run(RELEASE); // Turns off motor 1
motor2.run(RELEASE); // Turns off motor 2
}
void AllForward() { // Makes the robot go forward
motor1.run(FORWARD); // Motor 1 goes forward
motor2.run(FORWARD); // Motor 2 goes forward
Serial.println("Going forward"); // Prints a line in the serial monitor
}
void turnRight() { // Makes the robot go right
motor2.run(BACKWARD); // Turns off motor 2
motor1.run(FORWARD); // Motor 1 goes forward
delay(1600); // Time required to turn right (1.6 seconds)
Serial.println("Motors going Right"); // Prints a line in the serial monitor
}
void GoBack(){ // Makes the robot go back
motor2.run(BACKWARD); // Motor 2 goes back
motor1.run(BACKWARD); // Motor 1 goes back
delay(1600); // Time Required to go back (1.6 seconds)
Serial.println("Backward"); // Prints a line in the serial monitor
}
void turnLeft() { // Makes the robot go Left
motor2.run(FORWARD); // Motor 2 goes forward
motor1.run(BACKWARD); // turns off motor 1
delay(1600); //Time Required to turn left (1.6)Seconds
Serial.println("Motors going Left");// Prints a line in the serial monitor
}
// Starts the loop to decide what to do
void loop()
{
LookAhead();
Serial.print(inches);
Serial.println(" inches"); // Prints a line in the serial monitor
if((inches >= minSafeObstacleDist) && (inches <= minSafeGroundDist))/* If the inches in front of an object is greater than or equal to the minimum safe distance A (11 inches) and
floor distance is less than or equal to the minimum safe distance B, react*/
{
AllForward(); // All wheels forward
delay(110); // Wait 0.11 seconds
}else // If not:

{
AllStop(); // Stop all motors
LookAround(); // Check your surroundings for best route
if(rightDist > leftDist) // If the right distance is greater than the left distance , turn right
{
turnRight();
}else if (leftDist > rightDist) // If the left distance is greater than the right distance , turn left
{
turnLeft();
}else if (leftDist&&rightDist<minSafeObstacleDist) // If the left and right distance is smaller than the min safe distance (11 inch) go back
{
GoBack();
}
}
}

unsigned long ping() {
pinMode(pingPin, OUTPUT); // Make the Pingpin to output
digitalWrite(pingPin, LOW); //Send a low pulse
delayMicroseconds(2); // wait for two microseconds
digitalWrite(pingPin, HIGH); // Send a high pulse
delayMicroseconds(5); // wait for 5 micro seconds
digitalWrite(pingPin, LOW); // send a low pulse
pinMode(pingPin,INPUT); // switch the Pingpin to input
duration = pulseIn(pingPin, HIGH); //listen for echo
unsigned long ping();
pinMode(TrigPin, OUTPUT); // Make the Pingpin to output
digitalWrite(TrigPin, LOW); //Send a low pulse
delayMicroseconds(2); // wait for two microseconds
digitalWrite(TrigPin, HIGH); // Send a high pulse
delayMicroseconds(5); // wait for 5 micro seconds
digitalWrite(TrigPin, LOW); // send a low pulse
pinMode(EchoPin,INPUT); // switch the Pingpin to input
duration = pulseIn(EchoPin, HIGH); //listen for echo

/Convert micro seconds to Inches
-------------------------------------
/

inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
}

long microsecondsToInches(long microseconds) // converts time to a distance
{
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds) // converts time to a distance
{
return microseconds / 29 / 2;
}

void LookAhead() {
PingServo.write(90);// angle to look forward
delay(175); // wait 0.175 seconds
ping();
}

void LookAround(){
PingServo.write(180); // 180° angle
delay(320); // wait 0.32 seconds
ping();
rightDist = inches; //get the right distance
PingServo.write(0); // look to the other side
delay(620); // wait 0.62 seconds
ping();
leftDist = inches; // get the left distance
PingServo.write(90); // 90° angle
delay(275); // wait 0.275 seconds

// Prints a line in the serial monitor
Serial.print("RightDist: ");
Serial.println(rightDist);
Serial.print("LeftDist: ");
Serial.println(leftDist);
Serial.print("CenterDist: ");
Serial.println(centerDist);
}
[/quote]

Nice try, but still not quite right.

Highlight your code in the post, click the # above the smileys and it will put code tags around your code.

OP:
Doesn't
your
tab
key
work
properly?

The problem I'm having is that the Center Distance Reading from the ultrasonic sensor is always reading zero (0) even if there's nothing in front of the ultrasonic sensors. I have used both of these ultrasonic sensors in another project and they both work perfectly so I now there's nothing wrong with the ultrasonic sensors.

More serious :

  • have you tried the sensors in a smallest sketch possible? test them to be sure.
    best way is to make a copy of your failing sketch and strip it. (This way you might see the code glitch)
  • do the servo's motors draw too much current causing the sensor to fail?
  • double check wiring (almost all robots have wring problems at some time)

In the middle of your unsigned long ping() function you have the line unsigned long ping();

Are you doing something clever or is that line a mistake ?

Psst, Bob, reply #4 :wink:

Psst indeed !
Note to self. When coming back to threads it is sometimes a good idea to reread earlier replies.

The OP has PM'd me and will reply to the thread soon.

(If you've got two sensors, on different pins, make a single function with the pin number as parameter.)

Thank you for this pointer. I will do some changes on the code.

Thank you all for the help. The Arduino is now reading the 2 ultrasonic sensors simultaneously. I will be posting the updated code soon. Thank you again for everyone's help.