Datasheet

Chapter 8 • Robot Control with Distance Detection
258 • Robotics with the BOE Shield-Bot
// 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); // Left servo speed
servoRight.writeMicroseconds(1500 - speedRight); // 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.