' SumoBot_Line_Follower.bs2
' {$STAMP BS2}
' {$PBASIC 2.5}
                '---[I/O DEFINITIONS]----------
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
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]----------------
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
                '---[Initialization]-----------
Reset:
LOW LMotor               'initialize motor outputs
LOW RMotor
PAUSE 2000               'time to disconnect cable
                '---[Program Code]-------------
Main:
GOSUB Read_Line_Sensors
SELECT lineBits          'display actions
CASE %00
PULSOUT LMotor, LFwdSlow
PULSOUT RMotor, RFwdSlow
PAUSE 20
GOTO Main
CASE %01
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RStop
PAUSE 20
GOTO Main
CASE %10
PULSOUT LMotor, LStop
PULSOUT RMotor, RFwdFast
PAUSE 20
GOTO Main
CASE %11
PULSOUT LMotor, LStop
PULSOUT RMotor, RStop
ENDSELECT
GOTO Main
                '---[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, >=[500, 0], lbLeft '0=black, 1 = line
LOOKDOWN rLine, >=[500, 0], lbRight
RETURN