Parallax Boe-Bot - Page 306

Page 292 · Robotics with the Boe-Bot
Kpr CON -35 ' Change from 35 to -35
SetPoint CON 3 ' Change from 2 to 3.
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 ]--------------------------------------
Get_Ir_Distances:
distanceLeft = 0
distanceRight = 0
FOR freqSelect = 0 TO 4
LOOKUP freqSelect,[37500,38250,39500,40500,41500], irFrequency
FREQOUT 8,1,irFrequency
irDetectLeft = IN9
distanceLeft = distanceLeft + irDetectLeft
FREQOUT 2,1,irFrequency
irDetectRight = IN0
distanceRight = distanceRight + irDetectRight
NEXT
RETURN