Obstacle avoidance with an UGV -HELP PLEASE-

Hello SFE hackers, programmers, builders and all!!! I've been working on my robot a bit lately and I've been trying to attempt to perfect my obstacle avoidance algorithm. Basically for my robot (I call it the ASRB) I'm using 5 SHARP sensors and servo to detect objects and avoid them. Also on the ASRB I have 5 battery packs, an Arduino Mega, and Uno, an electronic compass, a GPS, and a piezo speaker. There are a bunch of other random things too, but I'll just upload a few pictures later. I've included my code below--

Inside my main loop is my problem:
I've attempted to make an obstacle avoidance algorithm using if statements, but I want to make it more gradual so when the SHARP sensors detect an object 25 Inches away, it turns the servo a degree. Then when it goes to 24, turns the servo 2 degrees and so on and so forth. I've attempted to use for loops and while loops, but I've had no success---Can anyone help? Thanks!!

-Matt

/* Using the 5 IR Distance to control the servo to avoid obstacles

Created August 13th, 2012

Edited last on November 23rd, 2012
--made turns less jerky

*/

#include "pitches.h"

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

#include <DistanceGP2Y0A21YK.h>  //includes the proximity sensor's library

Servo Steering;  //Creates the servo object named "Steering"
Servo Motor;     //Creates the servo object for the motor

DistanceGP2Y0A21YK Dist01;  //defines Dist01 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist02;  //defines Dist02 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist03;  //defines Dist03 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist04;  //defines Dist04 as the sharp GP2Y0A21YK
DistanceGP2Y0A21YK Dist05;  //defines Dist05 as the sharp GP2Y0A21YK

int Prox01;  //creates the integer for the first IR sensor
int Prox02;  //creates the integer for the second IR sensor
int Prox03;  //creates the integer for the third IR sensor
int Prox04;  //creates the integer for the fourth IR sensor
int Prox05;  //creates the integer for the fifth IR sensor

int spd = 100;  //sets the speed of the dc motor

int melody[] = {
  NOTE_F6, 0, NOTE_F6, 0, NOTE_F6, 0, NOTE_F6, 0};  //melody to be played when backing the car up
int noteDurations[] = {
   2,6,2,6,2,6,2,6 };

int ResetLed = 12;    //Reset LED
int ReverseLed = 11;  //Reverse LED
int AlertLed = 13;    //Alert (Backwards) LED

void setup()
{
  Serial.begin(9600);  //opens the serial monitor at 9600 baud
  Dist01.begin(A0);    //tells program "Dist01" will be used by the serial monitor
  Dist02.begin(A1);    //tells program "Dist02" will be used by the serial monitor
  Dist03.begin(A2);    //tells program "Dist03" will be used by the serial monitor
  Dist04.begin(A3);    //tells program "Dist04" will be used by the serial monitor
  Dist05.begin(A4);    //tells program "Dist05" will be used by the serial monitor
 
 
  Steering.attach(7);  //attaches the steering servo to pin 10
  Motor.attach(9);      //attached the motor to PWM pin 9
 
   
  Serial.println("\n  --Sensor01--Sensor02--Sensor03--Sensor04--Sensor05--");

  pinMode(ReverseLed, OUTPUT);
  digitalWrite(ReverseLed, LOW);

  pinMode(AlertLed,OUTPUT);
  digitalWrite(AlertLed, LOW);

  pinMode(ResetLed, OUTPUT);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  delay(300);
  digitalWrite(ResetLed, HIGH);
  delay(300);
  digitalWrite(ResetLed, LOW);
  Motor.write(spd);
 
 
 
}

/*

Although below, it says "getDistanceCentimeter", it really converts the data into Inches---make a very big note for this!!

*/




void loop()
{
  Prox01 = Dist01.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist01)
  Prox02 = Dist02.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist02)
  Prox03 = Dist03.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist03)
  Prox04 = Dist04.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist04)
  Prox05 = Dist05.getDistanceCentimeter();  //Obtains and converts the voltage from the sensor to Inches (Dist05)

Serial.print(" ------");
Serial.print(Prox01);
Serial.print("--------");
Serial.print(Prox02);
Serial.print("--------");
Serial.print(Prox03);
Serial.print("--------");
Serial.print(Prox04);
Serial.print("--------");
Serial.print(Prox05);
Serial.println("------");


if ((Prox01 <= 10) || (Prox02 <= 10))
  Steering.write(125);
