Follow Objects with Ultrasound

Instead of finding and avoiding obstacles, how about making the ActivityBot follow a target instead?  Same sensor system, different code.  All the program has to do is check for a desired distance, and if the object is further away than that, move forward.  If the object is closer than the desired distance, move backward.  If the object’s distance is equal to the desired distance, stay still.

 

Following Objects

This example makes the ActivityBot try to keep a 32 cm distance between itself and an object in front of it.  With no object detected, the robot will go forward until it finds one, then hone in and maintain that 32 cm distance.  If you pull the object away from the ActivityBot, it will go forward to catch up.  Likewise, if you push the object toward it, it will back up to restore that 32 cm distance.

  • Click SimpleIDE’s Open Project button.
  • Open Follow with Ping))) from ...Documents\SimpleIDE\Learn\Examples\Robots\ActivityBot. 
  • Click the Load EEPROM & Run button.
  • Send it toward a wall, and verify that it stops and maintains a 32 cm distance.
  • Send it toward an object, and after it locks on, try pushing the object toward and away from the ActivityBot.  It should respond to maintain that 32 cm distance.

 

How it Works

The program declares five variables: distance, setPoint, errorVal, kp, and speed. 

  • distance stores the measured distance of an object in front of the ActivityBot. 
  • setPoint stores the distance the ActivityBot should try to maintain between itself and an object it detects in front of it. 
  • errorVal stores the difference between the measured distance and the desired setPoint distance. 
  • kp stores a proportionality constant; errorVal can be multiplied by kp for a speed that will help maintain the setPoint distance.
  • speed stores the result of errorVal * kp.
/*
  Follow with Ping.c

  Maintain a constant distance between ActivityBot and object.

*/

#include "simpletools.h"                      // Include simpletools header
#include "abdrive.h"                          // Include abdrive header
#include "ping.h"                             // Include ping header  

int distance, setPoint, errorVal, kp, speed;  // Navigation variables

int main()                                    // main function
{
  setPoint = 32;                              // Desired cm distance
  kp = -10;                                   // Proportional control

  drive_setRampStep(6);                       // 7 ticks/sec / 20 ms

  while(1)                                    // main loop
  {
    distance = ping_cm(8);                    // Measure distance
    errorVal = setPoint - distance;           // Calculate error
    speed = kp * errorVal;                    // Calculate correction speed

    if(speed > 128) speed = 128;              // Limit top speed
    if(speed < -128) speed = -128;

    drive_rampStep(speed, speed);             // Use result for following
  }
}

The program starts off by making the setPoint 32 cm and the proportionality constant kp -10.  It also sets the ramp step size to 6, which will cushion sudden changes in speed.  Inside the while(1) loop the program makes the ActivityBot maintain a distance between itself and the target object by repeating five steps very rapidly: 

  1. Get the measured distance with distance = ping_cm(8).
  2. Calculate the difference between the desired and measured distance with errorVal = setPoint – distance.
  3. Calculate the speed needed to correct any differences between desired and measured distance with speed = kp * errorVal.
  4. Limit the output to +/- 128 with two if statements.
  5. Drive both wheels at the corrected speed.

 

Did You Know?

An automated system that maintains a desired physical output level is called a control system.  In the case of our ActivityBot, the level it maintains is the distance between itself and the object in front of it.  Many control systems use a feedback loop to maintain the level, and our ActivityBot is a prime example of this.  The measured distance is fed back into the system and compared against the set point for calculating the next speed, and this happens over and over again in a process called a control loop.

Engineers use drawings like this one to think through a control loop’s design and also to convey how it works to technicians and other engineers.  If the set point (distance the ActivityBot tries to maintain) is 32 and the measured distance is 38, there is an error value.  This error value is the set point – measured distance.  That’s 32 – 38 = -6.  This error is multiplied by a proportionality constant (kp) for an output that’s the speed to correct the error this time through the loop.  That’s speed = kp x error value, or 60 = -10 x -6.   


 

The circle is called a summing junction (for addition or subtraction), and the rectangular blocks designate operations that can vary.  In this example, the top one is a proportional control block that multiplies error by a proportionality constant.  The System block represents things we do not necessarily have control over, like if you move the object toward or away from the ActivityBot.  Other common control blocks that were not used in this example are Integral, which allows corrections to build up over loop repetitions, and Derivative, which responds to sudden system changes. 

 

Try This

It’s really helpful to see how the values respond to different measured distances.  To do this, all you have to do is add some print values to the screen.

  • Use the Save Project As button to save a copy of your project.  Name it Display Ping Control System and save it in …Documents\SimpleIDE\My Projects.
  • Modify the main function as shown below.
  • Set the power switch to 1 (so the ActivityBot does not drive off your desk) and click Run with Terminal.
  • Run the program and monitor the output values as you try placing a target object different distances from the Ping))) sensor.  Focus on distances ranging from 20 to 44 cm.

WARNING: This modified program is just for display, not navigation.  For navigation, re-open Following with Ping.

If the measured distance is 40, errorVal = setPoint – distance results in errorVal = 32 – 40 = -8.  Then, speed = kp * error is speed = -10 * -8 = + 80.  So, the ActivityBot will try 80 ticks per second forward.  This falls in the +/- 128 range, so the ActivityBot goes a little over half speed forward to catch up with the object.  Now, try this for a distance of 24.  The correct answer is speed = -80 which will make the ActivityBot back up.  If the distance is 28 instead, the ActivityBot will back up less quickly, with a speed of -49.

  • Repeat these calculations for distances of 30, 32, 34, and 36 cm. 

 

Your Turn

There are lots of values to experiment with here.  For example, if you change the setPoint variable from 32 to 20, the ActivityBot will try to maintain a distance of 20 cm between itself and an object in front of it. The value of kp can be increased to make it try to go faster to correct with smaller error distances, or slower to correct over larger error distances. 

  • Adjust the values of setPoint and kp to see what happens to the performance. 
  • Make notes of what you tried and how the ActivityBot responded. What values of setPoint and kp seem optimal to you?

If you decrease the drive_setRampStep value, it will cushion the changes, but could cause it to respond so slowly that it runs into the object.  If you increase it, it will respond more abruptly.  Too large, and it could actually change direction so fast that it could even eject the Ping))) sensor from the breadboard!

  • Adjust the drive_setRampStep value downward to smooth out motions, or upward to make them more abrupt. 
  • Keep experimenting with values as you search for the performance that seems best to you.
  • Again, make notes of what you tried and how the ActivityBot’s performance responded.