Hey,
Working on an object follower instead of an object avoidance now, using ping ultrasound sensor and arduino.. found a pretty cool code online. It works good but after a while my robot seems to have a mind of its own and not do what the code says at times. It starts to roam around spinning, backing up and speeding forward. Im guessing it gets confused sensing too many objects in its way... Any ideas for improvements? check out the code below, feel free to post suggested changes to the code, nothing wrong with learning!
/*
* 07_Object_Following
*
* Makey robot follows an object ahead of it
* by keeping the object at a set distance away.
*
* Makey Robot, MAKE Magazine, Volume 19, p. 77
* Created: June 2009 Kris Magri
* Modified:
*
*/
#define servoPin 10 // Servomotor on pin D10.
#define pingPin 12 // Ping ultrasonic sensor on pin D12.
// 2 drive motors, motor driver TB6612FNG.
#define leftSpeedPin 9 // Left motor speed control, to PWM_A.
#define leftDir1 3 // Left motor direction 1, to AIn1.
#define leftDir2 4 // Left motor direction 2, to AIn2.
#define rightDir1 5 // Right motor direction 1, to BIn1.
#define rightDir2 6 // Right motor direction 2, to BIn2.
#define rightSpeedPin 10 // Right motor speed control, to PWM_B.
#define SERVOCENTER 1500 // Tweak so sensor looks straight ahead.
#define SERVORIGHT 1800 // Servo position when looking right.
#define SERVOLEFT 1200 // Servo position when looking left.
#define MAXSPEED 125 // Limit speed of both drive motors.
// setup
// Delays for 5 seconds and centers servo.
// Gives time so Makey doesn't run away on you.
// Sets I/O pins. Don't set Ping sensor as OUTPUT b/c
// it is used as both in and out in readDistance() subroutine.
void setup()
{
pinMode(leftDir1, OUTPUT); // Set motor direction pins as outputs.
pinMode(leftDir2, OUTPUT);
pinMode(rightDir1, OUTPUT);
pinMode(rightDir2, OUTPUT);
pinMode(servoPin, OUTPUT); // Servomotor as output.
centerServo(); // Move servo to center position.
delay(5000); // Wait 5 seconds.
}
// Main program
// Makey attempts to keep objects at a fixed distance away, if object is
// very far away, Makey will speed up to catch up. If object is just
// a bit too far away, Makey will go forward, but slowly, so as not to
// slam into the object. If object is too close, Makey backs up.
//
// Sets speed of motors in proportion to the error between the desired
// distance and the actual distance reading from rangefinder.
// Calculates error, multiplies it by some constant to obtain a motor
// speed. Takes distance readings to left and right independently,
// then calculates speed and direction for each wheel independently.
//
// Look left, take distance reading, calculate speed & direction
// of left motor, move left motor.
// Look right,take distance reading, calculate speed & direction
// of right motor, move right motor.
// Repeat.
void loop()
{
int leftDistance, rightDistance; // Distance readings from Ping sensor.
int leftError, rightError; // Difference between desired distance
// and actual distance reading.
int leftMotorSpeed; // From 0->255, slow->fast.
int rightMotorSpeed;
int numPulses; // Number of pulses to servomotor.
#define desiredDistance 20 // Desired distance from object (cm),
// Makey tries to keep 20 cm away.
#define multiplier 16 // Tweak for best results.
// error * multiplier = speed
// Error | x16 | x12 | x20 |
// Speed
// 1 16 12 20
// 2 32 24 40
// 3 48 36 60
// 4 64 48 80
// 5 80 60 100
// 6 96 72 120
// 7 112 84 140
// 8 128 96 160
// 9 144 108 180
// 10 160 120 200
// 11 176 132 220
// Note that motor speed will be limited to MAXSPEED by the program.
// Look left.
// Send 10 pulses to move servomotor to the left.
for (numPulses = 0; numPulses < 10; numPulses++)
{
pulseOut(servoPin, SERVOLEFT);
delay(20);
}
// Take a distance reading, looking left.
leftDistance = readDistance();
// Calculate left motor speed and direction settings.
// Error: Negative -- Object too far away, go foward.
// Positive -- Too close, back up. Zero: Perfect.
// Calculate error.
leftError = desiredDistance - leftDistance;
// Calculate motor speed, force positive.
leftMotorSpeed = (abs(leftError) * multiplier);
// Limit motor speed to some maximum.
leftMotorSpeed = constrain(leftMotorSpeed, 0, MAXSPEED);
// Decide what to do and move left motor.
if ( leftError > 0)
// Object too Close -- back up.
{
// Set the speed as calculated.
analogWrite(leftSpeedPin, leftMotorSpeed);
// Set the direction -- left motor backward.
digitalWrite(leftDir1, HIGH);
digitalWrite(leftDir2, LOW);
}
if (leftError < 0)
// Object too far away -- go forward.
{
// Set the speed.
analogWrite(leftSpeedPin, leftMotorSpeed);
// Set the direction -- left motor forward.
digitalWrite(leftDir1, LOW);
digitalWrite(leftDir2, HIGH);
}
// Look right -- move servomotor to the right
for (numPulses = 0; numPulses < 10; numPulses++)
{
pulseOut(servoPin, SERVORIGHT);
delay(20);
}
// Take a distance reading, looking right.
rightDistance = readDistance();
// Calculate right motor speed and direction settings.
rightError = desiredDistance - rightDistance;
rightMotorSpeed = (abs(rightError) * multiplier);
rightMotorSpeed = constrain(rightMotorSpeed, 0, MAXSPEED);
// Decide what to do and move right motor.
if ( rightError > 0)
// Object too close -- back up.
{
analogWrite(rightSpeedPin, rightMotorSpeed);
digitalWrite(rightDir1, HIGH);
digitalWrite(rightDir2, LOW);
}
if (rightError < 0)
// Object too far away -- go forward.
{
analogWrite(rightSpeedPin, rightMotorSpeed);
digitalWrite(rightDir1, LOW);
digitalWrite(rightDir2, HIGH);
}
} // End main program.
// pulseOut
// Send a single pulse to a servomotor for specified duration,
// in microseconds.
void pulseOut(byte pinNumber, int duration)
{
digitalWrite(servoPin, HIGH);
delayMicroseconds(duration);
digitalWrite(servoPin, LOW);
}
// centerServo
// Center servomotor so Makey's Ping sensor is looking
// straight ahead. Move the servomotor to midpoint of its range,
// 1500us (1.5ms).
void centerServo()
{
byte index;
for (index = 0; index < 20; index++)
{
pulseOut(servoPin, SERVOCENTER);
delay(20);
}
}
// readDistance
// Take a distance reading from Ping ultrasonic rangefinder.
// from http://arduino.cc/en/Tutorial/Ping?from=Tutorial.UltrasoundSensor
long readDistance()
{
long duration, inches, cm;
// The Ping is triggered by a HIGH pulse of 2 or more microseconds.
// We 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.
cm = microsecondsToCentimeters(duration);
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 traveled.
return microseconds / 29 / 2;
}