else if ((Prox04 <= 10) || (Prox05 <= 10))
  Steering.write(65);
else if ((Prox01 <= 15) || (Prox02 <= 15))
  Steering.write(115);
else if ((Prox04 <= 15) || (Prox05 <= 15))
  Steering.write(75);
else if ((Prox01 <= 20) || (Prox02 <= 20))
  Steering.write(110);
else if ((Prox04 <= 20) || (Prox05 <= 20))
  Steering.write(90);
else if ((Prox01 <= 25) || (Prox02 <= 25))
  Steering.write(105);
else if ((Prox04 <= 25) || (Prox05 <= 25))
  Steering.write(85);
else if ((Prox01 >15) && (Prox02 >15) && (Prox03 >15) && (Prox04 >15) && (Prox05 >15))
  Steering.write(95);

/*else if (Prox03 <= 20)
  FrontAlert();

delay(100);
*/


/* 
else if ((Prox03 <= 15) || ((Prox01 >15) && (Prox02 >15) && (Prox03 >15) && (Prox04 >15) && (Prox05 >15)) || ((Prox01 <=15) && (Prox02 <=15) && (Prox04 <=15) && (Prox05 <=15)))  //If the distance is less then 15 inches
  Steering.write(105);                       //Go Straight
else if (((Prox01 <=15) && (Prox02 <=15)) || ((Prox01 <=15) && (Prox03 <=15)) || ((Prox01 <=15) && (Prox04 <=15)))
  Steering.write(140);
else if (((Prox01 <=15) && (Prox05 <=15)) || ((Prox02 <=15) && (Prox04 <=15)))
  Steering.write(105);
else if (((Prox05 <=15) && (Prox04 <=15)) || ((Prox05 <= 15) && (Prox03 <=15)) || ((Prox05 <=15) && (Prox02 <=15)))
  Steering.write(50);
*/

//delay(100);
}


void BackUpNoise() {                                   //Makes the "Backing Up" noise
  for (int thisNote = 0; thisNote < 9; thisNote++) {
    int noteDuration = 1000/noteDurations[thisNote];
    tone(8, melody[thisNote], noteDuration);
    int pauseBetweenNotes = noteDuration * 1.30;
    delay(pauseBetweenNotes);
    noTone(8);
  }
}


void REVERSE() {                                        //Throws the car in reverse and turns left
  Motor.write(140);
  digitalWrite(ReverseLed, HIGH);
  delay(500);
  Steering.write(95);
  Motor.write(130);
  delay(500);
  Motor.write(110);
  delay(500);
  Motor.write(115);
  BackUpNoise();
  Motor.write(110);
  Steering.write(120);
  delay(1500);
  Steering.write(60);
  Motor.write(100);
  delay(500);
  Motor.write(spd);
  digitalWrite(ReverseLed, LOW);
 
}

void FrontAlert() {                                     //Warns that the car senses something in front of it
  Steering.write(95);
  digitalWrite(AlertLed, HIGH);
  delay(100);
  digitalWrite(AlertLed, LOW);
  delay(100);
  digitalWrite(AlertLed, HIGH);
  delay(100);
  digitalWrite(AlertLed, LOW);
  delay(100);
  digitalWrite(AlertLed, HIGH);
  delay(100);
  digitalWrite(AlertLed, LOW);
  delay(100);
  REVERSE();
 
}


/*  Serial.println("\nDistance in Inches Sensor 1: ");  //Prints the line "Distance in Inches Sensor 1:"
  Serial.print(Prox01);                               //Prints the data from Prox01
  Serial.println("\nDistance in Inches Sensor 2: ");  //Prints the line "Distance in Inches Sensor 2:"
  Serial.print(Prox02);                               //Prints the data from Prox02
  Serial.println("\nDistance in Inches Sensor 3: ");  //Prints the line "Distance in Inches Sensor 3:"
  Serial.print(Prox03);                               //Prints the data from Prox03
  Serial.println("\nDistance in Inches Sensor 4: ");  //Prints the line "Distance in Inches Sensor 4:"
  Serial.print(Prox04);                               //Prints the data from Prox04
  Serial.println("\nDistance in Inches Sensor 5: ");  //Prints the line "Distance in Inches Sensor 5:"
  Serial.print(Prox05);                               //Prints the data from Prox05
  Serial.println("\n-------------------------------");
*/

Hi Matt,

Sorry, but I didn't read all of your code... I just jumped to your comment:

