'{$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 IRout VAR Byte 'Sensor output to Branch Pattern VAR Byte 'Information from Scratch Pad RAM RightSearch VAR Bit 'If rightsearch = 1 search to right 'If rightsearch = 0 search to left RtHit VAR Byte LtHit VAR Byte NoHit VAR Byte OtherHit VAR Byte ' --------------Constants------------------------ ' 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------------------ ' Initial_IO_Pins: LOW 1 'Preset sensors and outputs LOW 2 LOW 3 LOW 4 LOW 5 LOW 8 LOW 9 LOW 10 HIGH 11 Initial_Motor_Settings: OldSpdRt = 128 'Start at zero OldSpdLeft = 128 'Start at zero NewSpdRt = 128 'Start at zero NewSpdLeft = 128 'Start at zero Initialize_Left_Right: GET 0, Pattern RightSearch = PATtern.LOWBIT Initialize_Motor_Control: PAUSE 10 LOW RPin 'Reset motor control HIGH RPin SEROUT SPin, Bmode, [$80, 0, RFWD, 0] IRout = 0 RtHit = 0 LtHit = 0 NoHit = 0 OtherHit = 0 '************************************************************************** '************************************************************************** '***************Main Program*********************************************** Main: GOSUB Sensors GOSUB Branching GOSUB Drive PAUSE 100 GOTO Main '************************************************************************** '************************************************************************** '***************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: OUT14 = ~IN13 'Remote Kill 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******************************************* 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: IF IROUt = 0 OR IROUt = 1 OR IRoUT = 16 THEN Alarmon LOW 8 RETURN Alarmon: HIGH 8 RETURN '************************************************************************** '************************************************************************** '***************Branching************************************************** Branching: 'IRout = 0 'Use for testing motor constants '0 for Straight '12 for Hard Right '5 for Hard Left '8 for Std Right '2 for Std Left '9 for Easy Right '18 for Easy Left '26 for Search Right '11 for Search Left '10 for Search BRANCH IRout, [Other, Other, Other, Other, Other, Other, Other, LeftHit, Other, Other, Other, Other, Other, Other, Other, LeftHit, Other, Other, Other, Other, Other, Other, Other, LeftHit, Other, Other, Other, Other, RightHit, RightHit, RightHit, NotaHit] '0 = hit 00000 00001 00010 00011 00100 00101 00110 00111 01000 01001 01010 01011 01100 01101 01110 01111 10000 10001 10010 10011 10100 10101 10110 10111 11000 11001 11010 11011 11100 11101 11110 11111 Other: OtherHit = OtherHit + 1 IF OtherHIT < 2 THEN BranchAgain OtherHit = 0: LtHit = 0: RtHit = 0: NoHit = 0 GOTO BranchAgain LeftHit: LtHit = LtHit + 1 IF LtHIT < 2 THEN BranchAgain OtherHit = 0: RtHit = 0: NoHit = 0 IF LtHIT < 45 THEN BranchAgain '45 is about six to seven seconds NewSpdLeft = 0 'Drive backwards for about eight NewSpdRt = 95 'inches as the controller changes speeds LtHit = 25 'If no change, do again in two seconds GOSUB Drive RETURN 'Return to main program RightHit: RtHit = RtHit + 1 IF RtHIT < 2 THEN BranchAgain OtherHit = 0: LtHit = 0: NoHit = 0 IF RtHIT < 45 THEN BranchAgain '45 is about six to seven seconds NewSpdLeft = 95 'Drive backwards for about eight NewSpdRt = 0 'inches as the controller changes speed RtHit = 25 GOSUB Drive RETURN 'Return to main program NotaHit: NoHit = NoHit + 1 IF NoHIT < 2 THEN BranchAgain OtherHit = 0: LtHit = 0: RtHit = 0 IF NoHiT < 100 THEN BranchAgain '100 is about eleven seconds NoHit = 0 'Zero out counter NewSpdLeft = 0 'Drive backwards for about NewSpdRt = 0 'a foot as the controller GOSUB Drive 'changes speeds PAUSE 1000 'Pause 1 sec for a total of 3 1/2 feet backwards RETURN 'Return to main program BranchAgain: BRANCH IRout, [Straight,EasyLeft,Left,Left,Straight,HardLeft,HardLeft,HardLeft,Right,EasyRight,Search,SearchLeft,HardRight,Search,Search,SearchLeft,EasyRight,Straight,EasyLeft,EasyLeft,HardRight,Search,Search,Left,Right,EasyRight,SearchRight,StraightR,HardRight,Right,SearchRight,Search] '0 = hit 00000 00001 00010 00011 00100 00101 00110 00111 01000 01001 01010 01011 01100 01101 01110 01111 10000 10001 10010 10011 10100 10101 10110 10111 11000 11001 11010 11011 11100 11101 11110 11111 StraightR: '****************Straight, change search direction Rightsearch = Rightsearch ^ 1 GOTO Straight SearchRight: '****************Search to right Rightsearch = 1 GOTO Search SearchLeft: '****************Search to left Rightsearch = 0 GOTO Search Search: '****************Search to right if Rightsearch = 1, Search to left if rightsearch = 0 NewSpdLeft = 0+(Rightsearch*255) NewSpdRt = 255-(Rightsearch*255) RETURN Straight: '****************Straight**************** NewSpdLeft = 255 NewSpdRt = 255 RETURN HardRight: '****************Hard right**************** Rightsearch = 1 NewSpdLeft = 255 NewSpdRt = 135 RETURN HardLeft: '****************Hard left**************** Rightsearch = 0 NewSpdLeft = 135 NewSpdRt = 255 RETURN Right: '****************Right**************** Rightsearch = 1 NewSpdLeft = 255 NewSpdRt = 160 RETURN Left: '****************Left**************** Rightsearch = 0 NewSpdLeft = 160 NewSpdRt = 255 RETURN EasyRight: '****************Easy right**************** Rightsearch = 1 NewSpdLeft = 255 NewSpdRt = 185 RETURN EasyLeft: '****************Easy left**************** Rightsearch = 0 NewSpdLeft = 185 NewSpdRt = 255 RETURN