#pragma config(Sensor, in1, #pragma config(Sensor, in2, #pragma config(Sensor, in3, #pragma config(Sensor, in4, #pragma config(Sensor, in5,

#pragma config(Sensor, in6, #pragma config(Sensor, in7, #pragma config(Sensor, in8, #pragma config(Sensor, in10, #pragma config(Sensor, in11, #pragma config(Sensor, in12, #pragma config(Sensor, in13, #pragma config(Sensor, in14, #pragma config(Motor, port1, #pragma config(Motor, port2, #pragma config(Motor, port3, eversed)

left4, left3, left2, left1, right1, right2, right3, right4, OnOffButton, LEDRight, LEDLeft, LEDLineLost, LEDBothMotorFast, Audio, leftMotor, rightMotor,

sensorLineFollower) sensorLineFollower) sensorLineFollower) sensorLineFollower) sensorLineFollower) sensorLineFollower) sensorLineFollower) sensorLineFollower) sensorTouch) sensorDigitalOut) sensorDigitalOut) sensorDigitalOut) sensorDigitalOut) tmotorAudio, openLoop) tmotorNormal, openLoop) tmotorNormal, openLoop, r

//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

//////////////////////////////////////////////////////////////////////////////// // // // // 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. VEX Maze Solving Robot

// // Program written in ROBOTC. See www.robotc.net for more details. // //////////////////////////////////////////////////////////////////////////////// #pragma platform (VEX)

static unsigned char nSaveNumbLeft static unsigned char nSaveNumbRight static unsigned char nSaveFilterNone static unsigned char nSaveFilterCenter = 0; = 0;

= 0; = 0;

const TTimers kPIDStartupTimer = T3; const int kMinimumStraightLineTime = 50;

//int nNumbLeftHits; int nNumbCenterHits; //int nNumbRightHits;

bool bMotorsDisabled = true; int nErrorValue; int nLastError; int nTurnSpeed int nTargetSpeed int nTargetClassicalSpeed llower int nMaxAllowedSpeed = = = 55; 90; // Target power level for PID algorithm 45; // Target power level for classical line fo

= 125;

int nAdjustment; int nLeftMotor; int nRightMotor;

int nPFactor = int nIFactor =

6; //10; 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 va rious 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 inter section // - the center elements are used for a PID algorithm for following a straigh t line. Let's number then // -3, -2, -1, +1, +2, +3. Typically the line is under sensors -1 or +1. Wh en it strays beyond these the // PID algorithm is very good at corecting movement. Very rarely will the l ine reach -3 or +3! //

// For turn detection, the eight sensors are reduced to 3-bits. // // // // // 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. 1. Left detector 2. Center detector 3. Right detector

// 2. Leading edge of a turn (i.e. left or right sensor over black line) dete cted, 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 senso rs to see white. // //////////////////////////////////////////////////////////////////////////////// ///////////////////////////////

typedef enum { driveNone, driveFollowLineViaPID, driveDetectIntersection, drivePerformingTurn } TDriveState;

TDriveState nDriveState = driveNone;

typedef enum