detect an object 25 Inches away, it turns the servo a degree. Then when it goes to 24, turns the servo 2 degrees and so on and so forth

Why not use a simple formula to calculate the avoidance angle? Something simple like the following: 26 degrees - Distance in inches, although this may not work at the extremes.

You could alternatively use if statements to divide the problem up. If 20 to 25 inches away, do what I said earlier... else if 10 to 20 inches away, turn closer to 90 degrees... else go in reverse.

That, or shoot a Nerf gun at it!

How are your five sensors oriented? Ignoring code for a moment, what's your navigation algorithm for using the data from them? If you can describe that, it'll likely make it easier to translate into your sketch.

Sorry about that--what better way to describe it than show pictures??

Very cool. Giraffe might be a better name for it!

I forget... what exactly are you asking about? Are you looking for help writing the algorithm, or for help controlling the motors?

Pat.

Haha thanks! I'm looking to improve the algorithm for obstacle avoidance, but I'm not sure how to do it. Instead of listing a million if statements, I wasn't sure if I could accomplish this with a few for or while loops. Can it be done?

DistanceGP2Y0A21YK Dist01;  //defines Dist01 as the sharp GP2Y0A21YK

Really? I'd never have guessed. :slight_smile:

  Dist01.begin(A0);    //tells program "Dist01" will be used by the serial monitor

That statement does NOT do what the comment says.

You should look at arrays for similar things.

/*

Although below, it says "getDistanceCentimeter", it really converts the data into Inches---make a very big note for this!!

*/

Or get smart and rename the function.

if ((Prox01 <= 10) || (Prox02 <= 10))
  Steering.write(125);
else if ((Prox04 <= 10) || (Prox05 <= 10))
  Steering.write(65);
else if ((Prox01 <= 15) || (Prox02 <= 15))
  Steering.write(115);

This collection of if tests is way too complicated. There has to be a simpler way.

How are the sensors oriented?

On non-program matters, you might want to try mounting the IR rangers in vertical
orientation, rather than horizontal, see page 6 here,

Also note, these devices take 50-msec to average a bunch of readings, so if your bot is
moving or especially rotating, you can get flukey results. It's always best to use different
kinds of sensors on robots for best results, eg adding sonars.

well, I probably should have looked at that before I mounted them....the readings seem to be fine though.......and Paul, sorry about that...I'm a bit lazy with commenting and stuff.....And aren't sonars a lot of money? Do they make cheap ones? And I'm going to end up buying longer ranged sensors to go along with these, probably mounted higher up--and the whole reason I posted on the forum was to make the algorithm better Paul!! :slight_smile: Any ideas?

sora628:
Haha thanks! I'm looking to improve the algorithm for obstacle avoidance, but I'm not sure how to do it. Instead of listing a million if statements, I wasn't sure if I could accomplish this with a few for or while loops. Can it be done?

It seems that the fundamental thing you need to decide is what you want to do with the data from the sensors. Steer towards the one that sees no obstacle? SLAM? Build a more rudimentary map of your surroundings? Turn towards the biggest gap when the forward sensor sees an obstacle? Learning and mapping will be hard to do given the limited memory the Arduino has of course.

The end goal for this robot is for it to be able to navigate threw an "obstacle course" getting from point a to b using the gps, compass, and sensors.

Hi,

Just to make sure I understand what you what the sensors to do, no code thoughts here.

You are using the sensors strictly to determine how close you are to an object and ideal stay a given distance away to avoid collisions, or to stay centered in a "lane" between obstacles.

If I am correct, try mapping the sensor cones on paper. Then put obstacles in to the cones figure out what you would need to do to avoid the collision. In most cases it will be nothing, I think your approach will result in constant left / right servo movements that will result in a net nothing.

I agree with PaulS that there has to be a simpler way to code it. I think you are over thinking the collision sensors.

Start with the front, left and right, ignore the two corner sensors. Set a given distance where you think a collision is likely to occur for each group, all you initially do is check if you are over that range, if you are keep on motoring. At the point any of the groups goes under the range then deal with the problem. I would also set a dead stop range, where you know there is no way to avoid a hit easily. Get the code for the 3 sensors working then deal with the corners.

I may be telling you how to suck an egg, in which case I apologize. But when I look at it I think of driving down nasty little roads in my 4 wheel drive. If in doubt stop look and hedge to one side. Move ahead slow and easy, with small corrections,and always remember hit the mountain don't go over the cliff.

