In Chapter 7 [1], we used the infrared LEDs and receivers to detect whether an object is in the BOE Shield-Bot’s way without actually touching it. Wouldn’t it be nice to also get some distance information from the IR sensors? There is a way to accomplish some rough, close-range detection with the very same IR circuit from the previous chapter. It’s just enough information to allow the BOE Shield-Bot to follow a moving object without colliding into it.
This chapter uses the same IR LED/receiver circuits from the previous chapter.
You will also need:
(1) ruler
(1) sheet of paper
Here's a graph from one specific brand of IR detector’s datasheet (Panasonic PNA4602M; a different brand may have been used in your kit).
The graph shows that the IR detector is most sensitive at 38.5 kHz—its peak sensitivity —at the top of the curve. Notice how quickly the curve drops on both sides of the peak. This IR detector is much less sensitive to IR signals that flash on/off at frequencies other than 38 kHz. It’s only half as sensitive to an IR LED flashing at 40 kHz as it would be to 38 kHz signals. For an IR LED flashing at 42 kHz, the detector is only 20% as sensitive. The further from 38 kHz an IR LED’s signal rate is, the closer the IR receiver has to be to an object to see that IR signal’s reflection.
The most sensitive frequency (38 kHz) will detect the objects that are the farthest away, while less-sensitive frequencies can only detect closer objects. This makes rough distance detection rather simple: pick some frequencies, then test them from most sensitive to least sensitive. Try the most sensitive frequency first. If an object is detected, check and see if the next-most sensitive frequency detects it. Depending on which frequency makes the reflected infrared no longer visible to the IR detector, your sketch can infer a rough distance.
Frequency Sweep is the technique of testing a circuit’s output using a variety of input frequencies.
The next diagram shows an example of how the BOE Shield-Bot can test for distance using frequency. In this example, an object is in Zone 3. That means that the object can be detected when 38000 and 39000 Hz is transmitted, but it cannot be detected with 40000, 41000, or 42000 Hz. If you were to move the object into Zone 2, then the object would be detected when 38000, 39000, and 40000 Hz are transmitted, but not with 41000 or 42000 Hz.
The irDistance function from the next sketch performs distance measurement using the technique shown above. Code in the main loop calls irDistance and passes it values for a pair of IR LED and receiver pins. For example, the loop function uses int irLeft = irDistance(9, 10) to check distance to an object in the left side of the BOE Shield-Bot’s detection zone “field of vision.”
// IR distance measurement function int irDistance(int irLedPin, int irReceivePin) { int distance = 0; for(long f = 38000; f <= 42000; f += 1000) { distance += irDetect(irLedPin, irReceivePin, f); } return distance; }
The irDetect function returns a 1 if it doesn’t see an object, or a zero if it does. The expression distance += irDetect(irLedPin, irReceivePin, f) counts the number of times the object is not detected. After the for loop is done, that distance variable stores the number of times the IR detector did not see an object. The more times it doesn’t see an object, the further away it must be. So, the distance value the function returns represents the zone number in the drawing above.
It’s important to test that the detection distances are roughly the same for both IR LED/receiver pairs. They don’t have to be identical, but if they are too far apart, the BOE Shield-Bot might have difficulty following an object in front of it because it will keep trying to turn to one side. More subsystem testing!
A common cause of mismatched distance measurement is mismatched resistors used with the IR LEDs. For example, if one side’s IR LED has a 1 kΩ resistor and the other has a 2 kΩ resistor, one side will need objects to be much closer to see them. Another possibility, though rare, is that one IR detector is far more sensitive than the other. In that case, a larger resistor can be used in series with the IR LED on that side to make its IR headlight dimmer and correct the mismatched measurements.
This screencapture shows some detection zone measurements from DisplayBothDistances in the Serial Monitor. Though there’s fluctuation in the values, commonly called noise, what matters is that the numbers match, or are off by only 1, in each pair of measurements.
/* * Robotics with the BOE Shield - DisplayBothDistances * Display left and right IR states in Serial Monitor. * Distance range is from 0 to 5. Only a small range of several centimeters * in front of each detector is measureable. Most of it will be 0 (too * close) or 5 (too far). */ void setup() // Built-in initialization block { tone(4, 3000, 1000); // Play tone for 1 second delay(1000); // Delay to finish tone pinMode(10, INPUT); pinMode(9, OUTPUT); // Left IR LED & Receiver pinMode(3, INPUT); pinMode(2, OUTPUT); // Right IR LED & Receiver Serial.begin(9600); // Set data rate to 9600 bps } void loop() // Main loop auto-repeats { int irLeft = irDistance(9, 10); // Measure left distance int irRight = irDistance(2, 3); // Measure right distance Serial.print(irLeft); // Display left distance Serial.print(" "); // Display spaces Serial.println(irRight); // Display right distance delay(100); // 0.1 second delay } // IR distance measurement function int irDistance(int irLedPin, int irReceivePin) { int distance = 0; for(long f = 38000; f <= 42000; f += 1000) { distance += irDetect(irLedPin, irReceivePin, f); } return distance; } // IR Detection function int irDetect(int irLedPin, int irReceiverPin, long frequency) { tone(irLedPin, frequency, 8); // IRLED 38 kHz for at least 1 ms delay(1); // Wait 1 ms int ir = digitalRead(irReceiverPin); // IR receiver -> ir variable delay(1); // Down time before recheck return ir; // Return 1 no detect, 0 detect }
For a BOE Shield-Bot to follow a leader-object, it has to know the rough distance to the leader. If the leader is too far away, the sketch has to be able to detect that and move the BOE Shield-Bot forward. Likewise, if the leader is too close, the sketch has to detect that and move the BOE Shield-Bot backward. The purpose of the sketch is to make the BOE Shield-Bot maintain a certain distance between itself and the leader-object.
When a machine is designed to automatically maintain a measured value, it generally involves a control system. The value that the system is trying to maintain is called the set point. Electronic control systems often use a processor to take sensor measurements and respond by triggering mechanical actuators to return the machine to the set point.
Our machine is the BOE Shield-Bot. The measured value we want it to maintain is the distance to the leader-object, with a set point of 2 (for zone 2). The machine’s processor is the Arduino. The IR LED/receiver pairs are the sensors that take distance value measurements to the leader-object. If the measured distance is different from the set-point distance, the servos are the mechanical actuators that rotate to move the BOE Shield-Bot forward or backward as needed.
Closed loop control—repeatedly measuring a value and adjusting output in proportion to error for maintaining a set point—works very well for the BOE Shield-Bot shadow vehicle. In fact, the majority of the control loop shown in the diagram below reduces to just one line of code in a sketch. This block diagram describes the proportional control process that the BOE Shield-Bot will use to control the wheel speed based on detection distance measured with the IR LED/receiver pairs.
This block diagram could apply to either the left IR distance sensor and servo output or the right. In fact, your code will maintain two identical control loops, one for each side of the BOE Shield-Bot. Let’s walk through this example.
In the upper left, the set point = 2; we want the BOE Shield-Bot to maintain a zone-2 distance between itself and its leader-object. Below that, the measured distance is zone 4, so the leader-object is too far away. The arrows towards the symbols in the circle (called a summing junction) indicate that you add (+) the set point and subtract (-) the measured distance together to get the error, in this case 2 – 4 = -2.
Next, the error value feeds into the top square—an operator block. This block shows that the error gets multiplied by -50, a proportionality constant (Kp). In this example, the operator block gives us -2 × -50 = 100, so 100 is the output. In a sketch, this output value gets passed to the maneuver function. It turns the servo full speed forward to move the BOE Shield bot closer to the leader-object.
The next block diagram shows another example. This time, the measured distance is 1, meaning the leader-object is too close. So, the error is 1, and 1×–50 = -50. Passing -50 to the maneuver function turns the servo half-speed in reverse, backing the BOE Shield-Bot away from the leader-object.
The next time through the loop, the measured distance might change, but that’s okay. Regardless of the measured distance, this control loop will calculate a value that will cause the servo to move to correct any error. The correction is always proportional to the error. The two calculations involved:
set point – measured distance = error; error x Kp = output for maneuver
…can be easily combined and re-ordered into one expression for your sketch:
Output for maneuver = (Distance set point – Measured distance) x Kp
If you want to take a look at the sketch in detail, see How FollowingShieldBot Works.
The FollowingShieldBot sketch repeats the proportional control loops just discussed at a rate of about 25 times per second. So, all the proportional control calculations and servo speed updates happen 25 times each second. The result is a BOE Shield-Bot that will follow your hand, a book, or another robot.
/* * Robotics with the BOE Shield - FollowingShieldBot * Use proportional control to maintain a fixed distance between * BOE Shield-Bot and object in front of it. */ #include <Servo.h> // Include servo library Servo servoLeft; // Declare left and right servos Servo servoRight; const int setpoint = 2; // Target distances const int kpl = -50; // Proportional control constants const int kpr = -50; void setup() // Built-in initialization block { pinMode(10, INPUT); pinMode(9, OUTPUT); // Left IR LED & Receiver pinMode(3, INPUT); pinMode(2, OUTPUT); // Right IR LED & Receiver tone(4, 3000, 1000); // Play tone for 1 second delay(1000); // Delay to finish tone servoLeft.attach(13); // Attach left signal to pin 13 servoRight.attach(12); // Attach right signal to pin 12 } void loop() // Main loop auto-repeats { int irLeft = irDistance(9, 10); // Measure left distance int irRight = irDistance(2, 3); // Measure right distance // Left and right proportional control calculations int driveLeft = (setpoint - irLeft) * kpl; int driveRight = (setpoint - irRight) * kpr; maneuver(driveLeft, driveRight, 20); // Drive levels set speeds } // IR distance measurement function int irDistance(int irLedPin, int irReceivePin) { int distance = 0; for(long f = 38000; f <= 42000; f += 1000) { distance += irDetect(irLedPin, irReceivePin, f); } return distance; } // IR Detection function int irDetect(int irLedPin, int irReceiverPin, long frequency) { tone(irLedPin, frequency, 8); // IRLED 38 kHz for at least 1 ms delay(1); // Wait 1 ms int ir = digitalRead(irReceiverPin); // IR receiver -> ir variable delay(1); // Down time before recheck return ir; // Return 1 no detect, 0 detect } void maneuver(int speedLeft, int speedRight, int msTime) { // speedLeft, speedRight ranges: Backward Linear Stop Linear Forward // -200 -100......0......100 200 servoLeft.writeMicroseconds(1500 + speedLeft); // Set left servo speed servoRight.writeMicroseconds(1500 - speedRight); // Set right servo speed if(msTime==-1) // if msTime = -1 { servoLeft.detach(); // Stop servo signals servoRight.detach(); } delay(msTime); // Delay for msTime }
FollowingShieldBot declares three global constants: setpoint, kpl, and kpr. Everywhere you see setpoint, it’s actually the number 2 (a constant). Likewise, everywhere you see kpl, it’s actually the number -50. Likewise with kpr.
const int setpoint = 2; // Target distances const int kpl = -50; // Proportional control constants const int kpr = -50;
The convenient thing about declaring constants for these values is that you can change them in one place, at the beginning of the sketch. The changes you make at the beginning of the sketch will be reflected everywhere these constants are used. For example, by changing the declaration for kpl from -50 to -45, every instance of kpl in the entire sketch changes from ‑50 to -45. This is exceedingly useful for experimenting with and tuning the right and left proportional control loops.
The first thing the loop function does is call the irDistance function for current distance measurements and copies the results to the irLeft and irRight variables.
void loop() // Main loop auto-repeats { int irLeft = irDistance(9, 10); // Measure left distance int irRight = irDistance(2, 3); // Measure right distance
Remember the simple control loop calculation?
Output for maneuver = (Distance set point – Measured distance) x Kp
The next two lines of code perform those calculations for the right and left control loops, and store the output-for-maneuver results to variables named driveLeft and driveRight.
// Left and right proportional control calculations int driveLeft = (setpoint - irLeft) * kpl; int driveRight = (setpoint - irRight) * kpr;
Now, driveLeft and driveRight are ready to be passed to the maneuver function to set the servo speeds.
maneuver(driveLeft, driveRight, 20); // Drive levels set speeds }
Since each call to maneuver lasts for 20 ms, it delays the loop function from repeating for 20 ms. The IR distance detection takes another 20 ms, so the loop repetition time is about 40 ms. In terms of sampling rate, that translates to 25 samples per second.
Sampling Rate vs. Sample Interval
The sample interval is the time between one sample and the next. The sampling rate is the frequency at which the samples are taken. If you know one term, you can always figure out the other:
sampling rate = 1 ÷ sample interval
Here's a leader BOE Shield-Bot followed by a shadow BOE Shield-Bot. The lead robot is running a modified version of FastIrRoaming (with maneuver speeds reduced to +/- 40). The shadow BOE Shield-Bot is running FollowingShieldBot. One lead robot can string along a chain of 6 or 7 shadow robots. Just add the paper panel to the rest of the shadow BOE Shield-Bots in the chain.
/* * Robotics with the BOE Shield - SlowerIrRoamingForLeaderBot * Adaptation of RoamingWithWhiskers with IR object detection instead of * contact switches */ #include <Servo.h> // Include servo library Servo servoLeft; // Declare left and right servos Servo servoRight; void setup() // Built-in initialization block { pinMode(10, INPUT); pinMode(9, OUTPUT); // Left IR LED & Receiver pinMode(3, INPUT); pinMode(2, OUTPUT); // Right IR LED & Receiver tone(4, 3000, 1000); // Play tone for 1 second delay(1000); // Delay to finish tone servoLeft.attach(13); // Attach left signal to pin 13 servoRight.attach(12); // Attach right signal to pin 12 } void loop() // Main loop auto-repeats { int irLeft = irDetect(9, 10, 38000); // Check for object on left int irRight = irDetect(2, 3, 38000); // Check for object on right if((irLeft == 0) && (irRight == 0)) // If both sides detect { maneuver(-40, -40, 20); // Backward 20 milliseconds } else if(irLeft == 0) // If only left side detects { maneuver(40, -40, 20); // Right for 20 ms } else if(irRight == 0) // If only right side detects { maneuver(-40, 40, 20); // Left for 20 ms } else // Otherwise, no IR detects { maneuver(40, 40, 20); // Forward 20 ms } } int irDetect(int irLedPin, int irReceiverPin, long frequency) { tone(irLedPin, frequency, 8); // IRLED 38 kHz for at least 1 ms delay(1); // Wait 1 ms int ir = digitalRead(irReceiverPin); // IR receiver -> ir variable delay(1); // Down time before recheck return ir; // Return 1 no detect, 0 detect } void maneuver(int speedLeft, int speedRight, int msTime) { // speedLeft, speedRight ranges: Backward Linear Stop Linear Forward // -200 -100......0......100 200 servoLeft.writeMicroseconds(1500 + speedLeft); // Set left servo speed servoRight.writeMicroseconds(1500 - speedRight); // Set right servo speed if(msTime==-1) // if msTime = -1 { servoLeft.detach(); // Stop servo signals servoRight.detach(); } delay(msTime); // Delay for msTime }
You can adjust the setpoint and proportionality constants to change the shadow BOE Shield-Bot’s behavior. Use your hand or a piece of paper to lead the shadow BOE Shield-Bot while doing these exercises:
You might notice some odd behaviors. For example, if the set point is 0, it won’t back up. Want to figure out why?
Congratulations! You've made it to the end of the book. Now that you've mastered the basics, you're ready to start exploring and adding capabilities with your BOE Shield-Bot! Here are some ideas:
Or, if you're interested in trying specific sensors or adapting projects from other platforms, take a look at some of the options below.
This chapter used the infrared IR LED/receiver pairs for simple distance detection, to make a BOE Shield-Bot shadow vehicle. Now-familiar skills combined with some new concepts got the job done:
for(long f = 38000; f <= 42000; f += 1000) { distance += irDetect(irLedPin, irReceivePin, f); }
const int setpoint = 2; // Target distances const int kpl = -45; // Proportional control constants const int kpr = -55;
void loop() // Main loop auto-repeats { int irLeft = irDistance(9, 10); // Measure left distance int irRight = irDistance(2, 3); // Measure right distance // Left and right proportional control calculations int driveLeft = (setpoint - irLeft) * kpl; int driveRight = (setpoint - irRight) * kpr; int drive = (driveLeft + driveRight)/2; // Average drive levels maneuver(drive, drive, 20); // Apply same drive to both delay(10); // 0.1 second delay }
Links
[1] https://learn.parallax.com/tutorials/robot/shield-bot/robotics-board-education-shield-arduino/chapter-7-navigating-infrared-12
[2] https://learn.parallax.com/sites/default/files/content/shield/robo_ch8/RoboticsBOEShield_Ch8_20120517.zip
[3] https://learn.parallax.com/tutorials/series/arduino-shield-bot-tutorial-series
[4] http://www.parallax.com/product/28015
[5] https://learn.parallax.com/KickStart/28015
[6] http://www.parallax.com/product/570-28015
[7] http://learn.parallax.com/project/shield-bot-roaming-ping
[8] http://learn.parallax.com/node/342
[9] http://www.parallax.com/product/28017
[10] https://learn.parallax.com/KickStart/28017
[11] https://learn.parallax.com/tutorials/robot/boe-bot/sumobot-or-boe-bot-tilt-controller-project
[12] http://learn.parallax.com/XBeeSumoBot
[13] http://www.parallax.com/go/XBee
[14] https://learn.parallax.com/KickStart/32440
[15] http://www.parallax.com/product/30055
[16] http://www.parallax.com/product/28106