' ========================================================================= ' ' File...... Mini Sumo (2.5).BS2 ' Purpose... Mini Sumo Competition Program with Tilt check ' Author.... Jon Williams ' E-mail.... jwilliams@parallax.com ' Started... ' Updated... 14 MAR 2003 ' ' {$STAMP BS2} ' {$PBASIC 2.5} ' ' ========================================================================= ' -----[ Program Description ]--------------------------------------------- ' -----[ Revision History ]------------------------------------------------ ' -----[ I/O Definitions ]------------------------------------------------- LMotor PIN 13 ' left servo motor RMotor PIN 12 ' right servo motor LLineSnsPwr PIN 10 ' left line sensor power LLineSnsIn PIN 9 ' left line sensor input RLineSnsPwr PIN 7 ' right line sensor power RLineSnsIn 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 Xin PIN 3 ' input from Memsic 2125 Speaker PIN 2 GreenLED PIN 1 RedLED PIN 0 ' -----[ Constants ]------------------------------------------------------- HiPulse CON 1 LoPulse CON 0 LFwdFast CON 1000 ' left motor forward; fast LFwdSlow CON 825 ' left motor forward; slow LStop CON 750 ' left motor stop LRevSlow CON 625 ' left motor reverse; slow LRevFast CON 500 ' left motor reverse; fast RFwdFast CON 500 ' right motor forward; fast RFwdSlow CON 625 ' right motor forward; slow RStop CON 750 ' right motor stop RRevSlow CON 825 ' right motor reverse; slow RRevFast CON 1000 ' right motor reverse; fast MaxTilt CON 2600 ' max tilt value allowed ' -----[ Variables ]------------------------------------------------------- task VAR Nib ' current task srvoClock VAR Byte ' servo update timer tiltClock VAR Nib ' Memsic access timer lSpeed VAR Word ' left motor speed rSpeed VAR Word ' right motoro speed pulses VAR Byte ' counter for motor control blackThresh VAR Word ' QTI black threshold leftSense VAR Word ' left sensor raw reading rightSense VAR Word ' right sensor raw reading lineBits VAR Nib ' decoded sensors value lineLeft VAR lineBits.BIT1 lineRight VAR lineBits.BIT0 irBits VAR Nib ' storage for IR data irLeft VAR irBits.BIT1 irRight VAR irBits.BIT0 xTilt VAR Word ' x axis tilt runOkay VAR Byte ' run after reset? ' -----[ EEPROM Data ]----------------------------------------------------- RunStatus DATA $FF ' run status ' -----[ Initialization ]-------------------------------------------------- ' Use SumoBot Reset button as an On-Off switch Run_Check: READ RunStatus, runOkay ' read current status runOkay = ~runOkay ' invert status WRITE RunStatus, runOkay ' save status IF (runOkay = 0) THEN END ' end now? Initialize: LOW LMotor ' make motors outputs LOW RMotor lSpeed = LFwdFast ' start aggressive! rSpeed = RFwdFast srvoClock = 20 ' force servo update ' Sets black threshold to 20% 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 / 10) + (rightSense / 10) ' Create mandatory 5-second delay ' -- with beep and LED flash Start_Delay: FOR pulses = 1 TO 5 HIGH RedLED ' show active PAUSE 850 LOW RedLED ' blink each second HIGH GreenLED FREQOUT Speaker, 147, 2500, 3000 ' beep each second LOW GreenLED NEXT ' -----[ Program Code ]---------------------------------------------------- Main: DO GOSUB Update_Motors ON task GOSUB Check_Line, Check_IR, Check_Tilt task = task + 1 // 3 LOOP END ' task processors Check_Line: GOSUB Read_Line_Sensors IF (lineBits > %00) THEN GOSUB Read_IR_Sensors SELECT lineBits CASE %01 ' spin left IF (irBits = 0) THEN ' no enemy, turn back in FOR pulses = 1 TO 25 PULSOUT LMotor, LRevFast PULSOUT RMotor, RFwdFast PAUSE 20 NEXT srvoClock = 20 ELSE lSpeed = LFwdSlow ' try to push him off slow rSpeed = RStop ENDIF CASE %10 ' spin right IF (irBits = 0) THEN ' no enemy, turn back in FOR pulses = 1 TO 25 PULSOUT LMotor, LFwdFast PULSOUT RMotor, LFwdFast PAUSE 20 NEXT srvoClock = 20 ELSE lSpeed = LStop ' try to push him off slow rSpeed = RFwdSlow ENDIF CASE %11 ' on line PULSOUT LMotor, LStop PULSOUT RMotor, RStop IF (irBits = %11) THEN ' enemy right in front GOSUB Read_Tilt_Sensor IF (xTilt < 2525) THEN Match_Over ' I'm not being tiltled ELSE FOR pulses = 1 TO 10 PULSOUT LMotor, LRevFast PULSOUT RMotor, RRevFast PAUSE 20 NEXT FOR pulses = 1 TO 30 ' do an about-face PULSOUT LMotor, LFwdFast PULSOUT RMotor, RRevFast PAUSE 20 NEXT srvoClock = 20 ENDIF ENDSELECT ENDIF RETURN Check_IR: GOSUB Read_IR_Sensors SELECT irBits CASE %01 ' turn right lSpeed = LFwdFast rSpeed = RFwdSlow CASE %10 ' turn left lSpeed = LFwdSlow rSpeed = RFwdFast CASE %11 ' jump on him! lSpeed = LFwdFast rSpeed = RFwdFast ENDSELECT GreenLED = irLeft ' display IR in on LEDs RedLED = irRight srvoClock = srvoClock + 1 ' update servo timer RETURN Check_Tilt: IF (tiltClock >= 5) THEN ' okay to read tilt? tiltClock = 0 ' yes -- reset tilt timer GOSUB Read_Tilt_Sensor ' read tilt sensor IF (xTilt > MaxTilt) THEN ' check tilt GOSUB Escape_Move ' if titled, try to escape ENDIF ELSE srvoClock = srvoClock + 1 ' update servo timer ENDIF RETURN ' -----[ Subroutines ]----------------------------------------------------- Update_Motors: IF (srvoClock >= 20) THEN ' if timer expired PULSOUT LMotor, lSpeed ' -- update servos PULSOUT RMotor, rSpeed srvoClock = 0 ' reset servo timer tiltClock = tiltClock + 1 ' increment tilt timer ELSE srvoClock = srvoClock + 1 ' account for check ENDIF RETURN Read_Line_Sensors: HIGH LLineSnsPwr ' activate sensors HIGH RLineSnsPwr HIGH LLineSnsIn ' discharge QTI caps HIGH RLineSnsIn PAUSE 0 RCTIME LLineSnsIn, 1, leftSense ' read left sensor RCTIME RLineSnsIn, 1, rightSense ' read right sensor LOW LLineSnsPwr ' deactivate sensors LOW RLineSnsPwr ' convert readings to bits lineBits = %00 LOOKDOWN leftSense, >=[blackThresh, 0], lineLeft LOOKDOWN rightSense, >=[blackThresh, 0], lineRight srvoClock = srvoClock + 15 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) srvoClock = srvoClock + 5 RETURN Read_Tilt_Sensor: PULSIN Xin, HiPulse, xTilt ' read pulse output srvoClock = srvoClock + 9 RETURN ' Attempt escape if tilted up -- enemy under scoop Escape_Move: FOR pulses = 1 TO 15 ' back-up PULSOUT LMotor, LRevFast PULSOUT RMotor, RRevFast PAUSE 20 NEXT FOR pulses = 1 TO 25 ' spin out of path PULSOUT LMotor, LStop PULSOUT RMotor, RRevFast PAUSE 20 NEXT FOR pulses = 1 TO 15 ' back up a bit more PULSOUT LMotor, LRevFast PULSOUT RMotor, RRevFast PAUSE 20 NEXT FOR pulses = 1 TO 5 ' turn toward enemy PULSOUT LMotor, LRevFast PULSOUT RMotor, RFwdFast PAUSE 20 NEXT lSpeed = LFwdFast rSpeed = RFwdFast srvoClock = 20 ' force servo update RETURN ' We won -- stop motors and play a tune Match_Over: FOR pulses = 1 TO 5 ' stop motors PULSOUT LMotor, LStop PULSOUT RMotor, RStop PAUSE 20 NEXT INPUT LMotor INPUT RMotor FOR pulses = 1 TO 10 ' make some noise HIGH RedLED LOW GreenLED FREQOUT Speaker, 100, 2500, 3000 ' beep LOW RedLED HIGH GreenLED PAUSE 100 NEXT DIRS = $0000 ' disable all outputs GOTO Run_Check ' reset for next round