Sumobot Test.bs2

' 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

LfIrOut      PIN        4                       ' left IR LED output
LfIrIn       PIN        11                      ' left IR sensor input
RtIrOut      PIN        15                      ' right IR LED output
RtIrIn       PIN        14                      ' right IR sensor input

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

' -----[ 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

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

' -----[ Initialization ]--------------------------------------------------

Reset:
  LOW LMotor                                    ' initialize motor outputs
  LOW RMotor

' -----[ Program Code ]----------------------------------------------------

Main:
  GOSUB Read_Line_Sensors

  GOSUB Read_IR_Sensors  ' Read IR Sensors
  BRANCH irBits, [Scan, Follow_Right, Follow_Left, Target_Found]

Scan:
  BRANCH lastIR, [Hold, Turn_Right, Turn_Left, Move_Forward]


'''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'               Sensor Reactions
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''
Target_Found:                ' attack on target found
  lastIr = irBits
  GOTO Move_Forward

Handle_Line_Sensors:
  SELECT lineBits                               ' display actions
    CASE %00
      DEBUG "Continue forward", CLREOL
    CASE %01
      DEBUG "Spin Left", CLREOL
    CASE %10
      DEBUG "Spin Right", CLREOL
    CASE %11
      DEBUG "Back up and turn around", CLREOL
    ENDSELECT

  GOTO Main

Follow_Right:                                   ' spin right, fast
  lastIr = irBits                               ' save last direction found
  GOTO Turn_Right

Follow_Left:                                    ' spin left, fast
  lastIr = irBits
  GOTO Turn_Left

'''''''''''''''''''''''''''''''''''''''''''''''''''''''''
'                     Movement
'''''''''''''''''''''''''''''''''''''''''''''''''''''''''
Move_Forward:
  PULSOUT LMotor, LFwdFast
  PULSOUT RMotor, RFwdFast
  PAUSE 20
  GOTO Main

Move_Reverse:
  PULSOUT LMotor, LRevFast
  PULSOUT RMotor, RRevFast
  PAUSE 20
  GOTO Main

Turn_Left:                         ' scan to the left
  PULSOUT LMotor, LRevFast
  PULSOUT RMotor, RFwdFast
  PAUSE 20
  GOTO Main

Turn_Right:                    ' scan to the right
  PULSOUT LMotor, LFwdFast
  PULSOUT RMotor, RRevFast
  PAUSE 20
  GOTO Main

Hold:                                           ' hold position
  FOR pulses = 1 TO 3
    PULSOUT LMotor, LStop
    PULSOUT RMotor, RStop
    PAUSE 20
  NEXT
  lastIr = %00
  GOTO Main

  END

' Subroutines

Read_IR_Sensors:
  FREQOUT LfIrOut, 1, 38500                     ' modulate left IR LED
  irLeft = ~LfIrIn                              ' read input (1 = target)
  FREQOUT RtIrOut, 1, 38500                     ' modulate right IR LED
  irRight = ~RtIrIn                             ' read input (1 = target)
  RETURN

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