LEARN.PARALLAX.COM
Published on LEARN.PARALLAX.COM (https://learn.parallax.com)

Home > Robotics with the Board of Education Shield for Arduino > Chapter 8. Robot Control with Distance Detection

Chapter 8. Robot Control with Distance Detection

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.    

Determining Distance with the IR LED/Receiver Circuit

This chapter uses the same IR LED/receiver circuits from the previous chapter.

You will also need:

(1) ruler
(1) sheet of paper

  • If the circuit from the last chapter is still built on your BOE Shield-Bot, make sure your IR LEDs are using the 2 kΩ resistors.
  • If you already disassembled the circuit from the previous chapter, repeat the steps in Chapter 7, Activity #1.  Also, make sure to use TestBothIrAndIndicators to verify that the system is working properly.
  • Download Chapter 8 Arduino Code [2]

 

Activity 1: Testing the Frequency Sweep

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). 

Sensitivity curve graph for an Infrared Receiver

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.

Programming Frequency Sweep for Distance Detection

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.

BOE Shield-Bot object etection zones using different infrared LED signal frequencies

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.

 

Displaying Both Distances

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.

Example Sketch – DisplayBothDistances

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.

Serial Monitor output tests the distance detection zone of each IR LED/receiver pair

  • Enter, save, and upload DisplayBothDistances to your Arduino.
  • Use a box, book, bottle, or similar object as a target for the left distance detector.
  • Start by moving the object toward and away from the BOE Shield-Bot until you find the small range where a small movement will result in a change in distance measurement.
  • Find and record the midpoint of each distance detection zone.
  • Repeat for the right IR detector. 
  • If it turns out that the detection range of one side is twice as far as the other (or more), check the resistors connected to the IR LEDs.  You may have a mismatch there; make sure both resistors are 2 kΩ (red-black-red).  If there isn’t a mismatch, try adjusting IR LED resistor values until the detection ranges of both sides are in the same neighborhood.
