Hello,
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
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
Servo servoRight;
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
int distance;
unsigned long pulseduration=0;
void setup()
{
servoRight.attach(12);
servoLeft.attach(13);
turret.attach(11); //Attach servo
pinMode(signal, OUTPUT); //Set Ping))) pin as output
}
void measureDistance()
{
// set pin as output so we can send a pulse
pinMode(signal, OUTPUT);
// set output to LOW
digitalWrite(signal, LOW);
delayMicroseconds(5);
// now send the 5uS pulse out to activate Ping)))
digitalWrite(signal, HIGH);
delayMicroseconds(5);
digitalWrite(signal, LOW);
// now we need to change the digital pin
// to input to read the incoming pulse
pinMode(signal, INPUT);
// finally, measure the length of the incoming pulse
pulseduration=pulseIn(signal, HIGH);
}
void loop()
{
scan();
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)))
measureDistance();
// divide the pulse length by half
pulseduration=pulseduration/2;
// 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
delay(1);
}
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
{
servoLeft.detach();
servoRight.detach();
}
delay(ms);
}
Thank you very much for helping me, I am just starting out.