{ / / Left scanNone = 0, 0 0 scanRight = 1, 0 1 scanCenter = 2, 1 scanCenterRight // 1 scanLeft = 4, 1 0 scanLeftRight = 5, 0 scanLeftCenter // 1 scanLeftCenterRight // 1 1 } TSensorStates; = 7, 1 1 0 // 1 1 = 6, 0 // 0 1 = 3, // 0 0 0 // // 0 Center Right

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 static unsigned char nFilterNonePostIntersection ; static unsigned char nFilterCenterPostIntersection static int nNumbBadIntersections = 0; = 0;

= = 0; = 0

void resetToFollowingStraightLine() { // // Reset state to following straight line and looking for a turn detecti on // nNumbLeft nNumbRight //nFilterNone = 0; = 0; = 0;

//nFilterCenter = 0;

nDetectionState = stateSingleLinePreIntersection; return; }

void badIntersectionDetection() { // Failure during intersection detection

++nNumbBadIntersections; return; }

////////////////////////////////////////////////////////////////////////////// // // // // 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. kGoalWidth

// //////////////////////////////////////////////////////////////////////////////

const int kGoalWidth = 25;

////////////////////////////////////////////////////////////////////////////// // // // // Function is used to detect an intersection. // // The detection is driven by a state machine. The eight individual sensors // are simplified to three elements. // // // // // The state machine looks at these three values to determine the type of // intersection. Hit filtering is used. // ////////////////////////////////////////////////////////////////////////////// 1. A left branch is detected, i.e. the leftmost sensor detectIntersection

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.

TIntersectionTypes detectIntersection() { switch (nDetectionState) { case stateSingleLinePreIntersection: case stateSingleLinePreIntersectionPlus1: case stateSingleLinePreIntersectionPlus2: //

// Following a single straight ahead line waiting to detect an i ntersection or a dead end // // Possible valid sensor values are: // none -- dead end reached // // // // switch (nSensorState) { center center + right center + left + right + left

// 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 err or and then ignore. badIntersectionDetection(); break;

case scanCenter: break;

case scanNone: //++nFilterNone; nDetectionState = stateDetectIntersection;

break;

// Start of an intersection

case scanCenterRight: ++nNumbRight; ++n DetectionState; case scanLeftCenter: ++nNumbLeft; e; ; break; } nFilterNonePostIntersection = 0; break; case scanLeftCenterRight: ++nNumbLeft; ++nNumbRight ++nDetectionState; ++nDetectionStat break;

nFilterCenterPostIntersection = 0; break;

case stateDetectIntersection: // // Performing an intersection detection // // Exit intersection detection when following is reached // // none center when repeated "N" times the g

// center + left + right oal has been reached. //

// Use the 'nNumbRight' and 'nNumbLeft' counts to determine the type of intersection // switch (nSensorState) {

case scanNone: break; case scanCenter: ePostIntersection = 0; break; default: ePostIntersection = 0; break; } nFilterCenterPostIntersection = 0; nFilterNon nFilterNon

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 typeLeftR ight; else return typeRight ; } else { if (nNumbLeft >= 2) return typeLeft; else

return typeDeadE nd; } break;

case scanCenter: ++nFilterCenterPostIntersection; if (nFilterCenterPostIntersection >= 8) { if (nNumbRight >= 2) { if (nNumbLeft >= 2) { nSaveNumbLeft nSaveNumbRight nSaveFilterNone nSaveFilterCenter return typeCross; } else { nSaveNumbLeft nSaveNumbRight nSaveFilterNone nSaveFilterCenter = nNumbRight; = nFilterNonePostIntersection; = nFilterCenterPostIntersection; = nNumbLeft; = nNumbRight; = nFilterNonePostIntersection; = nFilterCenterPostIntersection; = nNumbLeft;

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 intersecti on 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; eExpected; case scanLeft: bLeft; ++nNum nDetectionState = stateEndOfIntersectionNoneExpected; break; nDetectionState = stateEndOfIntersectionNon break;

case scanLeftRight: ++nNumbRight; DetectionState = stateEndOfIntersectionNoneExpected; reak;

++nNumbLeft;

n b

// Still crossing intersection

case scanCenterRight: ++nNumbRight; bre ak; case scanLeftCenter: ++nNumbLeft; case scanLeftCenterRight: ++nNumbRight; ++nNumbLeft; if ((nNumbRight > kGoalWidth) && (nNumbLeft > kG oalWidth)) return typeGoalReached; break; } break; break;

case stateEndOfIntersectionNoneExpected: // // State is reached from intersection detection and left or righ t found but not center. This may occur // as the robot is just leavig the intersection. Need to hit fil ter 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: ++nNum bLeft; break; case scanLeftRight: ++nNumbRight; break; ++nNumbLeft;

case scanCenter: case scanCenterRight: case scanLeftCenter: case scanLeftCenterRight: // Badly screwed up. Totally invalid badIntersectionDetection(); resetToFollowingStraightLine(); break; } break; } return typeStraight; }

//////////////////////////////////////////////////////////////////////////////

// // // // 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 sp eeds 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. // o // // // // NOTE: Another trick is to adjust the cumulative by '75' to make the values a smaller adjustment // // ////////////////////////////////////////////////////////////////////////////// to minimize the overflow. Then re-apply Then get the average value for the 100 samples and use this as input t the average calculation. 2. Keep a average and cumulative value for each of (1) getLineErrorPosition

int nLeftMotorAverage = 0; int nRightMotorAverage = 0;

int nLeftMotor100Sum = 0; int nRightMotor100Sum = 0; int nSumCount = 0;

////////////////////////////////////////////////////////////////////////////// // // // // 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: // // // // // // 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. // ////////////////////////////////////////////////////////////////////////////// 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. White / Black Threshold Adjustment

const int kThreshold = 75;

////////////////////////////////////////////////////////////////////////////// // // // // // The robot uses a line following sensor from Pololu. This sensor has eight ana log // 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. Some times, // 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 li ne. // // 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 fo und // on the detector array. // // It handles multiple detectors finding the line by calculating a weighted aver age // value based on all the detectors that see the line. // // Each detector is assigned a different "weight" ranging from -100 to +100. Not e that // the scale is not uniform -- there's a smaller change between detectors near t he // center vs the outside edges. // 1. This results in very small motor speed adjustments when following a st getLineErrorPosition

raight // line. Typically only the center two detectors see the line.

// 2. When the line is near the outside edges -- as in following a sharp cur ve -// // ////////////////////////////////////////////////////////////////////////////// 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, checkSensor(left3, checkSensor(left2, checkSensor(left1, checkSensor(right1,

-100); -70); -30); -7); +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 occasi onally // 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 outpu t 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; return nLinePosition; } // Save the last detected position

