I am trying to control simple bot using a sharp IR sensor mounted on hitec servo with arduino mega. The pseudocode i have so far is as follows:
Set servo at 90deg and read IR sensor value
If val > 200, then stop the bot for a second
a. Turn servo 0 deg completely and read ir val
b. if val < 200 then turn bot left
c. else turn servo 180 deg and read ir val
d. if val < 200 then turn bot right
e. else if cant turn right or left, reverse a bit and then turn the bot 180 degree and go straight.
repeat
What happens now is that the bot goes forward, if the IR reading comes within 200, the bot turns 180 and goes in that new direction. It doesnt pan the servo left or right. On two occasions it did turn the servo, but that was only to the left (0 deg).
Have i missed something here? Im just learning how to program, so perhaps someone could suggest a far better method? Thanks very much
#include <MegaServo.h>
#define IR_pin 0
int IR_val = 0;
MegaServo servo1;
void setup() {
Serial.begin(9600);
pinMode(50,OUTPUT);
servo1.attach(50);
servo1.write(90); //center servo intially
}
void loop() {
IR_val = analogRead(IR_pin);
if (IR_val > 200) // obstacle detected
{
motor_stop(); // Stop both motors...
delay(1000); // ...and pause for a second
servo1.write(0); // Turn servo left completely and scan
IR_val = analogRead(IR_pin);
if (IR_val < 200) // if left seems to be clear
{
turn_bot_left();
delay(535); // Turn till bot has turned 90 deg left
servo1.write(90); // center the servo
}
else
{
servo1.write(180); // Turn servo completely right and scan
IR_val = analogRead(IR_pin);
if (IR_val < 200) // if right seems to be clear
{
turn_bot_right();
delay(540); // Turn till bot has turned 90 deg right
servo1.write(90); // center the servo
}
}
servo1.write(90); // position servo back at center
go_reverse();
delay(500); // wait for sometime till it reverses
turn_bot_180();
delay(1030);
} // end of routine that checks if IR_val > 200
else
go_straight();
Serial.println(IR_val);
}
/* Motor movements...removed to save space!! Btw..all these functions work fine. I've tested them.
void motor_stop()
void go_straight()
void go_reverse()
void turn_bot_right()
void turn_bot_left()
void turn_bot_180()
*/
it looks like you are taking the IR readings too soon after commanding the servo to move. I suggest you wait around a half second after you write a new servo position before doing the next IR read.
.
.
servo1.write(0); // Turn servo left completely and scan
delay(500);
IR_val = analogRead(IR_pin);
.
.
servo1.write(180); // Turn servo completely right and scan
delay(500);
IR_val = analogRead(IR_pin);
.
.
It does scan left and right, if the reading comes above 200. But what happens is, say it went into a narrow gap with both left and right edges of the wall very close to the bot; it will stop, scan left, turn the bot 180, centre the servo and go forward. I wanted it to stop, scan left, scan right and then reverse a bit and then finally turn 180. It seems to skip the scanning right and reversing bit completely.
How does the narrow gap differ from the robot approaching a dead end corner.
It may help if you had a table that showed all of the possible cases you expect to encounter, the IR readings associated, and what action you want to take. You may then want to structure your code accordingly.
Left Center Right Action
<200 <200 <200 move forward
<200 >200 <200 turn left
<200 >200 >200 turn left
Mem, this is what i meant when describing a narrow gap.
In case it ran into something like this, i wanted it to turn 180 and then proceed from there.
I am going to try and code what you have suggested. Would it be easier to do it with an ' if...else' or a 'switch...case' set of statements? Just asking coz i dont have much programming experience.
mmm, I wrote some code, but i think its gotten worse now..
void loop() {
go_straight();
scan();
if (left_val < 200 && center_val < 200 && right_val < 200) // Left, center and right paths clear??
{
go_straight();
}
else if (left_val < 200 && center_val > 200 && right_val < 200) // If center is blocked, then turn left or right
{
turn_bot_left();
servo1.write(90); // turn servo back to center
delay(500);
}
else if (left_val < 200 && center_val > 200 && right_val > 200) // If center and right are blocked, turn left
{
turn_bot_left();
servo1.write(90);
delay(500);
}
else if (left_val > 200 && center_val > 200 && right_val < 200) // If center and left are blocked, turn right
{
turn_bot_right();
servo1.write(90);
delay(500);
}
else if (left_val > 200 && center_val > 200 && right_val > 200) // All paths blocked..turn 180, center servo..
{
motor_stop();
delay(1000);
turn_bot_180();
servo1.write(90);
delay(500);
}
}
void scan()
{
servo1.write(0);
delay(650);
left_val = analogRead(IR_pin);
servo1.write(90);
delay(600);
center_val = analogRead(IR_pin);
servo1.write(180);
delay(600);
right_val = analogRead(IR_pin);
if(left_val < center_val && left_val < right_val) // Trying find out smallest of three values...
{
min_val = left_val;
}
else if(center_val < left_val && center_val < right_val)
{
min_val = center_val;
}
else
{
min_val = right_val;
}
}
/* motor movements...
void motor_stop()
void go_straight()
void go_reverse()
void turn_bot_right()
void turn_bot_left()
void turn_bot_180()
*/
What happens now is that it goes straight, scans and even if it comes to an obstacle in the front, it tries to keep going. It still keeps scanning, then it keeps turning 180...ie (crazy 360 turns)...
in the function 'scan' give below, i've setup a variable that stores the minimum value captured by the sensor. How do i get the bot to turn in that specific direction? Thank you
