Pages: [1] 2 3 4   Go Down
Author Topic: Obstacle avoidance with an UGV -HELP PLEASE-  (Read 6214 times)
0 Members and 1 Guest are viewing this topic.
New York
Offline Offline
Jr. Member
**
Karma: 0
Posts: 63
digitalWrite( 5, HIGH);
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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



Code:
/* 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-------------------------------");
*/
« Last Edit: November 24, 2012, 02:17:15 pm by sora628 » Logged

-Matt-

Indiana, US
Offline Offline
Full Member
***
Karma: 12
Posts: 204
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi Matt,

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

Quote
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!
Logged

There are 10 types of people in the world, those that understand binary, and those that don't.

New Jersey
Offline Offline
Faraday Member
**
Karma: 67
Posts: 3702
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

New York
Offline Offline
Jr. Member
**
Karma: 0
Posts: 63
digitalWrite( 5, HIGH);
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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



* 100_2298.JPG (753.54 KB, 1536x2048 - viewed 70 times.)

* 100_2299.JPG (758.22 KB, 2048x1536 - viewed 52 times.)

* 100_2300.JPG (762.83 KB, 2048x1536 - viewed 64 times.)
Logged

-Matt-

Indiana, US
Offline Offline
Full Member
***
Karma: 12
Posts: 204
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

There are 10 types of people in the world, those that understand binary, and those that don't.

New York
Offline Offline
Jr. Member
**
Karma: 0
Posts: 63
digitalWrite( 5, HIGH);
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

-Matt-

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 617
Posts: 49463
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
DistanceGP2Y0A21YK Dist01;  //defines Dist01 as the sharp GP2Y0A21YK
Really? I'd never have guessed.  smiley

Code:
  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.

Code:
/*

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.

Code:
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?
Logged

the land of sun+snow
Offline Offline
Faraday Member
**
Karma: 159
Posts: 2916
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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

http://sharp-world.com/products/device/lineup/data/pdf/datasheet/gp2y0a21yk_e.pdf

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.
Logged

New York
Offline Offline
Jr. Member
**
Karma: 0
Posts: 63
digitalWrite( 5, HIGH);
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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!! smiley   Any ideas?
Logged

-Matt-

New Jersey
Offline Offline
Faraday Member
**
Karma: 67
Posts: 3702
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

New York
Offline Offline
Jr. Member
**
Karma: 0
Posts: 63
digitalWrite( 5, HIGH);
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

-Matt-

Offline Offline
Newbie
*
Karma: 0
Posts: 46
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

New Jersey
Offline Offline
Faraday Member
**
Karma: 67
Posts: 3702
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

New York
Offline Offline
Jr. Member
**
Karma: 0
Posts: 63
digitalWrite( 5, HIGH);
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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-
Logged

-Matt-

New Jersey
Offline Offline
Faraday Member
**
Karma: 67
Posts: 3702
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Pages: [1] 2 3 4   Go Up
Jump to: