Choosing closest object (PING Ultrasonic Distance Sensor) & Servo Motor

Hi I am rather new to the Arduino community, so I would appreciate any help or guidance. :slight_smile:

I am currently working on a project where I must design and construct an autonomous robot that, amongst others, should choose between two objects (or more).

I bought the PING))) Ultrasonic Distance Sensor kit from Parallax (http://www.parallax.com/product/570-28015) and have started the following code for the programming:
.
//PING_assembly variables
#include <Servo.h>
Servo myservo; // create servo object to control a servo
int pos; // variable to store the servo position
const int pingPin = 41; // pin number of the ping’s output
char see_bit = 0; // bit to indicate that object is seen
int save_pos = 0;
const int stored_distance1 = 320;
int stored_distace2;

void setup()
{

//PING setup
myservo.attach(43); // attaches the servo on pin 9 to the servo object
Serial.begin(9600); // initialize serial communication
Serial1.begin(115200);
myservo.write(85); // tell servo to go to position in variable ‘pos’
}

void loop()
{

if (see_bit == 0) {
for(pos = 10; pos < 170; pos += 5) // goes from 0 degrees to 180 degrees
{ // in steps of 1 degree
myservo.write(pos); // tell servo to go to position in variable ‘pos’
delay(15); // waits 15ms for the servo to reach the position
int testDistance = pingread(); //get object distance
if (testDistance <= stored_distance1 ) {
see_bit = 1;
save_pos = pos;
break;
}
}
for(pos = 170; pos>=10; pos-=5) // goes from 180 degrees to 0 degrees
{
if (see_bit == 0) {
myservo.write(pos); // tell servo to go to position in variable ‘pos’
delay(15); // waits 15ms for the servo to reach the position
int testDistance = pingread(); //get object distance
if (testDistance <= 10) {
see_bit = 1;
save_pos = pos;
break;
}
}
}
}
else {
int testDistance = pingread();
Serial.println(pos,DEC);
delay(500);
}

What I am trying to do is, place the PING in an open room, and start the robot. It should then move forward (I have already completed the motor setup) and constantly search for an object. If, say, two objects are in its path, it should ‘store’ each object’s distance and then choose to move to the closest of the two.

What additional code should I add?

Furthermore, I have a function to derive the distance:

//PING functions

int pingread()
{
// establish variables for duration of the ping,
// and the distance result in inches and centimeters:
long duration, cm;

// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);

// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);

// convert the time into a distance
//inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);

//Serial.print(inches);
//Serial.print("in, ");
Serial.print(cm);
Serial.print(“cm”);
Serial.println();

delay(100);
return cm;
}

long microsecondsToCentimeters(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}

Thank you. :blush: :blush:

I would take a set of readings, 5, 10 or 15 degrees apart, and put each reading into an array. Then go through the array to find the minimum distance. Once you know the which element holds the minimum distance you use the index of that element to calculate where to turn towards.

for(pos = 10; pos < 170; pos += 5)  // goes from 0 degrees to 180 degrees
    {                                  // in steps of 1 degree

If you are going to have comments that state the obvious. at least get them right!

During the calibration my servo was a bit off, so for(pos = 10; pos < 170; pos += 5) actually makes it turn from 0 degrees to 180 degrees. Will change the increments though, thanks. Still experimenting a bit