Let's Make Robots!

Programming help, sharp IR mounted on servo

Hello

I am trying to control my bot using a sharp IR sensor mounted on hitec servo with arduino mega. The pseudocode i have so far is as follows:

1. Set servo at 90deg and read IR sensor value
2. 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.
3. 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?

 

I know this code is extremely crappy, but what i was hoping, if it works, was to modify it so that the servo pans say in intervals of 30 degrees, takes a reading and stores it. It then calulates based on the readings the best route(ie with least obstacle distance) and moves the bot in that direction. Im not very sure how to implement this either.

 

Any help would be appreciated. Thank you

Code:

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

 

 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.
Ok, that seems to be exactly what the problem is. Thanks for the suggestion. Im going to give it another go.

Oki..i got it work, and im guessing it cant go wrong this time... the code so far...

void loop() { 
 scan_center();
 while (center_val < 45)  //keep scanning center and going forward until obstacle is detected..

{
   go_straight();
   scan_center();
 }
 
// if center > 45 , then above loop exits, and the bot does the following:
 
 motor_stop(); // stop for a bit
 delay(200);
 
 scan_around(); //scan left and right and record both values
 servo1.write(90); 
 
 if (left_val < 50 && left_val < right_val) //if there is a better chance for turning left, do so
 {
   turn_bot_left();
   delay(250);
   motor_stop();
   delay(200);
 }
 else if (right_val < 50 && right_val < left_val) // if right seems better, turn right
 {
   turn_bot_right();
   delay(250);
   motor_stop();
   delay(200);
 }
 else //if both left, right and center paths are blocked, go reverse and turn 180
 {
   go_reverse();
   delay(600);
   motor_stop();
   turn_bot_180();
   delay(600);
   motor_stop();
   delay(20);
}
} // end of main routine..repeat


// 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. The servo is mounted upside down, hence the range is from 160 - 20 (or 180 - 0)
void scan_around() 
{
  servo1.write(160);
  delay(750);
  left_val = analogRead(IR_pin);
  servo1.write(20);
  delay(800);
  right_val = analogRead(IR_pin);

 

Video here > http://www.youtube.com/watch?v=KtxbcUZFjpk

 

Thanks for all the suggestions :)