Posted 10 June 2021,
In my last post on this issue, I described using a small test program to explore an in-line version of the PID (Proportional-Integral-Differential) algorithm for turn rate control with Wall-E2, my autonomous wall-following robot. This post describes some follow-on work on this same subject.
The fundamental problem with all the available Arduino PID libraries is they all require the user to wait in a loop for the PID::Compute() function to decide when to actually produce a new output value, and since this computation is inside the function, it is difficult or impossible to synchronize any other related timed element with the PID function. In my case where I want to control the turn rate of a robot, the input to the PID engine is, obviously, the turn rate, in degrees/sec. But, calculation of the turn rate is necessarily a timed function, i.e. (curent_heading – last_heading) / elapsed_time, where the ‘elapsed_time’ parameter is usually a constant. But, all the Arduino PID libraries use an internal private class member that defines the measurement period (in milliseconds), and this value isn’t available externally (well, it is, but only because the user can set the sample time – it can’t be read). So, the best one can do with the current libraries is to use the same constant for PID::SetSampleTime() and for any external time-based calculations, and hope there aren’t any synchronization issues. With this setup, it would be quite possible (and inevitable IMHO) for the PID::Compute() function to skip a step, or to be ‘phase-locked’ to producing an output that is one time constant off from the input.
The solution to this problem is to not use a PID library at all, and instead place the PID algorithm in-line with the rest of the code. This ensures that the PID calculation any related time-based calculations are operating on the same schedule. The downside of this arrangement is loss of generality; all the cool enhancements described by Brett Beauregard in his wonderful PID tutorial go away or have to be implemented in-line as well. From Brett’s tutorial, here’s ‘the beginner’s PID algorithm’:
The difficulty with all the current PID libraries is the ‘dt’ parameter in the above expression; the implementation becomes much easier if ‘dt’ is a constant – i.e. the time between calculations is a constant. However, this constraint also requires that the library, not the user program, controls the timing. This just doesn’t work when the ‘Input’ parameter above also requires a constant time interval for calculation. In my case of turn rate control, the turn rate calculation requires knowledge of the time interval between calculations, and the calculation itself should be done immediately after the turn rate is determined, using the same time interval. So, the turn rate is calculated and then PID::Compute() is called, but Compute() may or may not generate a new output value, because it can return without action if it’s internal time duration criteria isn’t met; see the problem? It may, or even might, generate a new output value each time, but there is no way to ensure that it will!
After figuring this out the hard way (by trying and failing to make the library work), I finally decided to forget the library – at least for my turn rate problem, and in-line all the needed code. Once I had it all running, I abstracted just the PID algorithm to its own function so I could use it elsewhere. This function is shown below:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 |
void PIDCalcs(double input, double setpoint, uint16_t sampleTime, double& lastError, double& lastInput, double& lastIval, double& lastDerror, double Kp, double Ki, double Kd, double& output) { //Purpose: Encapsulate PID algorithm so can get rid of PID library. Library too cumbersome and won't synch with TIMER5 ISR //Inputs: // input = double denoting current input value (turn rate, speed, whatever) // setpoint = double denoting desired setpoint in same units as input // sampleTime = int denoting sample time to be used in calcs. // lastError = ref to double denoting error saved from prev calc // lastInput = ref to double denoting input saved from prev calc // Kp/Ki/Kd = doubles denoting PID values to be used for calcs // Output = ref to double denoting output from calc double error = setpoint - input; double dInput = (input - lastInput); lastIval += (error); double dErr = (error - lastError); //mySerial.printf("PIDCalcs: error/dInput/lastIval/dErr = %2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr); /*Compute PID Output*/ output = Kp * error + Ki * lastIval + Kd * dErr; /*Remember some variables for next time*/ lastError = error; lastInput = input; lastDerror = dErr; } |
As you can see, the explanatory comments are much bigger than the function itself, which is really just eight lines long. Also, it has a huge number of arguments, five of which are references that are updated by the function. This function wouldn’t win any awards for good design, as it has too many arguments (wide coupling), but it does have high cohesion (does just one thing), and the coupling is at least ‘data’ coupling only.
Once this function was implemented, the calling function (in this case ‘SpinTurn()’ looks like this:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 |
bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC) { //Purpose: Make a numDeg CW or CCW 'spin' turn //Inputs: // b_ccw - True if turn is to be ccw, false otherwise // numDeg - angle to be swept out in the turn // ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg // IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR // degPerSec = float value denoting desired turn rate //Plan: // Step1: Get current heading as starting point // Step2: Compute new target value // Step3: 11/14/20 activate turn rate PID // Step4: Set motor speeds based on b_ccw // Step5: Run motors until target reached, using PID to control turn rate //Notes: // 06/27/20 copied from RollingTurn & adapted // 09/02/20 experiment with adjustable turn rate // 11/02/20 rev to use ISR-updated IMUHdgValDeg vs call to UpdateIMUHdgValDeg // 11/06/20 rev to use bTimeForNavUpdate flag to space out hdg/dist updates // 11/06/20 rev to make target turn rate variable for slowdown portion of spin // 11/14/20 rev to use PID for turn rate managaement // 04/05/21 rewritten to use ISR for timing and simplified // 04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC) // 05/29/21 replaced TurnRatePID with SetTurnRateMotorSpeeds() // 06/06/21 added re-try for 180.00 return from IMU - could be bad value float tgt_deg; float timeout_sec; bool bDoneTurning = false; bool bTimedOut = false; bool bResult = true; //04/21/20 added so will be only one exit point //unsigned long prev_uSec; //added 09/02/20 //DEBUG!! mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec); //DEBUG!! mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n", TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC); //no need to continue if the IMU isn't available if (!dmpReady) { return false; } UpdateIMUHdgValDeg(); //refreshes IMHHdgValDeg //06/06/21 it is possible for IMU to return 180.00 on failure //so try one more time in this case. If it really IS 180, then //the second try should confirm this. if (IMUHdgValDeg == 180.f) { mySerial.printf("Got 180.00 exactly from IMU - retrying...\n"); UpdateIMUHdgValDeg(); } //Step2: Compute new target value & timeout value timeout_sec = 2 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter //05/17/20 limit timeout_sec to 1 sec or more timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec; //12/05/19 added #define back in to manage which direction increases yaw values #ifdef MPU6050_CCW_INCREASES_YAWVAL tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg; #else tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg; #endif // MPU6050_CCW_INCREASES_YAWVAL //correct for -180/180 transition if (tgt_deg < -180) { tgt_deg += 360; } //07/29/19 bugfix if (tgt_deg > 180) { tgt_deg -= 360; } //DEBUG!! mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n", IMUHdgValDeg, numDeg, tgt_deg, timeout_sec); //DEBUG!! float curHdgMatchVal = 0; //09/08/18 added to bolster end-of-turn detection float prevHdgMatchVal = 0; float matchSlope = 0; TIMSK5 = 0; //disable timer compare interrupt //Step5: Run motors until target reached, using PID to control turn rate Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop elapsedMillis sinceLastTimeCheck = 0; elapsedMillis sinceLastComputeTime = 0; double lastError = 0; double lastInput = 0; double lastIval = 0; double lastDerror = 0; while (!bDoneTurning && !bTimedOut) { //11/06/20 now just loops between ISR hits CheckForUserInput(); UpdateIMUHdgValDeg(); //now Prev_HdgDeg & IMUHdgValDeg can be different (but probably aren't the first time through) //runs every 30mSec if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC) { sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC; //here Prev_HdgDeg & IMUHdgValDeg should be different, as TURN_RATE_UPDATE_INTERVAL_MSEC should have elapsed since last synch double dHdg = IMUHdgValDeg - Prev_HdgDeg; double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC); Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput); int speed = 0; if (TurnRatePIDOutput > MOTOR_SPEED_MAX) speed = MOTOR_SPEED_MAX; else if (TurnRatePIDOutput <= MOTOR_SPEED_LOW) speed = MOTOR_SPEED_LOW; else speed = (int)TurnRatePIDOutput; SetLeftMotorDirAndSpeed(!b_ccw, speed); SetRightMotorDirAndSpeed(b_ccw, speed); //07/31/19 now running as fast as we can get valid hdgs from IMU //check for nearly there and all the way there curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg); matchSlope = curHdgMatchVal - prevHdgMatchVal; prevHdgMatchVal = curHdgMatchVal; //look for full match bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL || (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start } bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000); if (bTimedOut) { //DEBUG!! mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal); //DEBUG!! bResult = false; } } TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt //04/21/20 added 'StopBothMotors()' at the end for symmetry & better understanding StopBothMotors(); delay(1000); //added 04/27/21 for debug return bResult; } |
In the above code, the lines dealing with ‘TIMSK5’ are there to disable and then re-enable the TIMER5 interrupt I have set up to update external sensor values every 100 mSec. I’m not really sure that this HAS to be done, but once I learned how to do it I figured it wouldn’t hurt, either ☺
Now that I have this ‘PIDCalcs()’ function working properly, I plan to use it in several other places where I currently use the PID library; it’s just so much simpler now, and because all the relevant parameters are visible to the calling program, debugging is now a piece of cake where before it was just an opaque black box.
12 June 2021 Update:
After chasing down and eliminating a number of bugs and edge-case issues, I think I now have a pretty stable/working version of the ‘SpinTurn’ function, as shown below:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 |
bool SpinTurn(bool b_ccw, float numDeg, float degPersec) //04/25/21 added turn-rate arg (default = TURN_RATE_TARGET_DEGPERSEC) { //Purpose: Make a numDeg CW or CCW 'spin' turn //Inputs: // b_ccw - True if turn is to be ccw, false otherwise // numDeg - angle to be swept out in the turn // ROLLING_TURN_MAX_SEC_PER_DEG = const used to generate timeout proportional to turn deg // IMUHdgValDeg = IMU heading value updated by UpdateIMUHdgValDeg() //11/02/20 now updated in ISR // degPerSec = float value denoting desired turn rate //Plan: // Step1: Get current heading as starting point // Step2: Disable TIMER5 interrupts // Step3: Compute new target value & timeout value // Step4: Run motors until target reached, using inline PID algorithm to control turn rate // Step5: Re-enable TIMER5 interrupts //Notes: // 06/06/21 we-written to remove PID library - now uses custom 'PIDCalcs()' function // 06/06/21 added re-try for 180.00 return from IMU - could be bad value // 06/11/21 added code to correct dHdg errors due to 179/-179 transition & bad IMU values // 06/12/21 cleaned up & commented out debug code float tgt_deg; float timeout_sec; bool bDoneTurning = false; bool bTimedOut = false; bool bResult = true; //04/21/20 added so will be only one exit point ////DEBUG!! //mySerial.printf("In SpinTurn(%s, %2.2f, %2.2f)\n", b_ccw == TURNDIR_CCW ? "CCW" : "CW", numDeg, degPersec); ////DEBUG!! //mySerial.printf("TurnRatePID started with Kp/Ki/Kd = %2.1f,%2.1f,%2.1f, SampleTime(mSec) = %d\n", // TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TURN_RATE_UPDATE_INTERVAL_MSEC); //no need to continue if the IMU isn't available if (!dmpReady) { return false; } //Step1: Get current heading as starting point //06/06/21 it is possible for IMU to return 180.00 on failure //so try again. If it really IS 180, then //it will eventually time out and go on int retries = 0; if (IMUHdgValDeg == 180.f && retries < 5) { //DEBUG!! //mySerial.printf("Got 180.00 exactly from IMU - retrying...\n"); //DEBUG!! UpdateIMUHdgValDeg(); retries++; delay(30); } //Step2: Compute new target value & timeout value timeout_sec = 3 * numDeg / degPersec; //05/29/21 rev to use new turn rate parmeter //05/17/20 limit timeout_sec to 1 sec or more timeout_sec = (timeout_sec < 1) ? 1.f : timeout_sec; UpdateIMUHdgValDeg(); //12/05/19 added #define back in to manage which direction increases yaw values #ifdef MPU6050_CCW_INCREASES_YAWVAL tgt_deg = b_ccw ? IMUHdgValDeg + numDeg : IMUHdgValDeg - numDeg; #else tgt_deg = b_ccw ? IMUHdgValDeg - numDeg : IMUHdgValDeg + numDeg; #endif // MPU6050_CCW_INCREASES_YAWVAL //correct for -180/180 transition if (tgt_deg < -180) { tgt_deg += 360; } //07/29/19 bugfix if (tgt_deg > 180) { tgt_deg -= 360; } //DEBUG!! //mySerial.printf("Init hdg = %4.2f deg, Turn = %4.2f deg, tgt = %4.2f deg, timeout = %4.2f sec\n\n", // IMUHdgValDeg, numDeg, tgt_deg, timeout_sec); //DEBUG!! float curHdgMatchVal = 0; //09/08/18 added to bolster end-of-turn detection float prevHdgMatchVal = 0; float matchSlope = 0; //Step2: Disable TIMER5 interrupts TIMSK5 = 0; //disable timer compare interrupt //Step3: Run motors until target reached, using PID algorithm to control turn rate Prev_HdgDeg = IMUHdgValDeg; //06/10/21 synch Prev_HdgDeg & IMUHdgValDeg just before entering loop //DEBUG!! //mySerial.printf("Just before while() loop: hdg/prvhdg = %2.2f\t%2.2f\n\n", IMUHdgValDeg, Prev_HdgDeg); //mySerial.printf("Msec\tHdg\tPrvHdg\tDhdg\trate\tSet\terror\tKp*err\tKi*err\tKd*Din\tOutput\n"); //DEBUG!! elapsedMillis sinceLastTimeCheck = 0; elapsedMillis sinceLastComputeTime = 0; double lastError = 0; double lastInput = 0; double lastIval = 0; double lastDerror = 0; bool bFirstIMUHdg = true; while (!bDoneTurning && !bTimedOut) { //11/06/20 now just loops between ISR hits CheckForUserInput(); //runs every 30mSec if (sinceLastComputeTime >= TURN_RATE_UPDATE_INTERVAL_MSEC) { sinceLastComputeTime -= TURN_RATE_UPDATE_INTERVAL_MSEC; UpdateIMUHdgValDeg(); //update IMUHdgValDeg double dHdg = IMUHdgValDeg - Prev_HdgDeg; if (dHdg > 180) { dHdg -= 360; mySerial.printf("dHdg > 180 - subtracting 360\n"); } else if (dHdg < -180) { dHdg += 360; mySerial.printf("dHdg < -180 - adding 360\n"); } //watch for turn rates that are wildly off double rate = abs(1000 * dHdg / TURN_RATE_UPDATE_INTERVAL_MSEC); if (rate > 3*degPersec) { //DEBUG!! //mySerial.printf("hdg/prevhdg/dHdg/rate = %2.2f\t%2.2f\t%2.2f\t%2.2f, excessive rate - replacing with %2.2f\n", IMUHdgValDeg, Prev_HdgDeg, dHdg, rate, degPersec); //DEBUG!! rate = degPersec; } //06/11/21 don't do calcs first time through, as Prev_HdgDeg isn't valid yet if (bFirstIMUHdg) { bFirstIMUHdg = false; Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time } else { PIDCalcs(rate, degPersec, TURN_RATE_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, TurnRate_Kp, TurnRate_Ki, TurnRate_Kd, TurnRatePIDOutput); int speed = 0; (TurnRatePIDOutput > MOTOR_SPEED_MAX) ? speed = MOTOR_SPEED_MAX : speed = (int)TurnRatePIDOutput; (TurnRatePIDOutput <= MOTOR_SPEED_LOW)? speed = MOTOR_SPEED_LOW : speed = (int)TurnRatePIDOutput; SetLeftMotorDirAndSpeed(!b_ccw, speed); SetRightMotorDirAndSpeed(b_ccw, speed); //check for nearly there and all the way there curHdgMatchVal = GetHdgMatchVal(tgt_deg, IMUHdgValDeg); matchSlope = curHdgMatchVal - prevHdgMatchVal; prevHdgMatchVal = curHdgMatchVal; //DEBUG!! //mySerial.printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\n", // millis(), // IMUHdgValDeg, // Prev_HdgDeg, // dHdg, // rate, // degPersec, // lastError, // TurnRate_Kp * lastError, // TurnRate_Ki * lastIval, // TurnRate_Kd * lastDerror, // speed); //DEBUG!! Prev_HdgDeg = IMUHdgValDeg; //re-synch prev to curr hdg for next time //look for full match bDoneTurning = (curHdgMatchVal >= HDG_FULL_MATCH_VAL || (curHdgMatchVal >= HDG_MIN_MATCH_VAL && matchSlope < -0.1)); //have to use < vs <= as slope == 0 at start } } bTimedOut = (sinceLastTimeCheck > timeout_sec * 1000); if (bTimedOut) { //DEBUG!! //mySerial.printf("timed out with yaw = %3.2f, tgt = %3.2f, and match = %1.3f\n", IMUHdgValDeg, tgt_deg, curHdgMatchVal); //DEBUG!! bResult = false; } } //DEBUG!! //mySerial.printf("%lu: Exiting SpinTurn() at %3.2f deg\n", millis(), IMUHdgValDeg); //DEBUG!! //Step4: Re-enable TIMER5 interrupts TIMSK5 |= (1 << OCIE5A);// enable timer compare interrupt StopBothMotors(); delay(1000); //added 04/27/21 for debug return bResult; } |
With this code in place, I made some 180º turns at 45 & 90 deg/sec, both on my benchtop and on carpet, as shown in the plots and video below:
Stay Tuned!
Frank
Pingback: Wall-E3 Replacing Mega 2560 With Teensy 3.5 Part IV | Paynter's Palace