Example Sketch: FollowingShieldBot
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.
- Enter, save, and upload FollowingShieldBot.
- Point the BOE Shield-Bot at an 8 ½ x 11” sheet of paper held in front of it as though it’s a wall-obstacle. The BOE Shield-Bot should maintain a fixed distance between itself and the sheet of paper. (It will dance around a little because of the sensor noise mentioned earlier.)
- Rotate the sheet of paper slightly; the BOE Shield-Bot should rotate with it.
- Try using the sheet of paper to lead the BOE Shield-Bot around. The BOE Shield-Bot should follow it.
- Move the sheet of paper too close to the BOE Shield-Bot, and it should back up, away from the paper.
/*
* 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
}
How FollowingShieldBot Works
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