SparkFun Forums 

Where electronics enthusiasts find answers.

Have questions about a SparkFun product or board? This is the place to be.
By sora62896
#152235
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!!

Code: Select all
/* 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-------------------------------");
*/
By Mee_n_Mac
#152236
Are you asking why your code doesn't work as intended or asking if there's a better way to code to implement what you describe ? Or asking what's a good way to implement obstacle avoidance ?

I don't know how you have your 5 sensors arranged so it's hard to comment on what to do when sensor X "sees" something ahead. But let's assume that at some distance "D" you want the 'bot to turn it's wheels some angle "A". Furthermore if the object continues to be detected at closer and closer distances, you want the angle to be increased. Presumably at some point the 'bot will have altered course enough that the object is no longer ahead and so isn't detected and the steering goes back to zero (straight ahead). If you want the angle to be inversely proportional to the detected distance, why not just compute the angle ? Here's some pseudo-code that illustrates the general idea.
Code: Select all
If objected detected = true
   If distance < D
      steering_angle = k*(D-distance)+A
   else
      steering_angle = 0
else
   steering_angle = 0
k is a constant that sets the steering angle/inch or steering angle/cm or ... you get the idea. When the distance = D the result is A degrees. If the distance decreases to D-1, then the result is k + A degrees, at D-2.5 you get 2.5*k + A, etc, etc. This works gradually, increasing the steering angle as the distance decreases and not just at 1 inch (or 1 cm or 1 unit) intervals.

I would probably do some other things to make the 'bots response less twitchy to false detections but I think the above is the bare minimum to do what you've asked for.

ps - How are you using the 5 sensors to determine which way (left or right) to turn ?
By Mee_n_Mac
#152241
It would seem that some algorithm using the 5 sensors would have a good chance of steering the 'bot away from simple obstacles, like a wall or a can or rock in it's path. I don't know the sensors field of view so I can't comment on how you've got them "aimed" but it would seem a simple star would be to go away from the direction (left or right) that has detection(s). If both sides detection something then you might as well turn towards the side where the distance is the largest.
By sora62896
#152242
If you look at the code, I've already done that!! The way the statements in the if/else statements are ordered, the car turns to the largest gap, and stays away from the closest object--I want to know how to use a for or while loop to get the car to VERY gradually turn, based on the closeness of the object
By Duane Degn
#152249
Why not use an angorithm to turn based on how close the obstical is.

Instead of "digitalWrite(constant);"

Assuming "digitalWrite(constant);" makes the car turn with a large value making the car turn more than a small value.

use:
" x = SOME_CONSTANT / distance;
digitalWrite(x);"

So as the distance increases the amount of turn decreases.

This is known as proportional control (I guess it could be argued this is really inverse proportional control).

(Sorry, I don't work with Arduinos much so I'm not sure if I have the syntax correct.)
Last edited by Duane Degn on Sat Nov 24, 2012 6:00 pm, edited 1 time in total.
By Mee_n_Mac
#152250
I wouldn't use nested If statements or a for loop to compute the steering angle. I'd use the the distances to decide to turn left or right or go straight and compute the steering angle deviation from straight ahead as I did above. Let's say 0 is straight ahead, negative steering angle is turning left and positive angle is turning right. Use the distances to set a new variable, L_R, where a left turn is -1, a right turn is 1 and straight ahead is a 0. Then compute the steering angle by (a slight modification to what I had above) :
Code: Select all
   If distance < D
      steering_angle = L_R*(k*(D-distance)+A)
   else
      steering_angle = 0
By sora62896
#152262
I don't fully understand--can we put in the actual variables I'm using just to make sure i've got this down? Straight is 95 degrees, any thing less is left, more is right as you stated. But I don't understand what L_R is; Is that a new variable to store the desired direction? And I'm still not understanding what "K" is--Can you explain? Can you break down the variables to help me understand what I'm doing here?
By Duane Degn
#152273
Let's see if I can save Mee_n_Mac some typing.

D would be distance an object needs to be in order to start avoiding it. Let's assume you set it equal to 25 somewhere earlier in your code.

You would also set the value of "L_R" base on which side the ostacle was found. For example let's say you found the obstacle was on the right (so you want to turn left) so you set "L_R" to -1.

"k" is some constant to make the math turn out correctly. I think we can use one for now.

Set "A" to 95 since that's straight ahead.

So now let's say your sensor picks up an obstacle on the right at a distance of 21. Let's run it through Mee_n_Mac's code (modified based on the additional information you've given).
Code: Select all
   If distance < D
      steering_angle = (L_R*(k*(D-distance)))+A
   else
      steering_angle = A
