Sumobot

Explanation
Program to convert the Boe-bot into a Sumo-bot.

'-----[ Title ]----------------------------------------------------------------
' Mini Sumo 5.2 : Basic Competition Program For Boe Bot
' {$STAMP BS2}
' {$PBASIC 2.5}
' -----[ I/O Definitions ]------------------------------------------------------

LMotor          CON     13                      ' left servo motor
RMotor          CON     12                      ' right servo motor

LfIrOut         CON      8                      ' left IR LED output
LfIrIn          VAR     IN9                     ' left IR sensor input
RtIrOut         CON      2                      ' right IR LED output
RtIrIn          VAR     IN0                     ' right IR sensor input

leftSense       VAR     IN5                     ' left sensor raw reading
rightSense      VAR     IN4                     ' right sensor raw reading

Speaker         CON      3                      ' piezo speaker
StartLED        CON      15                      ' display start delay

' -----[ Constants ]------------------------------------------------------------
LFwdFast        CON      850                    ' left motor forward; fast
LFwdSlow        CON      775                    ' left motor forward; slow
LStop           CON      750                    ' left motor stop
LRevSlow        CON      725                    ' left motor reverse; slow
LRevFast        CON      650                    ' left motor reverse; fast

RFwdFast        CON      650                    ' right motor forward; fast
RFwdSlow        CON      725                    ' right motor forward; slow
RStop           CON      750                    ' right motor stop
RRevSlow        CON      775                    ' right motor reverse; slow
RRevFast        CON      850                    ' right motor reverse; fast

' -----[ Variables ]------------------------------------------------------------
blackThresh     VAR     Word                    ' QTI black threshold setting
lineBits        VAR     Nib                     ' decoded sensors value
lineLeft        VAR     lineBits.BIT1
lineRight       VAR     lineBits.BIT0

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
temp            VAR     Byte

' -----[ EEPROM Data ]----------------------------------------------------------
RunStatus       DATA    $00                     ' run status

' -----[ Initialization ]-------------------------------------------------------
Run_Check:                                      ' user Reset button as On-Off
  READ RunStatus, temp                          ' read current status
  temp = ~temp                                  ' invert status
  WRITE RunStatus, temp                         ' save status for next reset
  IF (temp = 0) THEN Set_Threshold              ' run now?
  END                                           '   -- no ... next time

' 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 = (leftSense / 8) + (rightSense / 8)

Start_Delay:                                    ' mandatory 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!

' -----[ Main 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 line
  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 15
    PULSOUT LMotor, LFwdFast
    PULSOUT RMotor, RFwdFast
    GOSUB Read_Line_Sensors
    IF (lineBits = %11) THEN Match_Over         ' in sight and 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 Run_Check                                ' reset for next round

' -----[ Subroutines ]----------------------------------------------------------
Read_Line_Sensors:
  lineLeft = ~leftSense
  lineRight = ~rightSense

  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 10
    PULSOUT LMotor, LFwdSlow
    PULSOUT RMotor, RFwdSlow
    PAUSE 20
  NEXT
  RETURN