Go Down

Topic: Tracking algorithm with Ping))) ultrasonic sensor (Read 6586 times) previous topic - next topic


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 :
Code: [Select]

#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()
    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);

// now send the 5uS pulse out to activate Ping)))
digitalWrite(signal, HIGH);
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()
  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.
Send Bitcoin tips to: 1G2qoGwMRXx8az71DVP1E81jShxtbSh5Hp
See who has no social life: https://forum.arduino.cc/index.php?action=stats :)


Thank you,
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?

Go Up