So now we end up with:
"steering_angle = ((-1) * (1 * (25 - 21)))+ 95"
Which gives us a steering_angle of 91 or slightly left.

You could get rid of this part of the code:
Code: Select all
   else
      steering_angle = A
By either setting the distance to "D" when no obstacle was detected or by setting "L_R" to zero when no obstacle was detected since the above code evaluates to 95 (straight ahead).

If you want the robot to turn sharper when seeing an obstacle, use a larger value for k.
By jremington
#152278
Looks like you've built a fun robot!

Mathematically the expression given by Mee_n_Mac resembles a linear repulsive force, except that it changes sign if distance>D. Repulsive forces, modeled as an inverse spring: -k/(distance_to_object), or repulsive energies: k/(distance_to_object^2) are often used in robotic obstacle avoidance as they are easy to calculate and allow for smooth response. The positive constant k is chosen by experiment.

There are a number of free and/or open source driving simulators that allow you to experiment with steering algorithms. They are very useful, as they eliminate lots of code burning, vehicle crashes, etc. See an overview at: http://www.templetons.com/brad/robocars/simulator.html

I found MobotSim, featuring selectable ranging indicators, differential steering and programmable avoidance algorithms, to be quite easy to use and very instructive. A demo version is downloadable here: http://www.mobotsoft.com/?page_id=9

There are far more advanced open source programs (google "robot simulation" etc. ) such as http://sourceforge.net/projects/usarsim/
By Mee_n_Mac
#152297
sora62896 wrote: Can you break down the variables to help me understand what I'm doing here?
Hopefully "DD" has explained the equation well enough it now makes sense. I'm not sure I can add anything useful and I sure don't need the typing practice. :mrgreen: Again it simply does what I think you asked for in the OP.
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
Whether that reaction to avoid collisions is the best possible reaction is another question, but before going down that route you should become comfortable with the idea already proposed. In summary the code should ;
- read the sensors
- interpret the data from all 5 and decide which direction to go (continue straight, turn left or right)
- calculate the steering command using the above decision and how close to the obstacle (if any) the 'bot is.
- wash, rinse, repeat
By Mee_n_Mac
#152298
jremington wrote:I found MobotSim, featuring selectable ranging indicators, differential steering and programmable avoidance algorithms, to be quite easy to use and very instructive. A demo version is downloadable here: http://www.mobotsoft.com/?page_id=9
Interesting ! I was starting to setup an Excel SS to do a simple simulation of the 'bot in the post (I don't have MatLab at home) but the above seems both easier to use and more flexible in constructing obstacles. Does it work only with diff drive 'bots or can the model be modified to include a steering/car-like 'bot as in this thread ? Do the models for the sensors include some FOV modeling or just a simple straight-ahead-line-to-whatever model ?
By jremington
#152301
Sadly, differential steering only, however the ranging sensors have adjustable beamwidth and range, so they are quite realistic.

Here is a list of features, taken from the MobotSim site. I think it is worth the asked-for $30, but you get 30 days to decide. With the simple BASIC language, in just part of a day's work I was able to implement an obstacle-avoidance algorithm that followed a minimum energy path through repulsive obstacles, and the path taken was quite smooth!
Unlimited number of robots and obstacles.
Several obstacle shapes (line, rectangle, round rectangle, arc, ellipse, sector, chord) and free-hand drawing.
Mobots are differential drive.
Simulation of Ranging Sensors (typically ultrasonic sensor)
Flexible Configuration of Mobots – Platform diameter, wheels diameter, distance between wheels, number of ranging sensors, angle between sensors.
Configuration of ranging sensors – radiation cone, range, misreading percentage.
Each mobile robot has a configurable grid to map the environment. Automatic Certainty Grid method available with use of Ranging Sensor functions.
Development of simulation through quick and easy BASIC macro writing. Fully Visual Basic for Applications – compatible.
Control program flow and set breakpoints, step-over, step-into, and step-out.
View debug info such as watch variables, view the call-stack and loaded modules.
Easy integration of third parties ActiveX controls and dll files. You can add specific tools to the BASIC editor developed to make use of Fuzzy Logic, Genetic Algorithms, Neural Networks, etc.
By Mee_n_Mac
#152309
I was curious because to answer the next question the OP will come up with isn't so easy. There are all sorts of obstacles to overcome and it's not at all obvious to me what the best approach is to handle them all given the array of sensors he has. Especially if the intent of the 'bot isn't to roam aimlessly, avoiding hitting things along the way ... but rather to go from A to B, getting around obstacles in the way. In this case you have to avoid the obstacle but return to a "good" course afterwards to get to B. This is where a sim program can shine, pointing out errors in your logic that you'd find hard (or impossible) to test for using the real 'bot in real life.