Go Down

Topic: Arduino Obstacle Avoidance and Cliff Avoidance Robot (Read 5128 times) previous topic - next topic

Feb 11, 2013, 08:54 pm Last Edit: Feb 11, 2013, 11:11 pm by CaliforniAduino Reason: 1
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.

Code: [Select]
#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);
}

PaulS

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.

UKHeliBob

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.....
Please do not send me PMs asking for help.  Post in the forum then everyone will benefit from seeing the questions and answers.

UKHeliBob

Please do not send me PMs asking for help.  Post in the forum then everyone will benefit from seeing the questions and answers.

AWOL

Code: [Select]
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.
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

robtillaart


Just wondering, how do you test a cliff avoidance robot ?  ;)

Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

Docedison

--> WA7EMS <--
"The solution of every problem is another problem." -Johann Wolfgang von Goethe
I do answer technical questions PM'd to me with whatever is in my clipboard

robtillaart

Quote
With a very small cliff?

LOL
(so not by playing Cliff Richard mp3's ;)
Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

UKHeliBob



Just wondering, how do you test a cliff avoidance robot ?  ;)


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.
Please do not send me PMs asking for help.  Post in the forum then everyone will benefit from seeing the questions and answers.

#9
Feb 11, 2013, 11:01 pm Last Edit: Feb 11, 2013, 11:11 pm by CaliforniAduino Reason: 1

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.

Code: [Select]
#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]

UKHeliBob

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.
Please do not send me PMs asking for help.  Post in the forum then everyone will benefit from seeing the questions and answers.

PaulS


robtillaart

Quote
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)
Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

UKHeliBob

In the middle of your unsigned long ping() function you have the line
Code: [Select]
unsigned long ping();

Are you doing something clever or is that line a mistake ?
Please do not send me PMs asking for help.  Post in the forum then everyone will benefit from seeing the questions and answers.

AWOL

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

Go Up