Given the gps and compass, I'd be tempted to start there, assuming that points a and b are known gps coordinates. Solve it for the simple case where there are no obstacles and simply drive along the rhumb line until you arrive. Then add obstacle avoidance algorithms.

The reason I posted here was to get help improving the algorithm!! Is it possible to use for loops to improve the algorithm? My current setup with this algorithm works well with hallways and enclosed spaces with walls. I'm going to have to buy longer ranged sensors soon, but my main provider is out of stock! And I'm not entirely sure where I will mount them.....that block of wood in the front actually adds a lot of weight and makes it hard for the car to steer if it isn't moving--maybe I'll buy a more powerful servo while I'm at it.....

And wildbill, I've tried starting there, and I've also made a post about that, but I've had no one really help me with that, and everyone is saying my GPS will not be accurate enough, and that I'll need to set up GPS differential--but I plan on saving that for the next model of the robot. Is there any way you'd be able to start me off with the basics of going from point a to b? And doesn't that require the use of interrupts? I started to use interrupts and I've not been very successful--Is it possible to have all the statements in the same main loop instead of having to use interrupts?

-Matt-

What I've been trying to suggest is that you're confusing the algorithm with the code. There are very likely more efficient ways to write the code, but the real problem is figuring out how to use the information the sensors are giving you and the code follows from that. Assuming that you were wearing a blacked out motorcycle helmet with the compass, gps and five ping sensor data on a heads up display, what would you do to navigate between your waypoints using that info?

Having said that, what scale is your course? You mention walls and enclosed spaces, so I'm guessing not very big, and likely has no gps reception, so my previous suggestion seems moot. You're down to dead reckoning now, so encoders on the wheels would be nice. Without that, how do you plan to tell where you are and where your destination is? How nasty will the obstacle course be - will you need to navigate a little maze or is it just avoiding a few small cushions strewn along the way?

Depending on the answers to the above, this could turn out to be very hard and likely require additional storage for mapping data, or reasonably easy but with no guarantee that the robot will always be able to find its way.

It isn't really a set made course, most likely going to be a local park (with trees) and another test using this algorithm specifically would be inside my high school hallways, which are about 10 feet from wall to wall. It won't be navigating through tiny obstacles, most likely large courses to get from point a to b, and there will most likely be a large gap between the two points. I'm thinking of using some rotary encoders on the wheels not to see how far the car has traveled, rather to see how fast it is going. I want to keep this as simple as I can, without spending too much. Have you ever heard of the AVC by sparkfun?

well, I probably should have looked at that before I mounted them....the readings seem to be fine though.......and Paul, sorry about that...I'm a bit lazy with commenting and stuff.....And aren't sonars a lot of money? Do they make cheap ones?

Ping sonars cost $25 or so. Rather than spend a lot on many fixed sensors, I've usually mounted
one [of each type] on a panning servo head. The reason I mentioned the measurement problem
with the Sharp IR rangers is that I once built a robot to test that device, although it only had a
single ranger, and a lot of times it tried to climb up the wall instead of turning at it. And it wasn't
my algorithm was the problem either.

The thing about robot sensors is that the people who have the most experience with them realize
they are the real Achille's Heel of robotics, and you need to use multiple redundant types for
best results. Jo Jones goes on and on about this in his book, good read,

It isn't really a set made course, most likely going to be a local park (with trees) and another test using this algorithm specifically would be inside my high school hallways

These are two very different requirements. If the ultimate goal is the park and the distances are large enough, gps and the compass can play a major part. The moment you go indoors though for testing, all that's changed. Can you test outside instead at your school? You can of course do both - use the gps if it's getting signal and revert to dead reckoning if not, but then you've made everything significantly harder.

I usually do testing outside, but given the season and the weather in New York, U.S., it's rather difficult to do that kind of testing---And I don't dare take those electronics outside unless everything is dry and the parts have been somewhat warmed up. The reason I want the longer ranged sensors as well is so I can work the code both inside and out, with accurate results. But the mainly they'll be used outside. And are there any good tutorials on how to get an arduino rc car to work the way I want it to with a GPS? I've definitely seen tens of hundreds of AGV's, but no real tutorials on how to get an AGV from point "a" to "b"

I suspect the longer-range Sharp IR rangers will do even less well than the shorter
range ones with a moving/turning robot, for the reasons mentioned = triangulation
and 50-msec sampling times with multiple samples. OTOH, sonars should do a little
better, especially if you get the ones with larger beam patterns. Check out Maxsonars.