Pages: [1]   Go Down
Author Topic: Tracking algorithm with Ping))) ultrasonic sensor  (Read 2376 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 6
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
#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.
Logged

Massachusetts, USA
Offline Offline
Tesla Member
***
Karma: 201
Posts: 8648
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Send Bitcoin tips to: 1L3CTDoTgrXNA5WyF77uWqt4gUdye9mezN
Send Litecoin tips to : LVtpaq6JgJAZwvnVq3ftVeHafWkcpmuR1e

Offline Offline
Newbie
*
Karma: 0
Posts: 6
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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 ?
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 285
Posts: 25630
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Why don't you add some debug prints to your code so you can see exactly what it is doing?
Logged

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

Pages: [1]   Go Up
Jump to: