Datasheet

Chapter 4: Navigation Tips · Page 153
NO_ROT CON 750 ' No rotation
' -----[ Variables ]----------------------------------------------------------
temp VAR Word ' Temporary variable
counter VAR Byte ' Loop counting variable.
maneuver VAR Nib ' SumoBot travel maneuver
' -----[ Main Routine ]-------------------------------------------------------
Test_Search_Pattern:
DO
FOR counter = 1 TO 35 ' Forward 35 pulses
maneuver = Forward
GOSUB Pulse_Servos
NEXT
FOR counter = 1 TO 12 ' Rotate left 12 pulses
maneuver = RotateLeft
GOSUB Pulse_Servos
NEXT
FOR counter = 1 TO 24 ' Rotate right 24 pulses
maneuver = RotateRight
GOSUB Pulse_Servos
NEXT
FOR counter = 1 TO 12 ' Rotate Left 12 pulses
maneuver = RotateLeft
GOSUB Pulse_Servos
NEXT
LOOP
' -----[ Subroutine - Pulse_Servos ]------------------------------------------
Pulse_Servos:
' Pulse to left servo
LOOKUP maneuver, [FS_CCW, FS_CW, FS_CW, FS_CCW], temp
PULSOUT ServoLeft, temp
' Pulse to right servo
LOOKUP maneuver, [FS_CW, FS_CCW, FS_CW, FS_CCW], temp
PULSOUT ServoRight, temp
' Pause between pulses (remove when using IR object detectors + QTIs).
PAUSE 20
RETURN