I am using the Parallax BOE shield for arduino and am trying to create a tracking algorithm with a Ping))) ultrasonic sensor mounted on a servo. I am trying to imitate this sumo bot's algorithm : http://www.societyofrobots.com/robot_sumo.shtml (http://www.societyofrobots.com/robot_sumo.shtml)
But I cannot get my code to work, I tried re-doing it but it won't work, the servo turret simply gets stuck at the far end and stops reacting. I think it is because I don't fully understand the servo object. Also, my servo isn't moving as fast as the one on the sumo bot which has a Sharp IR mounted on it.
Here is the commented code :
#include <Servo.h> //Include servo library
Servo turret; //Create servo objects
Servo servoLeft; //Continuous rotation servos for robot differential steering
int pos = 90; //Variable to store the servo (sensor turret) position
// Connect Ping))) signal pin to Arduino digital 10
int signal=10; //Variables to calculate distance with Ping))) ultrasonic sensor
unsigned long pulseduration=0;
turret.attach(11); //Attach servo
pinMode(signal, OUTPUT); //Set Ping))) pin as output
// set pin as output so we can send a pulse
// set output to LOW
// now send the 5uS pulse out to activate Ping)))
// now we need to change the digital pin
// to input to read the incoming pulse
// finally, measure the length of the incoming pulse
if (pos>135) //If servo far right
maneuver(0,200,20); //Go right towards target
pos=pos-1; //While going right turn sensor left
else if (pos<45) //If far left
maneuver(200,0,20); //Go left towards target
pos=pos+1; //While going left turn sensor right
else //If straight ahead
maneuver(200,200,20); //Drive forward
void scan() //Scan code
// if sharp IR detects object
// scanning IR turns left
// else //no object detected
// scanning IR turns right
// //robot motion code
// if scanner is pointing far left
// robot turns left
// else if scanner is pointing far right
// robot turns right
// else //scanner pointing forward
// robot drives straight
// get the raw measurement data from Ping)))
// divide the pulse length by half
// now convert to centimetres. We're metric here people...
distance = int(pulseduration/29);
if (pos > 20) //If doesn't detect object
if (pos >10) //Overflow protection
pos = pos - 2; //Turn right
else //Full right
pos = 170; // Reset left
else //detects object
if (pos <170) //Overflow protection
pos = pos +2; //Turn left
turret.write (pos) ; //Turn servo
void maneuver (int left,int right,int ms) //speed between -200 and 200; time in ms //Function to control drive servos
// Full back Linear Stop Linear Full forward
// -200 -100........0...........100 200
servoLeft.writeMicroseconds (1500 + left);
servoRight.writeMicroseconds (1500 - right);
if (ms == -1) //if ms == -1 stop the robots
Thank you very much for helping me, I am just starting out.
IR is faster than sound so you may want to take larger angle steps between distance readings.
servo.write(position) sets a servo to a position between 0 and 180 degrees.
...unless the servo has been modified for 'continuous rotation'. In that case the servo always thinks it is at (about) 90 degrees, even when it isn't. If the servo.write() tries to send it to 80 degrees it will turn slowly in a direction that would normally get it to 80 degrees and since the internal reference says 90 it will continue to turn in that direction. For 60 it goes faster. For 100 it goes the other way (trying to get to 100) and for 120 it goes faster. You can get it to stop if you send the value (near 90) that the servo thinks it's already at.
I know understand better how the continuous rotation servos work but I still don"t know where the problem is coming from. Does anyone know why my code isn't working ?
Why don't you add some debug prints to your code so you can see exactly what it is doing?