Arduino Object Follower cool project, need guidance

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;
}

Sounds like a wiring problem to me.

wrt code; well documented!

  // 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);                           
  }

think it is better to set direction first and then speed. And maybe stop before change. so first the LOW then the HIGH

the case leftError == 0 is not handled; probably OK but it leaves a 'gap' in the semantics

furthermore there is a 200 millis timing difference between left and right wheel. So th left decide to backup while the right is still going forward.
Go through your code line by line (comments may be skipped) with paper and pencil and you will see what I mean.

hopes this helps
Rob

Have you been introduced to the servo library?

@Robtillart thanks alot! Im going to add some code to make it back up if lefterror == 0 and righterror == 0. Surprisingly I noticed it would stop moving after tracking a still object for sometime.
Also you are right with the delay between the left and right wheels. Its noticeable when I first turn the robot on it always begins on a slight turn. I'm fairly new to arduino programming, any suggestions on how to correct this delay between the wheels?

use this genral structure:

loop()
{
// MAKE MEASUREMENTS LEFT & RIGHT

// TAKE ACTIONS LEFT & RIGHT
}

Note that with one sweep you can read both distances! that means iso 2 x 10 x 20 = 400 millisec, you only need 10 x 20 = 200 millisec before reacting on the info

bool sweepLeft = true;

loop()
{
  // MAKE MEASUREMENTS
  sweepLeft = !sweepLeft;  // reverse every loop
  if (sweepLeft)
  {
    rightDistance = readDistance();
    for (numPulses = 0; numPulses < 10; numPulses++)
    {
      pulseOut(servoPin, SERVOLEFT);
      delay(20);
    } 
    leftDistance = readDistance();
  }
  else // sweep right
  {
    leftDistance = readDistance();
    for (numPulses = 0; numPulses < 10; numPulses++)
    {
      pulseOut(servoPin, SERVORIGHT);
      delay(20);
    } 
    rightDistance = readDistance();
  }

  // TAKE ACTIONS
  
  leftError = desiredDistance - leftDistance;  
  leftMotorSpeed = constrain( (abs(leftError) * multiplier), 0, MAXSPEED); 

// if you remember the previous speed you may do smarter things than from 100% forward to 100% reverse ... (use pencil and paper to think)

  if ( leftError >= 0)     // Object too Close -- back up. 
  {
    analogWrite(leftSpeedPin, leftMotorSpeed);
    // Set the direction -- left motor backward.            
     digitalWrite(leftDir2, LOW);  
     digitalWrite(leftDir1, HIGH);                          
  } 
  else         // Object too far away -- go forward.                                    
  {
    analogWrite(leftSpeedPin, leftMotorSpeed);    
    // Set the direction -- left motor forward.         
    digitalWrite(leftDir1, LOW);                           
    digitalWrite(leftDir2, HIGH);
  }

  rightDistance = readDistance();
  rightError = desiredDistance - rightDistance;
  rightMotorSpeed = (abs(rightError) * multiplier);
  rightMotorSpeed = constrain(rightMotorSpeed, 0, MAXSPEED);  

  if ( rightError >= 0)   
  // Object too close -- back up.                                    
  {
    analogWrite(rightSpeedPin, rightMotorSpeed);          
    digitalWrite(rightDir1, HIGH);                           
    digitalWrite(rightDir2, LOW);
  }
 else  // Object too far away -- go forward.                                      
  {
    analogWrite(rightSpeedPin, rightMotorSpeed);          
    digitalWrite(rightDir1, LOW);                           
    digitalWrite(rightDir2, HIGH);
  }
}

IN a more elaborate way you can fill an array by making a distance measurement in every of the 10 steps. Then you can steer to the place with the most space.

Hopes this helps
Rob

http://letsmakerobots.com/node/26101