' SumoBot_4.2_IR_Scan.bs2
' {$STAMP BS2}
' {$PBASIC 2.5}
                '---[I/O DEFINITIONS]----------
LMotor PIN 13            'left servo motor
RMotor PIN 12            'right servo motor
LflrOut PIN 4            'left IR LED output
Lflrln PIN 11            'left ir sensor input
RtlrOut PIN 15           'right IR LED output
RTlrln PIN 14            'right IR sensor input
                '---[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]----------------
irBits VAR Nib           'storage for IR target data
irLeft VAR irBits.BIT1
irRight VAR irBits.BIT0
lastIR VAR Nib           'info from last reading
pulses VAR Byte          'counter for motor control
                '---[Initialization]----------------
Reset:
LOW LMotor               'initialize motor outputs
LOW RMotor
                '---[Program Code]-------------
Main:
GOSUB Read_IR_Sensors
BRANCH irBits, [Scan, Follow_Right, Follow_Left, Hold]
Scan:
BRANCH lastIR, [Move_Fwd, Scan_Right, Scan_Left, Move_Fwd]
Move_Fwd:
DEBUG HOME, "Forward", CLREOL
GOTO Main
Scan_Right:               'spin right, slow
DEBUG HOME, "Scan Right", CLREOL
PULSOUT LMotor, LFwdSlow
PULSOUT RMotor, RRevSlow
PAUSE 20
GOTO Main
Scan_Left:                 'spin left, slow
DEBUG HOME, "Scan Left", CLREOL
PULSOUT LMotor, LRevSLow
PULSOUT RMotor, RFwdSlow
PAUSE 20
GOTO Main
Follow_Right:            'spin right, fast
DEBUG HOME, "Follow Right", CLREOL
PULSOUT LMotor, LFwdFast
PULSOUT RMotor, RRevFast
PAUSE 20
lastIR = irBits          'save last direction found
GOTO Main
Follow_Left:             'spin left, fast
DEBUG HOME, "Follow Left", CLREOL
PULSOUT LMotor, LRevFast
PULSOUT RMotor, RFwdFast
PAUSE 20
lastIR = irBits
GOTO Main
Hold:                    'on target
DEBUG HOME, "On Target", CLREOL
FOR pulses =1 TO 3
PULSOUT LMotor, LStop
PULSOUT RMotor, RStop
PAUSE 20
NEXT
lastIR = %00
GOTO Main
END
                '---[Subroutines]--------------
Read_IR_Sensors:
FREQOUT LflrOut, 1, 38500 'modulate left IR LED
irLeft = ~Lflrln          'read input (1 = target)
FREQOUT RtlrOut, 1, 38500 'modulate right IR LED
irRight = ~Rtlrln         'read input (1 = target)
RETURN