Go Down

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

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 :
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()
{
    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.

johnwasser

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: 1L3CTDoTgrXNA5WyF77uWqt4gUdye9mezN
Send Litecoin tips to : LVtpaq6JgJAZwvnVq3ftVeHafWkcpmuR1e

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 ?

AWOL

Why don't you add some debug prints to your code so you can see exactly what it is doing?
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

Go Up