' CanCan.bsx - Can Collector program for Rover ' 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 10 irpdLeftLED con 9 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 2250 '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 10 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 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 irStart var bit leftIRCount var byte rightIRCount var byte fwdIRCount var byte turns var byte onCan var bit numCans var nib photoIgnoreCount var nib laps var nib ' Initialization dirs = %0000011011001011 irStart = 1 leftIRCount = 0 rightIRCount = 0 fwdIRCount = 0 turns = %11111111 onCan = 0 numCans = 0 photoIgnoreCount = 0 laps = 0 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 = 1 then Continue run 0 Continue: motorDelay = 15 '============================== 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 turns & %00011111 = %01101 or turns & %00111111 = %011001 then FinishedLap IRAction: branch action, [IrpdGoRight, IrpdGoForward, IrpdGoHardLeft, IrpdGoHardLeft] FinishedLap: laps = laps + 1 if numCans = 6 then Finished sonalert = 1 pause 500 sonalert = 0 goto IRAction Finished: for i = 1 to 3 sonalert = 1 pause 250 sonalert = 0 pause 250 next run 0 IrpdGoRight: overrideDirection = DirSlightRight goalInOverrideDir = Rotate10 timeInOverrideDir = 0 rightIRCount = rightIRCount + 1 leftIRCount = 0 fwdIRCount = 0 if rightIRCount < 30 then NoIrpd turns = turns << 1 rightIRCount = 0 serout lcd,N2400,[12,bin8 turns] pause 150 goto NoIrpd IrpdGoHardRight: overrideDirection = DirHardRight goalInOverrideDir = Rotate10 timeInOverrideDir = 0 goto NoIrpd IrpdGoLeft: overrideDirection = DirSlightLeft goalInOverrideDir = Rotate10 timeInOverrideDir = 0 goto NoIrpd IrpdGoHardLeft: overrideDirection = DirHardLeft goalInOverrideDir = Rotate45 timeInOverrideDir = 0 leftIRCount = leftIRCount + 1 rightIRCount = 0 fwdIRCount = 0 if leftIRCount < 5 then NoIrpd turns = turns << 1 + 1 leftIRCount = 0 serout lcd,N2400,[12,bin8 turns] pause 150 goto NoIrpd IrpdGoForward: overrideDirection = DirForward goalInOverrideDir = 1 timeInOverrideDir = 0 fwdIRCount = fwdIRCount + 1 if fwdIRCount < 3 then Move leftIRCount = 0 goto NoIrpd IrpdGoBack: overrideDirection = DirBack goalInOverrideDir = Dist1 timeInOverrideDir = 0 goalDirection = DirHardLeft goalInDirection = Rotate180 timeInDirection = 0 goto NoIrpd NoIrpd: '============================== END OF IRPD ROUTINE ================================= '============================== PHOTO ROUTINE ================================= PHOTO: if photoIgnoreCount = 0 then CheckPhoto photoIgnoreCount = photoIgnoreCount - 1 goto NoPhoto 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, dec photoLeftCount] if photoLeftCount < 500 then NotOnCan if onCan then NoPhoto onCan = 1 numCans = numCans + 1 for i = 1 to numCans sonalert = 1 pause 100 sonalert = 0 pause 100 next photoIgnoreCount = 3 goto NoPhoto NotOnCan: onCan = 0 photoIgnoreCount = 3 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 ' 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: 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