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