' 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