#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, leftMotor, tmotorNormal, openLoop) #pragma config(Motor, port2, rightMotor, tmotorNormal, openLoop, reversed) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*// //////////////////////////////////////////////////////////////////////////////// // // Fast VEX Line Following Robot // // Source code for a fast VEX line following robot. The robot was described in // the July/August issue of ROBOT magazine. // // Lots of comments. Read the comments for insight on the operation. // // Search youtube for "VEX Line Following ROBOTC" for several videos of the robot // in action. Published week of July 20, 2009. // //////////////////////////////////////////////////////////////////////////////// #pragma platform (VEX) bool bMotorsDisabled = true; int nErrorValue; int nLastError; int nTargetSpeed = 85; // 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}; // Variables to "instrument" performance of robot. Non-essential. int nSensorOutOfRange = 0; int nCycles = 0; int nCyclesPerSecond = 0; int nNumbOfHitsPerSensor[10] = {0,0,0,0,0, 0,0,0,0,0}; int nNumbOfSimultaneousSensors[10] = {0,0,0,0,0, 0,0,0,0,0}; ////////////////////////////////////////////////////////////////////////////// // // 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 nLeftMotorCumulative = 0; int nRightMotorCumulative = 0; int nCumulativeCounts = 0; int nLeftMotor100Sum = 0; int nRightMotor100Sum = 0; int nSumCount = 0; void addSampleToAverageCalculation() { nLeftMotorCumulative += nLeftMotor100Sum / 100; nRightMotorCumulative += nRightMotor100Sum / 100; nLeftMotorCumulative -= 75; nRightMotorCumulative -= 75; ++nCumulativeCounts; nLeftMotorAverage = nLeftMotorCumulative / nCumulativeCounts; nLeftMotorAverage += 75; nRightMotorAverage = nRightMotorCumulative / nCumulativeCounts; nRightMotorAverage += 75; return; } ////////////////////////////////////////////////////////////////////////////// // // 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 nNumbOfHits = 0; int nNumbOfSensors = 0; int nLinePosition = 0; void getSensorWeights() { const int kThreshold = 45; int nTemp; int nDarkness; nNumbOfHits = 0; nNumbOfSensors = 0; nLinePosition = 0; #define checkSensor(nSensor, nWeight)\ {\ nTemp = SensorValue[nSensor];\ if (nTemp >= kThreshold)\ {\ nDarkness = nTemp / kThreshold;\ nLinePosition = nWeight * nDarkness;\ nNumbOfHits += nDarkness;\ ++nNumbOfSensors;\ ++nNumbOfHitsPerSensor[nSensor];\ }\ } checkSensor(left4, -100); checkSensor(left3, -70); checkSensor(left2, -30); checkSensor(left1, -7); checkSensor(right1, +7); checkSensor(right2, +30); checkSensor(right3, +70); checkSensor(right4, +100); } int getLineErrorPosition() { static int nLastLinePosition = 0; getSensorWeights(); if (!bMotorsDisabled) { // // Some useful measurements accumulated for ease of debugging. // // 'nNumbOfSimultaneousSensors' is an array containing a histogram of how // many detectors were found in a single scan. On the test robot >75% of the // samples were for a single hit. Less than 5% were for more than 2 sensors // and likely occurred when navigating curves. if (nNumbOfSensors < 10) ++nNumbOfSimultaneousSensors[nNumbOfSensors]; else ++nNumbOfSimultaneousSensors[9]; } // Convert the weighted sum of values into a number in the range =100 to +100. switch (nNumbOfHits) { 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. // if (!bMotorsDisabled) ++nSensorOutOfRange; SensorValue[LEDLineLost] = false; //Turns LED on return nLastLinePosition; case 1: SensorValue[LEDLineLost] = true; //Turns LED off break; default: nLinePosition /= nNumbOfHits; SensorValue[LEDLineLost] = true; //Turns LED off break; } nLastLinePosition = nLinePosition; // Save the last detected position return nLinePosition; } ////////////////////////////////////////////////////////////////////////////// // // 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; } ////////////////////////////////////////////////////////////////////////////// // // 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; } } ////////////////////////////////////////////////////////////////////////////// // // task main // ////////////////////////////////////////////////////////////////////////////// task main() { const int kMinSpeed = -30; int nNumbCyclesAtTimePeriodStart = 0; 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. // // Some debugging code to calculate how many loop iterations are performed // per second. // ++nCycles; if (time1[T2] > 1000) { nCyclesPerSecond = nCycles - nNumbCyclesAtTimePeriodStart; nNumbCyclesAtTimePeriodStart = nCycles; time1[T2] = 0; } processMotorEnableDisableButton(); // // PID Algorithm calculations: // nErrorValue = getLineErrorPosition(); if (false ) { // // 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(); } // // 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; nLeftMotor100Sum += nLeftMotor; nRightMotor100Sum += nRightMotor; // Some additional "instrumentation" to calculate the average power levels // applied to each motor. ++nSumCount; if (nSumCount >= 100) { addSampleToAverageCalculation(); nSumCount = 0; nLeftMotor100Sum = 0; nRightMotor100Sum = 0; } } // // Wait until each iteration takes 8 milliseconds // while (time1[T1] < 8) {} } return; }