////////////////////////////////////////////////////////////////////////////// // // // // NOTE: // // // // // // // // // // // // // // // // // // // ////////////////////////////////////////////////////////////////////////////// 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. So, if we only used the derivative value for a single iteration, then sometimes it may not even be seen by the master CPU! 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. PID Derivative Calculation

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; }

////////////////////////////////////////////////////////////////////////////// // // // // Use a PID algorithm to follow a straight line. // ////////////////////////////////////////////////////////////////////////////// FollowStraightLine

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 spe ed // // // 2. Coerse power levels into a maximum and minimum range. based on 'nAdjustment' calculated by PID algorithm.

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(); } }

////////////////////////////////////////////////////////////////////////////// // // // // 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. // ////////////////////////////////////////////////////////////////////////////// kDelayCenterOnIntersection

static int kDelayCenterOnIntersection = 400;

////////////////////////////////////////////////////////////////////////////// // // // // 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 // // // 2. Keep turning until the sensors all detect white. // // 3. Now keep turning until one of the outer sensors detects the line we // // // 4. when the sensors start to detect the "center" region, i.e. the sensors // // // 5. Return to the caller. // ////////////////////////////////////////////////////////////////////////////// are nearing center alignment over the line brake (stop) both motors. are trying to turn to. "intersection". The robot will then pivot in place making the turn. doTurn

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 ver y short // straighaway (say at the end of an 'unterminated' T-intersection and whi le 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: break; case goRight: break; case goReverse: break; case goStraight: return true; case goStop: StopAllTasks(); } nTurningState = stateWaitForBlankSensor; nHits = 0; break; nLeftMotor = - nTurnSpeed; nRightMotor = + nTurnSpeed; nLeftMotor = + nTurnSpeed; nRightMotor = - nTurnSpeed; nLeftMotor = - nTurnSpeed; nRightMotor = - nTurnSpeed; nLeftMotor = + nTurnSpeed; nRightMotor = + nTurnSpeed; nLeftMotor = break; 0; nRightMotor = 0;

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. Ne ed to detect the type of intersection // if (time1[kPIDStartupTimer] < kM inimumStraightLineTime) { 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 a nd // 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; }

////////////////////////////////////////////////////////////////////////////// // // // // 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 // // // 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 typeLeftStraight typeLeftRight Turn right go straight. Turn right. other choice. Always Turn Right Algorithm

// //////////////////////////////////////////////////////////////////////////////

void AlwaysTurnRight() { switch (nIntersection) { default: case typeStraight: case typeLeft: case typeRight: case typeLeftStraight: case typeRightStraight: case typeLeftRight: case typeCross: case typeDeadEnd: case typeGoalReached: } } /* Invalid */ nTurnType = goLeft; nTurnType = goRight; break; doTurn(true); break; doTurn(true); break;

nTurnType = goStraight; doTurn(true); break; nTurnType = goRight; nTurnType = goRight; nTurnType = goRight; nTurnType = goRight; nTurnType = goStop; doTurn(true); break; doTurn(true); break; doTurn(true); break; doTurn(true); break; doTurn(true); break;

