Posted 09 September 2019
After successfully demonstrating heading-based wall tracking with my little 2-motor robot, I am now trying to add this capability to Wall-E2’s repertoire.
Current Algorithm:
Every MIN_PING_INTERVAL_MSEC millisec Wall-E2 measures left/right/forward distances, and then calls ‘GetOpMode()’ to determine its current operating mode. The currently defined modes are:
- MODE_NONE: (0) default case
- MODE_CHARGING (1): connected to the charger and not finished
- MODE_IRHOMING (2): coded IR signal from a charger has been detected
- MODE_WALLFOLLOW (3): trying to run parallel to a wall
- MODE_DEADBATTERY (4): battery exhausted
The MODE_WALLFOLLOW operational mode is returned from GetOpMode() when none of the other modes are active – in other words it is the real ‘default mode’. The MODE_WALLFOLLOW mode has three distinct sub-modes;
-
- TRACKING_LEFT (1): actively tracking the left wall
- TRACKING_RIGHT (2): actively tracking the right wall
- TRACKING_NEITHER (3): not tracking either wall (both distances > 200cm)
TRACKING_LEFT & TRACKING_RIGHT are identical for purposes of heading-based wall tracking, and the TRACKING_NEITHER case isn’t relevant, so we just have to come up with a way of integrating heading-based tracking into either the LEFT or RIGHT case. The TRACKING_LEFT case block 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 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 |
case TRACKING_LEFT: #pragma region TRACKLEFT trackstr = "Left"; //used for telemetry printouts //Bkp & Rot Right = Tracking Left && (Near || Stuck) if (frontdistval <= MIN_OBS_DIST_CM || IsStuck(frontvar)) { Serial.println("---- Backup & Turn to the Right"); //09/19/18 bugfix - 1st param is 'nearest wall', not 'turn direction' //BackupAndTurn(false, MOTOR_SPEED_HALF); //backup and turn 90 deg right //c/o 02/23/17 for IR Track debug BackupAndTurn(true, MOTOR_SPEED_HALF); //backup and turn 90 deg right //c/o 02/23/17 for IR Track debug //go back to normal tracking mode after B&T completes NavCase = NAV_WALLTRK; } //Step-turn Right = Tracking Left && Step && !Stuck //this section *starts* the step-turn. It is terminated as soon as frontdistval exits step-turn window else if (frontdistval > MIN_OBS_DIST_CM && frontdistval <= STEPTURN_DIST_CM) { //if we aren't already in Step-turn mode, start it. Otherwise, skip if (NavCase != NAV_STEPTURN) { Serial.println("---- Starting Stepturn to the Right"); NavCase = NAV_STEPTURN; leftspeednum = MOTOR_SPEED_MAX; rightspeednum = MOTOR_SPEED_LOW; MoveAhead(leftspeednum, rightspeednum); } } //Wall-tracking Left = Tracking Left && Far away from obstacles else if (frontdistval > STEPTURN_DIST_CM) { if (NavCase == NAV_STEPTURN) { mySerial.printf("Exiting Step-turn to the right\n"); } NavCase = NAV_WALLTRK; UpdateWallFollowMotorspeeds(leftdistval, rightdistval, leftspeednum, rightspeednum); MoveAhead(leftspeednum, rightspeednum); } PrintWallFollowTelemetry(frontvar); //added 04/15/19 #pragma endregion Tracking Left Wall Case break; |
The first thing that happens is a check for an obstacle within the MIN_OBS_DIST_CM or the ‘stuck’ condition (determined via a call to ‘IsStuck()’). If this test passes, a check is made for an obstacle that is farther away than MIN_OBS_DIST_CM but closer than STEPTURN_DIST_CM, so a gradual ‘step-turn’ can be made to negotiate an upcoming corner. If that check too fails, then the robot is assumed to be tracking the left wall, safely far from any oncoming obstacles. In this case, a new set of motor speeds is computed using a PID setup to keep the left PING distance constant – no consideration is given for whether or not the PING distance is appropriate – just that it is either greater or less than the one measured in the last pass.
So, the thing we want to change is the way the robot responds to PING distances to the left wall. We want the robot to acquire and maintain a selected standoff distance from the wall being tracked (the LEFT wall in this case). Pretty much by definition, this requires two distinct activities – the first to acquire the selected standoff distance, and the second to maintain that distance for the duration of the wall-tracking activity. So now we have three state variables for wall tracking, and only the last one (ACQUIRE/MAINTAIN) is new
- TRACKING_LEFT/RIGHT
- NAV_WALLTRK (as opposed to NAV_STEPTURN)
- PHASE_ACQUIREDIST/PHASE_MAINTAINDIST
- NAV_WALLTRK (as opposed to NAV_STEPTURN)
In the PHASE_ACQUIREDIST phase, a heading change of up to 20 deg is made in the appropriate direction to move toward the selected offset distance. The situation is then monitored in subsequent loop passes to determine if a larger or small heading cut is required. When the robot gets close to the desired distance, all the added heading offset is removed and the robot enters the PHASE_MAINTAINDIST phase. In this phase, small(er) ping distance variations cause larg(er) heading changes, resulting in a sawtooth pattern about the selected distance.