Skip to content

Instantly share code, notes, and snippets.

@Spartan322

Spartan322/Sumobot Comp.bs2

Created April 7, 2016 14:02
Show Gist options
  • Save Spartan322/053ed9e710e1f63f2a06fe87c3a31e27 to your computer and use it in GitHub Desktop.
Save Spartan322/053ed9e710e1f63f2a06fe87c3a31e27 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
Sumobot Comp.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 ]-------------------------------------------------------
LFwdSS CON 1400
RFwdSS CON 100
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
endCounter VAR Nib
' -----[ 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:
GOTO Spin_Left ' 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:
endCounter = 0
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, LFwdSS
PULSOUT RMotor, LFwdSS
PAUSE 20
NEXT
lastIr = %00 ' clear scan direction
GOTO Lunge
Spin_Right: ' left sensor was active
FOR pulses = 1 TO 20
PULSOUT LMotor, RFwdSS
PULSOUT RMotor, RFwdSS
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, LFwdSS
PULSOUT RMotor, RFwdSS
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:
endCounter = endCounter+1
IF (endCounter < 6) THEN About_Face
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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment