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-------------------------------");
*/