#pragma config(Sensor, in1, left4, sensorLineFollower) #pragma config(Sensor, in2, left3, sensorLineFollower) #pragma config(Sensor, in3, left2, sensorLineFollower) #pragma config(Sensor, in4, left1, sensorLineFollower) #pragma config(Sensor, in5, right1, sensorLineFollower) #pragma config(Sensor, in6, right2, sensorLineFollower) #pragma config(Sensor, in7, right3, sensorLineFollower) #pragma config(Sensor, in8, right4, sensorLineFollower) #pragma config(Sensor, in10, OnOffButton, sensorTouch) #pragma config(Sensor, in11, LEDRight, sensorDigitalOut) #pragma config(Sensor, in12, LEDLeft, sensorDigitalOut) #pragma config(Sensor, in13, LEDLineLost, sensorDigitalOut) #pragma config(Sensor, in14, LEDBothMotorFast, sensorDigitalOut) #pragma config(Motor, port1, Audio, tmotorAudio, openLoop) #pragma config(Motor, port2, leftMotor, tmotorNormal, openLoop) #pragma config(Motor, port3, rightMotor, tmotorNormal, openLoop, reversed) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// //////////////////////////////////////////////////////////////////////////////// // // VEX Maze Solving Robot // // Source code for a VEX maze solving robot. The robot was described in // the Sept/Oct 2009 issue of ROBOT magazine. // // Lots of comments. Read the comments for insight on the operation. // // Search youtube for "VEX ROBOTC Maze Solving Robot" for several videos of the robot // in action. Published on YouTube Sept 18, 2009. // // Program written in ROBOTC. See www.robotc.net for more details. // //////////////////////////////////////////////////////////////////////////////// #pragma platform (VEX) static unsigned char nSaveNumbLeft = 0; static unsigned char nSaveNumbRight = 0; static unsigned char nSaveFilterNone = 0; static unsigned char nSaveFilterCenter = 0; const TTimers kPIDStartupTimer = T3; const int kMinimumStraightLineTime = 50; //int nNumbLeftHits; int nNumbCenterHits; //int nNumbRightHits; bool bMotorsDisabled = true; int nErrorValue; int nLastError; int nTurnSpeed = 55; int nTargetSpeed = 90; // Target power level for PID algorithm int nTargetClassicalSpeed = 45; // Target power level for classical line follower int nMaxAllowedSpeed = 125; int nAdjustment; int nLeftMotor; int nRightMotor; int nPFactor = 6; //10; int nIFactor = 0; int nDFactor = 120; //60; int nDerivativeAdjustment; int nDerivativeCircIndex = 0; const int kDerivativeSize = 4; int nDerivative[4] = {0, 0, 0, 0}; typedef enum { goLeft, goRight, goStraight, goReverse, goStop, } TTurnType; TTurnType nTurnType; typedef enum { typeStraight, typeLeft, typeRight, typeLeftStraight, typeRightStraight, typeLeftRight, typeCross, typeDeadEnd, typeGoalReached, } TIntersectionTypes; void displayIntersectionTypeOnLCD(TIntersectionTypes nTurnType); void TurnClockwise(); void SmartMazeSolver(); void AlwaysTurnRight(); void processMotorEnableDisableButton(); void classicalZigZagLineFollower(); /////////////////////////////////////////////////////////////////////////////////////////////////////////////// // // Intersection Detection // // The maze follower program traverses a maze consisting of a black line with various intersections. All turns // are right angle turns; the maze does not contain any curves. // // A eight element array of line detection sensors is used. // - the outer left and right sensor are used for detecting the type of intersection // - the center elements are used for a PID algorithm for following a straight line. Let's number then // -3, -2, -1, +1, +2, +3. Typically the line is under sensors -1 or +1. When it strays beyond these the // PID algorithm is very good at corecting movement. Very rarely will the line reach -3 or +3! // // For turn detection, the eight sensors are reduced to 3-bits. // 1. Left detector // 2. Center detector // 3. Right detector // // A state machine is used for turn detection based on the above three bits. The state machine has three states: // 1. Following a straight line waiting for detection of the leading edge (i.e. left or right) of a turn. Or for // a dead end. // 2. Leading edge of a turn (i.e. left or right sensor over black line) detected, waiting for end of turn // detection (i.e. left right sensors see white). // 3. Hit filtering end of left, right or T turn. Waiting for all three sensors to see white. // /////////////////////////////////////////////////////////////////////////////////////////////////////////////// typedef enum { driveNone, driveFollowLineViaPID, driveDetectIntersection, drivePerformingTurn } TDriveState; TDriveState nDriveState = driveNone; typedef enum { // Left Center Right scanNone = 0, // 0 0 0 scanRight = 1, // 0 0 1 scanCenter = 2, // 0 1 0 scanCenterRight = 3, // 0 1 1 scanLeft = 4, // 1 0 0 scanLeftRight = 5, // 1 0 1 scanLeftCenter = 6, // 1 1 0 scanLeftCenterRight = 7, // 1 1 1 } TSensorStates; TSensorStates nSensorState; TIntersectionTypes detectIntersection(); TIntersectionTypes nIntersection; typedef enum { stateSingleLinePreIntersection, stateSingleLinePreIntersectionPlus1, stateSingleLinePreIntersectionPlus2, stateDetectIntersection, stateEndOfIntersectionNoneExpected, stateSingleLinePostIntersection, stateNoLine, stateInvalidTransition, } TDetectionState; TDetectionState nDetectionState = stateSingleLinePreIntersection; static unsigned char nNumbLeft = 0; static unsigned char nNumbRight = 0; static unsigned char nFilterNonePostIntersection = 0; static unsigned char nFilterCenterPostIntersection = 0; static int nNumbBadIntersections = 0; void resetToFollowingStraightLine() { // // Reset state to following straight line and looking for a turn detection // nNumbLeft = 0; nNumbRight = 0; //nFilterNone = 0; //nFilterCenter = 0; nDetectionState = stateSingleLinePreIntersection; return; } void badIntersectionDetection() { // Failure during intersection detection ++nNumbBadIntersections; return; } ////////////////////////////////////////////////////////////////////////////// // // kGoalWidth // // The end of the maze is signalled by a large black rectange that all sensors // will detect. // // The rectangle is much wider than a regular line. // // Initially we cannot tell whether we've reached an intersection (a "T" or a // "cross") or the goal. // // The "goal" is detected by a high reading on number of hits on both the left // and right sensors. The value below works fine for my robot. You may have to // adjust for other robots or other mazes where the "goal" rectangle is a // different size. // ////////////////////////////////////////////////////////////////////////////// const int kGoalWidth = 25; ////////////////////////////////////////////////////////////////////////////// // // detectIntersection // // Function is used to detect an intersection. // // The detection is driven by a state machine. The eight individual sensors // are simplified to three elements. // 1. A left branch is detected, i.e. the leftmost sensor // 2. A right branch is detected, i.e. the rightmost sensor // 3. A center region is detected, i.e. any of the inner six sensors. // // The state machine looks at these three values to determine the type of // intersection. Hit filtering is used. // ////////////////////////////////////////////////////////////////////////////// TIntersectionTypes detectIntersection() { switch (nDetectionState) { case stateSingleLinePreIntersection: case stateSingleLinePreIntersectionPlus1: case stateSingleLinePreIntersectionPlus2: // // Following a single straight ahead line waiting to detect an intersection or a dead end // // Possible valid sensor values are: // none -- dead end reached // center + left // center + right // center + left + right // switch (nSensorState) { // Unexpected states. Recover as best we can case scanRight: case scanLeft: case scanLeftRight: // when following a line we should alway see the "center" detector if a "left" or "right" detector is triggered. // About the only thing we can do is log the error and then ignore. badIntersectionDetection(); break; case scanCenter: break; case scanNone: //++nFilterNone; nDetectionState = stateDetectIntersection; break; // Start of an intersection case scanCenterRight: ++nNumbRight; ++nDetectionState; break; case scanLeftCenter: ++nNumbLeft; ++nDetectionState; break; case scanLeftCenterRight: ++nNumbRight; ++nNumbLeft; ++nDetectionState; break; } nFilterNonePostIntersection = 0; nFilterCenterPostIntersection = 0; break; case stateDetectIntersection: // // Performing an intersection detection // // Exit intersection detection when following is reached // none // center // center + left + right when repeated "N" times the goal has been reached. // // Use the 'nNumbRight' and 'nNumbLeft' counts to determine the type of intersection // switch (nSensorState) { case scanNone: break; case scanCenter: nFilterNonePostIntersection = 0; break; default: nFilterCenterPostIntersection = 0; nFilterNonePostIntersection = 0; break; } switch (nSensorState) { // End of intersection. Filter at least two hits. case scanNone: ++nFilterNonePostIntersection; if (nFilterNonePostIntersection >= 4) goto detectStatesNone; break; detectStatesNone: if (nNumbRight >= 2) { if (nNumbLeft >= 2) return typeLeftRight; else return typeRight; } else { if (nNumbLeft >= 2) return typeLeft; else return typeDeadEnd; } break; case scanCenter: ++nFilterCenterPostIntersection; if (nFilterCenterPostIntersection >= 8) { if (nNumbRight >= 2) { if (nNumbLeft >= 2) { nSaveNumbLeft = nNumbLeft; nSaveNumbRight = nNumbRight; nSaveFilterNone = nFilterNonePostIntersection; nSaveFilterCenter = nFilterCenterPostIntersection; return typeCross; } else { nSaveNumbLeft = nNumbLeft; nSaveNumbRight = nNumbRight; nSaveFilterNone = nFilterNonePostIntersection; nSaveFilterCenter = nFilterCenterPostIntersection; return typeRightStraight; } } else { if (nNumbLeft >= 2) return typeLeftStraight; else { badIntersectionDetection(); return typeStraight; // Invalid condition. Try to recover } } } break; // These states can occur as we reach the end of some intersection types. The center sensor looses the line // but the left or right sensor is still visible. This can occur on a left turn, right turn or tee. The next // scan should result in a "scanNone". case scanRight: ++nNumbRight; nDetectionState = stateEndOfIntersectionNoneExpected; break; case scanLeft: ++nNumbLeft; nDetectionState = stateEndOfIntersectionNoneExpected; break; case scanLeftRight: ++nNumbRight; ++nNumbLeft; nDetectionState = stateEndOfIntersectionNoneExpected; break; // Still crossing intersection case scanCenterRight: ++nNumbRight; break; case scanLeftCenter: ++nNumbLeft; break; case scanLeftCenterRight: ++nNumbRight; ++nNumbLeft; if ((nNumbRight > kGoalWidth) && (nNumbLeft > kGoalWidth)) return typeGoalReached; break; } break; case stateEndOfIntersectionNoneExpected: // // State is reached from intersection detection and left or right found but not center. This may occur // as the robot is just leavig the intersection. Need to hit filter until Left, Right and Center are all "white" // and no line detected. // switch (nSensorState) { // End of intersection. Filter at least two hits. case scanNone: ++nFilterNonePostIntersection; if (nFilterNonePostIntersection >= 4) goto detectStatesNone; break; case scanRight: ++nNumbRight; break; case scanLeft: ++nNumbLeft; break; case scanLeftRight: ++nNumbRight; ++nNumbLeft; break; case scanCenter: case scanCenterRight: case scanLeftCenter: case scanLeftCenterRight: // Badly screwed up. Totally invalid badIntersectionDetection(); resetToFollowingStraightLine(); break; } break; } return typeStraight; } ////////////////////////////////////////////////////////////////////////////// // // getLineErrorPosition // // Calculate average left and right motor values. This gives us an indication of how well // th PID aglorithm is performing. If the PID algorithm needs to use negative speeds to // adjust PID, then the average values will go down. // // VEX only supports 16-bit signed 'int'. With lots of sample counts, this could easily overflow. // So the calculation is done in two stages. // 1. Accumulate 100 cycles of the PID loop, summing the motor settings for each cycle. // Then get the average value for the 100 samples and use this as input to // the average calculation. // 2. Keep a average and cumulative value for each of (1) // // NOTE: Another trick is to adjust the cumulative by '75' to make the values a smaller adjustment // to minimize the overflow. Then re-apply // ////////////////////////////////////////////////////////////////////////////// int nLeftMotorAverage = 0; int nRightMotorAverage = 0; int nLeftMotor100Sum = 0; int nRightMotor100Sum = 0; int nSumCount = 0; ////////////////////////////////////////////////////////////////////////////// // // White / Black Threshold Adjustment // // A simple threshold is used to detect whether a sensor is detecting the black // line or not. // // This VEX robot used a 8-element analog line sensor from Pololu. Low analog // values indicated white and high values black. // // The actual threshold setting is quite sensitive to the specific configuration. // This includes: // 1. The height of the sensor above the floor. I used about 1/8". // 2. The white background I used was 12" x 12" vinyl floo tiles with the // maze spray painted on with black paint. // 3. How much ambient light is available. // // I found that white background ranged from 30 to 60 on the analog scale. The // higher reading occurred when there was lots of ambient lighting. // // I originally used a threshold of 45. When I moved the maze to a different // room I had to change the value to 75. // // You should play with different values to find the optimum setting for your // maze configuration. // ////////////////////////////////////////////////////////////////////////////// const int kThreshold = 75; ////////////////////////////////////////////////////////////////////////////// // // getLineErrorPosition // // // The robot uses a line following sensor from Pololu. This sensor has eight analog // detectors. They typically read values of <50 when over white line and >600 // when detecting black line. The sensors are spaced about 5/8 inch apart. // // The line is 0.5 inch wide. Often only a single sensor will see the line. Sometimes, // when the line is between two sensor then both sensors can "see" the line. And, // when traversing a sharp curve, more than three detectors may all "see" the line. // // This low level routine handles all three scenarios described above. // // It's used to generate a value from -100 to +100 based on where the line is found // on the detector array. // // It handles multiple detectors finding the line by calculating a weighted average // value based on all the detectors that see the line. // // Each detector is assigned a different "weight" ranging from -100 to +100. Note that // the scale is not uniform -- there's a smaller change between detectors near the // center vs the outside edges. // 1. This results in very small motor speed adjustments when following a straight // line. Typically only the center two detectors see the line. // 2. When the line is near the outside edges -- as in following a sharp curve -- // then more extreme motor adjustments result. // ////////////////////////////////////////////////////////////////////////////// int nNumbOfSensors = 0; int nLinePosition = 0; void getSensorWeights() { int nTemp; int nDarkness; nNumbCenterHits = 0; nNumbOfSensors = 0; nLinePosition = 0; #define checkSensor(nSensor, nWeight)\ {\ nTemp = SensorValue[nSensor];\ if (nTemp >= kThreshold)\ {\ nDarkness = nTemp / kThreshold;\ nLinePosition += nWeight * nDarkness;\ nNumbCenterHits += nDarkness;\ ++nNumbOfSensors;\ }\ } nSensorState = scanNone; if (SensorValue[left4] > kThreshold * 4) nSensorState |= scanLeft; if (SensorValue[right4] > kThreshold * 4) nSensorState |= scanRight; //checkSensor(left4, -100); checkSensor(left3, -70); checkSensor(left2, -30); checkSensor(left1, -7); checkSensor(right1, +7); checkSensor(right2, +30); checkSensor(right3, +70); //checkSensor(right4, +100); if (nNumbCenterHits != 0) nSensorState |= scanCenter; } int getLineErrorPosition() { static int nLastLinePosition = 0; getSensorWeights(); // Convert the weighted sum of values into a number in the range =100 to +100. switch (nNumbCenterHits) { case 0: // Line was not detected. Use the last detected value. This situation occasionally // occurred on sharp curves. By not updating the "nLinePosition" variable we use the last // sample which almost always performs a recovery. // // To aid in debugging, a LED is connected to one of the digital output ports and // it is turned on whenever no detectors are triggered. This provides an excellent // visual indication of when this situation occurs. // SensorValue[LEDLineLost] = false; //Turns LED on return nLastLinePosition; case 1: SensorValue[LEDLineLost] = true; //Turns LED off break; default: nLinePosition /= nNumbCenterHits; SensorValue[LEDLineLost] = true; //Turns LED off break; } nLastLinePosition = nLinePosition; // Save the last detected position return nLinePosition; } ////////////////////////////////////////////////////////////////////////////// // // PID Derivative Calculation // // NOTE: // Standard PID algorithm calculates the "derivative" value and uses it for // one iteration of the PID calculations. This will not work for the VEX // because: // 1. VEX motors are updated from the slave (user) to master CPU every 18.5 // milliseconds. It may then take some time for master to send updates // to the motors. // 2. This PID loop is set up to take 8 milliseconds per iteration. // // So, if we only used the derivative value for a single iteration, then // sometimes it may not even be seen by the master CPU! // // To overcome above, derivative values are used for several iterations of the // loop. Currently it is set up to use them for three iterations: // 1. Derivative values are stored in a 3-element circulat buffer. // 2. The PID algorithm uses the average value of the items in the circular // buffer. // This way, a change in the derivative value will influence three iterations // of the PID loop. // ////////////////////////////////////////////////////////////////////////////// void calculatePID_Derivative() { if (nDerivativeCircIndex >= (kDerivativeSize - 1)) nDerivativeCircIndex = 0; else ++nDerivativeCircIndex; nDerivative[nDerivativeCircIndex] = nErrorValue - nLastError; // // Handle varying sizes of circular buffers. // switch (kDerivativeSize) { case 1: nDerivativeAdjustment = nDerivative[0]; break; case 2: nDerivativeAdjustment = (nDerivative[0] + nDerivative[1]) / 2; break; case 3: nDerivativeAdjustment = (nDerivative[0] + nDerivative[1] + nDerivative[2]) / 3; break; case 4: nDerivativeAdjustment = (nDerivative[0] + nDerivative[1] + nDerivative[2] + nDerivative[3]) / 4; break; default: nDerivativeAdjustment = 0; break; } } void resetPIDCalculations() { memset(&nDerivative, 0, sizeof(nDerivative)); nLastError = nErrorValue; time1[kPIDStartupTimer] = 0; return; } ////////////////////////////////////////////////////////////////////////////// // // FollowStraightLine // // Use a PID algorithm to follow a straight line. // ////////////////////////////////////////////////////////////////////////////// const int kMinSpeed = -30; void FollowStraightLine() { // // PID Algorithm calculations: // if (true) { // // Use PID algorithm for motor power levels // calculatePID_Derivative(); nAdjustment = ((nErrorValue * nPFactor) + (nDerivativeAdjustment * nDFactor))/ 10; nLastError = nErrorValue; // 1. Adjust right and left motors to be greater and smaller than target speed // based on 'nAdjustment' calculated by PID algorithm. // // 2. Coerse power levels into a maximum and minimum range. if (nAdjustment > 0) { nLeftMotor = nTargetSpeed + nAdjustment / 2; nRightMotor = nTargetSpeed - nAdjustment; if (nRightMotor < kMinSpeed) nRightMotor = kMinSpeed; if (nLeftMotor > nMaxAllowedSpeed) nLeftMotor = nMaxAllowedSpeed; } else { nAdjustment = - nAdjustment; nLeftMotor = nTargetSpeed - nAdjustment; nRightMotor = nTargetSpeed + nAdjustment / 2; if (nLeftMotor < kMinSpeed) nLeftMotor = kMinSpeed; if (nMaxAllowedSpeed > 127) nRightMotor = nMaxAllowedSpeed; } } else { // // Use a classical "zig-zag" algorithm to compare performance with PID // classicalZigZagLineFollower(); } } ////////////////////////////////////////////////////////////////////////////// // // kDelayCenterOnIntersection // // After an intersection has been encountered, the robot needs to keep moving // forward until the rear wheels are positioned over the "intersection". This // is done with a simple timing loop. // // You may have to adjust the timing duration based on the speed and mechanical // characteristics of your robot. // // The value below works fine for the robot used in the article. It may need // to be adjusted to fit other robots. // ////////////////////////////////////////////////////////////////////////////// static int kDelayCenterOnIntersection = 400; ////////////////////////////////////////////////////////////////////////////// // // doTurn // // Perform a turn until it encounters a line. There are two types of turns: // // 1. A right angle (90 degree) turn // // 2. A 180 degree turn used when reversing robot when a dead end is reached. // // The same algorithm works fine for both types. It works as follows. // // 1. Keep moving forward until the drive wheels are positioned over the // "intersection". The robot will then pivot in place making the turn. // // 2. Keep turning until the sensors all detect white. // // 3. Now keep turning until one of the outer sensors detects the line we // are trying to turn to. // // 4. when the sensors start to detect the "center" region, i.e. the sensors // are nearing center alignment over the line brake (stop) both motors. // // 5. Return to the caller. // ////////////////////////////////////////////////////////////////////////////// bool doTurn(bool bFirstTime) { typedef enum { stateStartup, stateWaitForBlankSensor, stateWaitForFirstDetect, stateWaitForCenterDetect, stateWaitForCenterBrake, stateBraking, } TTurningState; static TTurningState nTurningState; static int nHits; const int kHitFilter = 5; if (bFirstTime) nTurningState = stateStartup; switch (nTurningState) { case stateStartup: // // Keep driving until axle lines up with center of intersection. // Because we want to pivot about this point. // if (nIntersection == typeDeadEnd) { // Need to go a little bit furthur for dead end because it may be on a very short // straighaway (say at the end of an 'unterminated' T-intersection and while reversing // we don't want it to connect with any of the branches of the T. wait1Msec((kDelayCenterOnIntersection * 5) / 4); } else wait1Msec(kDelayCenterOnIntersection); // // Start the turn; // switch (nTurnType) { case goLeft: nLeftMotor = - nTurnSpeed; nRightMotor = + nTurnSpeed; break; case goRight: nLeftMotor = + nTurnSpeed; nRightMotor = - nTurnSpeed; break; case goReverse: nLeftMotor = - nTurnSpeed; nRightMotor = - nTurnSpeed; break; case goStraight: nLeftMotor = + nTurnSpeed; nRightMotor = + nTurnSpeed; return true; case goStop: nLeftMotor = 0; nRightMotor = 0; StopAllTasks(); break; } nTurningState = stateWaitForBlankSensor; nHits = 0; break; case stateWaitForBlankSensor: if (nSensorState != scanNone) break; ++nHits; if (nHits < kHitFilter) break; nHits = 0; nTurningState = stateWaitForFirstDetect; break; case stateWaitForFirstDetect: if (nSensorState == scanNone) break; ++nHits; if (nHits < kHitFilter) break; // Line is now detected on outer sensor. Start to slow down nHits = 0; if (nLeftMotor > 0) nLeftMotor = 0; if (nRightMotor > 0) nRightMotor = 0; nTurningState = stateWaitForCenterDetect; break; case stateWaitForCenterDetect: if ((nSensorState & (scanLeft | scanRight)) != 0) break; if ((nSensorState & scanCenter) == 0) break; nHits = 0; nTurningState = stateWaitForCenterBrake; break; case stateWaitForCenterBrake: if ((nLinePosition > 60) || (nLinePosition < -60)) break; time1[T4] = 0; nLeftMotor = - nLeftMotor; nRightMotor = - nRightMotor; nTurningState = stateBraking; break; case stateBraking: if (time1[T4] < 35) break; nLeftMotor = 0; nRightMotor = 0; return true; } return false; } ////////////////////////////////////////////////////////////////////////////// // // task main // // The main driving loop. // ////////////////////////////////////////////////////////////////////////////// task main() { int nNumbCyclesAtTimePeriodStart = 0; resetPIDCalculations(); nDriveState = driveFollowLineViaPID; time1[T2] = 0; while (true) { // // Repeat "forever" calculating motor power levels based on the value of the // line following detector from Pololu. // time1[T1] = 0; // used to time how long each iteration is. processMotorEnableDisableButton(); nErrorValue = getLineErrorPosition(); switch (nDriveState) { case driveNone: break; case driveFollowLineViaPID: displayIntersectionTypeOnLCD(typeStraight); switch (nSensorState) { case scanCenter: FollowStraightLine(); break; case scanRight: case scanLeftCenter: case scanLeft: case scanLeftRight: case scanNone: case scanCenterRight: case scanLeftCenterRight: // // No longer a straight line. Need to detect the type of intersection // if (time1[kPIDStartupTimer] < kMinimumStraightLineTime) { FollowStraightLine(); break; } nDriveState = driveDetectIntersection; nLeftMotor = nTargetSpeed; nRightMotor = nTargetSpeed; goto detect; } break; case driveDetectIntersection: detect: nIntersection = detectIntersection(); if (nIntersection != typeStraight) { // End of intersection detection reached //motor[leftMotor] = 0; //Temp //motor[rightMotor] = 0; //Temp displayIntersectionTypeOnLCD(nIntersection); resetToFollowingStraightLine(); nLeftMotor = 0; nRightMotor = 0; AlwaysTurnRight(); nDriveState = drivePerformingTurn; } break; case drivePerformingTurn: if (doTurn(false)) { resetToFollowingStraightLine(); resetPIDCalculations(); nDriveState = driveFollowLineViaPID; } break; } // Useful optional debugging feature // // Use 'left' and 'right' LEDs (mounted on front sides of robot) to indicate // when motors are making an extreme turn; i.e. one motor at maximum power and // the other motor at minimum power. // // NOTE: LED is illuminated when digital output is zero! // SensorValue[LEDLeft] = nLeftMotor > kMinSpeed; SensorValue[LEDRight] = nRightMotor > kMinSpeed; SensorValue[LEDBothMotorFast] = (nLeftMotor < 100) < (nRightMotor < 100); if (bMotorsDisabled) { motor[leftMotor] = 0; motor[rightMotor] = 0; } else { motor[leftMotor] = nLeftMotor; motor[rightMotor] = nRightMotor; } if (nDriveState == driveFollowLineViaPID) { // // Wait until each iteration takes 8 milliseconds // while (time1[T1] < 8) {} } } return; } ////////////////////////////////////////////////////////////////////////////// // // Always Turn Right Algorithm // // The simplest maze solving algorithm does not keep a history of the maze. // Whenever an intersection is detected, it always takes the "rightmost" // branch when there is a decision to be made. Alternatively, it could always // turn left. // // For many mazes, this will eventually solve the maze. It does not work on // mazes that have "loops". Fortunately, the test mazes can be designed to // prevent this occurrence. // // "Always turn right" means that it does the following: // // 1. For simple right angle turns, it must follow the turn. There is no // other choice. // // 2. Similarly for dead end, it must do a 180 degree turn and go back. // // 3. For a cross intersection it turns right. // // 4. There are three orientations of the T-intersection // typeRightStraight Turn right // typeLeftStraight go straight. // typeLeftRight Turn right. // ////////////////////////////////////////////////////////////////////////////// void AlwaysTurnRight() { switch (nIntersection) { default: case typeStraight: /* Invalid */ break; case typeLeft: nTurnType = goLeft; doTurn(true); break; case typeRight: nTurnType = goRight; doTurn(true); break; case typeLeftStraight: nTurnType = goStraight; doTurn(true); break; case typeRightStraight: nTurnType = goRight; doTurn(true); break; case typeLeftRight: nTurnType = goRight; doTurn(true); break; case typeCross: nTurnType = goRight; doTurn(true); break; case typeDeadEnd: nTurnType = goRight; doTurn(true); break; case typeGoalReached: nTurnType = goStop; doTurn(true); break; } } ////////////////////////////////////////////////////////////////////////////// // // Smart Maze Solver // // The fastest maze solving robots make multiple passes through the maze. // // 1. The initial passes are used to "map" the maze and find a solution. // Software should remember the turns taken during a solution. // // 2. On the final pass, the robot simply follows the optimal path through // the maze that it has detected. // // 3. An enhancement is that it makes several "final" passes through the // maze. // * The first pass is at a slow speed where it will always be able // to follow the line. // * On subsequent final passes it increases the motor speed used // to get a faster solution but with a higher risk that it will // not be able to track the line. // // Initial Pass Maze Solving: // ========================= // // There are many references on the web describing different maze mapping // algorithms. Generally these are related to a "MicroMouse" contest so // be sure to use "micromouse" as one of the search keywords. Most (all?) of // these algorithms rely on wheel odometry so that you can build a complete // map of the maze. The robot used here has no wheel encoders. And the VEX // platform with ROBOTC is a little short or memory to store any kind of // large map. // // One algorithm that will work with this robot is the following: // // 1. Use the "rightmost" turn algorithm to find a solution to the maze. // // 2. Record the turns in a stack as they are made. // // NOTE: Simple right angle turns do not need to be recorded. There is no // decision to be made as the robot always has to follow the turn. // // 3. When a dead end is detected and the robot has to backtrack, "prune" // the recorded turns of the backtracked map. // // 4. When the goal is reached, you will have a list of turns to make to find // a path through the maze without dead ends. // ////////////////////////////////////////////////////////////////////////////// void SmartMazeSolver() { // Left to the reader for implementation } ////////////////////////////////////////////////////////////////////////////// // // TurnClockwise // // During debugging of the software, it was convenient to organize a temporary // maze that was a "simple" rectange or loop. The four corners of the rectange were // configured to be one of the various intersection types. // // Here's a sample of 3 x 3 tile arrangement: // // =============================== // | * | | * | // | * | | * | // | * * *|* * * * *|* * * | // | * | | * | // | * | | * | // =============================== // | * | | * | // | * | | * | // | * | | * | // | * | | * | // | * | | * | // =============================== // | * | | * | // | * | | * | // | * * *|* * * * *|* * * | // | * | | * | // | * | | * | // =============================== // // The objective was to test the reliability of the "intersection detection" // algorithm and the "make a turn" algorithm. // // So the robot was configured to continuously run following the simple maze // for several minutes. If everything was working well, then it would never // loose the path. // // // "TurnClockwise means that it does the following: // // 1. For simple right angle turns, it must follow the turn. There is no // other choice. // // 2. Similarly for dead end, it must do a 180 degree turn and go back. [But // of course a slightly different maze is needed for dead ends // // 3. For a cross intersection it turns right. // // 4. There are three orientations of the T-intersection // typeRightStraight Turn right // typeLeftStraight go straight. // typeLeftRight Turn left. // ////////////////////////////////////////////////////////////////////////////// void TurnClockwise() { switch (nIntersection) { default: case typeStraight: /* Invalid */ break; case typeLeft: nTurnType = goLeft; doTurn(true); break; case typeRight: nTurnType = goRight; doTurn(true); break; case typeLeftStraight: nTurnType = goStraight; doTurn(true); break; case typeRightStraight: nTurnType = goRight; doTurn(true); break; case typeLeftRight: nTurnType = goRight; doTurn(true); break; case typeCross: nTurnType = goRight; doTurn(true); break; case typeDeadEnd: nTurnType = goRight; doTurn(true); break; case typeGoalReached: nTurnType = goStop; doTurn(true); break; } } ////////////////////////////////////////////////////////////////////////////// // // processMotorEnableDisableButton // // A button is connected to one of the VEX digital inputs. It is used to enable // and disable the motors. It is mounted on the top of the robvot. // // When disabled, the program will perform all the calculations but will leave // the motors stopped. // // This capability is useful for: // 1. Initially placing your robot on the course. Then push the button to // start operation. // 2. Stopping the robot in mid course. // 3. Debugging // // Every time the button is pushed, the "enable motors" flag is toggled. // ////////////////////////////////////////////////////////////////////////////// void processMotorEnableDisableButton() { static bool bClickInProgress = false; if (bClickInProgress) { if (SensorValue[OnOffButton]) bClickInProgress = false; } else { if (!SensorValue[OnOffButton]) { bMotorsDisabled = !bMotorsDisabled; bClickInProgress = true; } } return; } ////////////////////////////////////////////////////////////////////////////// // // classicalZigZagLineFollower // // The classical "zig-zag" and simplest line following algorithm simply looks // to see if the robot is to the right or left of the line. // // 1. If to the left of line: // Turn left motor on at maximum power. // Turn right motor off // // 2. If to the right of line: // Turn right motor on at maximum power. // Turn left motor off. // // Keep adjusting the "maximum power" until the robot can no longer follow // the line. // // A simple adjustment of a "true" or "false" evaluation in the main task() // enables switching between "classical" and "PID" based line following. // // On my sample robot the maximum achievable "target" speed for the classical // algorithm was about half of that for a PID based algorithm. This does not // fully reflect the speed difference achieved. // // 1. With classical algorithm, one motor is always at zero power and the // other at "target" power. So the average power level of the two motors // is half of the "target" power. // // 2. With the PID algorithm, the power difference is variable based on how // far from the line the robot is. One motor is adjust to be more than // the target power and one motor is adjusted to be less. The average // power is a much higher perecentage of the target power. // // For example, on a straightaway, both motors are typically operating // at the target power level. // // Another advanage of PID algorithm is that robot tends to smoothly // follow the line without a lot of zig-zagging back and forth. // // The PID algorithm will typically have a maximum speed of three or four // time that of the classica "zig-zag" algorithm. // ////////////////////////////////////////////////////////////////////////////// void classicalZigZagLineFollower() { if (nErrorValue > 0) { nLeftMotor = nTargetClassicalSpeed; nRightMotor = 0; } else { nLeftMotor = 0; nRightMotor = nTargetClassicalSpeed; } return; } //////////////////////////////////////////////////////////////////////////////////// // // displayIntersectionTypeOnLCD // // This is a useful debugging routine to display the types of intersections on the // VEX LCD. The LCD is an optional VEX peripheral and not essential to this robot. // // Even without a LCD, this is still a useful feature. The ROBOTC IDE has a PC based // window that provides a "remote LCD" function -- the contents of the LCD is copied // to the PC where it is displayed. // // So you can still get the visual effect, even without a physical LCD. // // Of course, for a working robot you would then want to use the optional VEXNET // wireless communications between the VEX robot and the PC! // //////////////////////////////////////////////////////////////////////////////////// void displayIntersectionTypeOnLCD(TIntersectionTypes nTurnType) { clearLCDLine(1); displayLCDPos(1, 0); switch (nTurnType) { default: displayNextLCDString("??? BAD ???"); break; case typeStraight: displayNextLCDString("Straight"); break; case typeLeft: displayNextLCDString("Left"); break; case typeRight: displayNextLCDString("Right"); break; case typeLeftStraight: displayNextLCDString("Left Straight"); break; case typeRightStraight: displayNextLCDString("Right Straight"); break; case typeLeftRight: displayNextLCDString("Left Right"); break; case typeCross: displayNextLCDString("Cross"); break; case typeDeadEnd: displayNextLCDString("Dead End"); break; case typeGoalReached: displayNextLCDString("Goal Reached"); break; } }