Chapter 8: Robot Control with Distance Detection
· Page 297
intersection is a 3-way or 4-way intersection, the Boe-Bot will arbitrarily turn in
the direction that black is first detected. A constant,
Turn90Degree
, is provided
to tune the 90 degree turn. Some audible and visual indicators are included,
which aid in troubleshooting and understanding what the Boe-Bot is seeing and
deciding, as well as adding a bit of personality and fun.
' -----[ Title ]-------------------------------------------------------
' Robotics with the Boe-Bot - IntersectionsBoeBot.bs2
' Navigate 90 degree left/right, 3-way, and 4-way intersections.
' Based on StripeFollowingBoeBot.bs2
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
DEBUG "Program Running!"
' -----[ Constants ]---------------------------------------------------
Kpl CON 35 ' Left proportional constant
Kpr CON -35 ' Right proportional constant
SetPoint CON 3 ' 0-1 is White, 4-5 is Black
CenterPulse CON 750
Turn90Degree CON 30 ' Pulses needed for 90 turn
RightLED PIN 1 ' LED Indicators
LeftLED PIN 10
' -----[ Variables ]---------------------------------------------------
freqSelect VAR Nib ' Sweep through 5 frequencies
irFrequency VAR Word ' Freq sent to IR emitter
irDetectLeft VAR Bit ' Store results from detectors
irDetectRight VAR Bit
distanceLeft VAR Nib ' Calculate distance zones
distanceRight VAR Nib
pulseLeft VAR Word ' Servo pulseWidths
pulseRight VAR Word
numPulses VAR Byte ' Count total pulses
fwdPulses VAR Byte ' Count forward pulses
counter VAR Byte
isStuck VAR Bit ' Boolean variable,is bot stuck?
' -----[ Initialization ]----------------------------------------------
FREQOUT 4, 2000, 3000
' -----[ Main Routine ]------------------------------------------------
DO
GOSUB Get_Ir_Distances ' Read IR sensors