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.
starscorched's picture

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

starscorched's picture
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..

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

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

I tried lowering the value of the center reading..( while center_val < 50 condition); 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 really want it to do is to read values in increments of 20 or 30 degrees, and then turn into the most suitable path. I think ive figured out how to do the reading part, but im not so sure about how to make it actually turn into that specific angle. Any suggestions on how to go about doing this?

Thanks very much.
Gareth's picture

One simple idea to improve the current setup .....................

From your Video you detect 0°....90°.....180°  ........ does the bot need to detect far left and far right ?

Try narrowing the angle .... ie set the servo to 45° ......90°....... 135° (or 30....90...150) this will allow it to detect the corners of the bot  and will speed up the scan rate.

edit :- sorry ezekiel181 i should have read your post before posting this - its so easy to duplicate ideas.....(i will read in more detail next time)

starscorched's picture

lol!

 

Thanks a mill for the suggestions Gareth! Cheers!

ezekiel181's picture

Your bot moves too fast for the scanning to keep up with. If it`s inside the loop and moving forward when it starts scanning by the time the scan is finished it`s too late.

Your robot doesn`t need to check its left and right ALL the time unless it`s looking for something specific. If you just want it to avoid obstacles, have it go forward until it detects an obstacle ahead, make it stop and scan left/right and then it can decide where to go.

Alternatively you could have the robot continuously scanning a much smaller arc, say 20 degrees either side of center. Then you can get him to scoot around obstacles without slowing down.

starscorched's picture
Ok, that seems to be exactly what the problem is. Thanks for the suggestion. Im going to give it another go.
starscorched's picture

@ezekiel181 & Gareth > The introdcution of a delay before the analogRead is exactly what it needed. Thank you very much.

 I tried modifying the code based on this table below:

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

 

Its all gone completely haywire now....all it does is that it bangs straight into a wall, then starts turning 360 degrees. 

Im gonna check and see if it can work with a very simple case of stopping when it encounters an obstacle. I might actually upload a vid before i do that...its hilarious!! :)

 

mmm...crazy code below...

 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) // Routine to find out smallest sensor reading and store it in a variable. Perhaps the bot can then be programmed to turn in that direction...

  {
    min_val = left_val;
  }
  else if(center_val < left_val && center_val < right_val)
  {
    min_val = center_val;
  }
  else
  {
    min_val = right_val;
  }
 
}

}

Gareth's picture

I would suggest fixing the IR_val to a value above 200 (ie direct after  IR_val = analogRead(IR_pin) put on next line IR_val=250) see what happen and then......... fix it to value below 200 and see what happen. To eliminate the sharp sensor.

You may also need a small delay after the servo commands - as they take a while to settle - before you take a sharp reading.

ezekiel181's picture
I`ve never used that library but I don`t think the servo position routines are blocking. Try putting some delays after assigning the servos a new position, perhaps they simply don`t have time to move and it`s affecting the readings.
robologist's picture

Try removing the

pinMode(50,OUTPUT); 

line, as I don't see it used in other servo examples. Think pinmode is for digital IO, not PWM.