' SumoBot_2.2_Motor_Test.BS2
' {$STAMP BS2}
' {$PBASIC 2.5}
                '---[I/O DEFINITIONS]----------
LMotor PIN 13            'left servo motor
RMotor PIN 12            'right servo motor
                '---[Constants]----------------
LFwdFast CON 1000        'left motor fwd; fast
LFwdSlow CON 800         'left motor fwd; slow
LStop CON 750            'left motor stop
LRevSlow CON 700         'left motor rev; slow
LRevFast CON 500         'left motor rev; fast
RFwdFast CON 500         'right motor fwd; fast
RFwdSlow CON 700         'right motor fwd; slow
RStop CON 750            'right motor stop
RRevSlow CON 800         'right motor rev; slow
RRevFast CON 1000        'right motor rev; fast
                '---[Variables]----------------
pulses VAR Byte
                '---[Initialization]-----------
Reset:
LOW LMotor               'initialize motor outputs
LOW RMotor
PAUSE 2000               'time to disconnect cable
                '---[Program Code]-------------
Main:
FOR pulses = 1 TO 65     'fwd slow, ~12 inches
PULSOUT LMotor, LFwdSlow
PULSOUT RMotor, RFwdSlow
PAUSE 20
NEXT
FOR pulses = 1 TO 30     'pivot 90 on left wheel
PULSOUT LMotor, LStop
PULSOUT RMotor, RFwdSlow
PAUSE 20

NEXT
FOR pulses = 1 TO 25     'fwd fast
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RFwdFast
PAUSE 20

NEXT
FOR pulses = 1 TO 60     'pivot 180 on right wheel
PULSOUT LMotor, LFwdSlow
PULSOUT RMotor, RStop
PAUSE 20

NEXT
FOR pulses = 1 TO 50     'fwd fast
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RFwdFast
PAUSE 20

NEXT
FOR pulses = 1 TO 55 'spin turn-clockwise
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RRevFast
PAUSE 20

NEXT
Hold_Position:
DO
PULSOUT LMotor, LStop
PULSOUT RMotor, RStop
PAUSE 20
LOOP
END