' SumoBot_3.3_Simple_Mini_Sumo.bs2
' {$STAMP BS2}
' {$PBASIC 2.5}
                '---[I/O DEFINITIONS]----------
 LMotor PIN 13           'left servo motor
 RMotor PIN 12           'right servo motor
 LLinePwr PIN 10         'left line sensor power
 LLineIn PIN 9           'left line sensor input
 RLinePwr PIN 7          'right line sensor power
 RLineIn PIN 8           'right line sensor input
 StartLED PIN 0          'display start delay
                '---[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]----------------
lLine VAR Word           'left sensor raw reading
rLine VAR Word           'right sensor raw reading
lineBits VAR Nib         'decoded sensors value
lbLeft VAR lineBits.BIT1
lbRight VAR lineBits.BIT0
pulses VAR Byte          'counter for motor control
temp VAR Byte
                '---[EEPROM Data]--------------
RunStatus DATA $00       'run status
                '---[Initialization]-----------
Reset:
READ RunStatus, temp     'read current status
temp = ~temp             'invert status
WRITE RunStatus, temp    'save for next reset
IF(temp > 0) THEN END      'run now?
Start_Delay:
HIGH StartLED            'show active
PAUSE 5000               'start delay
LOW StartLED             'LED off
                '---[Program Code]-------------
Main:
GOSUB Read_Line_Sensors  'sumo movement
BRANCH lineBits, [Go_Fwd, Spin_Left, Spin_Right, About_Face]

Go_Fwd:
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RFwdFast
GOTO Main

Spin_Left:
FOR pulses = 1 TO 20
PULSOUT LMotor, LRevFast
PULSOUT RMotor, RFwdFast
PAUSE 20
NEXT
GOTO main

Spin_Right:
FOR pulses = 1 TO 20
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RRevFast
PAUSE 20
NEXT
GOTO Main

About_Face:
FOR pulses = 1 TO 10      'back up from edge
PULSOUT LMotor, LRevFast
PULSOUT RMotor, RRevFast
PAUSE 20
NEXT
FOR pulses = 1 TO 30      'turn around
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RRevFast
PAUSE 20
NEXT
GOTO main

END
                '---[Subroutines]-------------
Read_Line_Sensors:
HIGH LLinePwr             'activate sensors
HIGH RLinePwr
HIGH LLineIn              'discharge caps
HIGH RLineIn
PAUSE 1
RCTIME LLineIn, 1, lLine  'read left sensor
RCTIME RLineIn, 1, rLine  'read right sensor
LOW LLinePwr              'deactivate sensors
LOW RLinePwr              'convert readings to bits
LOOKDOWN lLine, >=[1000, 0], lbLeft ' 0=black, 1 = line
LOOKDOWN rLine, >=[1000, 0], lbRight
RETURN