Posted 25 May 2024
Lately I have been working (or re-working) on Wall-E3’s ‘MoveToDesiredFrontDistCm()’ and ‘MoveToDesiredRearDistCm()’ capability. As I worked on these functions, I realized that the failure modes weren’t particularly straightforward. Both the ‘Front’ and ‘Rear’ functions can direct forward or rearward movement to achieve the desired distance, so in theory either function could experience a ‘stuck’ condition in either direction – ugh! In addition, I’m not sure what to do if either function experiences a ‘front obstacle’, ‘front offset distance’ or ‘rear obstacle’ condition.
Currently I have a set of enums that describe expected anomaly conditions, as shown below.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
enum AnomalyCode { ANOMALY_NONE = 0, ANOMALY_STUCK_AHEAD, ANOMALY_STUCK_BEHIND, ANOMALY_OBSTACLE_AHEAD, ANOMALY_WALL_OFFSET_DIST_AHEAD, ANOMALY_OBSTACLE_BEHIND, ANOMALY_DEAD_BATTERY, ANOMALY_CHARGER_CONNECTED, //added 05/22/22 ANOMALY_EXCESS_STEER_VAL, //added 05/19/23 ANOMALY_IR_BEAM_AVAILABLE, //added 08/20/23 ANOMALY_MIRRORED_SFC //added 01/21/24 }; |
And, in the current version of the ‘MoveTo..’ functions, the above are used as exit conditions from the movement loop, like the following snippet:
|
1 2 3 |
while (abs(gl_FrontCm - offsetCm) > 0.01 * MAX_MOVE_TO_DIST_ERROR_PCT * offsetCm && gl_LastAnomalyCode != ANOMALY_STUCK_AHEAD && gl_LastAnomalyCode != ANOMALY_STUCK_BEHIND) |
gl_LastAnomalyCode is updated in the ‘UpdateAllEnvironmentParameters()’ 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 |
void UpdateAllEnvironmentParameters(WallTrackingCases trkdir) { //Purpose: Update all 'global' distance measurements, distance arrays, variances and anomaly variables //Provenance: Created 03/07/22 //Inputs: // Left/Right/Rear/Front distance sensors //Outputs: // left rear/ctr/front, right rear/ctr/front, rear, front distance vars updated // Front and Rear variance vars updated // All boolean anomaly detection states updated //Plan: // Step1: Update all left/right/rear/front distances // Step2: Update all distance arrays // Step3: Update both front and rear variance calcs // Step4: Update all anomaly detection boolean vars //Notes: // 03/07/22 another attempt to corral the updates to 'global' environment vars // 08/20/23 repl all 'gl_bXXX = ' with 'if(???) gl_LastAnomalyCode = ANOMALY_XXX;' // 09/03/23 added UpdateIMUHdgValDeg() call // 05/19/24 just discovered that ANOMALY_WALL_OFFSET_DIST_AHEAD anomaly isn't cleared when the robot moves from // inside the offset distance to outside - ouch! Added 'else {gl_LastAnomalyCode = ANOMALY_NONE}' to bottom of checks. // 05/19/24 bugfix: chg 'gl_FrontCm < REAR_OBSTACLE_DETECTION_DIST_CM' to 'gl_RearCm < REAR_OBSTACLE_DETECTION_DIST_CM' // //DEBUG!! //gl_pSerPort->printf("%lu: UpdateAllEnvironmentParameters(%s) with anomaly code = %s\n", millis(), WallTrackStrArray[trkdir], AnomalyStrArray[gl_LastAnomalyCode]); //DEBUG!! digitalWrite(DURATION_MEASUREMENT_PIN1, HIGH);//start measurement pulse //Step1: Update all left/right/rear/front distances UpdateAllDistances(); UpdateIMUHdgValDeg(); //added 09/03/23 gl_AvgHeadingDeg = UpdateAvgHdgArray(IMUHdgValDeg); //gl_pSerPort->printf("in UpdateAllEnv()\n"); //gl_pSerPort->printf("Front\tRear\tLeftCtr\tRtCtr\n"); //gl_pSerPort->printf("%d\t%2.1f\t%2.1f\t%2.1f\n", gl_FrontCm, gl_RearCm, gl_LeftCenterCm, gl_RightCenterCm); ////Step2: Update all distance arrays // 05/19/22 no longer using incremental calcs, so no need for oldestfront/reardistval UpdateFrontDistanceArray(gl_FrontCm); UpdateRearDistanceArray(gl_RearCm); //Step3: Update both front and rear variance calcs gl_Frontvar = CalcBruteFrontDistArrayVariance();//05/19/22 bugfix gl_Rearvar = CalcBruteRearDistArrayVariance();//05/19/22 bugfix if (gl_FrontCm <= WALL_OFFSET_TGTDIST_CM) gl_LastAnomalyCode = ANOMALY_WALL_OFFSET_DIST_AHEAD; else if (IsExcessiveSteerVal(trkdir)) gl_LastAnomalyCode = ANOMALY_EXCESS_STEER_VAL;//08/22/23 moved here from below else if (gl_FrontCm < FRONT_OBSTACLE_DETECTION_DIST_CM) gl_LastAnomalyCode = ANOMALY_OBSTACLE_AHEAD; //else if (gl_FrontCm < REAR_OBSTACLE_DETECTION_DIST_CM) gl_LastAnomalyCode = ANOMALY_OBSTACLE_BEHIND; else if (gl_RearCm < REAR_OBSTACLE_DETECTION_DIST_CM) gl_LastAnomalyCode = ANOMALY_OBSTACLE_BEHIND;//05/19/24 bugfix else if (IsChargerConnected(gl_bChgConnect)) gl_LastAnomalyCode = ANOMALY_CHARGER_CONNECTED;//10/30/23 moved above IsIRBeamAvail() else if (IsIRBeamAvail()) gl_LastAnomalyCode = ANOMALY_IR_BEAM_AVAILABLE; //else if (IsChargerConnected(gl_bChgConnect)) gl_LastAnomalyCode = ANOMALY_CHARGER_CONNECTED; else if (IsStuckBehind()) gl_LastAnomalyCode = ANOMALY_STUCK_BEHIND; else if (IsStuckAhead()) gl_LastAnomalyCode = ANOMALY_STUCK_AHEAD; else if (IsDeadBattery()) { gl_LastAnomalyCode = ANOMALY_DEAD_BATTERY; gl_pSerPort->printf("Dead Battery detected with BattV = %2.2f\n", GetBattVoltage()); } else { gl_LastAnomalyCode = ANOMALY_NONE; }//added 05/19/24 digitalWrite(DURATION_MEASUREMENT_PIN1, LOW);//end measurement pulse //gl_pSerPort->printf("%lu: Bottom of UpdateAllEnvironmentParameters(%s) with anomaly code = %s\n", millis(), WallTrackStrArray[trkdir], AnomalyStrArray[gl_LastAnomalyCode]); } |
This function retrieves all the current distance readings, the current heading, and the current reading from the charger IR beam detection sensor. Then it assigns one of the ANOMALY enum values to gl_LastAnomalyCode based on one or more direct or derived sensor values. For instance, the ‘STUCK_AHEAD’ or ‘STUCK_BEHIND’ codes are assigned based on the value computed for the ‘front’ or ‘rear’ variance calculations.
The problem (or at least what I *think* is a problem) is that gl_LastAnomalyCode can only contain one value, and in the case of more than one ‘anomaly’ existing at the same time, the value assigned to gl_LastAnomalyCode is the one encountered first, as a result of the ‘else’ – ‘else if’ structure.
So, I started thinking that maybe I should instead implement a status byte or status word with bit positions assigned to possible error/anomaly conditions. Instead of an ‘else’ – ‘else if’ structure, the ‘UpdateAllEnvironmentParameters()’ would simply update each bit in the status object independently of all the others. Then functions whose behavior gets modified by one or more anomalies can consult the status object to determine what to do.
I think this approach is much more robust and generalized than the current ‘only one anomaly type at a time’ approach, but it is also much more complicated, in at least three different ways:
- Managing updates to the status object would require ‘bit diddling’ operations for clearing or setting individual bits.
- Deciding what to do could become much more complicated. Currently the ‘gl_LastAnomalyCode’ can contain only one value, so any behavior modification decisions are basically CASE blocks. With a status object behavior could depend on more than one parameter.
- sdaf;ljsadkf
I’m really intrigued by the power and generality of the status byte/word idea, but more than a little worried about ripping out the ANOMALY_CODE stuff and replacing it with status. Maybe I’ll just add the status byte/word stuff in parallel with the current setup and see how it goes.
Stay Tuned,
Frank