Parallax Boe-Bot - Page 296

Page 282 · Robotics with the Boe-Bot
√ Move the sheet of paper too close to the Boe-Bot, and it should back up, away
from the paper.
' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - FollowingBoeBot.bs2
' Boe-Bot adjusts its position to keep objects it detects in zone 2.
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
DEBUG "Program Running!"
' -----[ Constants ]----------------------------------------------------------
Kpl CON -35
Kpr CON 35
SetPoint CON 2
CenterPulse CON 750
' -----[ Variables ]----------------------------------------------------------
freqSelect VAR Nib
irFrequency VAR Word
irDetectLeft VAR Bit
irDetectRight VAR Bit
distanceLeft VAR Nib
distanceRight VAR Nib
pulseLeft VAR Word
pulseRight VAR Word
' -----[ Initialization ]-----------------------------------------------------
FREQOUT 4, 2000, 3000
' -----[ Main Routine ]-------------------------------------------------------
DO
GOSUB Get_Ir_Distances
' Calculate proportional output.
pulseLeft = SetPoint - distanceLeft * Kpl + CenterPulse
pulseRight = SetPoint - distanceRight * Kpr + CenterPulse
GOSUB Send_Pulse
LOOP
' -----[ Subroutine - Get IR Distances ]--------------------------------------