Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- ' 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
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