////////////////////////////////////////////////////////////////////////////// // // // // 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. // // // 2. On the final pass, the robot simply follows the optimal path through Software should remember the turns taken during a solution. Smart Maze Solver

// //

the maze that it has detected.

// 3. An enhancement is that it makes several "final" passes through the // // // // // // // // 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. 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.

// 3. When a dead end is detected and the robot has to backtrack, "prune" // // // 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. the recorded turns of the backtracked map.

void SmartMazeSolver() { // Left to the reader for implementation }

////////////////////////////////////////////////////////////////////////////// // // // // 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 w ere // configured to be one of the various intersection types. // // Here's a sample of 3 x 3 tile arrangement: // // // // // // // // =============================== | | | | | * * | | | | * * | | | | | TurnClockwise

* * *|* * * * *|* * * * * | | | | * *

===============================

// // // // // // // // // // // //

| | | | |

* * * * *

| | | | |

| | | | |

* * * * *

| | | | |

=============================== | | | | | * * | | | | * * | | | | |

* * *|* * * * *|* * * * * | | | | * *

===============================

// // 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 // // // 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 other choice.

// 3. For a cross intersection it turns right. // // 4. There are three orientations of the T-intersection // // // // ////////////////////////////////////////////////////////////////////////////// typeRightStraight typeLeftStraight typeLeftRight Turn right go straight. Turn left.

void TurnClockwise() { switch (nIntersection) { default: case typeStraight: case typeLeft: case typeRight: case typeLeftStraight: case typeRightStraight: case typeLeftRight: case typeCross: case typeDeadEnd: case typeGoalReached: } } ////////////////////////////////////////////////////////////////////////////// // // // // A button is connected to one of the VEX digital inputs. It is used to enable processMotorEnableDisableButton /* Invalid */ nTurnType = goLeft; nTurnType = goRight; break; doTurn(true); break; doTurn(true); break;

nTurnType = goStraight; doTurn(true); break; nTurnType = goRight; nTurnType = goRight; nTurnType = goRight; nTurnType = goRight; nTurnType = goStop; doTurn(true); break; doTurn(true); break; doTurn(true); break; doTurn(true); break; doTurn(true); break;

// 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: // // // // // // Every time the button is pushed, the "enable motors" flag is toggled. // ////////////////////////////////////////////////////////////////////////////// 1. Initially placing your robot on the course. Then push the button to start operation. 2. Stopping the robot in mid course. 3. Debugging

void processMotorEnableDisableButton() { static bool bClickInProgress = false;

if (bClickInProgress) { if (SensorValue[OnOffButton]) bClickInProgress = false; } else { if (!SensorValue[OnOffButton]) { bMotorsDisabled = !bMotorsDisabled; bClickInProgress = true;

} } return; }

////////////////////////////////////////////////////////////////////////////// // // // // 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. // // // // // // // // // // 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. // 2. If to the right of line: Turn right motor on at maximum power. Turn left motor off. 1. If to the left of line: Turn left motor on at maximum power. Turn right motor off classicalZigZagLineFollower

// // // // // // // // // // // // // // //

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; }

//////////////////////////////////////////////////////////////////////////////// //// // // // // This is a useful debugging routine to display the types of intersections on t he // VEX LCD. The LCD is an optional VEX peripheral and not essential to this robo t. // // Even without a LCD, this is still a useful feature. The ROBOTC IDE has a PC b ased // window that provides a "remote LCD" function -- the contents of the LCD is co pied // 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! // //////////////////////////////////////////////////////////////////////////////// //// displayIntersectionTypeOnLCD

void displayIntersectionTypeOnLCD(TIntersectionTypes nTurnType) { clearLCDLine(1); displayLCDPos(1, 0);

switch (nTurnType) { default: eak; case typeStraight: eak; case typeLeft: eak; case typeRight: eak; case typeLeftStraight: eak; case typeRightStraight: eak; case typeLeftRight: eak; case typeCross: eak; case typeDeadEnd: eak; case typeGoalReached: eak; } } displayNextLCDString("Goal Reached"); br displayNextLCDString("Dead End"); br displayNextLCDString("Cross"); br displayNextLCDString("Left Right"); br displayNextLCDString("Right Straight"); br displayNextLCDString("Left Straight"); br displayNextLCDString("Right"); br displayNextLCDString("Left"); br displayNextLCDString("Straight"); br displayNextLCDString("??? BAD ???"); br