Parallax Boe-Bot - Page 312

Page 298 · Robotics with the Boe-Bot
GOSUB Update_LEDs ' Indicate white/black line
' Calculate proportional output and move accordingly.
pulseLeft = SetPoint - distanceLeft * Kpl + CenterPulse
pulseRight = SetPoint - distanceRight * Kpr + CenterPulse
GOSUB Send_Pulse
GOSUB Check_For_Intersection ' Are we stuck at intersection?
IF (isStuck = 1) THEN
GOSUB Make_Noise ' Audible indication
GOSUB Navigate_Intersection ' Navigate through it
ENDIF
LOOP
' -----[ Subroutines ]-------------------------------------------------
Navigate_Intersection:
' Go forward until both sensors read white, through the intersection.
DO
pulseLeft = 850: pulseRight = 650 ' Forward
GOSUB Send_Pulse
GOSUB Get_Ir_Distances
GOSUB Update_LEDs
LOOP UNTIL (distanceLeft <=2) AND (distanceRight <=2)
GOSUB Stop_Quickly ' Don't coast forward
' Now back up until one detector sees the black.L & R turn will see
' black on one detector.3- or 4-way will see both black, turn toward
' whichever the bot sees first (random).
DO
pulseLeft = 650: pulseRight = 850 ' Backward
GOSUB Send_Pulse
GOSUB Get_Ir_Distances
GOSUB Update_LEDs
LOOP UNTIL (distanceLeft >=4) OR (distanceRight >=4)
GOSUB Stop_Quickly ' Don't coast backward
' Make 90 degree turn in direction of the detector which sees black
IF (distanceLeft >=4) THEN ' Left detector reads black
FOR counter = 1 TO Turn90Degree ' Turn 90 degrees left
PULSOUT 13, 750 ' without proportional control
PULSOUT 12, 650
PAUSE 20 ' so use PAUSE 20
NEXT
ELSEIF (distanceRight >=4) THEN ' Right detector reads black
FOR counter = 1 TO Turn90Degree ' Turn 90 degrees right
PULSOUT 13, 850
PULSOUT 12, 750