Datasheet

Chapter 4: Navigation Tips · Page 151
The servo control subroutine will send pulse values to the servos. Constants for full
speed counterclockwise (
FS_CCW), full speed clockwise (FS_CW), and no rotation
(
NO_ROT) will make the subroutine easier to write.
FS_CCW CON 850 ' Full speed counterclockwise
FS_CW CON 650 ' Full speed clockwise
NO_ROT CON 750 ' No rotation
Here is the Pulse_Servos subroutine. Remember that Forward is a constant, set to 0
by a constant declaration (
Forward CON 0). If maneuver is set to Forward before the
Pulse_Servos subroutine is called, the LOOKUP command takes the zeroth element in
the lookup table, and copies it to the
temp variable. The first lookup command copies
FS_CCW (850) to temp. Then PULSOUT ServoLeft, temp sends that pulse to the left
servo, which is connected to P13. It's equivalent to
PULSOUT 13, 850. The second
LOOKUP and PULSOUT commands in the subroutine place FS_CCW (650) into the temp
variable. So PULSOUT ServoRight, temp is equivalent to PULSOUT 12, 650, which
makes the right servo turn full speed clockwise.
' -----[ 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
If maneuver is set equal to the constant Backward, the LOOKUP command copies FS_CW
to the
temp variable for the PULSOUT ServoLeft, temp command, and FS_CCW to temp
for the
PULSOUT ServoRight, temp command. If maneuver is RotateLeft (2),
FS_CW is copied to temp for both PULSOUT commands. Finally, if maneuver is
RotateRight (3), FS_CCW gets copied to both temp variables, and the PULSOUT
commands send 850 to the servos.