Posted 22 June 2021
About nine months ago (October 2020) I made a run at getting offset tracking to work (see here and here). This post describes yet another attempt at getting this right, taking advantage of recent work on controlled-rate turns. I constructed a short single-task program to do just the wall tracking task, hopefully simplifying things to the point where I can understand what is happening.
One of the big issues that arose in previous work was the inability to synch my TIMER5 ISR with the PID library’s ‘Compute()’ function. The PID library insists on managing the update timing internally, which meant there was no way to ensure that Compute() would be called every time the ISR ran. I eventually came to the conclusion that I simply could not use the PID library version, and instead wrote my own small function that did the Compute() function, but with the timing value passed in as an argument rather than being managed internally. This forces the PID calculations to actually update in synch with the TIMER5 interrupt. Here’s the new PIDCalcs() function:
|
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/kp/ki/kd = %2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\n", error, dInput, lastIval, dErr, // Kp,Ki,Kd); /*Compute PID Output*/ output = Kp * error + Ki * lastIval + Kd * dErr; /*Remember some variables for next time*/ lastError = error; lastInput = input; lastDerror = dErr; } |
As can be seen from the above, this is a very simple routine that just does one thing, and doesn’t incorporate any of the improvements (windup suppression, sample time changes, etc) available in the PID library. The calling function has to manage the persistent parameters, but that’s a small price to pay for clarity and the assurance that the output value will indeed be updated every time the function is called.
With this function in hand, I worked on getting the robot to reliably track a specified offset, but was initially stymied because while I could get it to control the motors so that the robot stayed parallel to the nearest wall, I still couldn’t get it to track a specific offset. I solved this problem by leveraging my new-found ability to make accurate, rate-controlled turns; the robot first turns by an amount proportional to it’s distance from the desired offset, moves straight ahead until the offset is met, and then turns the same number of degrees in the other direction. Assuming the robot started parallel to the wall, this results in it facing the direction of travel, at the desired offset and parallel to the nearest wall. Here is the code for this algorithm.
|
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 |
void TrackLeftWallOffset(double kp, double ki, double kd, double offsetCm) { //Notes: // 06/21/21 modified to do a turn, then straight, then track 0 steer val elapsedMillis sinceLastComputeTime = 0; double lastError = 0; double lastInput = 0; double lastIval = 0; double lastDerror = 0; bool bFirstTime = true; WallTrackSteerVal = LeftSteeringVal; //computed by Teensy 3.5 //at 30mm from tgt offset, setpoint will be +/-0.3 WallTrackSetPoint = (double)(10 * WALL_OFFSET_TGTDIST_CM - Lidar_LeftCenter) / 100.f; //10/04/20 positive value drives robot toward wall //limit approach angle to reasonable values if (WallTrackSetPoint > WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = WALL_OFFSET_TRACK_SETPOINT_LIMIT; if (WallTrackSetPoint < -WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT; mySerial.printf("Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd); mySerial.printf("Msec\tFdir\tCdir\ttRdir\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n"); //06/21/21 modified to do a turn, then straight, then track 0 steer val GetRequestedVL53l0xValues(VL53L0X_LEFT); double cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(Lidar_LeftCenter / 10.f); mySerial.printf("dist = %d, cutAngleDeg = %2.2f\n", Lidar_LeftCenter, cutAngleDeg); SpinTurn((cutAngleDeg < 0), cutAngleDeg, 30); while (abs(Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM) > 10) { MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR); GetRequestedVL53l0xValues(VL53L0X_LEFT); mySerial.printf("%lu \t%d\t%d\t%d\t%d\n", millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, abs(Lidar_LeftFront - 10 * WALL_OFFSET_TGTDIST_CM)); delay(100); } //now turn back the same amount SpinTurn(!(cutAngleDeg < 0), cutAngleDeg, 30); //sinceLastComputeTime = 0; WallTrackSetPoint = 0; //moved here 6/22/21 while (true) { CheckForUserInput(); //this is a bit recursive, but should still work (I hope) //now using TIMER5 100 mSec interrupt for timing if (bTimeForNavUpdate) { //sinceLastComputeTime -= WALL_TRACK_UPDATE_INTERVAL_MSEC; bTimeForNavUpdate = false; //GetRequestedVL53l0xValues(VL53L0X_LEFT); //now done in TIMER5 ISR //have to weight value by both angle and wall offset WallTrackSteerVal = LeftSteeringVal + (Lidar_LeftCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f; //update motor speeds, skipping bad values if (!isnan(WallTrackSteerVal)) { //10/12/20 now this executes every time, with interval controlled by timer ISR PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, kp, ki, kd, WallTrackOutput); //PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, WALL_TRACK_UPDATE_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror, // kp, ki, kd, WallTrackOutput); int speed = 0; int leftspeednum = MOTOR_SPEED_QTR + WallTrackOutput; int rightspeednum = MOTOR_SPEED_QTR - WallTrackOutput; rightspeednum = (rightspeednum <= MOTOR_SPEED_FULL) ? rightspeednum : MOTOR_SPEED_FULL; rightspeednum = (rightspeednum > 0) ? rightspeednum : 0; leftspeednum = (leftspeednum <= MOTOR_SPEED_FULL) ? leftspeednum : MOTOR_SPEED_FULL; leftspeednum = (leftspeednum > 0) ? leftspeednum : 0; MoveAhead(leftspeednum, rightspeednum); mySerial.printf("%lu \t%d\t%d\t%d \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\t%d\n", millis(), Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval, kp * lastError, ki * lastIval, kd * lastDerror, WallTrackOutput, leftspeednum, rightspeednum); } } } } |
Here are a couple of videos showing the ability to capture and track a desired offset from either side.
In both of the above videos, the desired wall offset is 30 cm.
Stay Tuned,
Frank
Pingback: Another Try at Wall Offset Tracking, Part II | Paynter's Palace