'{$STAMP BS2sx} ' --------------Variables------------------------ ' OldSpdRt VAR Byte 'The last Right output NewSpdRt VAR Byte 'The new Right output OldSpdLeft VAR Byte 'The last Left output NewSpdLeft VAR Byte 'The new Left output Time VAR Word 'Button push time accumulator Pattern VAR Byte 'The Start pattern to be run C VAR Byte 'Counter Delay VAR Byte 'Counter IRout VAR Byte 'Sensor output to Branch RightSearch VAR Bit SPin CON 15 'Serial out pin RPin CON 14 'Reset pin Bmode CON 240 'Serial out Baudmode 240 = 9.6Kbit, 110 = 19.2Kbit RFWD CON 0 RBAK CON 1 LFWD CON 3 LBAK CON 2 Difference CON 10 'Maximum change for ramping ' --------------InitializatioNS------------------ ' LOW 1 'Preset sensors and outputs LOW 2 LOW 3 LOW 4 LOW 5 LOW 8 LOW 9 LOW 10 HIGH 11 IRout = 0 Initial_Motor_Settings: OldSpdRt = 128 'Start at zero OldSpdLeft = 128 'Start at zero NewSpdRt = 128 'Start at zero NewSpdLeft = 128 'Start at zero Initialize_Motor_Control: PAUSE 10 LOW RPin 'Reset motor control HIGH RPin SEROUT SPin, Bmode, [$80, 0, RFWD, 0] '************************************************************************** '************************************************************************** '***************Main Program*********************************************** GET 0, Pattern 'Retrieve the existing pattern from Scratch RAM IF PaTTERn <> 0 THEN Start 'If there is nothing in Scratch Ram, Then RUN 2 'go directly to the Search and Destroy program Start: WRITE 0, 0 'Clear old pattern from EEPROM so that 'another reset will NOT rerun the old start pattern GET 0, Pattern 'Retrieve the existing pattern from Scratch RAM RightSearch = PATtern.LOWBIT 'Retrieve RightSearch from Pattern Pattern = Pattern/2 'Shift right one bit so that Pattern is correct BRANCH Pattern, [SearchRL, SearchRL, Backward_Search, Run_Away, Loop] SearchRL: RUN 2 'Go directly to Search and Destroy Backward_Search: NewSpdLeft = 0 + (Rightsearch * 128) NewSpdRt = 128 - (Rightsearch * 128) GOSUB Drive Delay = 40 '40 SME Sumo GOSUB SensorDelay 'Watch sensors for hits RUN 2 Run_Away: '***********Forward FAST*********** OldSpdRt = 255 OldSpdLeft = 255 NEWSPdRt = 255 NewSpdLeft = 255 GOSUB Drive Delay = 15 '15 SME Sumo GOSUB SensorDelay 'Watch sensors for hits '***********Turn*********** NewSpdLeft = 0 + (Rightsearch * 255) NewSpdRt = 255 - (Rightsearch * 255) GOSUB Drive Delay = 15 '15 SME Sumo GOSUB SensorDelay 'Watch sensors for hits '***********Return*********** NewSpdRt = 255 NewSpdLeft = 255 GOSUB Drive Delay = 15 '15 SME Sumo GOSUB SensorDelay 'Watch sensors for hits RUN 2 Loop: NewSpdLeft = 160 + (Rightsearch * 95) NewSpdRt = 255 - (Rightsearch * 95) GOSUB Drive Delay = 70 '70 SME Sumo GOSUB SensorDelay 'Watch sensors for hits RUN 2 '************************************************************************** '************************************************************************** '***************Drive****************************************************** 'Subroutine Drive to Ramp the drive motors to the new speed 'The speed range is 0 to 255 with 0 being full reverse, 255 full forward, and 128 stop. Drive: IF ABS(OldSpdRt - NewSpdRt) < Difference THEN EasyR 'If there is a small difference, then go to the new speed directly IF OldSpdRt > NewSpdRt THEN SubtractR 'If the speed is to drop, then go to the subtract option OldSpdRt = OldSpdRt + Difference 'If the speed is to increase, then add. GOTO SeroutputR EasyR: OldSpdRt = NewSpdRt 'If there is a small difference, then go to the new speed directly GOTO SeroutputR SubtractR: OldSpdRt = OldSpdRt - Difference 'If the speed is to drop, then go to the subtract option SeroutputR: 'It is time to determine forward or reverse and to Serial out to the motors IF OldSpdRt > 127 THEN ForwardR 'Test Right motor for Forward SEROUT SPin, Bmode, [$80, 0, RBAK, (127 - OldSpdRt)] GOTO OutputL ForwardR: SEROUT SPin, Bmode, [$80, 0, RFWD, (OldSpdRt - 128)] OutputL: 'Repeat for the Left motor IF ABS(OldSpdLeft - NewSpdLeft) < Difference THEN EasyL IF OldSpdLeft > NewSpdLeft THEN SubtractL OldSpdLeft = OldSpdLeft + Difference GOTO SeroutputL EasyL: OldSpdLeft = NewSpdLeft GOTO SeroutputL SubtractL: OldSpdLeft = OldSpdLeft - Difference SeroutputL: IF OldSpdLeft > 127 THEN ForwardL SEROUT SPin, Bmode, [$80, 0, LBAK, (127 - OldSpdLeft)] PAUSE 5 IF OldSpdLeft <> NewSpdLeft OR OldSpdRt <> NewSpdRt THEN Drive 'Test to determine if motors are at the New speeds RETURN ForwardL: SEROUT SPin, Bmode, [$80, 0, LFWD, (OldSpdLeft - 128)] PAUSE 5 IF OldSpdLeft <> NewSpdLeft OR OldSpdRt <> NewSpdRt THEN Drive 'Test to determine if motors are at the New speeds RETURN '************************************************************************** '************************************************************************** '***************Check IR sensors******************************************* SensorDelay: C = 0 'Reset counter Again: C = C + 1 'Increment counter GOSUB Sensors IF IRout.BIT1 = 1 AND IRout.BIT2 = 1 AND IRout.BIT3 = 1 THEN Continue 'Check for sensor hits RUN 2 'If there is a sensor hit....Search and Destroy Continue PAUSE 50 IF C < DElay THEN Again RETURN Sensors: FREQOUT 3, 1, 14450 'Center IR sensor IRout.BIT2 = IN6 FREQOUT 2, 1, 14450 'Left Center IR sensor IRout.BIT1 = IN6 FREQOUT 4, 1, 14450 'Right Center IR sensor IRout.BIT3 = IN6 FREQOUT 1, 1, 14493 'Left IR sensor IRout.BIT0 = IN6 FREQOUT 5, 1, 14450 'Right IR sensor IRout.BIT4 = IN6 LOW 9 LOW 10 IF IRout = 31 THEN IRoff 'If no sensors respond then LED is off HIGH 10 'If any sensor responds then LED is green IRoff: IF IRout <> 0 THEN IRred LOW 10 'If all sensor responds then LED is red HIGH 9 IRred: RETURN