SumoBot-5.1-Basic-Competition-Program.BS2

' SumoBot-5.1-Basic-Competition-Program.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

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

Speaker      PIN        1                       ' piezo speaker
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
blackThresh  VAR        Word                    ' QTI black threshold
lineBits     VAR        Nib                     ' decoded sensors value
lbLeft       VAR        lineBits.BIT1
lbRight      VAR        lineBits.BIT0

irBits       VAR        Nib                     ' IR readings (l & r)
irLeft       VAR        irBits.BIT1
irRight      VAR        irBits.BIT0
lastIr       VAR        Nib                     ' info from last reading

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                        ' okay to run?

' Sets black threshold to 1/4 the average of the two sensor readings.
' SumoBot must be placed over black playing surface before this code runs.

Set_Threshold:                                  ' set QTI black threshold
  GOSUB Read_Line_Sensors
  blackThresh = (lLine / 10) + (rLine / 10)

  LOW LMotor                                    ' make more pins outputs
  LOW RMotor

Start_Delay:                                    ' five second delay
  FOR temp = 1 TO 5
    HIGH StartLED                               ' show active
    PAUSE 900
    INPUT StartLED                              ' blink each second
    FREQOUT Speaker, 100, 2500, 3000            ' beep each second
  NEXT

  GOTO Lunge                                    ' start aggressive!

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

Main:
  GOSUB Read_Line_Sensors

  ' If not on the Shikiri line (border), continue to look for opponent,
  ' otherwise, spin back toward center and resume search

  BRANCH lineBits, [Search_For_Opponent, Spin_Left, Spin_Right, About_Face]

' --[ Border Avoidance ]--

Spin_Left:                                      ' right sensor was active
  FOR pulses = 1 TO 20
    PULSOUT LMotor, LRevFast
    PULSOUT RMotor, RFwdFast
    PAUSE 20
  NEXT
  lastIr = %00                                  ' clear scan direction
  GOTO Lunge

Spin_Right:                                     ' left sensor was active
  FOR pulses = 1 TO 20
    PULSOUT LMotor, LFwdFast
    PULSOUT RMotor, RRevFast
    PAUSE 20
  NEXT
  lastIr = %00
  GOTO Lunge

About_Face:                                     ' both sensors on Shikiri
  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
  lastIr = %00
  GOTO Lunge

' --[ IR Processing ]--

Search_For_Opponent:
  GOSUB Read_IR_Sensors

  ' If opponent is not in view, scan last known direction. Turn toward
  ' opponent if seen by one "eye" -- if both, lunge forward

BRANCH irBits, [Scan, Follow_Right, Follow_Left, Lunge]

Scan:
  BRANCH lastIR, [Move_Fwd, Scan_Right, Scan_Left]

Move_Fwd:
  GOSUB Creep_Forward
  GOTO Main

Scan_Right:                                     ' spin right, slow
  FOR pulses = 1 TO 5
    PULSOUT LMotor, LFwdSlow
    PULSOUT RMotor, RRevSlow
    PAUSE 20
  NEXT
  GOSUB Creep_Forward                           ' keep moving
  GOTO Main

Scan_Left:                                      ' spin left, slow
  FOR pulses = 1 TO 5
    PULSOUT LMotor, LRevSlow
    PULSOUT RMotor, RFwdSlow
    PAUSE 20
  NEXT
  GOSUB Creep_Forward
  GOTO Main

Follow_Right:                                   ' spin right, fast
  PULSOUT LMotor, LFwdFast
  PULSOUT RMotor, RRevSlow
  lastIR = irBits                               ' save last direction found
  GOTO Main

Follow_Left:                                    ' spin left, fast
  PULSOUT LMotor, LRevSlow
  PULSOUT RMotor, RFwdFast
  lastIR = irBits
  GOTO Main

Lunge:                                          ' locked on -- go get him!
  FOR pulses = 1 TO 25
    PULSOUT LMotor, LFwdFast
    PULSOUT RMotor, RFwdFast
    GOSUB Read_Line_Sensors
    IF (lineBits = %11) THEN Match_Over         ' in sight, we're on the line
  NEXT
  GOTO Main

' If SumoBot can see the opponent with both "eyes" and both QTIs are
' detecting the border, we must have pushed the opponent out.

Match_Over:
  FOR pulses = 1 TO 10                          ' stop motors
    PULSOUT LMotor, LStop
    PULSOUT RMotor, RStop
    PAUSE 20
  NEXT
  INPUT LMotor
  INPUT RMotor

  FOR temp = 1 TO 10                            ' make some noise
    HIGH StartLED
    FREQOUT Speaker, 100, 2500, 3000            ' beep
    INPUT StartLED                              ' blink LED
    PAUSE 100
  NEXT

  DIRS = $0000                                  ' disable all outputs
  GOTO Reset                                    ' reset for next round

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

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

Creep_Forward:
  FOR pulses = 1 TO 20
    PULSOUT LMotor, LFwdSlow
    PULSOUT RMotor, RFwdSlow
    PAUSE 20
  NEXT
  RETURN