Go Down

Topic: Obstacle avoidance with an UGV -HELP PLEASE- (Read 6 times) previous topic - next topic

sora628

Nov 24, 2012, 07:48 pm Last Edit: Nov 24, 2012, 08:17 pm by sora628 Reason: 1
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: [Select]
/* 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-------------------------------");
*/
-Matt-

patduino

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!
There are 10 types of people in the world, those that understand binary, and those that don't.

wildbill

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.

sora628

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

-Matt-

patduino

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.
There are 10 types of people in the world, those that understand binary, and those that don't.

Go Up