' Linetrk.bsx - line tracker 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 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 2130 '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 1958 '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 ' 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 10 Rotate45 con 21 Rotate90 con 45 Rotate180 con 90 Rotate270 con 120 ' Distance constants Dist1 con 45 Dist2 con 90 ' Variables rightSpeed var word leftSpeed var word action var nib i var byte j var nib 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 lineColor var bit lineLastDir var nib endOfLine var bit irStart var bit ' Initialization dirs = %0000011011001011 get 0,lineColor lineLastDir = DirForward timeInDirection = 0 goalInDirection = 0 timeInOverrideDir = 0 goalInOverrideDir = 0 endOfLine = 0 get 1,irStart if irStart = 0 then Loop serout lcd,N2400,[16,64,"Awaiting IRStart"] WaitIRStart: if irpdSensor = 1 then WaitIRStart '========================= MAIN LOOP ========================== Loop: if buttonA = 1 then NoOverride run 0 Continue: motorDelay = 10 '========================= 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 CheckLeftBumper action = action + 1 CheckLeftBumper: input leftBumper if leftBumper <> BumperPressed then DoBumperAction action = action + 2 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 = DirHardRight goalInDirection = Rotate180 timeInDirection = 0 goto Move NoBumper: '======================== END OF BUMPER 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 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 timeInDirection = 0 goalInDirection = 1 serout lcd,N2400,[16,64+11,bin in13, bin in12, bin in11] if in11 <> lineColor or in12 <> lineColor or in13 <> lineColor then LineCheckRight endOfLine = 1 goto LineCheckCenter LineCheckRight: if in11 <> lineColor then LineCheckLeft goalDirection = DirSlightRight lineLastDir = DirSlightRight goto DoGoalDirection LineCheckLeft: if in13 <> lineColor then LineCheckCenter goalDirection = DirSlightLeft lineLastDir = DirSlightLeft goto DoGoalDirection LineCheckCenter: if in12 <> lineColor then LineLost goalDirection = DirForward lineLastDir = DirForward goto DoGoalDirection LineLost: if endOfLine = 0 then FindLine ' end of line marker seen followed by losing line = done! for i = 1 to 3 sonalert = 1 pause 250 sonalert = 0 pause 250 next run 0 FindLine: goalDirection = lineLastDir goto DoGoalDirection 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 rightMotor, rightFwdFull pulsout leftMotor, leftBackFull goto Loop GoLeft: pulsout rightMotor, rightFwdFull pulsout leftMotor, leftStop goto Loop GoSlightLeft: pulsout rightMotor, rightFwdFull pulsout leftMotor, leftFwdSlow goto Loop