'{$STAMP BS2sx, Start_Patterns.bsx, Search&Destroy.bsx, Remote_Control.bsx} '{$STAMP BS2sx} 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 INPUT 8 INPUT 11 INPUT 12 INPUT 13 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 N VAR Byte 'Counter Difference CON 10 'Maximum change for ramping OldSpdRt = 128 'Start at zero OldSpdLeft = 128 'Start at zero NewSpdRt = 128 'Start at zero NewSpdLeft = 128 'Start at zero LOW RPin 'Reset motor control HIGH RPin SEROUT SPin, Bmode, [$80, 0, RFWD, 0] '************************************************************************** '************************************************************************** main: PAUSE 100 IF IN11 = 1 THEN forward IF IN8 = 1 THEN left IF IN12 = 1 THEN right IF IN13 = 1 THEN back GOTO hault '*********Full forward********** Forward: NewSpdRt = 255 NewSpdLeft = 255 GOSUB Drive GOTO Main '*********Full reverse********** Back: NewSpdRt = 0 NewSpdLeft = 0 GOSUB Drive GOTO main '*********Left********** Left: NewSpdRt = 0 NewSpdLeft = 255 GOSUB Drive GOTO Main '*********Right********** Right: NewSpdRt = 255 NewSpdLeft = 0 GOSUB Drive GOTO Main '*********Stop********** Hault: NewSpdRt = 128 NewSpdLeft = 128 GOSUB Drive GOTO main '************************************************************************** '************************************************************************** '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