/*
 * 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
}     

Your Turn – More Distance Tests

  • Try measuring the detection range for objects with different colors and textures.  Which colors and surfaces are easiest to detect? Which are most difficult?

 

Activity 2: BOE Shield-Bot Shadow Vehicle

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.

Some Control System Vocabulary

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.

A Look Inside Proportional Control

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.  

Proportional control block diagram with set point 2 and error -2

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.

Proportional control block diagram with set point 2 and error 1

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.

Your Turn – Verify Control Loop with Other Distances

  • To prove that the proportional control loop responds correctly to all six measured distances, fill in the table below.  The block diagrams have been solved for two of the six conditions, so you’ve only got four to try.

 

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

 

Follow the Leader

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.

  • If you are working on your own with one BOE Shield-Bot, you will be the leader! The leader-object can be a book, bottle or even just your hand.  
  • If you are part of a class with two or more BOE Shield-Bots, mount a paper panel around the tail and both sides of a lead robot to make it more visible to the shadow robots, like in the picture. If you are making a chain of shadow robots, put a paper panel on each of them too.
  • Program the lead BOE Shield-Bot with SlowerIrRoamingForLeaderBot.  
  • Program each shadow BOE Shield-Bot with FollowingShieldBot.  Each shadow robot’s IR LEDs should be pointing slightly to the left and right, and level with horizontal (not up or down).
  • Place a shadow BOE Shield-Bot behind the lead BOE Shield-Bot or other leader-object.  The shadow BOE Shield-Bot should follow the leader at a fixed distance, so long as it is not distracted by another object such as a hand or a nearby wall.
/*
 * 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
}

Your Turn – Experiment with the Constants

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:

  • Try running FollowingShieldBot using values of kpr and kpl constants, ranging from 15 to 100.  Note the difference in how responsive the BOE Shield-Bot is when following an object.
  • Try making adjustments to the value of the setpoint constant.  Try values from 0 to 4. 

You might notice some odd behaviors.  For example, if the set point is 0, it won’t back up.  Want to figure out why?

  • Repeat the control loop exercises from Activity #1 with the set point at zero.  Can any measured distance cause it to back up with a set point of zero?

Activity 3: What's Next?

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:

  • Visit the Tutorial Series page and try some of the projects. [3]

 

Or, if you're interested in trying specific sensors or adapting projects from other platforms, take a look at some of the options below.

PING))) Ultrasonic Distance Sensor

  • PING))) sensor: #28015 at the Parallax store [4]
  • Check out the Ping))) KickStart code for Arduino here [5]
  • Mounting Bracket Kit #570-28015 at the Parallax store [6]
  • Try the Roaming Ping))) Shield-Bot project here [7]

BOE Shield-Bot out roaming with Ping))) sensor and traveling staff [8]

Dual-axis accelerometer for tilt sensing

  • #28017 at the Parallax store [9]
  • Check out the Accelerometer KickStart code for Arduino here [10].
  • See the XBee Tilt-control SumoBot/Boe-Bot project here [11]

XBee-linked SumoBot robot and Memsic accelerometer tilt controller [12]

XBee RF modules and adapters for wireless control and communication;

  • Visit the XBee page at the Parallax store for options [13]
  • Check out the XBee KickStart code for Arduino here [14]
  • See the XBee Tilt-control SumoBot/Boe-Bot project here [11]

A Crawler Kit to make your BOE Shield-Bot a 6-legged walker

  • #30055 at the Parallax store [15]

Crawler legs attach to the Boe-Bot chassis

A Tank Tread Kit for all-terrain navigation

  • #28106 at the Parallax store [16]

Keep Exploring and Have Fun!!

Chapter 8 Summary

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:

Electronics

  • Looking at the IR receiver’s peak sensitivity
  • Exploiting the IR receiver’s sensitivity curve properties with a frequency sweep to detect distance
  • Changing series resistor values to match up the sensitivity of two IR LED receiver pairs—making an adjustment in hardware

Programming 

  • Writing a routine with an indexing for loop to generate a frequency sweep
  • Writing a sketch that uses proportional control loops
  • Using const int to set up a proportional control loop’s set point and proportionality constants
  • changing a control loop’s proportionality constants to fine-tune the control system—making an adjustment in software
  • What sampling rate and sampling interval are, and how they relate to each other

Robotics Skills

  • Setting up a closed-loop proportional control system with a processor, sensors and actuators for autonomous shadow vehicle navigation

Engineering Skills

  • Reading a block diagram for a simple closed loop proportional control system, with a summing junction and operator blocks
  • Understanding and using a closed loop control system’s set point, error, proportionality constant, and output value
  • More subsystem testing and troubleshooting

Chapter 8 Challenges

Questions

  1. What would the relative sensitivity of the IR detector be if you use tone to send a 35 kHz signal?  What is the relative sensitivity with a 36 kHz signal?
  2. What keyword is used to declare a constant?
  3. What statement is used to sweep through the list of frequencies used for IR distance measurements?
  4. What’s a summing junction?
  5. In our closed loop proportional control block diagrams, what two values are used to calculate the error term?
  6. How do delays in the loop function contribute to setting the sampling rate?

Exercises

  1. Write a segment of code that does the frequency sweep for just four frequencies instead of five.
  2. Write global constant declarations that initialize kpl as –45 and kpr as –55. 

Projects 

  1. Write a sketch that allows you to hold your hand in front of the BOE Shield-Bot and push it backwards and forwards with no turning.

Chapter 8 Solutions

Question Solutions

  1. The relative sensitivity at 35 kHz is 30%.  For 36 kHz, it’s 50%.
  2. Precede a variable declaration with the const keyword.
  3. A for loop that starts indexing at 38000 and increases by 1000 with each repetition.
  4. A summing junction is a part of a block diagram that indicates two inputs are added together (or one subtracted from another) resulting in an output.
  5. The error term is the measured level subtracted from the desired set point level.
  6. If a distance sample is taken with each repetition of the loop function, then the delays more or less determine how long it takes between each sample.  That’s called the sample interval, and 1 ÷ sample interval = sampling rate.

Exercise Solutions

  1. Just reduce the for statement’s condition from f <=42000 to f <= 41000
  for(long f = 38000; f <= 42000; f += 1000) {
    distance += irDetect(irLedPin, irReceivePin, f);
  }
  1. Declarations
const int setpoint = 2;    // Target distances
const int kpl = -45;       // Proportional control constants
const int kpr = -55;

Project Solution

  1. One quick and simple solution would be to average the driveLeft and driveRight values in the FollowingShieldBot sketch.  The resulting single value can be applied both left and right speed parameters in the maneuver call.
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
}

DISCUSSION FORUMS | PARALLAX INC. STORE

About | Terms of Use | Feedback: learn@parallax.com | Copyright©Parallax Inc. 2022


Source URL: https://learn.parallax.com/tutorials/robot/shield-bot/robotics-board-education-shield-arduino/chapter-8-robot-control-9

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