Code not working properly > arduino + sharp IR mounted on servo

[starscorched]

Hello

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:

1. Go straight
2. scan left (0 degrees), center (90 degrees) and right (180 degrees) and read IR values
3. Limit range is 200, and based on this table, take action:

Left  Center Right  Action
<200 <200  <200  move forward
<200  >200 <200  turn left
<200  >200 >200  turn left
>200  >200 <200  turn right
>200  >200 >200  turn bot 180 and go straight

4. repeat

Code: [Select]

#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() {
   
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 bot 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...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()
*/

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

Two questions:
1. Could someone please give me a hand with the code, as i am new to programming?
2. In the function 'scan', 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?

Perhaps this code is extremely rudiementary, but id like to see this work and then try something like panning the servo in steps of 30 degrees, storing values and then calculating optimal path based on those readings.

Thanks very much for reading.

[wil.hamilton]

Have you ever tested code where you just have to robot go forward until it sees something blocking its path and then just stop?  If you havent done this I would try it first, to make sure it works.

From looking at the code, I'd say its something with the way go_straight() is working in combination with scanning and then the logic based on a scan.

Personally, i would change the logic of the program to something like this:

Code: [Select]

loop{

while(sensorReading > 200)
{
     go_straight();
}

scan();

//make turn based on scans


}

yea, thats sloppy pseudo code, but you get the general idea, just have the bot go forward until it cant anymore, then scan and turn based on the reading of the scan.

[starscorched]

I must try what u suggested; will try and see if it can get moving straight until it faces some obstacle.
Thank you

[starscorched]

Ok..i've changed the code to test for basic movement/detection

If the bot comes within a reading of 100, it stops, turns 180, scans and goes straight..

the code..

Code: [Select]

void loop() {

 scan(); // scan left, center and right
 while (center_val < 100)
 {
   go_straight(); // keep going straight until value of reading at center postion > 100
   scan();
 }
 motor_stop(); // if reading taken > 100, then stop motors for a second
 delay(1000);
 turn_bot_180(); // turn bot 180 degrees..
 delay(400); // wait until it completes turn...
 motor_stop(); // stop for a second..and loop..
 delay(1000);
}
 
This is what it does...

[youtube]http://www.youtube.com/watch?v=FO-h6mz1X7M[/youtube]

I tried lowering the value of the center reading..(center_val < 50); it kinda stops before it bangs into the wall. I am going to try and put in other conditions, ie to turn based on most feasible path.

Is there anyway i can speed up entire scanning process? What i would like it to do is to read values in increments of 20 or 30 degrees, and then turn into the most suitable path. Any suggestions on how to go about doing this?

Thanks very much.

[wil.hamilton]

i noticed in the video you keep scanning while you move forward,  i would only have it scan once you see something in front of you.  otherwise keep the sensor facing forward so you dont run into anything.  or, you could have the robot scan, go whichever direction for a bit, stop and scan again and then continue to do this.

[starscorched]

I think i got it working the way i wanted this time. Like you suggested, it keeps going straight until some obstacle comes across. Then it scans left or right and decides from there which direction to take. Code below

Code: [Select]


void loop() {
 scan_center();
 while (center_val < 60)
{
   go_straight();
   scan_center();
 }

// if center > 60 , then above loop exits, and the bot does the following:

 motor_stop();
 delay(150);
 
 scan_around();
 servo1.write(90);
 
 if (left_val < 60 && left_val < right_val) // If left has more room, turn left...
 {
   turn_bot_left();
   delay(450);
   motor_stop();
   delay(150);
 }
 else if (right_val < 60 && right_val < left_val) // if right has more room, turn right
 {
   turn_bot_right();
   delay(450);
   motor_stop();
   delay(150);
 }
 
 else // if both left and right are blocked, reverse a bit , turn 180 and proceed...
 {
   go_reverse();
   delay(700);
   motor_stop();
   turn_bot_180();
   delay(900);
   motor_stop();
   delay(150);
}
}

// Position servo at the center (90 degrees) read IR values

void scan_center()
{
  servo1.write(90);
  delay(500);
  center_val = analogRead(IR_pin);
}

// Turn servo left (160 degrees) and right (20 degrees), and read & store IR values. I've mounted the servo upside down, hence angle is from 160 - 20 (or 180-90)

void scan_around()
{
  servo1.write(160);
  delay(750);
  left_val = analogRead(IR_pin);

  servo1.write(20);
  delay(800);
  right_val = analogRead(IR_pin);

}


Many thanks for your help.