In the previous two posts on this subject, I described my efforts to replace the ultrasonic ‘ping’ sensors on my autonomous wall-following robot Wall-E2 with ST Microelectronics VL53L0X ‘Time-of-Flight’ LIDAR proximity sensors, with the aim of using an array of the ToF sensors to solve the problem of wall tracking at a specified offset. Part II demonstrated just such a capability, but only on the right side, and not in a way that was fully integrated with the rest of the robot operating system.
This post describes the work to fully integrate the new ‘parallel-find’ and ‘OffsetCapture/Track’ algorithms into the rest of the robot’s autonomous wall-following operating system. The robot operating system is a loop that runs every 200 mSec. Each time through the loop the current operating mode is determined in GetOpMode(), as follows:
- Dead Battery: Battery died before robot could find and connect to a charging station
- IR Homing: Robot has detected a charging station and is homing on it’s IR signal
- Charging: Robot is currently charging on one of the available charging stations
- Wall Follow: nothing else is going on, so continue tracking the nearest wall
In the above list, all but the last (Wall Follow) are ‘blocking’ in the sense that once they start, they run to completion without interruption. The ‘Wall Follow’ mode processing path however, typically makes a small adjustment to the left/right motor speeds depending on the situation and then exits back to the main loop, where GetOpMode() runs again. So wall following involves multiple passes through GetOpModes() so that higher-priority tasks (like dead battery recognition, IR homing, and battery charging are executed in a timely manner.
The result of GetOpMode() is passed to a CASE statement where actions appropriate to the determined mode are executed as follows:
- MODE_CHARGING: This mode blocks until charging is complete
- MODE_IRHOMING: This mode blocks while homing station to the charge station. If the robot isn’t ‘hungry’, it attempts to go around the charging station rather than connecting to it.
- MODE_DEADBATTERY: This mode blocks forever
- MODE_WALLFOLLOW: This is the default robot mode – whenever one of the above ‘special’ modes doesn’t apply. The MODE_WALLFOLLOW mode is further broken down into tracking cases as determined in GetTrackingDir(), which returns one of TRACKING_NONE, TRACKING_LEFT, TRACKING_RIGHT, or TRACKING_NEITHER.
With the replacement of the ‘ping’ sensors with the VL53L0X array) Wall-E2 can now accurately determine its orientation with respect to a nearby wall. So I now believe I can make the offset capture operation a blocking call, significantly simplifying the MODE_WALLFOLLOW code. Here is the code for the TRACKING_RIGHT case:
|
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 |
case TRACKING_RIGHT: //06/20/2020 rewritten to use PID engine and VL53L0X array trackstr = "Right"; //used for telemetry printouts if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar)) { //09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction' BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg left } //Step-turn Left = Tracking Right && Step && !Stuck else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM) { //02/26/20 StepTurn() is now a blocking fcn - doesn't return until step-turn is complete StepTurn(TRACKING_RIGHT); } //if we get to here, not stuck and no nearby obstacle issues else if (PrevOpMode != MODE_WALLFOLLOW)//02/26/20 first time through capture offset { //02/26/20 if capture successful, change ToFArrayPID params for wall tracking if (CaptureWallOffset(TRACKING_RIGHT, DESIRED_WALL_OFFSET_DIST_CM)) { ToFArrayPID.SetTunings(10, 0, 1); //try more aggresive than capture, but less than parallel rotate ToFArrayPID.SetOutputLimits(-MOTOR_SPEED_HALF, MOTOR_SPEED_HALF); //give PID full rein on motor speeds ToFSetpoint = Lidar_RightCenter; //now track the center distance, not steering value } PrevOpMode = MODE_WALLFOLLOW; //skip capture op after first time through } //01/30/17 added for manual remote control via Wixel CheckForUserInput(); GetRequestedVL53l0xValues(VL53L0X_RIGHT); ToFSteeringVal = Lidar_RightFront - Lidar_RightRear + Lidar_RightCenter; //use ALL info - not just distance! //update motor speeds, skipping bad values if (!isnan(ToFSteeringVal)) { //By default, computes new output approx 10 times/sec (use SetSampleTime() to change) if (ToFArrayPID.Compute())//if Compute returns TRUE, IRHomingOutput has new value { leftspeednum = MOTOR_SPEED_HALF - ToFOutput; rightspeednum = MOTOR_SPEED_HALF + ToFOutput; MoveAhead(leftspeednum, rightspeednum); } } break; |
In the above code the robot first checks for obstacles, and either maneuvers to capture the desired wall offset (first time through this case) or continues to track the previously captured offset.