' Rover3.bsx - roving robot controller ' I/O pins rightMotor con 0 leftMotor con 1 rightBumper var in0 leftBumper var in1 lcd con 2 buttonA var in4 buttonB var in5 leftLED var out6 sonalert var out6 rightLED var out7 irpdSensor var in8 irpdRightLED con 9 irpdLeftLED con 10 rightLineSensor var in11 centerLineSensor var in12 leftLineSensor var in13 rightPhotocell con 14 leftPhotocell con 15 ' Speed constants rightBackFull con 1250 '500 * 2.5 rightBackSlow con 1980 '792 * 2.5 rightStop con 2055 '822 * 2.5 rightFwdSlow con 2120 '852 * 2.5 rightFwdFull con 2500 '1000 * 2.5 leftBackFull con 2500 '1000 * 2.5 leftBackSlow con 2108 '843 * 2.5 leftStop con 2032 '813 * 2.5 leftFwdSlow con 1850 '783 * 2.5 leftFwdFull con 1250 '500 * 2.5 ' SerOut constants 'N2400 con 16780 ' for BS2 N2400 con 17405 ' for BS2-SX ' Sensor value constants BumperPressed con 1 IrpdSensed con 0 LineSensed con 0 ' Mode constants ModeBLineTracker con 0 ModeWLineTracker con 1 ModeRightWall con 2 ModeLeftWall con 3 ModeCanCan con 4 ModePhotophile con 5 ModePhotophobe con 6 ModePhotoAlt con 7 ModeWander con 8 ModeDance con 9 ModeDirTest con 10 ModeSensorTest con 11 ModeToggleIRSt con 12 ModeAbout con 13 LastMode con 13 ' Direction state constants DirStop con 0 DirForward con 1 DirSlightRight con 2 DirRight con 3 DirHardRight con 4 DirBackRight con 5 DirBackSlightRight con 6 DirBack con 7 DirBackSlightLeft con 8 DirBackLeft con 9 DirHardLeft con 10 DirLeft con 11 DirSlightLeft con 12 DirNoOverride con 13 ' Rotation constants Rotate10 con 3 Rotate45 con 20 Rotate90 con 40 Rotate180 con 80 Rotate270 con 100 ' Distance constants Dist1 con 30 Dist2 con 60 ' Variables action var nib i var byte j var nib irpdRightCount var nib irpdLeftCount var nib photoLeftCount var word photoRightCount var word avoidLight var bit photoLoopCount var word photoLoopTarget var byte mode var nib goalDirection var byte timeInDirection var byte goalInDirection var byte overrideDirection var nib timeInOverrideDir var byte goalInOverrideDir var byte randomNumber var byte motorDelay var byte danceIndex var byte dancePhase var bit lineColor var bit irStart var bit rightBumpCount var nib leftBumpCount var nib leftIRCount var nib rightIRCount var nib fwdIRCount var nib turns var byte danceTable data DirRight,DirBackRight,DirBackRight,DirRight danceTable2 data DirLeft,DirBackLeft,DirBackLeft,DirLeft danceTable3 data DirForward,DirBack,DirBack,DirForward danceTable4 data DirBack,DirForward,DirForward,DirBack danceTable5 data DirHardLeft,DirForward,DirBack,DirSlightLeft danceTable6 data DirHardLeft,DirForward,DirBack,DirSlightLeft danceTable7 data DirHardLeft,DirForward,DirBack,DirSlightLeft danceTable8 data DirHardLeft,DirHardRight,DirHardRight,DirHardLeft ' Initialization dirs = %0000011011001011 photoLoopCount = 0 avoidLight = 0 danceIndex = 0 dancePhase = 1 random photoLoopTarget irStart = 1 rightBumpCount = 0 leftBumpCount = 0 leftIRCount = 0 rightIRCount = 0 fwdIRCount = 0 turns = %11111111 'while waiting for lcd to init, position IRPD at center for i = 1 to 25 pulsout 3,1910 pause 20 next DisplayVersion: serout lcd,N2400,[12,"Encoder 04/01/01 Dennis Draheim"] pause 2000 mode = 8 SelectMode: serout lcd,N2400,[12,"A=next mode,B=go"] branch mode,[Mode0,Mode1,Mode2,Mode3,Mode4,Mode5,Mode6,Mode7,Mode8,Mode9,Mode10,Mode11,Mode12,Mode13] Mode0: serout lcd,N2400,["B. Line Tracker "] goto CheckButtons Mode1: serout lcd,N2400,["W. Line Tracker "] goto CheckButtons Mode2: serout lcd,N2400,["R. Wall Follower"] goto CheckButtons Mode3: serout lcd,N2400,["L. Wall Follower"] goto CheckButtons Mode4: serout lcd,N2400,[" Can Collector "] goto CheckButtons Mode5: serout lcd,N2400,[" Photophile "] goto CheckButtons Mode6: serout lcd,N2400,[" Photophobe "] goto CheckButtons Mode7: serout lcd,N2400,["Photophile/phobe"] goto CheckButtons Mode8: serout lcd,N2400,[" Wanderer "] goto CheckButtons Mode9: serout lcd,N2400,[" Gotta Dance! "] goto CheckButtons Mode10: serout lcd,N2400,[" Direction Test "] goto CheckButtons Mode11: serout lcd,N2400,[" Sensor Test "] goto CheckButtons Mode12: if irStart then DisplayIRStart serout lcd,N2400,[" Enable IRStart "] goto CheckButtons DisplayIRStart: serout lcd,N2400,["Disable IRStart "] goto CheckButtons Mode13: serout lcd,N2400,[" About Rover "] goto CheckButtons CheckButtons: ' pause 50 ' if buttonA = 1 then CheckButtonB ' mode = mode + 1 ' if mode <= LastMode then SelectMode ' mode = 0 ' goto SelectMode 'CheckButtonB: ' if buttonB = 1 then CheckButtons DoMode: serout lcd,N2400,[16,64,"Running: "] branch mode,[BLineTracker,WLineTracker,RWallFollower,LWallFollower,CanCan,Photophile,Photophobe,Photophile,Loop,Loop,DirTest,SensorTest,ToggleIRStart,DisplayVersion] Photophile: avoidLight = 0 goto Loop Photophobe: avoidLight = 1 goto Loop BLineTracker: lineColor = 0 goto LineTracker WLineTracker: lineColor = 1 LineTracker: put 0,lineColor put 1,irStart run 1 RWallFollower: for i = 1 to 10 pulsout 3, 2600 '1064 * 2.5 pause 12 next goto CkIRStart LWallFollower: for i = 1 to 10 pulsout 3, 1250 '500 * 2.5 pause 12 next goto CkIRStart CanCan: run 2 DirTest: run 3 SensorTest: run 4 ToggleIRStart: irStart = 1 - irStart goto SelectMode CkIRStart: if irStart = 0 then Loop serout lcd,N2400,[16,64,"Awaiting IRStart"] WaitIRStart: if irpdSensor = 1 then WaitIRStart serout lcd,N2400,[16,64,"Running: "] '============================== MAIN LOOP ================================= Loop: ' if buttonA = 0 then SelectMode motorDelay = 15 '============================== BUMPER ROUTINE ================================= 'input leftBumper 'input rightBumper 'serout lcd,N2400,[12, bin leftBumper, bin rightBumper] CheckBumpers: ' bump sensors are highest priority, so check them first action = 0 input rightBumper if rightBumper <> BumperPressed then NoRightBump rightBumpCount = rightBumpCount + 1 if rightBumpCount < 3 then CheckLeftBumper action = action + 1 goto CheckLeftBumper NoRightBump: rightBumpCount = 0 CheckLeftBumper: input leftBumper if leftBumper <> BumperPressed then NoLeftBump leftBumpCount = leftBumpCount + 1 if leftBumpCount < 3 then DoBumperAction action = action + 2 goto DoBumperAction NoLeftBump: leftBumpCount = 0 DoBumperAction: branch action, [NoBumper, BumpedRight, BumpedLeft, BumpedBoth] BumpedRight: overrideDirection = DirBack goalInOverrideDir = Dist1 timeInOverrideDir = 0 ' goalDirection = DirHardLeft ' goalInDirection = Rotate90 ' timeInDirection = 0 goto Move BumpedLeft: overrideDirection = DirBack goalInOverrideDir = Dist1 timeInOverrideDir = 0 ' goalDirection = DirHardRight ' goalInDirection = Rotate90 ' timeInDirection = 0 goto Move BumpedBoth: overrideDirection = DirBack goalInOverrideDir = Dist1 timeInOverrideDir = 0 ' goalDirection = DirHardLeft ' goalInDirection = Rotate180 ' timeInDirection = 0 goto Move NoBumper: '============================== END OF BUMPER ROUTINE ================================= '============================== IRPD ROUTINE ================================= IRPD: ' irpd sensors are next priority if overrideDirection <> DirNoOverride then Move motorDelay = 5 action = 0 irpdRightCount = 0 for i = 1 to 5 high irpdRightLED for j = 1 to 2 next ' pause 1 irpdRightCount = irpdRightCount + irpdSensor low irpdRightLED next if irpdRightCount > 3 then CheckLeftIrpd action = action + 1 CheckLeftIrpd: irpdLeftCount = 0 for i = 1 to 5 high irpdLeftLED for j = 1 to 2 next ' pause 1 irpdLeftCount = irpdLeftCount + irpdSensor low irpdLeftLED next if irpdLeftCount > 3 then DoIrpdAction action = action + 2 DoIrpdAction: ' rightLED = action.bit0 ' leftLED = action.bit1 ' serout lcd,N2400,[" ",dec irpdLeftCount," ",dec irpdRightCount] if mode <> ModeRightWall then CheckModeLeftWall if turns & %00011111 = %01101 or turns & %00111111 = %011001 then Finished branch action, [IrpdGoRight, IrpdGoForward, IrpdGoHardLeft, IrpdGoHardLeft] Finished: for i = 1 to 3 sonalert = 1 pause 250 sonalert = 0 pause 250 next run 0 CheckModeLeftWall: if mode <> ModeLeftWall then IrpdAvoid branch action, [IrpdGoLeft, IrpdGoHardRight, IrpdGoForward, IrpdGoHardRight] IrpdAvoid: pulsout 3, 1910 '764 * 2.5 branch action, [NoIrpd, IrpdGoHardLeft, IrpdGoHardRight, IrpdGoHardLeft] IrpdGoRight: overrideDirection = DirSlightRight goalInOverrideDir = Rotate10 timeInOverrideDir = 0 rightIRCount = rightIRCount + 1 leftIRCount = 0 fwdIRCount = 0 if rightIRCount < 5 then Move turns = turns << 1 rightIRCount = 0 serout lcd,N2400,[12,bin8 turns] pause 150 goto Move IrpdGoHardRight: overrideDirection = DirHardRight goalInOverrideDir = Rotate10 timeInOverrideDir = 0 goto Move IrpdGoLeft: overrideDirection = DirSlightLeft goalInOverrideDir = Rotate10 timeInOverrideDir = 0 goto Move IrpdGoHardLeft: overrideDirection = DirHardLeft goalInOverrideDir = Rotate10 timeInOverrideDir = 0 leftIRCount = leftIRCount + 1 rightIRCount = 0 fwdIRCount = 0 if leftIRCount < 5 then Move turns = turns << 1 + 1 leftIRCount = 0 serout lcd,N2400,[12,bin8 turns] pause 150 goto Move IrpdGoForward: overrideDirection = DirForward goalInOverrideDir = 1 timeInOverrideDir = 0 fwdIRCount = fwdIRCount + 1 if fwdIRCount < 3 then Move leftIRCount = 0 goto Move IrpdGoBack: overrideDirection = DirBack goalInOverrideDir = Dist1 timeInOverrideDir = 0 goalDirection = DirHardLeft goalInDirection = Rotate180 timeInDirection = 0 goto Move NoIrpd: '============================== END OF IRPD ROUTINE ================================= '============================== PHOTO ROUTINE ================================= PHOTO: ' photocells are next priority, if enabled if mode > ModePhotoAlt then NoPhoto if mode <> ModePhotoAlt then CheckPhoto photoLoopCount = photoLoopCount + 1 if photoLoopCount < photoLoopTarget then CheckPhoto avoidLight = 1 - avoidLight serout lcd,N2400,[16,64+15, bin avoidLight] photoLoopCount = 0 random photoLoopTarget CheckPhoto: 'serout lcd,N2400,[12, "P"] high rightPhotocell pause 1 rctime rightPhotocell, 1, photoRightCount high leftPhotocell pause 1 rctime leftPhotocell, 1, photoLeftCount 'serout lcd,N2400,[12,14, dec photoLeftCount] if (abs (photoRightCount - photoLeftCount)) < (photoRightCount / 10 + 2) then NoPhoto if avoidLight then PhotoAvoid 'serout lcd,N2400,["+"] if photoRightCount < photoLeftCount then GoRight 'serout lcd,N2400,["L"] goto GoLeft PhotoAvoid: 'serout lcd,N2400,["-"] if photoRightCount > photoLeftCount then GoRight 'serout lcd,N2400,["L"] goto GoLeft NoPhoto: '============================== END OF PHOTO ROUTINE ================================= '============================== MOVE ROUTINE ================================= ' Move according to the selected override direction or goal direction MOVE: pause motorDelay if overrideDirection = DirNoOverride then NoOverride timeInOverrideDir = timeInOverrideDir + 1 if timeInOverrideDir <= goalInOverrideDir then DoOverride overrideDirection = DirNoOverride goto noOverride DoOverride: ' serout lcd,N2400,[12, "Override: ", dec overrideDirection, 13, dec timeInOverrideDir, " of ", dec goalInOverrideDir] branch overrideDirection, [GoNowhere,GoForward,GoSlightRight,GoRight,GoHardRight,GoBackRight,GoBackSlightRight,GoBack,GoBackSlightLeft,GoBackLeft,GoHardLeft,GoLeft,GoSlightLeft,NoOverride] NoOverride: timeInDirection = timeInDirection + 1 if timeInDirection <= goalInDirection then DoGoalDirection ' calculate new goal direction if mode <> ModeWander then NoWander ' calculate new Wander direction random randomNumber lookup randomNumber.highNib,[DirForward,DirForward,DirSlightRight,DirSlightLeft,DirForward,DirForward,DirSlightRight,DirSlightLeft,DirRight,DirForward,DirSlightRight,DirSlightLeft,DirForward,DirLeft,DirSlightRight,DirSlightLeft],goalDirection goalInDirection = randomNumber.lowNib * 5 timeInDirection = 0 goto DoGoalDirection NoWander: if mode <> ModeDance then NoDance ' dance in an interesting pattern dancePhase = 1 - dancePhase if dancePhase then DancePause serout lcd,N2400,[16,64+12,dec2 danceIndex] read danceIndex,goalDirection goalInDirection = 12 timeInDirection = 0 danceIndex = danceIndex + 1 if danceIndex <= 31 then DoGoalDirection danceIndex = 0 goto DoGoalDirection DancePause goalDirection = DirStop goalInDirection = 4 timeInDirection = 0 goto DoGoalDirection NoDance: goalDirection = DirForward DoGoalDirection: ' serout lcd,N2400,[12, "Goal: ", dec goalDirection, 13, dec timeInDirection, " of ", dec goalInDirection] branch goalDirection, [GoNowhere,GoForward,GoSlightRight,GoRight,GoHardRight,GoBackRight,GoBackSlightRight,GoBack,GoBackSlightLeft,GoBackLeft,GoHardLeft,GoLeft,GoSlightLeft] '============================== END OF MOVE ROUTINE ================================= '============================== END OF MAIN LOOP ================================= '============================== MOVEMENT ROUTINES ================================= GoNowhere: pause 10 goto loop GoForward: pulsout rightMotor, rightFwdFull pulsout leftMotor, leftFwdFull goto Loop GoSlightRight: pulsout rightMotor, rightFwdSlow pulsout leftMotor, leftFwdFull goto Loop GoRight: pulsout rightMotor, rightStop pulsout leftMotor, leftFwdFull goto Loop GoHardRight: pulsout rightMotor, rightBackFull pulsout leftMotor, leftFwdFull goto Loop GoBackRight: pulsout rightMotor, rightStop pulsout leftMotor, leftBackFull goto Loop GoBackSlightRight: pulsout rightMotor, rightBackSlow pulsout leftMotor, leftBackFull goto Loop GoBack: pulsout rightMotor, rightBackFull pulsout leftMotor, leftBackFull goto loop GoBackSlightLeft: pulsout rightMotor, rightBackFull pulsout leftMotor, leftBackSlow goto Loop GoBackLeft: pulsout rightMotor, rightBackFull pulsout leftMotor, leftStop goto Loop GoHardLeft: pulsout leftMotor, leftBackFull pulsout rightMotor, rightFwdFull goto Loop GoLeft: pulsout rightMotor, rightFwdFull pulsout leftMotor, leftStop goto Loop GoSlightLeft: pulsout rightMotor, rightFwdFull pulsout leftMotor, leftFwdSlow goto Loop