Datasheet

Chapter 4: Navigation Tips · Page 161
temp VAR Word ' Temporary variable
counter VAR Byte ' Loop counting variable.
maneuver VAR Nib ' SumoBot travel maneuver
sensors VAR Byte ' Sensor flags byte
irLS VAR sensors.BIT3 ' State of Left Side IR
irLF VAR sensors.BIT2 ' State of Left Front IR
irRF VAR sensors.BIT1 ' State of Right Front IR
irRS VAR sensors.BIT0 ' State of Right Side IR
' -----[ Main Routine ]-------------------------------------------------------
DO
IF irLF = 1 AND irRF = 1 THEN ' Both?
maneuver = Forward ' State = Lunge forward
GOSUB Servos_And_Sensors
ELSEIF irLF = 1 THEN ' Just left?
counter = 0 ' State = track front left object
DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 15
maneuver = PivotLeft ' Pivot left 15
GOSUB Servos_And_Sensors
counter = counter + 1
LOOP
ELSEIF irRF = 1 THEN ' Just right?
counter = 0 ' State=track front right object
DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 15
maneuver = PivotRight ' Pivot right 15
GOSUB Servos_And_Sensors
counter = counter + 1
LOOP
ELSE ' No objects detected?
GOSUB Read_Object_Detectors ' State = search pattern
ENDIF
LOOP
' -----[ Subroutine - Servos_And_Sensors ]------------------------------------
Servos_And_Sensors:
GOSUB Pulse_Servos ' Call Pulse_Servos subroutine
' Call sensor subroutine(s).
sensors = 0 ' Clear previous sensor values
GOSUB Read_Object_Detectors ' Call Read_Object_Detectors