Tag Archives: robots

The way forward from here

Posted 31 January 2023

I think I have now arrived at the point where the major sub-systems in my anonymous wall-following robot program are working well now. The last piece (I think) of the puzzle was the offset tracking algorithm (see https://www.fpaynter.com/2023/01/walle3-wall-track-tuning-review/).

Wall-Track Tuning – just finished (I hope)
Move to Front/Rear/Left/Right Distance – These are generally working now.
Detection of and tracking/homing to a charging station via IR beam.
Error condition detection/handling – this is still an open question to me. The program handles a number of error conditions, but I’m sure there are some error conditions that it doesn’t handle, or doesn’t handle well (the ‘open doorway’ detection and handling issue, for one).

The last full program I see in my Arduino directory is ‘WallE3_WallTrack_V5’, although it appears that _V5 isn’t that much different than _V4. Below are the changes from V4 to V5:

There are a number of changes in _V5’s #pragma OFFSET_CAPTURE section, but as I just discovered in this post, the new wall tracking configuration with PID(350,0,0) and ‘tweak’ divisor 50 (as described at the bottom of this post) means that I don’t need a separate ‘offset capture’ feature at all – the normal offset tracking routine handles the capture portion quite well, thankyou.
There are some very minor changes in _V5’s TrackLeftWallOffset(), but this section will be replaced in its entirety with the algorithm from the above post
V5 has a function called OrientCorr() that doesn’t exist in _V4, but it was just for debugging support.

So, I think I will start by creating yet another Arduino project from scratch, with the intention of building up to a complete working program, with wall offset tracking, charging station detection/docking, and anomalous condition detection/handling. But what to call it? I think I will go with ‘WallE3_Complete_V1’ and see how that works. I think I will need to be careful during it’s construction, as I want to incorporate all the progress I have made in the various ‘part-task’ programs:

WallE3_AnomalyRecovery_V1/V2
WallE3_ChargingStn_V1/V2/V3
WallE3_FrontBackMotionTuning_V1
WallE3_ParallelFind_V1
WallE3_RollingTurn_V1
WallE3_SpinTurnTuning_V2
WallE3_WallTrack_V1-V5
WallE3_WallTrackTuning_V1-V5

I will start by creating WallE3_Complete_V1 as a new blank program, and then going carefully through each of the above ‘part-task’ programs (in alphabetical order each time, just to reduce the confusion factor) to pull in the relevant bits.

Includes:

It looks like the complete #includes section is:

#pragma region INCLUDES
#include <Wire.h>
#include "FlashTxx.h"		// TLC/T3x/T4x flash primitives
#include <elapsedMillis.h>
#include "MPU6050_6Axis_MotionApps612.h"  //01/18/22 changed to use the \I2CDevLib\Arduino\MPU6050\ version
#include "I2C_Anything.h" //needed for sending float data over I2C
#include "timelib.h" //added 01/01/22 for charge monitoring support
#include <math.h>
#include "enums.h" //12/20/2022 added to avoid intellisense errors assoc with enums
#pragma endregion Includes

#pragma region INCLUDES

#include <Wire.h>

#include "FlashTxx.h" // TLC/T3x/T4x flash primitives

#include <elapsedMillis.h>

#include "MPU6050_6Axis_MotionApps612.h" //01/18/22 changed to use the \I2CDevLib\Arduino\MPU6050\ version

#include "I2C_Anything.h" //needed for sending float data over I2C

#include "timelib.h" //added 01/01/22 for charge monitoring support

#include <math.h>

#include "enums.h" //12/20/2022 added to avoid intellisense errors assoc with enums

#pragma endregion Includes

Oddly though, many of my programs don’t #include <wire.h>, but they compile and run fine – no idea why. OK, the reason is – “I2C_Anything.h” also includes <wire.h>. When I look at ‘wire.h’, I see it has a ‘#ifndef TwoWire_h’ statement at the top, so adding #include <wire.h> at the top won’t cause a problem, and I like it just for its informational value.

After copying in the #includes, I right-clicked on the project name and selected ‘add->existing item…’ and added ‘enums.h’, ‘FlashTxx.h’ and ‘FlashTxx.cpp’ from the ‘…\Robot Common Files’ folder. Then I opened a CMD window and used mklink (see this link) to create hard links to ‘board.txt’ and ‘TeensyOTA1.ttl’. At this point, the minimalist program (only #defines and empty, setup() and loop() functions) compiles without error – yay!

#Define Section:

Next in line are all the #Defines:

#pragma region DEFINES
//02/29/16 hardware defines
//#define NO_MOTORS
//#define NO_MPU6050 //added 01/23/22
//#define IR_HOMING_ONLY
//#define NO_LIDAR
//#define NO_VL53L0X //01/08/22 now used for VL53L0X hardware
//#define NO_IRDET //added 04/05/17 for daytime in-atrium testing (too much ambient IR)
//#define DISTANCES_ONLY //added 11/14/18 to just display distances in infinite loop
//#define NO_STUCK //added 03/10/19 to disable 'stuck' detection
//#define BATTERY_DISCHARGE //added 03/04/20 to discharge battery safely
#define NO_POST //added 04/12/20 to skip all the POST checks

//11/07/2020 moved all I2C Address declarations here
#define IRDET_I2C_ADDR 0x08
#define MPU6050_I2C_ADDR 0x68
#pragma endregion Program #Defines

#pragma region DEFINES

//02/29/16 hardware defines

//#define NO_MOTORS

//#define NO_MPU6050 //added 01/23/22

//#define IR_HOMING_ONLY

//#define NO_LIDAR

//#define NO_VL53L0X //01/08/22 now used for VL53L0X hardware

//#define NO_IRDET //added 04/05/17 for daytime in-atrium testing (too much ambient IR)

//#define DISTANCES_ONLY //added 11/14/18 to just display distances in infinite loop

//#define NO_STUCK //added 03/10/19 to disable 'stuck' detection

//#define BATTERY_DISCHARGE //added 03/04/20 to discharge battery safely

#define NO_POST //added 04/12/20 to skip all the POST checks

//11/07/2020 moved all I2C Address declarations here

#define IRDET_I2C_ADDR 0x08

#define MPU6050_I2C_ADDR 0x68

#pragma endregion Program #Defines

TIME INTERVALS Section:

#pragma region TIME INTERVALS
const uint16_t MSEC_PER_IR_HOMING_ADJ = 150; //06/27/22 100ms is too fast for GetFrontDistCm()
const int MSEC_PER_DIST_UPDATE = 100; //10/02/22 rev to speed up rate indep of front LIDAR
const uint16_t WALL_TRACK_UPDATE_INTERVAL_MSEC = MSEC_PER_DIST_UPDATE;//10/02/22 rev to make these two intervals the same
const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //10/02/22 added to separate out slow front LIDAR from faster VL53L0X sensors
const uint16_t TURN_RATE_UPDATE_INTERVAL_MSEC = 30; //30 mSec is as fast as it can go

elapsedMillis MsecSinceLastAdj; //added 05/24/22 for ParallelOrientation() routine
elapsedMillis MsecSinceLastIRHomingAdj; //01/07/22 used for #ifdef IR_HOMING_ONLY block
elapsedMillis MsecSinceLastDistUpdate; //01/07/22 used for local dist update loops
elapsedMillis MsecSinceLastTurnRateUpdate;//heading/rate based turn support
elapsedMillis mSecSinceLastWallTrackUpdate;
elapsedMillis MsecSinceLastFrontDistUpdate; //10/02/22 slower update rate needed for front LIDAR
#pragma endregion TIME INTERVALS

#pragma region TIME INTERVALS

const uint16_t MSEC_PER_IR_HOMING_ADJ = 150; //06/27/22 100ms is too fast for GetFrontDistCm()

const int MSEC_PER_DIST_UPDATE = 100; //10/02/22 rev to speed up rate indep of front LIDAR

const uint16_t WALL_TRACK_UPDATE_INTERVAL_MSEC = MSEC_PER_DIST_UPDATE;//10/02/22 rev to make these two intervals the same

const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //10/02/22 added to separate out slow front LIDAR from faster VL53L0X sensors

const uint16_t TURN_RATE_UPDATE_INTERVAL_MSEC = 30; //30 mSec is as fast as it can go

elapsedMillis MsecSinceLastAdj; //added 05/24/22 for ParallelOrientation() routine

elapsedMillis MsecSinceLastIRHomingAdj; //01/07/22 used for #ifdef IR_HOMING_ONLY block

elapsedMillis MsecSinceLastDistUpdate; //01/07/22 used for local dist update loops

elapsedMillis MsecSinceLastTurnRateUpdate;//heading/rate based turn support

elapsedMillis mSecSinceLastWallTrackUpdate;

elapsedMillis MsecSinceLastFrontDistUpdate; //10/02/22 slower update rate needed for front LIDAR

#pragma endregion TIME INTERVALS

Note that the above const declarations for MSEC_PER_DIST_UPDATE and FRONT_DISTANCE_UPDATE_INTERVAL_MSEC could just as easily be in the DISTANCE_MEASUREMENT_SUPPORT section, but I decided to try and keep all the timing stuff together. I’ll also put these declarations in the DISTANCE_MEASUREMENT_SUPPORT section but commented out with a pointer to the timing section.

TELEMETRYSTRINGS Section:

#pragma region TELEMETRYSTRINGS
const char* AnomalyStrArray[] = { "NO_ANOMALIES", "STUCK_AHEAD", "STUCK_BEHIND", "OBSTACLE_AHEAD",
"WALL_OFFSET_DIST_AHEAD","OBSTACLE_BEHIND", "OPEN_CORNER", "DEAD_BATTERY", "CHARGER_CONNECTED" };//06/12/22 CHG_STN_AVAIL removed
//const char* TrkStrArray[] = { "None", "Left", "Left Capture", "Right", "Right Capture", "Neither" };
const char* TrkStrArray[] = { "None", "Left", "Right", "Neither" };//01/31/23 removed "_Capture" elements 
const char* IRHomingTelemStr = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\tFrontD\tRearD";
const char* IRHomingTelemStrNoPings = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\n";
const char* LeftWallFollowTelemStr = "Msec\tLFront\tLCtr\tLRear\tFront\tFrontVar\tRear\tSteer\tOutput\tSetPt\tAdjDist\tLSpd\tRSpd";
const char* WallFollowTelemHdrStr = "Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\tR\tRvar\tSteer\tSet\tOutput\tLSpd\tRSpd\tIRAvg\n";
const char* WallFollowTelemStr = "%lu\t%d\t%d\t%d\t\t%d\t%d\t%d\t\t%d\t%2.0f\t%d\t%2.0f\t%2.2f\t%2.2f\t%2.0f\t%d\t%d\t%lu\n";
//const char* ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tRearD\n"; //rev 01/30/21
const char* ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tbChging\n"; //rev 01/30/21
#pragma endregion Mode-specific telemetry header strings

#pragma region TELEMETRYSTRINGS

const char* AnomalyStrArray[] = { "NO_ANOMALIES", "STUCK_AHEAD", "STUCK_BEHIND", "OBSTACLE_AHEAD",

"WALL_OFFSET_DIST_AHEAD","OBSTACLE_BEHIND", "OPEN_CORNER", "DEAD_BATTERY", "CHARGER_CONNECTED" };//06/12/22 CHG_STN_AVAIL removed

//const char* TrkStrArray[] = { "None", "Left", "Left Capture", "Right", "Right Capture", "Neither" };

const char* TrkStrArray[] = { "None", "Left", "Right", "Neither" };//01/31/23 removed "_Capture" elements

const char* IRHomingTelemStr = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\tFrontD\tRearD";

const char* IRHomingTelemStrNoPings = "Time\tBattV\tFin1\tFin2\tSteer\tPID_Out\t\tLSpd\tRSpd\n";

const char* LeftWallFollowTelemStr = "Msec\tLFront\tLCtr\tLRear\tFront\tFrontVar\tRear\tSteer\tOutput\tSetPt\tAdjDist\tLSpd\tRSpd";

const char* WallFollowTelemHdrStr = "Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\tR\tRvar\tSteer\tSet\tOutput\tLSpd\tRSpd\tIRAvg\n";

const char* WallFollowTelemStr = "%lu\t%d\t%d\t%d\t\t%d\t%d\t%d\t\t%d\t%2.0f\t%d\t%2.0f\t%2.2f\t%2.2f\t%2.0f\t%d\t%d\t%lu\n";

//const char* ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tRearD\n"; //rev 01/30/21

const char* ChargingTelemStr = "ChgSec\tBattV\tTotalI\tRunI\tChgI\tbChging\n"; //rev 01/30/21

#pragma endregion Mode-specific telemetry header strings

I removed the “_Capture” elements from TrkStrArray[] as these are no longer needed (wall offset capture now just part of TrackLeftRightOffset()). The rest should be OK for now.

DISTANCE_MEASUREMENT_SUPPORT Section:

#pragma region DISTANCE_MEASUREMENT_SUPPORT
//misc LIDAR and Ping sensor parameters
const uint16_t MIN_FRONT_OBSTACLE_DIST_CM = 30; //rev 04/28/17 for better obstacle handling
const uint16_t CHG_STN_AVOIDANCE_DIST_CM = 40; //added 03/11/17 for charge stn avoidance
const uint16_t MAX_FRONT_DISTANCE_CM = 400;
const uint16_t MAX_LR_DISTANCE_CM = 200;  //04/19/15 now using sep parameters for front and side sensors

//04/01/2015 added for 'stuck' detection support
const uint16_t FRONT_DIST_ARRAY_SIZE = 50; //11/22/20 doubled to acct for 10Hz update rate
const uint16_t FRONT_DIST_AVG_WINDOW_SIZE = 3; //moved here & renamed 04/28/19
const uint16_t LR_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg
const uint16_t REAR_DIST_ARRAY_SIZE = 50; //02/28/22 bumped to 50
const uint16_t LR_AVG_WINDOW_SIZE = 3; //added 04/28/19 so front & lr averages can differ
const uint16_t STUCK_FRONT_VARIANCE_THRESHOLD = 50; //chg to 50 04/28/17
const uint16_t STUCK_REAR_VARIANCE_THRESHOLD = 50; //added 01/08/22
const uint16_t NO_LIDAR_FRONT_VAR_VAL = 10 * STUCK_FRONT_VARIANCE_THRESHOLD; //01/16/19

//04/28/19 added to reinstate l/r dist running avg
//06/28/20 chg to uint_16 to accommodate change from cm to mm
uint16_t aFrontDist[FRONT_DIST_ARRAY_SIZE]; //04/18/19 rev to use uint16_t vs byte
uint16_t aLeftDistCM[LR_DIST_ARRAY_SIZE];
uint16_t aRightDistCM[LR_DIST_ARRAY_SIZE];
uint16_t aRearDistCM[REAR_DIST_ARRAY_SIZE];
uint16_t curMinObstacleDistance = MIN_FRONT_OBSTACLE_DIST_CM;//added 03/11/17 for chg stn avoidance

//added 10/24/20
//const int STUCK_BACKUP_DISTANCE_CM = 25;
const uint16_t STUCK_BACKUP_DISTANCE_CM = 20; //11/21/28 shortened slightly
const uint16_t STUCK_FORWARD_DISTANCE_CM = 15;
const uint16_t MAX_REAR_DISTANCE_CM = 100; //03/01/22 rev to use actual experimental results
const uint16_t MIN_REAR_OBSTACLE_DIST_CM = 10;
const uint16_t STUCK_BACKUP_TIME_MSEC = 1000;
const uint16_t STUCK_FORWARD_TIME_MSEC = 1000;
//const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //01/31/23 this const was moved to 'TIME_INTERVALS'

#pragma endregion Distance Measurement Support

#pragma region DISTANCE_MEASUREMENT_SUPPORT

//misc LIDAR and Ping sensor parameters

const uint16_t MIN_FRONT_OBSTACLE_DIST_CM = 30; //rev 04/28/17 for better obstacle handling

const uint16_t CHG_STN_AVOIDANCE_DIST_CM = 40; //added 03/11/17 for charge stn avoidance

const uint16_t MAX_FRONT_DISTANCE_CM = 400;

const uint16_t MAX_LR_DISTANCE_CM = 200; //04/19/15 now using sep parameters for front and side sensors

//04/01/2015 added for 'stuck' detection support

const uint16_t FRONT_DIST_ARRAY_SIZE = 50; //11/22/20 doubled to acct for 10Hz update rate

const uint16_t FRONT_DIST_AVG_WINDOW_SIZE = 3; //moved here & renamed 04/28/19

const uint16_t LR_DIST_ARRAY_SIZE = 3; //04/28/19 added to reinstate l/r dist running avg

const uint16_t REAR_DIST_ARRAY_SIZE = 50; //02/28/22 bumped to 50

const uint16_t LR_AVG_WINDOW_SIZE = 3; //added 04/28/19 so front & lr averages can differ

const uint16_t STUCK_FRONT_VARIANCE_THRESHOLD = 50; //chg to 50 04/28/17

const uint16_t STUCK_REAR_VARIANCE_THRESHOLD = 50; //added 01/08/22

const uint16_t NO_LIDAR_FRONT_VAR_VAL = 10 * STUCK_FRONT_VARIANCE_THRESHOLD; //01/16/19

//04/28/19 added to reinstate l/r dist running avg

//06/28/20 chg to uint_16 to accommodate change from cm to mm

uint16_t aFrontDist[FRONT_DIST_ARRAY_SIZE]; //04/18/19 rev to use uint16_t vs byte

uint16_t aLeftDistCM[LR_DIST_ARRAY_SIZE];

uint16_t aRightDistCM[LR_DIST_ARRAY_SIZE];

uint16_t aRearDistCM[REAR_DIST_ARRAY_SIZE];

uint16_t curMinObstacleDistance = MIN_FRONT_OBSTACLE_DIST_CM;//added 03/11/17 for chg stn avoidance

//added 10/24/20

//const int STUCK_BACKUP_DISTANCE_CM = 25;

const uint16_t STUCK_BACKUP_DISTANCE_CM = 20; //11/21/28 shortened slightly

const uint16_t STUCK_FORWARD_DISTANCE_CM = 15;

const uint16_t MAX_REAR_DISTANCE_CM = 100; //03/01/22 rev to use actual experimental results

const uint16_t MIN_REAR_OBSTACLE_DIST_CM = 10;

const uint16_t STUCK_BACKUP_TIME_MSEC = 1000;

const uint16_t STUCK_FORWARD_TIME_MSEC = 1000;

//const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //01/31/23 this const was moved to 'TIME_INTERVALS'

#pragma endregion Distance Measurement Support

Copied this from ‘WallE3_AnomalyRecovery_V2’. It looks pretty complete. Note that I left the ‘STUCK_FORWARD/BACKUP_TIME_MSEC’ declarations here rather than in the ‘Timing’ section. Didn’t seem to warrant the attention.

PIN ASSIGNMENTS Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks pretty complete

MOVE TO DESIRED DISTANCE Section:

//04/25/21 moved here from MoveToDesiredFront/BackDist() functions
//01/31/23 TODO:  Globally change 'OffsetDistKx' to 'MoveToDist_Kx', 'OffsetDistOutput' to 'MoveToDistOutput', 'OffsetDistVal' to 'MoveToDistInput' 
#pragma region MoveToDesiredDist MOVE TO DESIRED DIST SUPPORT
//12/26/22 updated per https://www.fpaynter.com/2022/12/move-to-a-specified-distance-revisited/
const float OffsetDistKp = 1.5f; //neg value creates PID in REVERSE mode
const float OffsetDistKi = 0.1f;
const float OffsetDistKd = 0.2f;

//float OffsetDistSetpointCm, OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive
float OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive
double OffsetDistVal = 0; //has to be 'double'
#pragma endregion MoveToDesiredFront/Back/Left/RightDist()

//04/25/21 moved here from MoveToDesiredFront/BackDist() functions

//01/31/23 TODO: Globally change 'OffsetDistKx' to 'MoveToDist_Kx', 'OffsetDistOutput' to 'MoveToDistOutput', 'OffsetDistVal' to 'MoveToDistInput'

#pragma region MoveToDesiredDist MOVE TO DESIRED DIST SUPPORT

//12/26/22 updated per https://www.fpaynter.com/2022/12/move-to-a-specified-distance-revisited/

const float OffsetDistKp = 1.5f; //neg value creates PID in REVERSE mode

const float OffsetDistKi = 0.1f;

const float OffsetDistKd = 0.2f;

//float OffsetDistSetpointCm, OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive

float OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive

double OffsetDistVal = 0; //has to be 'double'

#pragma endregion MoveToDesiredFront/Back/Left/RightDist()

I changed the #pragma name from ‘FRONT_BACK OFFSET MOTION PID’ to ‘MOVE TO DESIRED DIST SUPPORT’ as that is a better description of what these parameters do. In addition, the ‘Offsetxxxx’ name is no longer relevant – it should be changed to ‘MoveToDistxxx’, but I don’t want to do that willy-nilly now. I’ll wait until the entire program will compile, and then (after one last check) I’ll make the change globally.

Charge Support Parameters Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks good.

MOTOR_PARAMETERS Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks good.

MPU6050_SUPPORT Section:

Copied from ‘WallE3_AnomalyRecovery_V2’ – looks good.

WALL_FOLLOW_SUPPORT Section:

#pragma region WALL_FOLLOW_SUPPORT
float WALL_OFFSET_TRACK_Kp = 200;//07/09/22 from WallE3_WallTrackTuning.ino
float WALL_OFFSET_TRACK_Ki = 20;//07/09/22 from WallE3_WallTrackTuning.ino
float WALL_OFFSET_TRACK_Kd = 0;
float WallTrackSteerVal, WallTrackOutput, WallTrackSetPoint;
const uint16_t WALL_OFFSET_TGTDIST_CM = 40;
const uint16_t REAR_OBSTACLE_RECOVERY_DISTANCE_CM = 10; //added 09/03/22
const uint16_t WALL_OFFSET_APPR_ANGLE_MINDEG = 30;//added 09/08/21
const float WALL_TRACK_CAPTURE_MAX_STEERING_VAL = 0.8;//09/06/22 added for use in RunToDaylight()
//const float LEFT_WALL_PARALLEL_STEER_VALUE = -0.35; //added 10/08/22
//const float RIGHT_WALL_PARALLEL_STEER_VALUE = 0.25; //added 10/08/22
#pragma endregion WALL_FOLLOW_SUPPORT

#pragma region WALL_FOLLOW_SUPPORT

float WALL_OFFSET_TRACK_Kp = 200;//07/09/22 from WallE3_WallTrackTuning.ino

float WALL_OFFSET_TRACK_Ki = 20;//07/09/22 from WallE3_WallTrackTuning.ino

float WALL_OFFSET_TRACK_Kd = 0;

float WallTrackSteerVal, WallTrackOutput, WallTrackSetPoint;

const uint16_t WALL_OFFSET_TGTDIST_CM = 40;

const uint16_t REAR_OBSTACLE_RECOVERY_DISTANCE_CM = 10; //added 09/03/22

const uint16_t WALL_OFFSET_APPR_ANGLE_MINDEG = 30;//added 09/08/21

const float WALL_TRACK_CAPTURE_MAX_STEERING_VAL = 0.8;//09/06/22 added for use in RunToDaylight()

//const float LEFT_WALL_PARALLEL_STEER_VALUE = -0.35; //added 10/08/22

//const float RIGHT_WALL_PARALLEL_STEER_VALUE = 0.25; //added 10/08/22

#pragma endregion WALL_FOLLOW_SUPPORT

Copied from ‘WallE3_AnomalyRecovery_V2’, except I commented out the ‘LEFT/RIGHT_WALL_PARALLEL_STEER_VALUEs as these are no longer used.

HEADING_AND_RATE_BASED_TURN_PARAMETERS Section:

Only the ‘TurnRate_Kx’ parameters, the ‘HDG_NEAR_MATCH’, HDG_FULL_MATCH, ‘HDG_MIN_MATCH’ and ‘DEFAULT_TURN_RATE’ should be in this section. Everything else should be local to the ‘turn’ functions (I kept the ‘Prev_HdgDeg’ and ‘TurnRatePIDOutput’ at global scope for now to avoid lots of compile errors, but they should also be removed.

PARALLEL_FIND_SUPPORT Section:

#pragma region PARALLEL_FIND_SUPPORT
//05/24/22 added for new RotateToParallelOrientation() routine
//const int ParallelFindKp = 100;   
const float ParallelFindKp = 50.f;
const float ParallelFindKi = 10.f;
const float ParallelFindKd = 0.f;
const float ParallelFindSetpoint = 0.0; //09/22/20 moved here
const float ParallelFindThreshold = 0.01; //05/24/22
float ParallelFindOutput = 0; //05/24/22
#pragma endregion PARALLEL_FIND_SUPPORT

#pragma region PARALLEL_FIND_SUPPORT

//05/24/22 added for new RotateToParallelOrientation() routine

//const int ParallelFindKp = 100;

const float ParallelFindKp = 50.f;

const float ParallelFindKi = 10.f;

const float ParallelFindKd = 0.f;

const float ParallelFindSetpoint = 0.0; //09/22/20 moved here

const float ParallelFindThreshold = 0.01; //05/24/22

float ParallelFindOutput = 0; //05/24/22

#pragma endregion PARALLEL_FIND_SUPPORT

03 February 2023: The entire PARALLEL_FIND_SUPPORT section has been removed, as the new RotateToParallelOrientation() function no longer uses a PID engine

IR_HOMING_SUPPORT Section:

#pragma region IR_HOMING_SUPPORT
//uint8_t IR_HOMING_MODULE_SLAVE_ADDR = 8;  //uint8_t type reqd here for Wire.requestFrom() call //mvd to #define 02/02/23
float IRHomingSetpoint, IRHomingOutput;//10/06/17 chg input variable name to something more descriptive
double IRHomingLRSteeringVal = 0;//04/12/22 has to be double (8 bytes)

//these values all come from IR Homing Teensy via I2C calls
uint32_t IRFinalValue1 = 0;
uint32_t IRFinalValue2 = 0;
uint32_t IRHomingValTotalAvg = 0;

//01/11/22 started over again with WallE3 and home-grown PID algorithm
//01/15/22 reduce Kp to 150, keeping others the same
const float IRHomingKp = 75.f;
const float IRHomingKi = 00.f;
const float IRHomingKd = 40.f; //worse

//04/14/22 chg from const long to const uint32_t
const uint32_t IR_BEAM_DETECTION_CHANNEL_MAX = 2621440;
//const uint32_t IR_BEAM_DETECTION_THRESHOLD = 15000;
const uint32_t IR_BEAM_DETECTION_THRESHOLD = 25000;//05/08/22

const float IR_HOMING_STEERING_VAL_CAPTURE_THRESHOLD = 0.3; //added 01/30/21
const float IR_HOMING_STEERING_VAL_DETECTION_THRESHOLD = 0.8; //added 03/30/21
const uint16_t IRHOMING_VALUE_ARRAY_SIZE = 3; //added 03/16/21 for value average support
long int aIRHOMINGVALTOTALS[IRHOMING_VALUE_ARRAY_SIZE];//added 03/16/21 for value average support
const uint16_t IRHOMING_IAP_OFFSET_LOW_THRESHOLD_PCT = 75; //03/21/21 if wall offset is less than this relative to IAP offset, then adjust
const uint16_t IRHOMING_IAP_OFFSET_HIGH_THRESHOLD_PCT = 110; //03/23/21 if wall offset is more than this relative to IAP offset, then adjust
//const float IRHOMING_IAP_FINE_TUNE_STEERING_VALUE_THRESHOLD = 0.2; //03/21/21 fine-tune if more off-boresight than this
const float IRHOMING_IAP_STEERING_VALUE_THRESHOLD = 0.8; //05/28/22

//added 05/28/22 for better TurnToHomingBeacon() performance
const uint16_t IRHOMING_STEERVALUE_ARRAY_SIZE = 10;
float aIRHOMINGSTEERVALS[IRHOMING_STEERVALUE_ARRAY_SIZE];
const float IRHOMING_DISTANCE_OFFSET_RATIO = 0.3; //added 06/17/22
#pragma endregion IR_HOMING_SUPPORT

#pragma region IR_HOMING_SUPPORT

//uint8_t IR_HOMING_MODULE_SLAVE_ADDR = 8; //uint8_t type reqd here for Wire.requestFrom() call //mvd to #define 02/02/23

float IRHomingSetpoint, IRHomingOutput;//10/06/17 chg input variable name to something more descriptive

double IRHomingLRSteeringVal = 0;//04/12/22 has to be double (8 bytes)

//these values all come from IR Homing Teensy via I2C calls

uint32_t IRFinalValue1 = 0;

uint32_t IRFinalValue2 = 0;

uint32_t IRHomingValTotalAvg = 0;

//01/11/22 started over again with WallE3 and home-grown PID algorithm

//01/15/22 reduce Kp to 150, keeping others the same

const float IRHomingKp = 75.f;

const float IRHomingKi = 00.f;

const float IRHomingKd = 40.f; //worse

//04/14/22 chg from const long to const uint32_t

const uint32_t IR_BEAM_DETECTION_CHANNEL_MAX = 2621440;

//const uint32_t IR_BEAM_DETECTION_THRESHOLD = 15000;

const uint32_t IR_BEAM_DETECTION_THRESHOLD = 25000;//05/08/22

const float IR_HOMING_STEERING_VAL_CAPTURE_THRESHOLD = 0.3; //added 01/30/21

const float IR_HOMING_STEERING_VAL_DETECTION_THRESHOLD = 0.8; //added 03/30/21

const uint16_t IRHOMING_VALUE_ARRAY_SIZE = 3; //added 03/16/21 for value average support

long int aIRHOMINGVALTOTALS[IRHOMING_VALUE_ARRAY_SIZE];//added 03/16/21 for value average support

const uint16_t IRHOMING_IAP_OFFSET_LOW_THRESHOLD_PCT = 75; //03/21/21 if wall offset is less than this relative to IAP offset, then adjust

const uint16_t IRHOMING_IAP_OFFSET_HIGH_THRESHOLD_PCT = 110; //03/23/21 if wall offset is more than this relative to IAP offset, then adjust

//const float IRHOMING_IAP_FINE_TUNE_STEERING_VALUE_THRESHOLD = 0.2; //03/21/21 fine-tune if more off-boresight than this

const float IRHOMING_IAP_STEERING_VALUE_THRESHOLD = 0.8; //05/28/22

//added 05/28/22 for better TurnToHomingBeacon() performance

const uint16_t IRHOMING_STEERVALUE_ARRAY_SIZE = 10;

float aIRHOMINGSTEERVALS[IRHOMING_STEERVALUE_ARRAY_SIZE];

const float IRHOMING_DISTANCE_OFFSET_RATIO = 0.3; //added 06/17/22

#pragma endregion IR_HOMING_SUPPORT

Copied these from WallE3_AnomalyRecovery_V2. At some point the ‘IRHomingSetpoint’ variable should be changed from global to local scope, and ‘IRHomingLRSteeringVal’ should be renamed to ‘gl_IRHomingLRSteeringVal’ to show global scope. Same with ‘IRFinalValue1’, ‘IRFinalValue2’ and ‘IRHomingValTotalAvg’

GLOBAL_VARIABLES Section:

#pragma region GLOBAL_VARIABLES
float glBatteryVoltage;
uint16_t glLeftspeednum = MOTOR_SPEED_HALF;
uint16_t glRightspeednum = MOTOR_SPEED_HALF;

NavCases glNavCase = NavCases::NAV_WALLTRK;
WallTrackingCases glTrackingCase = WallTrackingCases::TRACKING_NEITHER; //added 01/05/16
WallTrackingCases glPrevTrackingCase = WallTrackingCases::TRACKING_LEFT; //only used decide which way to turn in the TRACKING_NEITHER case  
OpModes glPrevOpMode = OpModes::MODE_NONE; //added 03/08/17, rev to MODE_NONE 04/04/17
OpModes glCurrentOpMode = OpModes::MODE_NONE; //added 10/13/17 so can use in motor speed setting routines

//04/10/20 added for experiment to port heading based wall tracking from two wheel robot
TrackingState glCurrentTrackingState = TrackingState::TRK_RIGHT_NONE;
TrackingState glPrevTrackingState = TrackingState::TRK_RIGHT_NONE;

int16_t glFinalLeftSpeed = 0;
int16_t glFinalRightSpeed = 0;

//02/15/22 added from FourWD_WallE2_V12.ino
uint16_t glFrontCm = 0;
float glFrontvar = 0;//chg to float 03/23/22
float glRearvar = 0;//chg to float 03/23/22
//float glFrontmean = 0; 
//float glRearmean = 0;//06/12/22 not needed - local var is OK

//03/07/22 prepended 'gl_' 
bool gl_bIRBeamAvail = false;
bool gl_bChgConnect = false;
bool gl_bObstacleAhead = false;
bool gl_bWallOffsetDist = false;//03/27/22 added
bool gl_bObstacleBehind = false;
bool gl_bStuckAhead = false;
bool gl_bStuckAhead_Slow = false;
bool gl_bStuckBehind = false;
bool gl_bDeadBattery = false;

//Sensor data values
//03/01/22 rev to store all dists in cm vs mm
uint16_t glRightFrontCm;
uint16_t glRightCenterCm;
uint16_t glRightRearCm;
uint16_t glRearCm; //added 10/24/20
float glRightSteeringVal; //added 08/06/20

uint16_t glLeftFrontCm;
uint16_t glLeftCenterCm;
uint16_t glLeftRearCm;
float glLeftSteeringVal; //added 08/06/20

bool bVL53L0X_TeensyReady = false; //11/10/20 added to prevent bad data reads during Teensy setup()
Stream* gl_pSerPort = 0; //09/26/22 made global so can use everywhere.


//02/05/22 ported from FourWD_WallTrackTest_V4
#pragma endregion GLOBAL_VARIABLES

#pragma region GLOBAL_VARIABLES

float glBatteryVoltage;

uint16_t glLeftspeednum = MOTOR_SPEED_HALF;

uint16_t glRightspeednum = MOTOR_SPEED_HALF;

NavCases glNavCase = NavCases::NAV_WALLTRK;

WallTrackingCases glTrackingCase = WallTrackingCases::TRACKING_NEITHER; //added 01/05/16

WallTrackingCases glPrevTrackingCase = WallTrackingCases::TRACKING_LEFT; //only used decide which way to turn in the TRACKING_NEITHER case

OpModes glPrevOpMode = OpModes::MODE_NONE; //added 03/08/17, rev to MODE_NONE 04/04/17

OpModes glCurrentOpMode = OpModes::MODE_NONE; //added 10/13/17 so can use in motor speed setting routines

//04/10/20 added for experiment to port heading based wall tracking from two wheel robot

TrackingState glCurrentTrackingState = TrackingState::TRK_RIGHT_NONE;

TrackingState glPrevTrackingState = TrackingState::TRK_RIGHT_NONE;

int16_t glFinalLeftSpeed = 0;

int16_t glFinalRightSpeed = 0;

//02/15/22 added from FourWD_WallE2_V12.ino

uint16_t glFrontCm = 0;

float glFrontvar = 0;//chg to float 03/23/22

float glRearvar = 0;//chg to float 03/23/22

//float glFrontmean = 0;

//float glRearmean = 0;//06/12/22 not needed - local var is OK

//03/07/22 prepended 'gl_'

bool gl_bIRBeamAvail = false;

bool gl_bChgConnect = false;

bool gl_bObstacleAhead = false;

bool gl_bWallOffsetDist = false;//03/27/22 added

bool gl_bObstacleBehind = false;

bool gl_bStuckAhead = false;

bool gl_bStuckAhead_Slow = false;

bool gl_bStuckBehind = false;

bool gl_bDeadBattery = false;

//Sensor data values

//03/01/22 rev to store all dists in cm vs mm

uint16_t glRightFrontCm;

uint16_t glRightCenterCm;

uint16_t glRightRearCm;

uint16_t glRearCm; //added 10/24/20

float glRightSteeringVal; //added 08/06/20

uint16_t glLeftFrontCm;

uint16_t glLeftCenterCm;

uint16_t glLeftRearCm;

float glLeftSteeringVal; //added 08/06/20

bool bVL53L0X_TeensyReady = false; //11/10/20 added to prevent bad data reads during Teensy setup()

Stream* gl_pSerPort = 0; //09/26/22 made global so can use everywhere.

//02/05/22 ported from FourWD_WallTrackTest_V4

#pragma endregion GLOBAL_VARIABLES

It looks like most, if not all, of the global variables associated with TrackingCases, OpModes, and TrackingStates are no longer used. I left them in for now, but will go back through and remove unused vars when WallE3_Complete_V1 is finished.

SETUP():

SERIAL_PORTS Section:

Copied verbatim from ‘WallE3_AnomalyRecovery_V2’ – looks good

PIN_INITIALIZATION, SERIAL_PORTS, I2C_PORTS, MPU6050, VL53L0X_TEENSY Sections:

Copied all these verbatim from ‘WallE3_AnomalyRecovery_V2’ – these haven’t changed in literally years, so shouldn’t be an issue

LR_FRONT DISTANCE ARRAYS, #IFDEF DISTANCES_ONLY, IRDET_TEENSY, #IFDEF IR_HOMING_ONLY, IR_BEAM_STEERVAL_ARRAY, POST_CHECKS Sections:

Copied all these verbatim from ‘WallE3_AnomalyRecovery_V2’ – these haven’t changed in literally years, so shouldn’t be an issue. I did note, however, that the ‘POST_CHECKS’ section always runs as the ‘NO_POST’ #define isn’t used. Will leave as it is for now. Thinking a bit more about this – it seems that what I originally thought would be a potentially long, onerous, and not very useful POST hasn’t turned out that way. It is long and onerous, but very necessary, as it initializes and connects to all the peripheral equipment. So I think I will simply remove the #define NO_POST line, and rename the section that ‘ripples’ the rear LED’s from ‘#pragma POST_CHECKS’ to ‘#pragma FLASH_REAR_LEDS’

This complete the ‘setup()’ function – on to ‘loop()’!

Obviously, the contents of the loop() function varies widely across all the ‘part-task’ programs, so this will probably require a lot of work to ‘harmonize’ all the part-task features into a complete program. I think I will try to go through the ‘part-task’ programs in alpha order to see if I can pick out the salient features that should be included.

WallE3_AnomalyRecovery_V2:

loop() has just three main sections – IR_HOMING, CHARGING, and WALL_TRACKING. The first two above are specific operations associated with homing and connecting to the charging station, and the last one is very simple – it just calls either TrackRightWallOffset() or TrackLeftWallOffset().

WallE3_ChargingStn_V2:

This one has the same three sections as WallE3_AnomalyRecovery_V2 and AFAICT, they are identical.

WallE3_FrontBackMotionTuning_V1:

In addition to the same three sections as WallE3_AnomalyRecovery_V2, this one has ‘PARAMETER CAPTURE’ and ‘FRONT_BACK_MOTION_TEST’ sections. The ‘PARAMETER CAPTURE’ section captures test parameter value input from the user, and the ‘FRONT_BACK_MOTION_TEST’ section actually performs the motion with the user-entered parameters. So, we need to make sure that the actual functions used for testing are copied over and the global front/back motion PID values as well. From the ‘Move to a Specified Distance’ post, I see that the final PID values are (1.5, 0.1, 0.2), and these values were incorporated into the ‘WallE3_WallTrackTuning_V5’ as follows:

#pragma region FRONT_BACK OFFSET MOTION PID
//12/26/22 updated per https://www.fpaynter.com/2022/12/move-to-a-specified-distance-revisited/
const float OffsetDistKp = 1.5f; //neg value creates PID in REVERSE mode
const float OffsetDistKi = 0.1f;
const float OffsetDistKd = 0.2f;

float OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive
double OffsetDistVal = 0; //has to be 'double'
#pragma endregion FRONT_BACK OFFSET MOTION PID

#pragma region FRONT_BACK OFFSET MOTION PID

//12/26/22 updated per https://www.fpaynter.com/2022/12/move-to-a-specified-distance-revisited/

const float OffsetDistKp = 1.5f; //neg value creates PID in REVERSE mode

const float OffsetDistKi = 0.1f;

const float OffsetDistKd = 0.2f;

float OffsetDistOutput;//10/06/17 chg input variable name to something more descriptive

double OffsetDistVal = 0; //has to be 'double'

#pragma endregion FRONT_BACK OFFSET MOTION PID

Uh-Oh, trouble ahead! When I looked at the OffsetDistKx values copied into _Complete_V1 from WallE3_AnomalyRecovery_V2, I see:

#pragma region FRONT_BACK OFFSET MOTION PID
const float OffsetDistKp = 2.f; //neg value creates PID in REVERSE mode
const float OffsetDistKi = 0.5f;
const float OffsetDistKd = 0.0f;

#pragma region FRONT_BACK OFFSET MOTION PID

const float OffsetDistKp = 2.f; //neg value creates PID in REVERSE mode

const float OffsetDistKi = 0.5f;

const float OffsetDistKd = 0.0f;

so, which set of values is correct? The WallE3_AnomalyRecovery_V2 project was created 9/26/22, while the Move to a Specified Distance, Revisited post is dated 24 December 2022, so much more recent. We’ll at least start with the later PID values of (1.5,0.1,0.2)

Copied the following functions from WallE3_FrontBackMotionTuning_V1 into WallE3_Complete_V1:

bool MoveToDesiredFrontDistCm(uint16_t offsetCm)
bool MoveToDesiredLeftDistCm(uint16_t offsetCm)
bool MoveToDesiredRightDistCm(uint16_t offsetCm)
bool MoveToDesiredRearDistCm(uint16_t offsetCm)

WallE3_ParallelFind_V1:

This program has the same structure in setup – with a PARAMETER CAPTURE section followed by the actual test code, all in setup(). However, when I tested this for functionality, I realized it a) only addressed the ‘left wall tracking’ case, and b) didn’t work even for that case.

So, I spent some quality time with this ‘part-task’ program and got it working fairly well, for both cases. See this post for the details and testing results.

After getting everything working, I copied the RotateToParallelOrientation() function from ‘WallE3_ChargingStn_V2’ into ‘WallE3_Complete_V1’ (in the WALL_TRACK_SUPPORT section) and then replaced the actual code with the code from the latest ‘WallE3_ParallelFind_V1’ part-test program.

After making the copy, there were a number of compile errors due to needed utility functions not being present. From ‘WallE3_ChargingStn_V2’ I copied in the following functions:

Entire HDG_BASED_TURN_SUPPORT section
Entire DISTANCE_MEASUREMENT_SUPPORT section
Entire MOTOR_SUPPORT section
Entire IR_HOMING_SUPPORT section
Entire VL53L0X_SUPPORT section
UpdateAllEnvironmentParameters()
Entire CHARGE_SUPPORT_FUNCTIONS section
copied ‘float glLeftCentCorrCm;’ from WallE3_ParallelFind_V1
IsStuckAhead(), IsStuckBehind(), IsIRBeamAvail(), GetWallOrientDeg(), CorrDistForOrient()

At this point the program still doesn’t compile, but I am going to stop and continue with looking at each part-task program in order, and then I’ll come back to the task of getting ‘Complete’ to compile

WallE3_RollingTurn_V1:

Based on the results described in ‘WallE3 Rolling Turn, Revisited’, I copied the ‘RollingTurn()’ function verbatim into ‘Complete’, and also added the ‘TURN_RATE_UPDATE_INTERVAL_MSEC’ constant to the ‘TIME INTERVALS’ section.

WallE3_SpinTurnTuning_V2:

Based on the ‘WallE3 Spin Turn, Revisited’ post, it looks like the original ‘SpinTurn()’ function is unaffected, but with a different set of PID values. The ‘final’ PID value set is (0.7,0.3,0). I did a file compare of the SpinTurn() functions between the SpinTurnTuning_V2 and ChargingStn_V2 programs, and found that they are functionally identical (ChargingStn_V2 uses TeePrint, and SpinTurnTuning_V2 uses gl_SerPort). So, I copied the SpinTurnTuning_V2 versions of both the SpinTurn() functions (one with and one without Kp/Ki/Kd as input parameters) to ‘Complete_V1’, and copied the Kp,Ki, & Kd values from SpinTurnTuning to ‘Complete_V1’.

WallE3_WallTrack_V1-V5:

From this post I see the following changes through ‘WallE3_WallTrack_V1-V5 series of programs:

WallE3_WallTrack_V2 vs WallE3_WallTrack_V1 (Created: 2/19/2022)

V2 moved all inline tracking code into TrackLeft/RightWallOffset() functions (later ported back into V1 – don’t know why)
V2 changed all ‘double’ declarations to ‘float’ due to change from Mega2560 to T3.5

WallE3_WallTrack_V3 vs WallE3_WallTrack_V2 (Created: 2/22/2022)

V3 Chg left/right/rear dists from mm to cm
V3 Concentrated all environmental updates into UpdateAllEnvironmentParameters();
V3 No longer using GetOpMode()

WallE3_WallTrack_V4 vs WallE3_WallTrack_V3 (Created: 3/25/2022)

V4 Added ‘RollingForwardTurn() function

WallE3_WallTrack_V5 vs WallE3_WallTrack_V4 (Created: 3/25/2022)

No real changes between V5 & V4

I *think* that I copied most pieces in from WallE3_WallTrack_V2, so I’m going to go back through the entire program, looking for V2-V5 differences.

Loop():

I think I have everything above loop() accounted for. Now to try and make some sense of loop(). I need to be cognizant of ‘WallE3_ChargingStn_V2’, ‘WallE3_WallTrack_V5’ and ‘WallE3_AnomalyRecovery_V2’ versions of ‘loop()’. Going through all the programs, I see the following differences in loop().

ChargingStn_V2 adds ‘UpdateAllEnvironmentParameters()’ compared to WallE3_WallTrack_V5.
WallE3_AnomalyRecovery_V2 also adds ‘UpdateAllEnvironmentParameters()’ and in addition changes all ‘myTeePrint.’ to ‘gl_SerPort->’ compared to ChargingStn_V2. So, I copied the WallE3_AnomalyRecovery_V2 versions of IR_HOMING, CHARGING, and WALL_TRACKING into Compare’s loop() function.

So, at this point I have all of the ‘pre-setup’, setup(), and loop() stuff in properly (I hope). This should compile, but probably won’t, so I’ll need to go through the PITA part of figuring out why, and fixing it – oh well.

I got IR_HOMING and CHARGING working (well, at least compiling), and now I’m working on WALL_TRACKING. For this section I need to decide which version of TrackRight/LeftWallOffset to use (or at least start with).

WallE3_WallTrackTuning_V5 ended up with a very successful setup with PID(350,0,0) and offset divisor of 50, but it only addressed left-side wall tracking, and didn’t use the actual ‘TrackLeftWallOffset()’ function. So first we need to move the test code into ‘TrackLeftWallOffset()’, and then port ‘TrackLeftWallOffset()’ into ‘TrackRightWallOffset()’.

Here’s the tracking code from WallE3_WallTrackTuning_V5:

if (isRightWall)
{
  //TrackRightWallOffset(WallTrack_Kp, WallTrack_Ki, WallTrack_Kd, OffCm);
}
else
{
  float lastError = 0;
  float lastInput = 0;
  float lastIval = 0;
  float lastDerror = 0;
  AnomalyCode errcode = CheckForAnomalies();

  //WallTrackSetPoint = -0.35; //04/17/22 this value holds robot parallel to left wall
  WallTrackSetPoint = 0; //04/17/22 this value holds robot parallel to left wall

  //01/14/23 added to bypass offset capture section
  gl_pSerPort->printf("Press any key to start\n");

  gl_pSerPort->printf("Front\tCenter\tRear\tSteer\n");
  while (!gl_pSerPort->available())
  {
	gl_pSerPort->read(); //remove this character from stream
	GetRequestedVL53l0xValues(VL53L0X_LEFT);
	gl_pSerPort->printf("\n%2.2f\t%2.2f\t%2.2f\t%2.2f\n", glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glLeftSteeringVal);
	delay(500);
  }

  gl_pSerPort->printf("\n%s: TrackLeftWallOffset: Start tracking offset of %2.1f cm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n",
	__FILENAME__, OffCm, WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);
  gl_pSerPort->printf(LeftWallFollowTelemHdrStr);

  //Step4: Track the wall using PID algorithm
  mSecSinceLastWallTrackUpdate = 0; //added 09/04/21
  MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate

  gl_pSerPort->printf("%s: In TrackLeftWallOffset: before while with errcode = %s\n", __FILENAME__, AnomalyStrArray[errcode]);
  //gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorr\tOffCm\tOffF\tSet\tSteer\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");
  gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorr\tSteer\tTweak\tOffCm\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");
  while (errcode == NO_ANOMALIES && !gl_bIRBeamAvail)
  {
	CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

	if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)
	{
	  mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

	  //03/08/22 abstracted these calls to UpdateAllEnvironmentParameters()
	  UpdateAllEnvironmentParameters();//03/08/22 added to consolidate sensor update calls
	  errcode = CheckForAnomalies();

	  //from Teensy_7VL53L0X_I2C_Slave_V3.ino: LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post
	  glLeftCentCorrCm = CorrDistForOrient(glLeftCenterCm, glLeftSteeringVal);
	  //float offset_factor = (glLeftCentCorrCm - OffCm) / 10.f;
	  //float offset_factor = (glLeftCentCorrCm - OffCm) / 100.f; //01/28/23 from https://www.fpaynter.com/2023/01/walle3-wall-track-tuning-review/
	  float offset_factor = (glLeftCentCorrCm - OffCm) / tweak_divisor; //01/28/23 from https://www.fpaynter.com/2023/01/walle3-wall-track-tuning-review/
	  WallTrackSteerVal = glLeftSteeringVal + offset_factor;

	  ////update motor speeds, skipping bad values
	  ////if (!isnan(WallTrackSteerVal))
	  //if (abs(WallTrackSteerVal) < maxSteerVal)
	  //{
	  //  WallTrackOutput = PIDCalcs(glLeftCentCorrCm, OffCm, lastError, lastInput, lastIval, lastDerror,
	  //    WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);


	  if (!isnan(WallTrackSteerVal))
	  //if (!isnan(glLeftCentCorrCm))
	  {
		//gl_pSerPort->printf("just before PIDCalcs call: WallTrackSetPoint  = %2.2f\n", WallTrackSetPoint);
		//WallTrackOutput = PIDCalcs(glLeftCentCorrCm, OffCm, lastError, lastInput, lastIval, lastDerror,
		//  WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);
		WallTrackOutput = PIDCalcs(WallTrackSteerVal, 0, lastError, lastInput, lastIval, lastDerror,
		  WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);

		//04/05/22 have to use local var here, as result could be negative
		int16_t leftSpdnum = MOTOR_SPEED_QTR + WallTrackOutput;
		int16_t rightSpdnum = MOTOR_SPEED_QTR - WallTrackOutput;

		//04/05/22 Left/rightSpdnum can be negative here - watch out!
		rightSpdnum = (rightSpdnum <= MOTOR_SPEED_HALF) ? rightSpdnum : MOTOR_SPEED_HALF; //result can still be neg
		glRightspeednum = (rightSpdnum > 0) ? rightSpdnum : 0; //result here must be positive
		leftSpdnum = (leftSpdnum <= MOTOR_SPEED_HALF) ? leftSpdnum : MOTOR_SPEED_HALF;//result can still be neg
		glLeftspeednum = (leftSpdnum > 0) ? leftSpdnum : 0; //result here must be positive

		MoveAhead(glLeftspeednum, glRightspeednum);

		//gl_pSerPort->printf(WallFollowTelemStr, millis(),
		//  glLeftFrontCm, glLeftCenterCm, glLeftRearCm,
		//  glLeftSteeringVal, orientdeg, orientcos, glLeftCentCorrCm,
		//  WallTrackSetPoint, lastError, WallTrackOutput,
		//  glLeftspeednum, glRightspeednum);
	  }
	//gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorrtOffCm\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");
	//gl_pSerPort->printf("Msec\tLF\tLC\tLR\tLCCorr\tSteer\tTweak
	  gl_pSerPort->printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f",
		millis(), glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glLeftCentCorrCm, glLeftSteeringVal, offset_factor);

	  //\tOffCm\tOffF\tSet\tSteer\tErr           
	  gl_pSerPort->printf("\t%2.2f\t%2.2f",
		//OffCm, offset_factor, WallTrackSetPoint, WallTrackSteerVal, lastError);
		OffCm, lastError);
	  //\tLastI\tLastD\tOut\Lspd\tRspd\n"); 
	  gl_pSerPort->printf("\t%2.2f\t%2.2f\t%2.2f\t%d\t%d\n",
		lastIval, lastDerror, WallTrackOutput, glLeftspeednum, glRightspeednum);
	}
  }
}

100

101

102

103

104

105

106

107

if (isRightWall)

{

//TrackRightWallOffset(WallTrack_Kp, WallTrack_Ki, WallTrack_Kd, OffCm);

}

else

{

float lastError = 0;

float lastInput = 0;

float lastIval = 0;

float lastDerror = 0;

AnomalyCode errcode = CheckForAnomalies();

//WallTrackSetPoint = -0.35; //04/17/22 this value holds robot parallel to left wall

WallTrackSetPoint = 0; //04/17/22 this value holds robot parallel to left wall

//01/14/23 added to bypass offset capture section

gl_pSerPort->printf("Press any key to start\n");

gl_pSerPort->printf("Front\tCenter\tRear\tSteer\n");

while (!gl_pSerPort->available())

{

gl_pSerPort->read(); //remove this character from stream

GetRequestedVL53l0xValues(VL53L0X_LEFT);

gl_pSerPort->printf("\n%2.2f\t%2.2f\t%2.2f\t%2.2f\n", glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glLeftSteeringVal);

delay(500);

}

gl_pSerPort->printf("\n%s: TrackLeftWallOffset: Start tracking offset of %2.1f cm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n",

__FILENAME__, OffCm, WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);

gl_pSerPort->printf(LeftWallFollowTelemHdrStr);

//Step4: Track the wall using PID algorithm

mSecSinceLastWallTrackUpdate = 0; //added 09/04/21

MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate

gl_pSerPort->printf("%s: In TrackLeftWallOffset: before while with errcode = %s\n", __FILENAME__, AnomalyStrArray[errcode]);

//gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorr\tOffCm\tOffF\tSet\tSteer\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");

gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorr\tSteer\tTweak\tOffCm\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");

while (errcode == NO_ANOMALIES && !gl_bIRBeamAvail)

{

CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)

{

mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

//03/08/22 abstracted these calls to UpdateAllEnvironmentParameters()

UpdateAllEnvironmentParameters();//03/08/22 added to consolidate sensor update calls

errcode = CheckForAnomalies();

//from Teensy_7VL53L0X_I2C_Slave_V3.ino: LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

glLeftCentCorrCm = CorrDistForOrient(glLeftCenterCm, glLeftSteeringVal);

//float offset_factor = (glLeftCentCorrCm - OffCm) / 10.f;

//float offset_factor = (glLeftCentCorrCm - OffCm) / 100.f; //01/28/23 from https://www.fpaynter.com/2023/01/walle3-wall-track-tuning-review/

float offset_factor = (glLeftCentCorrCm - OffCm) / tweak_divisor; //01/28/23 from https://www.fpaynter.com/2023/01/walle3-wall-track-tuning-review/

WallTrackSteerVal = glLeftSteeringVal + offset_factor;

////update motor speeds, skipping bad values

////if (!isnan(WallTrackSteerVal))

//if (abs(WallTrackSteerVal) < maxSteerVal)

//{

// WallTrackOutput = PIDCalcs(glLeftCentCorrCm, OffCm, lastError, lastInput, lastIval, lastDerror,

// WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);

if (!isnan(WallTrackSteerVal))

//if (!isnan(glLeftCentCorrCm))

{

//gl_pSerPort->printf("just before PIDCalcs call: WallTrackSetPoint = %2.2f\n", WallTrackSetPoint);

//WallTrackOutput = PIDCalcs(glLeftCentCorrCm, OffCm, lastError, lastInput, lastIval, lastDerror,

// WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);

WallTrackOutput = PIDCalcs(WallTrackSteerVal, 0, lastError, lastInput, lastIval, lastDerror,

WallTrack_Kp, WallTrack_Ki, WallTrack_Kd);

//04/05/22 have to use local var here, as result could be negative

int16_t leftSpdnum = MOTOR_SPEED_QTR + WallTrackOutput;

int16_t rightSpdnum = MOTOR_SPEED_QTR - WallTrackOutput;

//04/05/22 Left/rightSpdnum can be negative here - watch out!

rightSpdnum = (rightSpdnum <= MOTOR_SPEED_HALF) ? rightSpdnum : MOTOR_SPEED_HALF; //result can still be neg

glRightspeednum = (rightSpdnum > 0) ? rightSpdnum : 0; //result here must be positive

leftSpdnum = (leftSpdnum <= MOTOR_SPEED_HALF) ? leftSpdnum : MOTOR_SPEED_HALF;//result can still be neg

glLeftspeednum = (leftSpdnum > 0) ? leftSpdnum : 0; //result here must be positive

MoveAhead(glLeftspeednum, glRightspeednum);

//gl_pSerPort->printf(WallFollowTelemStr, millis(),

// glLeftFrontCm, glLeftCenterCm, glLeftRearCm,

// glLeftSteeringVal, orientdeg, orientcos, glLeftCentCorrCm,

// WallTrackSetPoint, lastError, WallTrackOutput,

// glLeftspeednum, glRightspeednum);

}

//gl_pSerPort->printf("\nMsec\tLF\tLC\tLR\tLCCorrtOffCm\tErr\tLastI\tLastD\tOut\tLspd\tRspd\n");

//gl_pSerPort->printf("Msec\tLF\tLC\tLR\tLCCorr\tSteer\tTweak

gl_pSerPort->printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f",

millis(), glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glLeftCentCorrCm, glLeftSteeringVal, offset_factor);

//\tOffCm\tOffF\tSet\tSteer\tErr

gl_pSerPort->printf("\t%2.2f\t%2.2f",

//OffCm, offset_factor, WallTrackSetPoint, WallTrackSteerVal, lastError);

OffCm, lastError);

//\tLastI\tLastD\tOut\Lspd\tRspd\n");

gl_pSerPort->printf("\t%2.2f\t%2.2f\t%2.2f\t%d\t%d\n",

lastIval, lastDerror, WallTrackOutput, glLeftspeednum, glRightspeednum);

}

Comparing the above to TrackLeftWallOffset() from WallE3_WallTrack_V5….

WallE3_WallTrack_V5 has an extra ‘float spinRateDPS = 30;’ line for use in it’s now unneeded ‘OFFSET_CAPTURE’ section.
WallE3_WallTrackTuning_V5 adds ‘MsecSinceLastFrontDistUpdate = 0;’ and I think this is needed for the ‘final’ version.
WallE3_WallTrackTuning_V5 adds ‘&& !gl_bIRBeamAvail’ to the initial ‘while()’ statement
WallE3_WallTrackTuning_V5 computes the ‘offset_factor’ and adds it to WallTrackSteerVal.
The rest of the function is essentially the same, but WallE3_WallTrackTuning_V5 uses a custom inline telemetry output section rather than ‘PrintWallFollowTelemetry();’
WallE3_WallTrackTuning_V5 doesn’t have the line ‘digitalToggle(DURATION_MEASUREMENT_PIN2);’ for debug purposes – I should probably keep this.
WallE3_WallTrackTuning_V5 doesn’t have ‘HandleAnomalousConditions(errcode, TRACKING_LEFT);’ at the end. This should be kept as well.
WallE3_WallTrack_V5 uses ‘myTeePrint.’ instead of ‘gl_SerPort->’

So, I copied ‘TrackLeftWallOffset()’ to Complete_V1 and made the above changes. I only copied in the ‘parameterized’ version of ‘TrackLeftWallOffset()’ – the ‘parameterless’ version is no longer needed.

Removed ‘float spinRateDPS = 30;’
added ‘MsecSinceLastFrontDistUpdate = 0;’
Removed entire OFFSET_CAPTURE section
added ‘&& !gl_bIRBeamAvail’ to the initial ‘while()’ statement
Edited the PIDCalcs line to match WallTrackTuning_V5, except with ‘kp,ki,kd’ symbols instead of ‘WallTrack_Kp, WallTrack_Ki, WallTrack_Kd’
- Replaced ‘PrintWallFollowTelemetry() with custom telemetry printout
kept ‘digitalToggle(DURATION_MEASUREMENT_PIN2);’ for debug purposes
‘HandleAnomalousConditions(errcode, TRACKING_LEFT);’ at the end, and copied in the function code from WallE3_WallTrack_V5.

After finishing all this up and adding some required MISCELLANEOUS section functions, the ‘Complete_V1’ program compiles with only one error, as follows:

WallE3_Complete_V1.ino: 983:114: error: 'TrackRightWallOffset' was not declared in this scope

1	WallE3_Complete_V1.ino: 983:114: error: 'TrackRightWallOffset' was not declared in this scope

This error is due to the fact that I haven’t yet ported the WallE3_WallTrackTuning_V5 code to the ‘right side wall’ case. I think I’ll comment this out for now, until I can confirm that the robot can actually track the left side wall.

04 February 2023 Update:

Well, what a miracle! I commented out the call to ‘TrackRightWallOffset’, compiled the program, ran the left-side tracking test on my ‘two-break’ wall configuration, and it actually worked – YAY!! Here’s a short video showing the run.

Stay tuned!

Frank

St Micro VL53L5CX Study – Single Module Parallel Detection?

1 Reply

Posted 15 January 2023

I currently use two 3-element arrays of ST Micro’s VL53L0X ‘time of flight’ distance sensors to determine my robot’s orientation with respect to a nearby wall. In a reply to a recent post of mine on the ST Micro Forum, St Micro guru John Kvam suggested that a single VL53L5CX sensor might take the place of all three VL53L0X’s – wow!

So, I’m currently at a stopping place on my mainline robot work (managed to kill a Teensy 3.5, and new ones are a few days out), I decided to take some time to study how the VL53L5CX module performs in detecting the parallel orientation condition.

John was kind enough to send me some experimental code – albeit with the warning that ‘some study would be required’ to see how it all works, so I’ll be working my way through the example code while also looking through the VL53L5CX datasheet.

I started this adventure by downloading the Sparkfun VL53L5CX library and looking at some of the examples. I was going to buy a couple of the SparkFun breakout boards as I like supporting their library development, but then I saw that I was going to have to pay about $11 (about 1/3 the cost of a single module) just for shipping! Now I know I’ve lost a mental step or two over the last few decades, but even I know that shipping a postage-stamp sized module can be accomplished via USPS for a dollar or two at the most, so I have some issues with a 10X profit margin just for shipping. Talk about ‘hidden fees’! So instead I purchased ST Micro’s eval kit (which contains 2ea modules) from DigiKey for about the price of one Sparkfun module, and shipping was about 1/4 the Sparkfun price.

After receiving the modules, I hooked it up to a Teensy 3.5 as shown below. I wasn’t able to find any information about how to wire this unit to an Arduino (ST’s information assumes you have a NUCLEO board), so I had to kind of piece information together from multiple sources. I used Sparkfun’s schematic (I used 51K instead of 47K resistors) and connected PWREN (power enable) to 3.3V via a 51K.

ST Micro eval board for VL53L5CX ToF distance sensor

Then I loaded Sparkfun’s Example1 code, which simply prints out the values from all 64 zones in a timed loop. Here is some representative output:

	222	215	207	198	196	192	189	186
	204	197	190	184	183	177	175	171
	190	184	179	175	171	169	167	163
	175	171	168	163	163	159	157	150
	167	165	160	158	156	152	149	144
	155	155	151	148	145	145	140	133
	147	144	143	141	139	136	132	129
	140	135	135	131	130	126	125	123

	706	214	203	201	195	190	189	188
	206	194	191	184	184	177	174	172
	190	182	179	173	170	169	165	162
	179	171	168	163	162	158	157	151
	167	162	160	155	155	151	147	145
	157	154	153	145	145	143	140	134
	146	145	143	142	137	136	129	128
	141	136	134	132	129	124	125	121

	511	217	207	201	193	193	189	187
	205	197	192	183	183	179	175	172
	187	184	180	176	171	167	168	164
	176	172	169	163	161	158	155	150
	167	164	162	157	153	153	147	144
	157	153	152	146	146	139	140	134
	148	143	143	142	138	135	131	128
	142	137	135	131	130	126	124	123

	584	215	206	201	198	193	189	189
	206	193	189	182	182	179	174	170
	189	183	177	175	171	168	166	163
	177	172	167	165	163	159	155	150
	167	164	160	155	154	151	148	145
	156	155	150	146	146	142	140	132
	148	144	144	140	136	133	131	128
	142	136	134	134	129	125	126	122

222 215 207 198 196 192 189 186

204 197 190 184 183 177 175 171

190 184 179 175 171 169 167 163

175 171 168 163 163 159 157 150

167 165 160 158 156 152 149 144

155 155 151 148 145 145 140 133

147 144 143 141 139 136 132 129

140 135 135 131 130 126 125 123

706 214 203 201 195 190 189 188

206 194 191 184 184 177 174 172

190 182 179 173 170 169 165 162

179 171 168 163 162 158 157 151

167 162 160 155 155 151 147 145

157 154 153 145 145 143 140 134

146 145 143 142 137 136 129 128

141 136 134 132 129 124 125 121

511 217 207 201 193 193 189 187

205 197 192 183 183 179 175 172

187 184 180 176 171 167 168 164

176 172 169 163 161 158 155 150

167 164 162 157 153 153 147 144

157 153 152 146 146 139 140 134

148 143 143 142 138 135 131 128

142 137 135 131 130 126 124 123

584 215 206 201 198 193 189 189

206 193 189 182 182 179 174 170

189 183 177 175 171 168 166 163

177 172 167 165 163 159 155 150

167 164 160 155 154 151 148 145

156 155 150 146 146 142 140 132

148 144 144 140 136 133 131 128

142 136 134 134 129 125 126 122

Then I set up an experiment with a nice white planar surface (the shoebox my recent B-ball shoes came in) at a distance of about 16cm, and observed the output for +30º, 0º (parallel), and -30º orientation conditions, as shown in the plots and photos below:

For comparison purposes, all three plots above were plotted on the same scale (100-250). Looking at the blue (Series1) and brown (Series8) lines in each of these plots, it appears that in the +30º orientation, all Series1 points are well above all Series8 plots, and in the -30º orientation the opposite is true – all Series8 points are well above all Series1 points. In the parallel case, all eight series points are crammed together between 140 & 160mm. The physical distance between the sensor and my ‘plane’ (shoebox) was measured at about 165mm, pretty close to the sensor values in the parallel case. So, it looks like it should be pretty easy to discern parallel, +30º, and -30º conditions, and probably reasonable to estimate the +/-10º and +/-20º cases as well.

18 January 2023 Update:

Today it occurred to me that I didn’t really know how the VL53L5CX sensor would respond to ‘wall’ angle changes when oriented in the ‘other’ direction (with the sensor oriented vertical to the floor instead of horizontally as it was above). So, I re-oriented the plugboard in my vise with the sensor mounted vertically, and redid the above experiments, with the results as shown below:

From the above results, it appears that the ‘sensor vertical’ case is more suited to plane orientation measurement than the ‘sensor horizontal’ one. Not sure why this is, but the data is pretty clear.

19 January 2023 Update:

In another email exchange with John Kvam, he wasn’t happy with the fact that the ‘sensor horizontal’ and ‘sensor vertical’ results were different, as the beam transmit/receive geometry is a square pyramid. He thinks some of the beam might be missing the top of the ‘wall’ or hitting the floor, distorting the results (presumably of the ‘horizontal sensor’ results, as the ‘vertical sensor’ results look pretty clean.

So, to address this question, I did another set of runs with the ‘resolution’ parameter set to “4×4” rather than the default “8×8”, and redid the experiments with both ‘horizontal’ and ‘vertical’ sensor orientation, with the following results:

The ‘4×4’ plots above agree quite well with the original set of ‘8×8’ plots, in both shape and magnitude. However, they still show differences between the ‘horizontal’ and ‘vertical’ sensor orientations, which is not what John was expecting.

It occurred to me that the output from my little Sparkfun example is either a 4×4 or 8×8 array of sensor values, so I wasn’t sure what the ‘proper’ way of plotting this data was – I had just accepted Excel’s default which was to plot row by row. As an experiment, I tried switching the rows and columns, whereupon I got the following plots:

So it appears you can get the same result from both horizontal and vertical orientations, but you have to switch the row/column plotting order to get it. Another way to put it would be that you can select which plot behavior you want by selecting the row/column plotting order – interesting!

To recap this point, here’s a typical 4×4 output:

283	262	238	213
296	265	243	218
295	270	245	220
297	272	245	218

283 262 238 213

296 265 243 218

295 270 245 220

297 272 245 218

If this is plotted in normal row by row order, you get this plot:

But the same data, plotted in column by column order produces this plot:

Here’s the actual code that produces the output data:

#include <Wire.h>

#include <SparkFun_VL53L5CX_Library.h> //http://librarymanager/All#SparkFun_VL53L5CX

SparkFun_VL53L5CX myImager;
VL53L5CX_ResultsData measurementData; // Result data class structure, 1356 byes of RAM

int imageResolution = 0; //Used to pretty print output
int imageWidth = 0; //Used to pretty print output

void setup()
{
  Serial.begin(115200);
  delay(1000);
  Serial.println("SparkFun VL53L5CX Imager Example");

  Wire.begin(); //This resets to 100kHz I2C

  Serial.println("Initializing sensor board. This can take up to 10s. Please wait.");
  if (myImager.begin() == false)
  {
    Serial.println(F("Sensor not found - check your wiring. Freezing"));
    while (1);
  }

  //myImager.setResolution(8 * 8); //Enable all 64 pads
  myImager.setResolution(4 * 4); //01/18/23 rev to use 4x4 array

  imageResolution = myImager.getResolution(); //Query sensor for current resolution - either 4x4 or 8x8
  imageWidth = sqrt(imageResolution); //Calculate printing width

  myImager.startRanging();
}

void loop()
{
  //Poll sensor for new data
  if (myImager.isDataReady() == true)
  {
    if (myImager.getRangingData(&measurementData)) //Read distance data into array
    {
      //The ST library returns the data transposed from zone mapping shown in datasheet
      //Pretty-print data with increasing y, decreasing x to reflect reality
      for (int y = 0; y <= imageWidth * (imageWidth - 1); y += imageWidth)
      {
        for (int x = imageWidth - 1; x >= 0; x--)
        {
          Serial.print("\t");
          Serial.print(measurementData.distance_mm[x + y]);
        }
        Serial.println();
      }
      Serial.println();
    }
  }

  delay(5); //Small delay between polling
}

#include <Wire.h>

#include <SparkFun_VL53L5CX_Library.h> //http://librarymanager/All#SparkFun_VL53L5CX

SparkFun_VL53L5CX myImager;

VL53L5CX_ResultsData measurementData; // Result data class structure, 1356 byes of RAM

int imageResolution = 0; //Used to pretty print output

int imageWidth = 0; //Used to pretty print output

void setup()

{

Serial.begin(115200);

delay(1000);

Serial.println("SparkFun VL53L5CX Imager Example");

Wire.begin(); //This resets to 100kHz I2C

Serial.println("Initializing sensor board. This can take up to 10s. Please wait.");

if (myImager.begin() == false)

{

Serial.println(F("Sensor not found - check your wiring. Freezing"));

while (1);

}

//myImager.setResolution(8 * 8); //Enable all 64 pads

myImager.setResolution(4 * 4); //01/18/23 rev to use 4x4 array

imageResolution = myImager.getResolution(); //Query sensor for current resolution - either 4x4 or 8x8

imageWidth = sqrt(imageResolution); //Calculate printing width

myImager.startRanging();

}

void loop()

{

//Poll sensor for new data

if (myImager.isDataReady() == true)

{

if (myImager.getRangingData(&measurementData)) //Read distance data into array

{

//The ST library returns the data transposed from zone mapping shown in datasheet

//Pretty-print data with increasing y, decreasing x to reflect reality

for (int y = 0; y <= imageWidth * (imageWidth - 1); y += imageWidth)

{

for (int x = imageWidth - 1; x >= 0; x--)

{

Serial.print("\t");

Serial.print(measurementData.distance_mm[x + y]);

}

Serial.println();

}

Serial.println();

}

delay(5); //Small delay between polling

}

The line

    if (myImager.getRangingData(&measurementData)) //Read distance data into array

1	if (myImager.getRangingData(&measurementData)) //Read distance data into array

Reads the entire 4×4 or 8×8 array into memory, and then the doubly-indexed loop prints it out. This is all pretty straightforward, except for how the data is interpreted graphically. As the comments in the code indicate, the printout indexing arrangement is intended to flip the data to undo the physical transformation produced by the physical arrangement of sensor’s transmitter and receiver.

My use case, where all I want is the robot’s orientation angle w/r/t a nearby wall, I don’t really care if the data is transposed horizontally or vertically, as long as it stays the same from measurement to measurement. So, I’m beginning to think that I could simply select the array loading and/or plotting arrangement that gives me the ‘sloped line’ version, and derive the wall orientation from the slope of any of the 4 (or 8) lines

21 January 2023 Update:

After getting the SparkFun VL53L5CXA demo to work, I modified it to compare the first and last values from the first line of the 4×4 measurement array, and used the difference between these values to drive a small RC servo with an attached VL53L5CX module. Then I moved my ‘wall’ around to verify that the setup would, in fact, track the wall plane. Here’s the entire program:

/*
    Name:       VL53L5CX_ParallelFind_V1.ino
    Created:	1/20/2023 8:04:48 PM
    Author:     FRANKNEWXPS15\Frank

    Experiment to see if I can use a VL53L5CX to find the parallel orientation to a nearby wall
    Starts with the code from Sparkfun's VL53L5CX 'Example 1' demo program.

    Uses a VL53L5CX mounted horizontally on a servo motor.  The program seeks to equalize 
    sensor readings across a row.
*/


#include <Wire.h>

#include <SparkFun_VL53L5CX_Library.h> //http://librarymanager/All#SparkFun_VL53L5CX
#include <Servo.h>

Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

uint16_t servopos = 0;// variable to store the servo position

SparkFun_VL53L5CX myImager;
VL53L5CX_ResultsData measurementData; // Result data class structure, 1356 byes of RAM

int imageResolution = 0; //Used to pretty print output
int imageWidth = 0; //Used to pretty print output
bool bAutoMode = true;//used to break out of manual positioning
int16_t curServoPos = 0;
int16_t newServoPos = 0;

void setup()
{
  Serial.begin(115200);//baud rate ignored by Teensy
  delay(1000);
  Serial.println("SparkFun VL53L5CX Imager Example");

  Wire.begin(); //This resets to 100kHz I2C

  Serial.printf("%lu: Initializing sensor board. This can take up to 10s. Please wait\n", millis());
  if (myImager.begin() == false)
  {
    Serial.println(F("Sensor not found - check your wiring. Freezing"));
    while (1);
  }

  Serial.printf("%lu: Sensor ready!\n", millis());

  //myImager.setResolution(8 * 8); //Enable all 64 pads
  myImager.setResolution(4 * 4); //01/18/23 rev to use 4x4 array

  imageResolution = myImager.getResolution(); //Query sensor for current resolution - either 4x4 or 8x8
  imageWidth = sqrt(imageResolution); //Calculate printing width

  myImager.startRanging();
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  //myservo.write(servopos); // tell servo to go to position 0

  delay(15); // waits 15ms for the servo to reach the position
}

void loop()
{
  CheckForUserInput();

  //Poll sensor for new data
  if (myImager.isDataReady() == true)
  {
    if (myImager.getRangingData(&measurementData)) //Read distance data into array
    {
      //The ST library returns the data transposed from zone mapping shown in datasheet
      //Pretty-print data with increasing y, decreasing x to reflect reality
      for (int y = 0; y <= imageWidth * (imageWidth - 1); y += imageWidth)
      {
        for (int x = imageWidth - 1; x >= 0; x--)
        {
          Serial.print("\t");
          Serial.print(measurementData.distance_mm[x + y]);
        }
        Serial.println();
      }
      Serial.println();
    }
  }

  //now determine which way to move the servo to 'flatten' the line
  for (int x = imageWidth - 1; x >= 0; x--)
  {
    Serial.print("\t");
    Serial.print(measurementData.distance_mm[x]);
  }

  Serial.print("\n");


  //if( D[imageWidth - 1 + y=0] > D[0 + y=0] then must be '+' angle; rotate CCW
  //else rotate CW

  int16_t delta = measurementData.distance_mm[imageWidth - 1] - measurementData.distance_mm[0];
  Serial.printf("first = %d, last = %d, delta = %d\n",
    measurementData.distance_mm[imageWidth - 1], 
    measurementData.distance_mm[0], 
    delta);

  if (delta > 0)
  {
    newServoPos -= 10;
  }
  else if (delta < 0)
  {
    newServoPos += 10;
  }
  //if delta == 0, no change

  Serial.printf("current pos = %d, delta = %d, new pos = %d\n",curServoPos, delta, newServoPos);

  Serial.printf("Writing %d to servo\n", newServoPos);
  myservo.write(newServoPos);

  curServoPos = newServoPos;

  delay(500); //Small delay between polling
}

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

Name: VL53L5CX_ParallelFind_V1.ino

Created: 1/20/2023 8:04:48 PM

Author: FRANKNEWXPS15\Frank

Experiment to see if I can use a VL53L5CX to find the parallel orientation to a nearby wall

Starts with the code from Sparkfun's VL53L5CX 'Example 1' demo program.

Uses a VL53L5CX mounted horizontally on a servo motor. The program seeks to equalize

sensor readings across a row.

#include <Wire.h>

#include <SparkFun_VL53L5CX_Library.h> //http://librarymanager/All#SparkFun_VL53L5CX

#include <Servo.h>

Servo myservo; // create servo object to control a servo

// twelve servo objects can be created on most boards

uint16_t servopos = 0;// variable to store the servo position

SparkFun_VL53L5CX myImager;

VL53L5CX_ResultsData measurementData; // Result data class structure, 1356 byes of RAM

int imageResolution = 0; //Used to pretty print output

int imageWidth = 0; //Used to pretty print output

bool bAutoMode = true;//used to break out of manual positioning

int16_t curServoPos = 0;

int16_t newServoPos = 0;

void setup()

{

Serial.begin(115200);//baud rate ignored by Teensy

delay(1000);

Serial.println("SparkFun VL53L5CX Imager Example");

Wire.begin(); //This resets to 100kHz I2C

Serial.printf("%lu: Initializing sensor board. This can take up to 10s. Please wait\n", millis());

if (myImager.begin() == false)

{

Serial.println(F("Sensor not found - check your wiring. Freezing"));

while (1);

}

Serial.printf("%lu: Sensor ready!\n", millis());

//myImager.setResolution(8 * 8); //Enable all 64 pads

myImager.setResolution(4 * 4); //01/18/23 rev to use 4x4 array

imageResolution = myImager.getResolution(); //Query sensor for current resolution - either 4x4 or 8x8

imageWidth = sqrt(imageResolution); //Calculate printing width

myImager.startRanging();

myservo.attach(9); // attaches the servo on pin 9 to the servo object

//myservo.write(servopos); // tell servo to go to position 0

delay(15); // waits 15ms for the servo to reach the position

}

void loop()

{

CheckForUserInput();

//Poll sensor for new data

if (myImager.isDataReady() == true)

{

if (myImager.getRangingData(&measurementData)) //Read distance data into array

{

//The ST library returns the data transposed from zone mapping shown in datasheet

//Pretty-print data with increasing y, decreasing x to reflect reality

for (int y = 0; y <= imageWidth * (imageWidth - 1); y += imageWidth)

{

for (int x = imageWidth - 1; x >= 0; x--)

{

Serial.print("\t");

Serial.print(measurementData.distance_mm[x + y]);

}

Serial.println();

}

Serial.println();

}

//now determine which way to move the servo to 'flatten' the line

for (int x = imageWidth - 1; x >= 0; x--)

{

Serial.print("\t");

Serial.print(measurementData.distance_mm[x]);

}

Serial.print("\n");

//if( D[imageWidth - 1 + y=0] > D[0 + y=0] then must be '+' angle; rotate CCW

//else rotate CW

int16_t delta = measurementData.distance_mm[imageWidth - 1] - measurementData.distance_mm[0];

Serial.printf("first = %d, last = %d, delta = %d\n",

measurementData.distance_mm[imageWidth - 1],

measurementData.distance_mm[0],

delta);

if (delta > 0)

{

newServoPos -= 10;

}

else if (delta < 0)

{

newServoPos += 10;

}

//if delta == 0, no change

Serial.printf("current pos = %d, delta = %d, new pos = %d\n",curServoPos, delta, newServoPos);

Serial.printf("Writing %d to servo\n", newServoPos);

myservo.write(newServoPos);

curServoPos = newServoPos;

delay(500); //Small delay between polling

}

And here is a short video showing the results:

In the above, the servo is moved in 10deg steps to attempt to zero out the difference between the first and last distance measurements from the first row of the 4×4 array. The loop timing (500mSec/loop) is very slow and the algorithm is a simple as it gets, but it appears that this very simple algorithm actually works pretty well.

Next steps are to reduce the loop delay, and reduce the servo step size from 10deg to 1deg, or even use a step size proportional to the error term. Here’s another short video showing the tracking behavior with a 30Hz ranging frequency and a slightly more aggressive servo positioning algorithm.

So it appears clear that the VL53L5CX can be used to track a nearby wall, at least in this servo-enabled configuration. The remaining question is, can I substitute my robot’s wheel motors in the place of the above RC servo and get the same sort of ‘parallel tracking’ behavior as shown above. If not, then I might still be able to use a single sensor per side by mounting it on a servo as in the above configuration. The servo-mounted configuration might even be better, as the sensor would remain broadside to the nearby wall, even if the robot wasn’t. This might facilitate better ‘move to desired left/right distance’ performance because no distance compensation for orientation would be required, and better wall offset tracking for the same reason.

20 March 2023 Update:

As one result of my recent ‘field’ tests with ‘WallE3_Complete_V2’ (see this post), I discovered that the maximum distance capability (approximately 120cm) of my VL53L0X sensors was marginal for some of the tracking cases. In particular, when the robot passes an open doorway on the tracking side, it attempts to switch to the ‘other’ wall, if it can find one. However, If the robot is tracking the near wall at a 40cm offset, and the other wall is more than 120cm further away (160cm total), then the robot may or may not ‘see’ the other wall during an ‘open doorway’ event. I could probably address this issue by setting the tracking offset to 50cm vs 40cm, but even that might still be marginal. This problem is exacerbated by any robot orientation changes while tracking, as even a few degrees of ‘off-perpendicular’ orientation could cause the distance to the other wall to fall outside the sensor range – bummer.

The real solution to the above problem is to get a better side-distance sensor, just as I did by changing from the Pulsed Light LIDAR front distance sensor to the Garmin LIDAR-Lite V4/LED one. For the VL53L0X side sensor, the logical choice is the VL53L5CX sensor; not only does the -5CX sensor have more range (200-400cm), but as I was able to demonstrate above, only one sensor per side is required for orientation sensing. The downside is it will require quite a bit of reconfiguration of the second deck, both in terms of hardware and software – ugh!

To confirm the distance specs for the VL53L5CX sensor, I reconstructed the above circuit from my and made some tests to measure the maximum distance, as shown below:

Unfortunately, the tests showed that I wasn’t really going to get any additional range out of the VL53L5CX unit, and in addition range determination would be complicated by the fact that the field-of-view for the VL53L5CX is much broader than for the VL53L0X. This would mean that when mounted on WallE3’s second deck, the sensor would ‘see’ the floor as well as the target wall, giving skewed readings across the array. I think I would still be able to figure that out (i.e. only look at the SPADS on the bottom or top, whichever corresponds to ‘not floor’) but still a PITA. The big problem though, is that I no longer think the -5CX would be enough better to justify the expense (time and money and aggravation) of changing from the -0X. Bummer!

More to follow, stay tuned

Frank

WallE3 Wall Tracking, Revisited

WallE3_WallTrackTuning_V5:

I am going to try and make WallE3_WallTrackTuning_V5 as ‘clean’ as possible, removing as much ‘dead’ code as possible and consolidating things like sensor measurement intervals.

Timing intervals:

Searching through the code for ‘elapsedMillis’ objects, I see the following global declarations:

elapsedMillis MsecSinceLastTurnRateUpdate;//heading/rate based turn support
elapsedMillis mSecSinceLastWallTrackUpdate;
elapsedMillis MsecSinceLastDistUpdate; //01/07/22 used for local dist update loops
elapsedMillis MsecSinceLastFrontDistUpdate; //10/02/22 slower update rate needed for front LIDAR

elapsedMillis MsecSinceLastTurnRateUpdate;//heading/rate based turn support

elapsedMillis mSecSinceLastWallTrackUpdate;

elapsedMillis MsecSinceLastDistUpdate; //01/07/22 used for local dist update loops

elapsedMillis MsecSinceLastFrontDistUpdate; //10/02/22 slower update rate needed for front LIDAR

Then I did a search for “MSEC” all upper case and found:

//in 'Distance Measurement Support
const uint16_t MSEC_PER_DIST_UPDATE = 200; //12/10/22 can't go any faster than 200mSec - due to longer meas budg for VL53L0X accuracy

//In 'Wall Follow Support'
const uint16_t TURN_RATE_UPDATE_INTERVAL_MSEC = 30; //30 mSec is as fast as it can go reliably
const uint16_t WALL_TRACK_UPDATE_INTERVAL_MSEC = 200;
const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //10/02/22 added to separate out slow front LIDAR from faster VL53L0X sensors
const uint16_t ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC = 200;

//in 'Distance Measurement Support

const uint16_t MSEC_PER_DIST_UPDATE = 200; //12/10/22 can't go any faster than 200mSec - due to longer meas budg for VL53L0X accuracy

//In 'Wall Follow Support'

const uint16_t TURN_RATE_UPDATE_INTERVAL_MSEC = 30; //30 mSec is as fast as it can go reliably

const uint16_t WALL_TRACK_UPDATE_INTERVAL_MSEC = 200;

const uint16_t FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250; //10/02/22 added to separate out slow front LIDAR from faster VL53L0X sensors

const uint16_t ROTATE_TO_PARALLEL_TELEMETRY_SPACING_MSEC = 200;

The front LIDAR sensor starts to generate errors for long distance measurements when the measurement interval falls below 200mSec

The VL53L0X time-of-flight sensors need a ‘measurement time’ of 50mSec or greater. This is handled by the VL53L0X array Teensy, but it means that UpdateAllEnvironmentParameters() shouldn’t be called more frequently than 20HZ.

The MPU6050 can support an update interval of 30mSec or greater, and this time is used for all turning operations.

Telemetry readouts should occur no more than once every 200mSec.

MoveToDesiredFront/Back/Left/RightDistCm()

Based on my recent work on these functions, it looks like PID = (1.5, 0.1, 0.2) will work for all cases, so all I have to do is modify the existing ‘OffsetDistKp/Ki/Kd’ values. Note that in my testing these were parameters to the function call instead of program constants, but now I can go back to just having the desired offset as the only parameter.

So, I copied each of the above functions to WallE3_WallTrackTuning_V5 from WallE3_FrontBackMotionTuning_V1, removed Kp,Ki,Kd from the sig, and replaced all occurrences with OffsetDistKp/Ki/Kd. I also ported the CorrDistForOrient() function, as it is required by the MoveToDesiredLeft/Right() versions

01 January 2023 Update:

After getting the ‘MoveTo…’ functions working, I discovered that ‘RotateToParallelOrientation()’ didn’t work well at all, and in fact found a note from my former self that the function was ‘fatally flawed’ – oops! So, I revisited my ‘WallE3_ParallelFind_V1’ part-task project to see if I could get it to work better now that VL53L0X distances are being reported as float vs integer objects, and after what I hope is much better sensor error compensation. As shown in this post, RotateToParallelOrientation() now works much better, albeit somewhat slowly, with PID = (20,4,0).

Tracking Left Wall with offset =  50 and PID = (10.0,0.0,0.0)
Enter 's' key to start
Msec	LF	LC	LR	Steer
In CaptureWallOffset(Left, 50.0)
R/C/F dists = 16.6	14.2	15.3 Steerval = -0.130, WallAngle = -7.43, Inside
In CaptureWallOffset():turndeg = -37.4
Just before call to  MoveToDesiredLeftDistCm(50.0);

In MoveToDesiredLeftDistCm(50) with Kp/Ki/Kd = 1.5/0.1/0.2
	MTLD: at start, tgt = 50cm, corr_dist = 13.9, tgt_window = 2.0, front/rear var = 12985.0/790.4
Msec	Left	Steer	LCorr	TgtD	err	Ival	Dval	Out	Speed	FVar	RVar
44703	20.4	0.6	12.6	50	37.4	3.7	37.4	52.3	52	12702.1	754.1
44903	22.1	0.5	14.3	50	35.7	7.3	-1.7	61.1	61	12477.4	722.9
45103	25.7	0.6	15.9	50	34.1	10.7	-1.6	62.1	62	12309.2	687.0
45303	33.3	0.5	23.8	50	26.2	13.3	-7.9	54.2	54	12195.2	652.3
45503	40.9	0.5	28.5	50	21.5	15.5	-4.7	48.7	48	12133.4	616.8
45703	49.9	0.5	33.3	50	16.7	17.1	-4.8	43.1	43	12121.4	583.1
45903	54.3	0.4	44.4	50	5.6	17.7	-11.1	28.3	28	12156.6	555.0
46103	63.5	0.7	31.5	50	18.5	19.6	12.8	44.7	44	12236.3	527.4
46303	67.3	0.7	35.9	50	14.1	21.0	-4.4	42.9	42	12357.8	501.8
46503	79.8	0.3	65.9	50	-15.9	19.4	-30.0	1.5	1	12518.0	489.6
46703	79.8	0.2	72.2	50	-22.2	17.1	-6.3	-15.0	-14	12713.8	474.1
46903	80.6	0.3	66.6	50	-16.6	15.5	5.7	-10.5	-10	12941.9	470.6
47103	81.6	0.6	50.6	50	-0.6	15.4	16.0	11.4	11	13198.9	456.7
	MTLD: Stopped with left dist = 50.6 and errcode = NO_ANOMALIES

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
48315	103.8	70.3	66.2	0.4	0.0	-0.4	-1.6	-9.8	-9
48515	103.8	74.5	68.7	0.6	0.4	-0.6	-4.0	-15.6	-15
48715	103.8	72.2	68.5	0.4	0.6	-0.4	-5.4	-12.8	-12
48915	103.8	74.1	67.4	0.7	0.4	-0.7	-8.1	-21.5	-21
49115	103.8	69.9	67.5	0.2	0.7	-0.2	-9.1	-13.9	-13
49315	103.8	68.7	65.1	0.4	0.2	-0.4	-10.5	-17.7	-17
49515	103.8	70.5	65.9	0.5	0.4	-0.5	-12.4	-21.6	-21
49715	103.8	70.2	66.3	0.4	0.5	-0.4	-13.9	-21.7	-21
49915	103.8	70.3	65.0	0.5	0.4	-0.5	-16.0	-26.6	-26
50115	103.8	65.8	63.0	0.3	0.5	-0.3	-17.2	-22.8	-22
50315	103.8	64.4	60.9	0.3	0.3	-0.3	-18.6	-25.6	-25
50515	103.8	61.8	58.8	0.3	0.3	-0.3	-19.8	-25.8	-25
50715	103.8	60.1	56.0	0.4	0.3	-0.4	-21.4	-29.6	-29
50915	103.8	56.9	53.9	0.3	0.4	-0.3	-22.6	-28.6	-28
51115	103.8	56.3	53.8	0.2	0.3	-0.2	-23.6	-28.6	-28
51315	103.8	55.3	53.2	0.2	0.2	-0.2	-24.4	-28.6	-28
51515	103.8	55.5	52.2	0.3	0.2	-0.3	-25.8	-32.4	-32
51715	103.8	55.0	53.2	0.2	0.3	-0.2	-26.5	-30.1	-30
51915	103.8	56.6	55.2	0.1	0.2	-0.1	-27.0	-29.8	-29
52115	103.8	56.9	56.1	0.1	0.1	-0.1	-27.4	-29.0	-28
52315	103.8	58.3	58.5	-0.0	0.1	0.0	-27.3	-26.9	-26
Exited RTPO coarse tune with hdg/steerval/prev_steerval = 103.8/-0.0/0.1

Tracking Left Wall with offset = 50 and PID = (10.0,0.0,0.0)

Enter 's' key to start

Msec LF LC LR Steer

In CaptureWallOffset(Left, 50.0)

R/C/F dists = 16.6 14.2 15.3 Steerval = -0.130, WallAngle = -7.43, Inside

In CaptureWallOffset():turndeg = -37.4

Just before call to MoveToDesiredLeftDistCm(50.0);

In MoveToDesiredLeftDistCm(50) with Kp/Ki/Kd = 1.5/0.1/0.2

MTLD: at start, tgt = 50cm, corr_dist = 13.9, tgt_window = 2.0, front/rear var = 12985.0/790.4

Msec Left Steer LCorr TgtD err Ival Dval Out Speed FVar RVar

44703 20.4 0.6 12.6 50 37.4 3.7 37.4 52.3 52 12702.1 754.1

44903 22.1 0.5 14.3 50 35.7 7.3 -1.7 61.1 61 12477.4 722.9

45103 25.7 0.6 15.9 50 34.1 10.7 -1.6 62.1 62 12309.2 687.0

45303 33.3 0.5 23.8 50 26.2 13.3 -7.9 54.2 54 12195.2 652.3

45503 40.9 0.5 28.5 50 21.5 15.5 -4.7 48.7 48 12133.4 616.8

45703 49.9 0.5 33.3 50 16.7 17.1 -4.8 43.1 43 12121.4 583.1

45903 54.3 0.4 44.4 50 5.6 17.7 -11.1 28.3 28 12156.6 555.0

46103 63.5 0.7 31.5 50 18.5 19.6 12.8 44.7 44 12236.3 527.4

46303 67.3 0.7 35.9 50 14.1 21.0 -4.4 42.9 42 12357.8 501.8

46503 79.8 0.3 65.9 50 -15.9 19.4 -30.0 1.5 1 12518.0 489.6

46703 79.8 0.2 72.2 50 -22.2 17.1 -6.3 -15.0 -14 12713.8 474.1

46903 80.6 0.3 66.6 50 -16.6 15.5 5.7 -10.5 -10 12941.9 470.6

47103 81.6 0.6 50.6 50 -0.6 15.4 16.0 11.4 11 13198.9 456.7

MTLD: Stopped with left dist = 50.6 and errcode = NO_ANOMALIES

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

48315 103.8 70.3 66.2 0.4 0.0 -0.4 -1.6 -9.8 -9

48515 103.8 74.5 68.7 0.6 0.4 -0.6 -4.0 -15.6 -15

48715 103.8 72.2 68.5 0.4 0.6 -0.4 -5.4 -12.8 -12

48915 103.8 74.1 67.4 0.7 0.4 -0.7 -8.1 -21.5 -21

49115 103.8 69.9 67.5 0.2 0.7 -0.2 -9.1 -13.9 -13

49315 103.8 68.7 65.1 0.4 0.2 -0.4 -10.5 -17.7 -17

49515 103.8 70.5 65.9 0.5 0.4 -0.5 -12.4 -21.6 -21

49715 103.8 70.2 66.3 0.4 0.5 -0.4 -13.9 -21.7 -21

49915 103.8 70.3 65.0 0.5 0.4 -0.5 -16.0 -26.6 -26

50115 103.8 65.8 63.0 0.3 0.5 -0.3 -17.2 -22.8 -22

50315 103.8 64.4 60.9 0.3 0.3 -0.3 -18.6 -25.6 -25

50515 103.8 61.8 58.8 0.3 0.3 -0.3 -19.8 -25.8 -25

50715 103.8 60.1 56.0 0.4 0.3 -0.4 -21.4 -29.6 -29

50915 103.8 56.9 53.9 0.3 0.4 -0.3 -22.6 -28.6 -28

51115 103.8 56.3 53.8 0.2 0.3 -0.2 -23.6 -28.6 -28

51315 103.8 55.3 53.2 0.2 0.2 -0.2 -24.4 -28.6 -28

51515 103.8 55.5 52.2 0.3 0.2 -0.3 -25.8 -32.4 -32

51715 103.8 55.0 53.2 0.2 0.3 -0.2 -26.5 -30.1 -30

51915 103.8 56.6 55.2 0.1 0.2 -0.1 -27.0 -29.8 -29

52115 103.8 56.9 56.1 0.1 0.1 -0.1 -27.4 -29.0 -28

52315 103.8 58.3 58.5 -0.0 0.1 0.0 -27.3 -26.9 -26

Exited RTPO coarse tune with hdg/steerval/prev_steerval = 103.8/-0.0/0.1

Offset Capture with ‘RotateToParallelOrientation’ ‘at end

02 January 2023 Update:

Starting to make some full-up left wall tracking runs, using the updated code from earlier work. In particular, I am trying to see if my older idea about combining an offset-driven steering angle modifier for the PID tracking algorithm will work. The ‘offset_factor’ incorporates the distance error into the reported steering angle, which in turn is used in the PID machine to drive the combined steering angle to 0.

glLeftCentCorrCm = CorrDistForOrient(glLeftCenterCm, glLeftSteeringVal);
float offset_factor = (glLeftCentCorrCm - OffCm) / 10.f;
WallTrackSteerVal = glLeftSteeringVal + offset_factor;

WallTrackSteerVal = (WallTrackSteerVal > 0.5) ? 0.5 : WallTrackSteerVal;
WallTrackSteerVal = (WallTrackSteerVal < -0.5) ? -0.5 : WallTrackSteerVal;

glLeftCentCorrCm = CorrDistForOrient(glLeftCenterCm, glLeftSteeringVal);

float offset_factor = (glLeftCentCorrCm - OffCm) / 10.f;

WallTrackSteerVal = glLeftSteeringVal + offset_factor;

WallTrackSteerVal = (WallTrackSteerVal > 0.5) ? 0.5 : WallTrackSteerVal;

WallTrackSteerVal = (WallTrackSteerVal < -0.5) ? -0.5 : WallTrackSteerVal;

Here’s an early run:

This worked, ‘sorta’. Part of the problem with this run is the robot’s orientation with respect to the wall at the start of the run. This is supposed to be parallel with the wall, but it obviously isn’t, and I don’t know why. Here’s the data from the ‘RotateToParallelOrientation()’ step

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
108955	27.61	39.40	36.60	0.28	0.00	-0.28	-1.12	-6.72	-6
109155	27.61	40.30	37.10	0.32	0.28	-0.32	-2.40	-8.80	-8
109355	27.61	38.80	36.50	0.23	0.32	-0.23	-3.32	-7.92	-7
109555	27.61	39.50	36.00	0.35	0.23	-0.35	-4.72	-11.72	-11
109755	27.61	40.00	36.90	0.31	0.35	-0.31	-5.96	-12.16	-12
109955	27.61	40.00	37.30	0.27	0.31	-0.27	-7.04	-12.44	-12
110155	27.61	39.80	36.30	0.35	0.27	-0.35	-8.44	-15.44	-15
110355	27.61	40.20	37.00	0.32	0.35	-0.32	-9.72	-16.12	-16
110555	27.61	39.40	36.50	0.29	0.32	-0.29	-10.88	-16.68	-16
110755	27.61	40.30	36.50	0.38	0.29	-0.38	-12.40	-20.00	-20
110955	27.61	40.30	36.80	0.35	0.38	-0.35	-13.80	-20.80	-20
111155	27.61	40.30	35.80	0.45	0.35	-0.45	-15.60	-24.60	-24
111355	27.61	39.90	36.40	0.35	0.45	-0.35	-17.00	-24.00	-24
111555	27.61	38.90	36.00	0.29	0.35	-0.29	-18.16	-23.96	-23
111755	27.61	37.20	35.40	0.18	0.29	-0.18	-18.88	-22.48	-22
111955	27.61	37.70	35.80	0.15	0.18	-0.15	-19.48	-22.48	-22
112155	27.61	37.70	35.70	0.20	0.15	-0.20	-20.28	-24.28	-24
112355	27.61	38.20	36.00	0.22	0.20	-0.22	-21.16	-25.56	-25
112555	27.61	37.50	37.00	0.05	0.22	-0.05	-21.36	-22.36	-22
112755	27.61	38.20	37.70	0.05	0.05	-0.05	-21.56	-22.56	-22
112955	27.61	39.00	38.80	0.02	0.05	-0.02	-21.64	-22.04	-22
113155	27.61	39.40	39.70	-0.03	0.02	0.03	-21.52	-20.92	-20
Exited RTPO coarse tune with hdg/steerval/prev_steerval = 27.6/-0.0/0.0

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

108955 27.61 39.40 36.60 0.28 0.00 -0.28 -1.12 -6.72 -6

109155 27.61 40.30 37.10 0.32 0.28 -0.32 -2.40 -8.80 -8

109355 27.61 38.80 36.50 0.23 0.32 -0.23 -3.32 -7.92 -7

109555 27.61 39.50 36.00 0.35 0.23 -0.35 -4.72 -11.72 -11

109755 27.61 40.00 36.90 0.31 0.35 -0.31 -5.96 -12.16 -12

109955 27.61 40.00 37.30 0.27 0.31 -0.27 -7.04 -12.44 -12

110155 27.61 39.80 36.30 0.35 0.27 -0.35 -8.44 -15.44 -15

110355 27.61 40.20 37.00 0.32 0.35 -0.32 -9.72 -16.12 -16

110555 27.61 39.40 36.50 0.29 0.32 -0.29 -10.88 -16.68 -16

110755 27.61 40.30 36.50 0.38 0.29 -0.38 -12.40 -20.00 -20

110955 27.61 40.30 36.80 0.35 0.38 -0.35 -13.80 -20.80 -20

111155 27.61 40.30 35.80 0.45 0.35 -0.45 -15.60 -24.60 -24

111355 27.61 39.90 36.40 0.35 0.45 -0.35 -17.00 -24.00 -24

111555 27.61 38.90 36.00 0.29 0.35 -0.29 -18.16 -23.96 -23

111755 27.61 37.20 35.40 0.18 0.29 -0.18 -18.88 -22.48 -22

111955 27.61 37.70 35.80 0.15 0.18 -0.15 -19.48 -22.48 -22

112155 27.61 37.70 35.70 0.20 0.15 -0.20 -20.28 -24.28 -24

112355 27.61 38.20 36.00 0.22 0.20 -0.22 -21.16 -25.56 -25

112555 27.61 37.50 37.00 0.05 0.22 -0.05 -21.36 -22.36 -22

112755 27.61 38.20 37.70 0.05 0.05 -0.05 -21.56 -22.56 -22

112955 27.61 39.00 38.80 0.02 0.05 -0.02 -21.64 -22.04 -22

113155 27.61 39.40 39.70 -0.03 0.02 0.03 -21.52 -20.92 -20

Exited RTPO coarse tune with hdg/steerval/prev_steerval = 27.6/-0.0/0.0

This certainly looks good – with a front/rear distance difference of only 0.3cm, and a steering value of 0.02. However, as shown in the following screengrab of the above video, the robot’s orientation just after the parallel find operation is anything but parallel

movie frame grab just after ‘RotateToParallelOrientation()’

I re-instrumented the ‘RotateToParallelOrientation’ function to print out 10 sets of front/back distances directly after completing it’s ‘ParallelFind’ operation, and made another run. The photo below shows the ending orientation, followed by the data

Robot orientation immediately after ‘RotateToParallelOrientation()’

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
33846	36.67	54.60	49.30	0.53	0.00	-0.53	-2.12	-12.72	-12
34046	36.67	54.90	50.10	0.48	0.53	-0.48	-4.04	-13.64	-13
34246	36.67	55.40	50.60	0.48	0.48	-0.48	-5.96	-15.56	-15
34446	36.67	56.60	50.10	0.65	0.48	-0.65	-8.56	-21.56	-21
34646	36.67	53.80	49.20	0.46	0.65	-0.46	-10.40	-19.60	-19
34846	36.67	54.10	48.50	0.56	0.46	-0.56	-12.64	-23.84	-23
35046	36.67	51.10	47.60	0.35	0.56	-0.35	-14.04	-21.04	-21
35246	36.67	49.90	46.30	0.36	0.35	-0.36	-15.48	-22.68	-22
35446	36.67	48.20	45.10	0.31	0.36	-0.31	-16.72	-22.92	-22
35646	36.67	46.70	43.90	0.28	0.31	-0.28	-17.84	-23.44	-23
35846	36.67	45.30	42.30	0.30	0.28	-0.30	-19.04	-25.04	-25
36046	36.67	43.80	41.30	0.25	0.30	-0.25	-20.04	-25.04	-25
36246	36.67	42.80	41.20	0.16	0.25	-0.16	-20.68	-23.88	-23
36446	36.67	42.10	40.90	0.12	0.16	-0.12	-21.16	-23.56	-23
36646	36.67	41.90	39.70	0.22	0.12	-0.22	-22.04	-26.44	-26
36846	36.67	41.60	39.80	0.18	0.22	-0.18	-22.76	-26.36	-26
37046	36.67	40.90	40.40	0.05	0.18	-0.05	-22.96	-23.96	-23
37246	36.67	40.50	40.30	0.02	0.05	-0.02	-23.04	-23.44	-23
37446	36.67	41.80	40.80	0.10	0.02	-0.10	-23.44	-25.44	-25
37646	36.67	42.40	42.00	0.04	0.10	-0.04	-23.60	-24.40	-24
37846	36.67	42.50	43.30	-0.08	0.04	0.08	-23.28	-21.68	-21
Exited RTPO coarse tune with hdg/steerval/prev_steerval = 36.67/-0.08/0.04
37854	36.67	42.50	43.30	-0.08
38057	36.67	44.00	44.50	-0.05
38259	36.67	44.00	44.10	-0.01
38462	36.67	43.50	44.00	-0.05
38664	36.67	43.50	44.10	-0.06
38867	36.67	43.20	44.00	-0.08
39069	36.67	43.50	44.30	-0.08
39272	36.67	43.50	44.30	-0.08
39474	36.67	44.40	44.50	-0.01
39676	36.67	44.40	44.20	0.02

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

33846 36.67 54.60 49.30 0.53 0.00 -0.53 -2.12 -12.72 -12

34046 36.67 54.90 50.10 0.48 0.53 -0.48 -4.04 -13.64 -13

34246 36.67 55.40 50.60 0.48 0.48 -0.48 -5.96 -15.56 -15

34446 36.67 56.60 50.10 0.65 0.48 -0.65 -8.56 -21.56 -21

34646 36.67 53.80 49.20 0.46 0.65 -0.46 -10.40 -19.60 -19

34846 36.67 54.10 48.50 0.56 0.46 -0.56 -12.64 -23.84 -23

35046 36.67 51.10 47.60 0.35 0.56 -0.35 -14.04 -21.04 -21

35246 36.67 49.90 46.30 0.36 0.35 -0.36 -15.48 -22.68 -22

35446 36.67 48.20 45.10 0.31 0.36 -0.31 -16.72 -22.92 -22

35646 36.67 46.70 43.90 0.28 0.31 -0.28 -17.84 -23.44 -23

35846 36.67 45.30 42.30 0.30 0.28 -0.30 -19.04 -25.04 -25

36046 36.67 43.80 41.30 0.25 0.30 -0.25 -20.04 -25.04 -25

36246 36.67 42.80 41.20 0.16 0.25 -0.16 -20.68 -23.88 -23

36446 36.67 42.10 40.90 0.12 0.16 -0.12 -21.16 -23.56 -23

36646 36.67 41.90 39.70 0.22 0.12 -0.22 -22.04 -26.44 -26

36846 36.67 41.60 39.80 0.18 0.22 -0.18 -22.76 -26.36 -26

37046 36.67 40.90 40.40 0.05 0.18 -0.05 -22.96 -23.96 -23

37246 36.67 40.50 40.30 0.02 0.05 -0.02 -23.04 -23.44 -23

37446 36.67 41.80 40.80 0.10 0.02 -0.10 -23.44 -25.44 -25

37646 36.67 42.40 42.00 0.04 0.10 -0.04 -23.60 -24.40 -24

37846 36.67 42.50 43.30 -0.08 0.04 0.08 -23.28 -21.68 -21

Exited RTPO coarse tune with hdg/steerval/prev_steerval = 36.67/-0.08/0.04

37854 36.67 42.50 43.30 -0.08

38057 36.67 44.00 44.50 -0.05

38259 36.67 44.00 44.10 -0.01

38462 36.67 43.50 44.00 -0.05

38664 36.67 43.50 44.10 -0.06

38867 36.67 43.20 44.00 -0.08

39069 36.67 43.50 44.30 -0.08

39272 36.67 43.50 44.30 -0.08

39474 36.67 44.40 44.50 -0.01

39676 36.67 44.40 44.20 0.02

According to the photo, the robot is definitely not oriented parallel to the wall. However, according to the telemetry data, it is (44.4 front, 44.2 rear, steerval = 0.02). Even curioser, the actual physical measurements taken using a tape measure show that the front/rear distances are about 47/44cm, or a steerval of about 0.3! Something is definitely wrong here.

Uncommented the #DISTANCES_ONLY define and re-ran, with the robot position/orientation unchanged:

Msec	LFront	LCenter	LRear	RFront	RCenter	RRear	Rear	LSteer	RSteer	LIDAR
8150	44.60	46.00	44.40	56.00	58.70	785.60	91.30	0.020	-72.960	0.00
8302	43.30	45.80	44.10	56.00	58.40	785.60	82.30	-0.080	-72.960	0.00
8502	44.00	45.80	44.70	56.90	59.50	785.60	85.60	-0.070	-72.870	0.00
8702	43.50	45.70	44.10	56.90	59.80	785.60	86.10	-0.060	-72.870	0.00
8902	43.70	46.00	44.30	56.80	58.70	785.60	94.60	-0.060	-72.880	0.00
9102	44.20	46.00	45.10	56.90	60.10	785.60	93.10	-0.090	-72.870	0.00
9302	43.00	46.70	44.70	56.30	56.80	785.60	80.50	-0.170	-72.800	0.00
9502	44.10	45.60	45.20	56.70	59.90	785.60	91.60	-0.110	-72.890	0.00
9702	44.10	46.00	44.30	56.90	59.80	785.60	81.00	-0.070	-72.870	0.00
9902	44.50	45.70	44.40	56.10	60.40	785.60	93.00	0.010	-72.950	0.00
10102	44.60	46.20	44.40	57.00	58.20	785.60	82.80	0.020	-72.860	0.00
10302	43.50	45.70	45.30	57.70	57.20	785.60	91.10	-0.180	-72.790	0.00
10502	44.20	46.30	44.50	58.00	58.40	785.60	80.00	-0.030	-72.760	0.00
10702	42.60	47.00	45.30	57.40	59.60	785.60	90.40	-0.270	-72.820	0.00
10902	43.50	45.60	45.20	56.30	59.40	785.60	84.60	-0.170	-72.930	0.00
11102	44.00	46.60	45.00	56.20	58.90	72.60	92.10	-0.100	-1.640	0.00
11302	42.70	46.30	44.90	57.30	58.40	785.60	80.80	-0.220	-72.830	0.00
11502	44.00	45.70	44.80	56.00	58.20	785.60	93.90	-0.080	-72.960	0.00
11702	43.20	46.00	44.70	57.70	58.30	785.60	81.40	-0.150	-72.790	0.00

Msec	LFront	LCenter	LRear	RFront	RCenter	RRear	Rear	LSteer	RSteer	LIDAR
11902	43.50	46.00	44.80	56.90	58.60	785.60	96.80	-0.130	-72.870	0.00
12102	43.40	45.50	44.20	56.60	59.60	785.60	81.80	-0.080	-72.900	0.00
12302	44.30	45.50	45.30	57.10	59.40	785.60	88.40	-0.100	-72.850	0.00
12502	43.80	45.30	45.10	56.90	58.50	785.60	88.60	-0.130	-72.870	0.00
12702	43.70	46.30	44.90	57.70	60.50	785.60	85.20	-0.120	-72.830	0.00
12902	43.50	45.70	45.10	56.10	60.00	785.60	97.40	-0.160	-72.950	0.00
13102	43.50	46.20	44.50	55.90	61.10	785.60	84.60	-0.100	-72.970	0.00
13302	44.00	46.40	44.60	57.60	61.60	785.60	93.60	-0.060	-72.800	0.00
13502	43.40	46.80	44.40	56.40	59.00	785.60	81.60	-0.100	-72.920	0.00
13702	43.60	46.40	44.30	56.50	60.50	785.60	91.90	-0.070	-72.910	0.00
13902	43.20	46.20	45.50	56.10	60.10	785.60	81.70	-0.230	-72.950	0.00
14102	43.40	46.80	44.80	56.40	59.50	67.30	91.70	-0.140	-1.090	0.00
14302	43.50	46.10	45.10	57.00	58.40	785.60	80.60	-0.160	-72.860	0.00
14502	44.50	46.50	45.00	56.10	58.60	785.60	90.90	-0.050	-72.950	0.00

Msec LFront LCenter LRear RFront RCenter RRear Rear LSteer RSteer LIDAR

8150 44.60 46.00 44.40 56.00 58.70 785.60 91.30 0.020 -72.960 0.00

8302 43.30 45.80 44.10 56.00 58.40 785.60 82.30 -0.080 -72.960 0.00

8502 44.00 45.80 44.70 56.90 59.50 785.60 85.60 -0.070 -72.870 0.00

8702 43.50 45.70 44.10 56.90 59.80 785.60 86.10 -0.060 -72.870 0.00

8902 43.70 46.00 44.30 56.80 58.70 785.60 94.60 -0.060 -72.880 0.00

9102 44.20 46.00 45.10 56.90 60.10 785.60 93.10 -0.090 -72.870 0.00

9302 43.00 46.70 44.70 56.30 56.80 785.60 80.50 -0.170 -72.800 0.00

9502 44.10 45.60 45.20 56.70 59.90 785.60 91.60 -0.110 -72.890 0.00

9702 44.10 46.00 44.30 56.90 59.80 785.60 81.00 -0.070 -72.870 0.00

9902 44.50 45.70 44.40 56.10 60.40 785.60 93.00 0.010 -72.950 0.00

10102 44.60 46.20 44.40 57.00 58.20 785.60 82.80 0.020 -72.860 0.00

10302 43.50 45.70 45.30 57.70 57.20 785.60 91.10 -0.180 -72.790 0.00

10502 44.20 46.30 44.50 58.00 58.40 785.60 80.00 -0.030 -72.760 0.00

10702 42.60 47.00 45.30 57.40 59.60 785.60 90.40 -0.270 -72.820 0.00

10902 43.50 45.60 45.20 56.30 59.40 785.60 84.60 -0.170 -72.930 0.00

11102 44.00 46.60 45.00 56.20 58.90 72.60 92.10 -0.100 -1.640 0.00

11302 42.70 46.30 44.90 57.30 58.40 785.60 80.80 -0.220 -72.830 0.00

11502 44.00 45.70 44.80 56.00 58.20 785.60 93.90 -0.080 -72.960 0.00

11702 43.20 46.00 44.70 57.70 58.30 785.60 81.40 -0.150 -72.790 0.00

Msec LFront LCenter LRear RFront RCenter RRear Rear LSteer RSteer LIDAR

11902 43.50 46.00 44.80 56.90 58.60 785.60 96.80 -0.130 -72.870 0.00

12102 43.40 45.50 44.20 56.60 59.60 785.60 81.80 -0.080 -72.900 0.00

12302 44.30 45.50 45.30 57.10 59.40 785.60 88.40 -0.100 -72.850 0.00

12502 43.80 45.30 45.10 56.90 58.50 785.60 88.60 -0.130 -72.870 0.00

12702 43.70 46.30 44.90 57.70 60.50 785.60 85.20 -0.120 -72.830 0.00

12902 43.50 45.70 45.10 56.10 60.00 785.60 97.40 -0.160 -72.950 0.00

13102 43.50 46.20 44.50 55.90 61.10 785.60 84.60 -0.100 -72.970 0.00

13302 44.00 46.40 44.60 57.60 61.60 785.60 93.60 -0.060 -72.800 0.00

13502 43.40 46.80 44.40 56.40 59.00 785.60 81.60 -0.100 -72.920 0.00

13702 43.60 46.40 44.30 56.50 60.50 785.60 91.90 -0.070 -72.910 0.00

13902 43.20 46.20 45.50 56.10 60.10 785.60 81.70 -0.230 -72.950 0.00

14102 43.40 46.80 44.80 56.40 59.50 67.30 91.70 -0.140 -1.090 0.00

14302 43.50 46.10 45.10 57.00 58.40 785.60 80.60 -0.160 -72.860 0.00

14502 44.50 46.50 45.00 56.10 58.60 785.60 90.90 -0.050 -72.950 0.00

In the above data, the front distance varies from 42.6 to 44.6cm with an average of 43.7cm, and the rear distance varies from 44.1 to 45.5cm with an average of 44.8cm.

So the program thinks the front/back distances are closer together than the tape measure does (44.5/45.0 vs 47/44). This is a pretty big discrepancy. Rotating the robot to be physically parallel with front/rear distances = 40cm, I get:

Msec	LFront	LCenter	LRear	RFront	RCenter	RRear	Rear	LSteer	RSteer	LIDAR
287902	38.40	41.10	38.10	78.60	92.40	785.60	819.00	0.030	-70.700	0.00
288102	38.70	40.50	38.20	76.70	94.20	785.60	750.30	0.050	-70.890	0.00
288302	39.00	40.40	38.30	77.10	93.40	785.60	819.00	0.070	-70.850	0.00
288502	38.60	40.70	38.30	78.70	93.50	785.60	819.00	0.030	-70.690	0.00
288702	39.20	40.50	38.10	75.80	92.70	785.60	750.30	0.110	-70.750	0.00
288902	38.90	41.60	37.70	78.10	91.90	92.20	819.00	0.120	-1.410	0.00
289102	38.30	40.80	38.10	77.90	92.70	87.60	750.30	0.020	-0.970	0.00
289302	38.20	41.40	37.60	78.50	92.00	785.60	819.00	0.060	-70.710	0.00
289502	39.60	40.50	37.90	79.20	91.40	84.90	750.30	0.170	-0.570	0.00
289702	39.20	40.40	38.30	76.70	94.80	785.60	819.00	0.090	-70.890	0.00
289902	38.90	39.50	38.00	78.10	92.70	92.70	750.30	0.090	-1.460	0.00
290102	39.10	41.10	37.70	78.00	92.40	785.60	819.00	0.140	-70.760	0.00
290302	38.50	40.60	38.10	77.50	93.50	785.60	750.30	0.040	-70.810	0.00
290502	38.80	41.10	36.70	77.10	93.10	95.00	819.00	0.210	-1.790	0.00
290702	38.90	41.30	37.50	77.30	93.10	91.70	750.30	0.140	-1.440	0.00

Msec LFront LCenter LRear RFront RCenter RRear Rear LSteer RSteer LIDAR

287902 38.40 41.10 38.10 78.60 92.40 785.60 819.00 0.030 -70.700 0.00

288102 38.70 40.50 38.20 76.70 94.20 785.60 750.30 0.050 -70.890 0.00

288302 39.00 40.40 38.30 77.10 93.40 785.60 819.00 0.070 -70.850 0.00

288502 38.60 40.70 38.30 78.70 93.50 785.60 819.00 0.030 -70.690 0.00

288702 39.20 40.50 38.10 75.80 92.70 785.60 750.30 0.110 -70.750 0.00

288902 38.90 41.60 37.70 78.10 91.90 92.20 819.00 0.120 -1.410 0.00

289102 38.30 40.80 38.10 77.90 92.70 87.60 750.30 0.020 -0.970 0.00

289302 38.20 41.40 37.60 78.50 92.00 785.60 819.00 0.060 -70.710 0.00

289502 39.60 40.50 37.90 79.20 91.40 84.90 750.30 0.170 -0.570 0.00

289702 39.20 40.40 38.30 76.70 94.80 785.60 819.00 0.090 -70.890 0.00

289902 38.90 39.50 38.00 78.10 92.70 92.70 750.30 0.090 -1.460 0.00

290102 39.10 41.10 37.70 78.00 92.40 785.60 819.00 0.140 -70.760 0.00

290302 38.50 40.60 38.10 77.50 93.50 785.60 750.30 0.040 -70.810 0.00

290502 38.80 41.10 36.70 77.10 93.10 95.00 819.00 0.210 -1.790 0.00

290702 38.90 41.30 37.50 77.30 93.10 91.70 750.30 0.140 -1.440 0.00

When the robot is physically parallel, the reported front distance varies from 38.2 to 39.2cm with an average of 38.8cm, and the rear distance varies from 36.7 to 38.3cm with an average of 37.9. The left steering value varies from 0.02 (38.8/38.1) to 0.14 (38.9/37.5) with an average of 0.09.

Well, it looks like the average reported distances and steering values are pretty close to reality, so maybe my original calibration efforts aren’t entirely screwed up. However, it is abundantly clear at this point that my current ‘RotateToParallelOrientation()’ algorithm isn’t reliable, due to very noisy distance value measurements.

01/09/23 Update:

After getting ‘RotateToParallelOrientation()’ working better (now it just uses the array front/rear distance measurements to calculate the off-parallel angle, and then does a ‘SpinTurn’ by that amount), I resumed the effort (see the 02 January Update above) trying to determine if my older algorithm for combining the raw steering value with an ‘offset adjustment factor’ based on the robot’s distance from the desired offset distance would now work better given the improvements I have made in VL53L0X sensor error compensation and off-parallel distance measurement compensation.

As it turns out, the answer seems to be ‘no’. After a multitude of runs with my test wall set up for two 30-45deg ‘breaks’, I couldn’t find any set of PID values that would allow the robot to track the wall – it always either took off for parts unknown, or crashed into the wall at some point.

So, back to the original algorithm of using the wall offset distance directly in the PID engine.

11 January 2023 Update:

I’m confused – not an unusual state for me to be in – but still…..

After all the above improvements, I still was unable to produce reasonable tracking performance using either the steering value or the offset distance as the parameter to be controlled. And, even more confusing, I have an entire post dedicated to demonstrating successful wall tracking using the orientation-angle-corrected distance to the wall as the input to the PID engine, with the desired wall offset as the setpoint, as shown here:

WallTrackOutput = PIDCalcs(glLeftCentCorrCm, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror,
            kp, ki, kd);

1 2	WallTrackOutput = PIDCalcs(glLeftCentCorrCm, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror, kp, ki, kd);

With this algorithm, I settled on PID(3,0,1) as the best parameter set, with the result shown in this short video (copied from the above post):

Wall tracking using corrected distance measure as input, and desired offset as the set-point

And then, I have another post demonstrating that using the steering value as input and 0 as the setpoint also works, as shown in this short video with PID(300,0,300)

Right-side wall tracking using steering value as input with PID = (300,0,300)

Here’s the data and short video from a run on my longer ‘4 meter’ test range with two 30º breaks:

In TrackLeftWallOffset with Kp/Ki/Kd = 300.00	0.00	0.00
R/C/F dists = 136	117	127 Steerval = -0.090, CutAngle = 29.00
(cutAngleDeg > 0 && abs(cutAngleDeg) < 30 -- setting to +30
R/C/F dists = 136	117	127 Steerval = -0.090, SteerAngle = -5.14, CutAngle = 30.00
CutAngDeg > 0, SteerAngleDeg < cutAngleDeg
In SpinTurn(CW, 35.14, 30.00)
Init hdg = -0.08 deg, Turn = 35.14  deg, tgt = 35.06 deg, timeout = 2.34 sec

Msec	Hdg	PrvHdg	dHdg	Rate	tgtDPS	err	Kp*err	Ival	Kd*Derr	speed	Match	Slope
11361	0.05	-0.08	0.13	4.49	30.00	25.51	45.92	25.51	10.20	61	0.81	0.81
11391	1.28	0.05	1.23	40.88	30.00	-10.88	-19.58	14.63	-14.56	50	0.81	0.01
11421	3.31	1.28	2.03	67.70	30.00	-37.70	-67.86	0.00	-10.73	50	0.82	0.01
11451	5.52	3.31	2.22	73.85	30.00	-43.85	-78.94	0.00	-2.46	50	0.84	0.01
11481	7.87	5.52	2.34	78.13	30.00	-48.13	-86.64	0.00	-1.71	50	0.85	0.01
11511	10.30	7.87	2.43	81.05	30.00	-51.05	-91.89	0.00	-1.17	50	0.86	0.01
11541	12.83	10.30	2.53	84.47	30.00	-54.47	-98.05	0.00	-1.37	50	0.88	0.01
11571	15.41	12.83	2.58	85.85	30.00	-55.85	-100.54	0.00	-0.55	50	0.89	0.01
11601	18.03	15.41	2.62	87.32	30.00	-57.32	-103.18	0.00	-0.59	50	0.91	0.01
11631	20.70	18.03	2.67	89.17	30.00	-59.17	-106.50	0.00	-0.74	50	0.92	0.01
11661	23.36	20.70	2.66	88.50	30.00	-58.50	-105.30	0.00	0.27	50	0.93	0.01
11691	26.02	23.36	2.66	88.82	30.00	-58.82	-105.87	0.00	-0.13	50	0.95	0.01
11721	28.79	26.02	2.76	92.13	30.00	-62.13	-111.83	0.00	-1.32	50	0.97	0.02
11751	31.48	28.79	2.69	89.76	30.00	-59.76	-107.57	0.00	0.94	50	0.98	0.01
11781	34.20	31.48	2.72	90.74	30.00	-60.74	-109.33	0.00	-0.39	50	1.00	0.02
Completed turn with yaw = 34.20, tgt = 35.06, and match = 0.995
fudgeFactorMM = -100

at approach start: cut angle = 30.000, adjfactor = 0.866, num = 400.00, tgt dist = 361.88
At start - err = prev_err = -138
R/C/F dists = 156	208	223 Steerval = 0.670, SteerAngle = 38.29, CutAngle = 30.00, err/prev_err = -138
Msec	LFront	LCtr	LRear	Front	Rear	Err	P_err
13913	228	206	155	170	206	-133	-138
14020	250	231	179	173	243	-111	-133
14127	312	310	237	168	304	-49	-111
14234	381	395	313	159	393	19	-49
At end of offset capture - prev_res*res = -931
correct back to parallel
In SpinTurn(CCW, 30.00, 30.00)
Init hdg = 45.41 deg, Turn = 30.00  deg, tgt = 15.41 deg, timeout = 2.00 sec

Msec	Hdg	PrvHdg	dHdg	Rate	tgtDPS	err	Kp*err	Ival	Kd*Derr	speed	Match	Slope
14294	45.26	45.41	-0.15	5.12	30.00	24.88	44.79	24.88	9.95	59	0.83	0.83
14324	44.29	45.26	-0.98	32.52	30.00	-2.52	-4.53	22.37	-10.96	50	0.84	0.01
14354	42.61	44.29	-1.68	55.95	30.00	-25.95	-46.71	0.00	-9.37	50	0.85	0.01
14384	40.64	42.61	-1.96	65.48	30.00	-35.48	-63.87	0.00	-3.81	50	0.86	0.01
14414	38.30	40.64	-2.34	78.08	30.00	-48.08	-86.55	0.00	-5.04	50	0.87	0.01
14444	35.51	38.30	-2.79	92.90	30.00	-62.90	-113.22	0.00	-5.93	50	0.89	0.02
14474	32.31	35.51	-3.21	106.88	30.00	-76.88	-138.39	0.00	-5.59	50	0.91	0.02
14504	28.88	32.31	-3.42	114.15	30.00	-84.15	-151.47	0.00	-2.91	50	0.93	0.02
14534	25.25	28.88	-3.64	121.22	30.00	-91.22	-164.19	0.00	-2.83	50	0.95	0.02
14564	21.33	25.25	-3.91	130.41	30.00	-100.41	-180.74	0.00	-3.68	50	0.97	0.02
14594	17.50	21.33	-3.84	127.92	30.00	-97.92	-176.26	0.00	1.00	50	0.99	0.02
14624	13.63	17.50	-3.87	128.94	30.00	-98.94	-178.09	0.00	-0.41	50	0.99	0.00
Completed turn with yaw = 13.63, tgt = 15.41, and match = 0.990

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	Fdir	Cdir	Rdir	Steer	Set	error	Ival	Kp*err	Ki*Ival	Kd*Din	Output	Lspd	Rspd
15692 	389	416	377 	0.14	0.00	-0.14	0.00 	-40.80	0.00	-0.00 	-40.80	34	115
15742 	390	404	377 	0.13	0.00	-0.13	0.00 	-40.20	0.00	0.00 	-40.20	34	115
15792 	391	405	371 	0.20	0.00	-0.20	0.00 	-61.50	0.00	-0.00 	-61.50	13	136
15842 	386	412	378 	0.09	0.00	-0.09	0.00 	-27.60	0.00	0.00 	-27.60	47	102
15892 	386	412	378 	0.09	0.00	-0.09	0.00 	-27.60	0.00	0.00 	-27.60	47	102
15942 	386	405	383 	0.04	0.00	-0.04	0.00 	-10.50	0.00	0.00 	-10.50	64	85
15992 	386	413	383 	0.04	0.00	-0.04	0.00 	-12.90	0.00	-0.00 	-12.90	62	87
16042 	388	407	381 	0.08	0.00	-0.08	0.00 	-23.10	0.00	-0.00 	-23.10	51	98
16092 	381	411	388 	-0.06	0.00	0.06	0.00 	17.70	0.00	0.00 	17.70	92	57
16142 	370	403	381 	-0.11	0.00	0.11	0.00 	32.10	0.00	0.00 	32.10	107	42
16192 	376	394	380 	-0.05	0.00	0.05	0.00 	13.80	0.00	-0.00 	13.80	88	61
16242 	366	384	374 	-0.10	0.00	0.10	0.00 	28.80	0.00	0.00 	28.80	103	46
16292 	362	384	374 	-0.10	0.00	0.10	0.00 	28.80	0.00	0.00 	28.80	103	46
16342 	362	375	370 	-0.10	0.00	0.10	0.00 	31.50	0.00	0.00 	31.50	106	43
16392 	351	372	357 	-0.09	0.00	0.09	0.00 	26.40	0.00	-0.00 	26.40	101	48
16442 	337	364	346 	-0.13	0.00	0.13	0.00 	37.80	0.00	0.00 	37.80	112	37
16492 	332	352	321 	0.06	0.00	-0.06	0.00 	-18.60	0.00	-0.00 	-18.60	56	93
16542 	325	336	319 	-0.00	0.00	0.00	0.00 	1.20	0.00	0.00 	1.20	76	73
16592 	315	323	308 	-0.01	0.00	0.01	0.00 	2.10	0.00	0.00 	2.10	77	72
16642 	310	324	300 	0.02	0.00	-0.02	0.00 	-7.20	0.00	-0.00 	-7.20	67	82
16692 	310	314	301 	0.00	0.00	-0.00	0.00 	-1.20	0.00	0.00 	-1.20	73	76
16742 	302	304	293 	-0.01	0.00	0.01	0.00 	1.80	0.00	0.00 	1.80	76	73
16792 	294	301	290 	-0.06	0.00	0.06	0.00 	17.70	0.00	0.00 	17.70	92	57
16842 	289	295	288 	-0.09	0.00	0.09	0.00 	28.50	0.00	0.00 	28.50	103	46
16892 	283	289	279 	-0.07	0.00	0.07	0.00 	21.30	0.00	-0.00 	21.30	96	53
16942 	283	287	275 	-0.03	0.00	0.03	0.00 	9.90	0.00	-0.00 	9.90	84	65
16992 	269	277	277 	-0.20	0.00	0.20	0.00 	60.90	0.00	0.00 	60.90	135	14
17042 	270	269	272 	-0.15	0.00	0.15	0.00 	45.30	0.00	-0.00 	45.30	120	29
17092 	263	268	255 	-0.05	0.00	0.05	0.00 	15.60	0.00	-0.00 	15.60	90	59
17142 	257	261	253 	-0.06	0.00	0.06	0.00 	17.70	0.00	0.00 	17.70	92	57
17192 	243	253	237 	-0.09	0.00	0.09	0.00 	26.10	0.00	0.00 	26.10	101	48
17242 	250	248	243 	-0.08	0.00	0.08	0.00 	24.60	0.00	-0.00 	24.60	99	50
17292 	241	249	238 	-0.12	0.00	0.12	0.00 	36.30	0.00	0.00 	36.30	111	38
17342 	259	255	234 	0.11	0.00	-0.11	0.00 	-31.50	0.00	-0.00 	-31.50	43	106
17392 	270	261	237 	0.19	0.00	-0.19	0.00 	-57.30	0.00	-0.00 	-57.30	17	132
17442 	273	269	245 	0.15	0.00	-0.15	0.00 	-44.70	0.00	0.00 	-44.70	30	119
17492 	287	282	245 	0.30	0.00	-0.30	0.00 	-90.60	0.00	-0.00 	-90.60	0	165
17542 	293	293	256 	0.26	0.00	-0.26	0.00 	-78.90	0.00	0.00 	-78.90	0	153
17592 	299	288	260 	0.28	0.00	-0.28	0.00 	-83.40	0.00	-0.00 	-83.40	0	158
17642 	302	292	272 	0.19	0.00	-0.19	0.00 	-57.60	0.00	0.00 	-57.60	17	132
17692 	300	292	272 	0.17	0.00	-0.17	0.00 	-51.60	0.00	0.00 	-51.60	23	126
17742 	302	290	276 	0.15	0.00	-0.15	0.00 	-45.00	0.00	0.00 	-45.00	30	120
17792 	288	306	282 	-0.03	0.00	0.03	0.00 	10.20	0.00	0.00 	10.20	85	64
17842 	298	305	278 	0.11	0.00	-0.11	0.00 	-31.50	0.00	-0.00 	-31.50	43	106
17892 	302	300	281 	0.11	0.00	-0.11	0.00 	-33.00	0.00	-0.00 	-33.00	42	108
17942 	304	305	288 	0.06	0.00	-0.06	0.00 	-19.50	0.00	0.00 	-19.50	55	94
17992 	302	296	292 	-0.00	0.00	0.00	0.00 	1.20	0.00	0.00 	1.20	76	73
18042 	294	307	288 	-0.03	0.00	0.03	0.00 	9.90	0.00	0.00 	9.90	84	65
18092 	301	307	285 	0.07	0.00	-0.07	0.00 	-20.10	0.00	-0.00 	-20.10	54	95
18142 	291	299	289 	-0.08	0.00	0.08	0.00 	24.30	0.00	0.00 	24.30	99	50
18192 	294	297	286 	-0.02	0.00	0.02	0.00 	6.90	0.00	-0.00 	6.90	81	68
18242 	283	300	283 	-0.10	0.00	0.10	0.00 	30.00	0.00	0.00 	30.00	105	45
18292 	290	274	290 	-0.13	0.00	0.13	0.00 	37.80	0.00	0.00 	37.80	112	37
18342 	280	281	277 	-0.09	0.00	0.09	0.00 	26.70	0.00	-0.00 	26.70	101	48
18392 	272	279	269 	-0.09	0.00	0.09	0.00 	27.30	0.00	0.00 	27.30	102	47
18442 	272	267	260 	-0.01	0.00	0.01	0.00 	3.90	0.00	-0.00 	3.90	78	71
18492 	269	275	267 	-0.11	0.00	0.11	0.00 	31.50	0.00	0.00 	31.50	106	43
18542 	268	274	264 	-0.09	0.00	0.09	0.00 	25.80	0.00	-0.00 	25.80	100	49
18592 	267	271	263 	-0.09	0.00	0.09	0.00 	26.70	0.00	0.00 	26.70	101	48
18642 	272	267	257 	0.02	0.00	-0.02	0.00 	-5.10	0.00	-0.00 	-5.10	69	80
18692 	269	262	248 	0.07	0.00	-0.07	0.00 	-21.60	0.00	-0.00 	-21.60	53	96
18742 	268	267	254 	0.01	0.00	-0.01	0.00 	-2.10	0.00	0.00 	-2.10	72	77
18792 	268	276	247 	0.09	0.00	-0.09	0.00 	-25.80	0.00	-0.00 	-25.80	49	100
18842 	271	267	258 	-0.00	0.00	0.00	0.00 	0.90	0.00	0.00 	0.90	75	74
18892 	264	270	256 	-0.05	0.00	0.05	0.00 	15.00	0.00	0.00 	15.00	90	60
18942 	272	268	248 	0.11	0.00	-0.11	0.00 	-32.40	0.00	-0.00 	-32.40	42	107
18992 	258	272	256 	-0.11	0.00	0.11	0.00 	32.40	0.00	0.00 	32.40	107	42
19042 	266	264	251 	0.01	0.00	-0.01	0.00 	-4.20	0.00	-0.00 	-4.20	70	79
19092 	262	265	253 	-0.05	0.00	0.05	0.00 	13.50	0.00	0.00 	13.50	88	61
19142 	265	274	253 	-0.01	0.00	0.01	0.00 	1.80	0.00	-0.00 	1.80	76	73
19192 	272	266	259 	-0.00	0.00	0.00	0.00 	1.20	0.00	-0.00 	1.20	76	73
19242 	268	273	264 	-0.09	0.00	0.09	0.00 	26.10	0.00	0.00 	26.10	101	48
19292 	274	279	263 	-0.01	0.00	0.01	0.00 	3.30	0.00	-0.00 	3.30	78	71
19342 	272	274	258 	0.01	0.00	-0.01	0.00 	-4.20	0.00	-0.00 	-4.20	70	79
19392 	280	276	269 	-0.01	0.00	0.01	0.00 	4.20	0.00	0.00 	4.20	79	70
19442 	282	283	264 	0.06	0.00	-0.06	0.00 	-18.90	0.00	-0.00 	-18.90	56	93
19492 	275	276	266 	-0.03	0.00	0.03	0.00 	10.20	0.00	0.00 	10.20	85	64
19542 	287	285	265 	0.10	0.00	-0.10	0.00 	-31.50	0.00	-0.00 	-31.50	43	106
19592 	287	285	265 	0.10	0.00	-0.10	0.00 	-31.50	0.00	0.00 	-31.50	43	106
19642 	286	286	267 	0.05	0.00	-0.05	0.00 	-13.80	0.00	0.00 	-13.80	61	88
19692 	289	281	267 	0.10	0.00	-0.10	0.00 	-30.30	0.00	-0.00 	-30.30	44	105
19742 	283	296	274 	-0.01	0.00	0.01	0.00 	4.20	0.00	0.00 	4.20	79	70
19792 	293	290	271 	0.11	0.00	-0.11	0.00 	-33.00	0.00	-0.00 	-33.00	42	108
19842 	289	290	284 	-0.06	0.00	0.06	0.00 	18.00	0.00	0.00 	18.00	93	57
19892 	293	284	275 	0.06	0.00	-0.06	0.00 	-19.20	0.00	-0.00 	-19.20	55	94
19942 	294	283	277 	0.05	0.00	-0.05	0.00 	-15.90	0.00	0.00 	-15.90	59	90
19992 	284	285	272 	0.00	0.00	-0.00	0.00 	-1.50	0.00	0.00 	-1.50	73	76
20042 	288	284	278 	-0.02	0.00	0.02	0.00 	4.80	0.00	0.00 	4.80	79	70
20092 	284	279	278 	-0.06	0.00	0.06	0.00 	18.30	0.00	0.00 	18.30	93	56
20142 	291	290	280 	0.00	0.00	0.00	0.00 	0.00	0.00	-0.00 	0.00	75	75
20192 	285	284	281 	-0.08	0.00	0.08	0.00 	22.80	0.00	0.00 	22.80	97	52
20242 	291	290	276 	0.04	0.00	-0.04	0.00 	-12.00	0.00	-0.00 	-12.00	63	87
20292 	285	286	285 	-0.11	0.00	0.11	0.00 	34.20	0.00	0.00 	34.20	109	40
20342 	268	268	282 	-0.27	0.00	0.27	0.00 	81.60	0.00	0.00 	81.60	156	0
20392 	248	246	276 	-0.43	0.00	0.43	0.00 	130.20	0.00	0.00 	130.20	200	0
20442 	248	246	276 	-0.43	0.00	0.43	0.00 	130.20	0.00	0.00 	130.20	200	0
20492 	227	236	253 	-0.42	0.00	0.42	0.00 	127.20	0.00	-0.00 	127.20	200	0
20542 	215	209	237 	-0.41	0.00	0.41	0.00 	123.30	0.00	-0.00 	123.30	198	0
20592 	198	204	222 	-0.44	0.00	0.44	0.00 	130.80	0.00	0.00 	130.80	200	0
20642 	185	181	198 	-0.35	0.00	0.35	0.00 	104.70	0.00	-0.00 	104.70	179	0
20692 	171	160	190 	-0.43	0.00	0.43	0.00 	129.00	0.00	0.00 	129.00	200	0
20742 	161	143	173 	-0.38	0.00	0.38	0.00 	113.10	0.00	-0.00 	113.10	188	0
20792 	151	127	158 	-0.34	0.00	0.34	0.00 	102.90	0.00	-0.00 	102.90	177	0
20842 	140	126	149 	-0.36	0.00	0.36	0.00 	109.20	0.00	0.00 	109.20	184	0
20892 	140	126	141 	-0.28	0.00	0.28	0.00 	85.20	0.00	-0.00 	85.20	160	0
20942 	152	129	142 	-0.17	0.00	0.17	0.00 	51.30	0.00	-0.00 	51.30	126	23
20992 	152	129	142 	-0.17	0.00	0.17	0.00 	51.30	0.00	0.00 	51.30	126	23
21042 	169	136	144 	-0.01	0.00	0.01	0.00 	4.20	0.00	-0.00 	4.20	79	70
21092 	186	156	149 	0.13	0.00	-0.13	0.00 	-37.80	0.00	-0.00 	-37.80	37	112
21142 	201	176	164 	0.15	0.00	-0.15	0.00 	-43.80	0.00	-0.00 	-43.80	31	118
21192 	230	223	181 	0.31	0.00	-0.31	0.00 	-93.90	0.00	-0.00 	-93.90	0	168
21242 	255	230	199 	0.39	0.00	-0.39	0.00 	-117.00	0.00	-0.00 	-117.00	0	192
21292 	261	254	216 	0.30	0.00	-0.30	0.00 	-91.20	0.00	0.00 	-91.20	0	166
21342 	275	260	227 	0.34	0.00	-0.34	0.00 	-102.00	0.00	-0.00 	-102.00	0	177
21392 	274	260	227 	0.34	0.00	-0.34	0.00 	-102.00	0.00	0.00 	-102.00	0	177
21442 	274	255	236 	0.23	0.00	-0.23	0.00 	-70.50	0.00	0.00 	-70.50	4	145
21492 	280	264	241 	0.25	0.00	-0.25	0.00 	-76.20	0.00	-0.00 	-76.20	0	151
21542 	275	265	240 	0.21	0.00	-0.21	0.00 	-64.50	0.00	0.00 	-64.50	10	139
21592 	275	261	250 	0.11	0.00	-0.11	0.00 	-33.30	0.00	0.00 	-33.30	41	108
21642 	273	260	253 	0.06	0.00	-0.06	0.00 	-18.00	0.00	0.00 	-18.00	57	93
21692 	275	263	258 	0.03	0.00	-0.03	0.00 	-9.90	0.00	0.00 	-9.90	65	84
21742 	274	265	263 	-0.03	0.00	0.03	0.00 	7.50	0.00	0.00 	7.50	82	67
21792 	273	261	269 	-0.10	0.00	0.10	0.00 	29.70	0.00	0.00 	29.70	104	45
21842 	273	261	269 	-0.10	0.00	0.10	0.00 	29.70	0.00	0.00 	29.70	104	45
21892 	272	265	276 	-0.18	0.00	0.18	0.00 	52.50	0.00	0.00 	52.50	127	22
21942 	268	263	263 	-0.09	0.00	0.09	0.00 	26.10	0.00	-0.00 	26.10	101	48
21992 	265	256	266 	-0.15	0.00	0.15	0.00 	46.20	0.00	0.00 	46.20	121	28
22042 	257	255	257 	-0.14	0.00	0.14	0.00 	43.50	0.00	-0.00 	43.50	118	31
22092 	256	243	252 	-0.12	0.00	0.12	0.00 	35.10	0.00	-0.00 	35.10	110	39
22142 	254	245	247 	-0.09	0.00	0.09	0.00 	25.50	0.00	-0.00 	25.50	100	49
22192 	250	237	249 	-0.15	0.00	0.15	0.00 	45.90	0.00	0.00 	45.90	120	29
22242 	248	237	243 	-0.11	0.00	0.11	0.00 	33.90	0.00	-0.00 	33.90	108	41
22292 	250	245	242 	-0.08	0.00	0.08	0.00 	22.50	0.00	-0.00 	22.50	97	52
22342 	258	236	242 	-0.00	0.00	0.00	0.00 	1.20	0.00	-0.00 	1.20	76	73
22392 	258	236	242 	-0.00	0.00	0.00	0.00 	1.20	0.00	0.00 	1.20	76	73
22442 	251	239	244 	-0.09	0.00	0.09	0.00 	27.30	0.00	0.00 	27.30	102	47
22492 	265	245	247 	0.03	0.00	-0.03	0.00 	-7.50	0.00	-0.00 	-7.50	67	82
22542 	262	239	241 	0.05	0.00	-0.05	0.00 	-14.70	0.00	-0.00 	-14.70	60	89
22592 	272	243	252 	0.04	0.00	-0.04	0.00 	-12.90	0.00	0.00 	-12.90	62	87
22642 	271	255	248 	0.09	0.00	-0.09	0.00 	-25.50	0.00	-0.00 	-25.50	49	100
22692 	276	255	252 	0.09	0.00	-0.09	0.00 	-28.50	0.00	-0.00 	-28.50	46	103
22742 	281	257	258 	0.09	0.00	-0.09	0.00 	-26.10	0.00	0.00 	-26.10	48	101
22792 	276	259	259 	0.03	0.00	-0.03	0.00 	-8.70	0.00	0.00 	-8.70	66	83
22842 	280	255	259 	0.06	0.00	-0.06	0.00 	-19.50	0.00	-0.00 	-19.50	55	94
22892 	279	265	260 	0.05	0.00	-0.05	0.00 	-16.50	0.00	0.00 	-16.50	58	91
22942 	279	260	266 	-0.01	0.00	0.01	0.00 	3.00	0.00	0.00 	3.00	78	72
22992 	277	259	261 	0.02	0.00	-0.02	0.00 	-5.70	0.00	-0.00 	-5.70	69	80
23042 	276	253	260 	0.01	0.00	-0.01	0.00 	-3.90	0.00	0.00 	-3.90	71	78
23092 	281	258	265 	0.02	0.00	-0.02	0.00 	-5.40	0.00	-0.00 	-5.40	69	80
23142 	280	252	262 	0.03	0.00	-0.03	0.00 	-9.60	0.00	-0.00 	-9.60	65	84
23192 	274	249	261 	-0.02	0.00	0.02	0.00 	6.30	0.00	0.00 	6.30	81	68
23242 	274	243	260 	-0.02	0.00	0.02	0.00 	5.10	0.00	-0.00 	5.10	80	69
23292 	267	244	256 	-0.05	0.00	0.05	0.00 	13.80	0.00	0.00 	13.80	88	61
23342 	266	240	252 	-0.02	0.00	0.02	0.00 	6.00	0.00	-0.00 	6.00	81	69
23392 	261	243	253 	-0.08	0.00	0.08	0.00 	23.10	0.00	0.00 	23.10	98	51
23442 	252	240	249 	-0.13	0.00	0.13	0.00 	39.00	0.00	0.00 	39.00	114	36
23492 	254	240	249 	-0.13	0.00	0.13	0.00 	39.00	0.00	0.00 	39.00	114	36

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

In TrackLeftWallOffset with Kp/Ki/Kd = 300.00 0.00 0.00

R/C/F dists = 136 117 127 Steerval = -0.090, CutAngle = 29.00

(cutAngleDeg > 0 && abs(cutAngleDeg) < 30 -- setting to +30

R/C/F dists = 136 117 127 Steerval = -0.090, SteerAngle = -5.14, CutAngle = 30.00

CutAngDeg > 0, SteerAngleDeg < cutAngleDeg

In SpinTurn(CW, 35.14, 30.00)

Init hdg = -0.08 deg, Turn = 35.14 deg, tgt = 35.06 deg, timeout = 2.34 sec

Msec Hdg PrvHdg dHdg Rate tgtDPS err Kp*err Ival Kd*Derr speed Match Slope

11361 0.05 -0.08 0.13 4.49 30.00 25.51 45.92 25.51 10.20 61 0.81 0.81

11391 1.28 0.05 1.23 40.88 30.00 -10.88 -19.58 14.63 -14.56 50 0.81 0.01

11421 3.31 1.28 2.03 67.70 30.00 -37.70 -67.86 0.00 -10.73 50 0.82 0.01

11451 5.52 3.31 2.22 73.85 30.00 -43.85 -78.94 0.00 -2.46 50 0.84 0.01

11481 7.87 5.52 2.34 78.13 30.00 -48.13 -86.64 0.00 -1.71 50 0.85 0.01

11511 10.30 7.87 2.43 81.05 30.00 -51.05 -91.89 0.00 -1.17 50 0.86 0.01

11541 12.83 10.30 2.53 84.47 30.00 -54.47 -98.05 0.00 -1.37 50 0.88 0.01

11571 15.41 12.83 2.58 85.85 30.00 -55.85 -100.54 0.00 -0.55 50 0.89 0.01

11601 18.03 15.41 2.62 87.32 30.00 -57.32 -103.18 0.00 -0.59 50 0.91 0.01

11631 20.70 18.03 2.67 89.17 30.00 -59.17 -106.50 0.00 -0.74 50 0.92 0.01

11661 23.36 20.70 2.66 88.50 30.00 -58.50 -105.30 0.00 0.27 50 0.93 0.01

11691 26.02 23.36 2.66 88.82 30.00 -58.82 -105.87 0.00 -0.13 50 0.95 0.01

11721 28.79 26.02 2.76 92.13 30.00 -62.13 -111.83 0.00 -1.32 50 0.97 0.02

11751 31.48 28.79 2.69 89.76 30.00 -59.76 -107.57 0.00 0.94 50 0.98 0.01

11781 34.20 31.48 2.72 90.74 30.00 -60.74 -109.33 0.00 -0.39 50 1.00 0.02

Completed turn with yaw = 34.20, tgt = 35.06, and match = 0.995

fudgeFactorMM = -100

at approach start: cut angle = 30.000, adjfactor = 0.866, num = 400.00, tgt dist = 361.88

At start - err = prev_err = -138

R/C/F dists = 156 208 223 Steerval = 0.670, SteerAngle = 38.29, CutAngle = 30.00, err/prev_err = -138

Msec LFront LCtr LRear Front Rear Err P_err

13913 228 206 155 170 206 -133 -138

14020 250 231 179 173 243 -111 -133

14127 312 310 237 168 304 -49 -111

14234 381 395 313 159 393 19 -49

At end of offset capture - prev_res*res = -931

correct back to parallel

In SpinTurn(CCW, 30.00, 30.00)

Init hdg = 45.41 deg, Turn = 30.00 deg, tgt = 15.41 deg, timeout = 2.00 sec

Msec Hdg PrvHdg dHdg Rate tgtDPS err Kp*err Ival Kd*Derr speed Match Slope

14294 45.26 45.41 -0.15 5.12 30.00 24.88 44.79 24.88 9.95 59 0.83 0.83

14324 44.29 45.26 -0.98 32.52 30.00 -2.52 -4.53 22.37 -10.96 50 0.84 0.01

14354 42.61 44.29 -1.68 55.95 30.00 -25.95 -46.71 0.00 -9.37 50 0.85 0.01

14384 40.64 42.61 -1.96 65.48 30.00 -35.48 -63.87 0.00 -3.81 50 0.86 0.01

14414 38.30 40.64 -2.34 78.08 30.00 -48.08 -86.55 0.00 -5.04 50 0.87 0.01

14444 35.51 38.30 -2.79 92.90 30.00 -62.90 -113.22 0.00 -5.93 50 0.89 0.02

14474 32.31 35.51 -3.21 106.88 30.00 -76.88 -138.39 0.00 -5.59 50 0.91 0.02

14504 28.88 32.31 -3.42 114.15 30.00 -84.15 -151.47 0.00 -2.91 50 0.93 0.02

14534 25.25 28.88 -3.64 121.22 30.00 -91.22 -164.19 0.00 -2.83 50 0.95 0.02

14564 21.33 25.25 -3.91 130.41 30.00 -100.41 -180.74 0.00 -3.68 50 0.97 0.02

14594 17.50 21.33 -3.84 127.92 30.00 -97.92 -176.26 0.00 1.00 50 0.99 0.02

14624 13.63 17.50 -3.87 128.94 30.00 -98.94 -178.09 0.00 -0.41 50 0.99 0.00

Completed turn with yaw = 13.63, tgt = 15.41, and match = 0.990

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00 0.00 0.00

Msec Fdir Cdir Rdir Steer Set error Ival Kp*err Ki*Ival Kd*Din Output Lspd Rspd

15692 389 416 377 0.14 0.00 -0.14 0.00 -40.80 0.00 -0.00 -40.80 34 115

15742 390 404 377 0.13 0.00 -0.13 0.00 -40.20 0.00 0.00 -40.20 34 115

15792 391 405 371 0.20 0.00 -0.20 0.00 -61.50 0.00 -0.00 -61.50 13 136

15842 386 412 378 0.09 0.00 -0.09 0.00 -27.60 0.00 0.00 -27.60 47 102

15892 386 412 378 0.09 0.00 -0.09 0.00 -27.60 0.00 0.00 -27.60 47 102

15942 386 405 383 0.04 0.00 -0.04 0.00 -10.50 0.00 0.00 -10.50 64 85

15992 386 413 383 0.04 0.00 -0.04 0.00 -12.90 0.00 -0.00 -12.90 62 87

16042 388 407 381 0.08 0.00 -0.08 0.00 -23.10 0.00 -0.00 -23.10 51 98

16092 381 411 388 -0.06 0.00 0.06 0.00 17.70 0.00 0.00 17.70 92 57

16142 370 403 381 -0.11 0.00 0.11 0.00 32.10 0.00 0.00 32.10 107 42

16192 376 394 380 -0.05 0.00 0.05 0.00 13.80 0.00 -0.00 13.80 88 61

16242 366 384 374 -0.10 0.00 0.10 0.00 28.80 0.00 0.00 28.80 103 46

16292 362 384 374 -0.10 0.00 0.10 0.00 28.80 0.00 0.00 28.80 103 46

16342 362 375 370 -0.10 0.00 0.10 0.00 31.50 0.00 0.00 31.50 106 43

16392 351 372 357 -0.09 0.00 0.09 0.00 26.40 0.00 -0.00 26.40 101 48

16442 337 364 346 -0.13 0.00 0.13 0.00 37.80 0.00 0.00 37.80 112 37

16492 332 352 321 0.06 0.00 -0.06 0.00 -18.60 0.00 -0.00 -18.60 56 93

16542 325 336 319 -0.00 0.00 0.00 0.00 1.20 0.00 0.00 1.20 76 73

16592 315 323 308 -0.01 0.00 0.01 0.00 2.10 0.00 0.00 2.10 77 72

16642 310 324 300 0.02 0.00 -0.02 0.00 -7.20 0.00 -0.00 -7.20 67 82

16692 310 314 301 0.00 0.00 -0.00 0.00 -1.20 0.00 0.00 -1.20 73 76

16742 302 304 293 -0.01 0.00 0.01 0.00 1.80 0.00 0.00 1.80 76 73

16792 294 301 290 -0.06 0.00 0.06 0.00 17.70 0.00 0.00 17.70 92 57

16842 289 295 288 -0.09 0.00 0.09 0.00 28.50 0.00 0.00 28.50 103 46

16892 283 289 279 -0.07 0.00 0.07 0.00 21.30 0.00 -0.00 21.30 96 53

16942 283 287 275 -0.03 0.00 0.03 0.00 9.90 0.00 -0.00 9.90 84 65

16992 269 277 277 -0.20 0.00 0.20 0.00 60.90 0.00 0.00 60.90 135 14

17042 270 269 272 -0.15 0.00 0.15 0.00 45.30 0.00 -0.00 45.30 120 29

17092 263 268 255 -0.05 0.00 0.05 0.00 15.60 0.00 -0.00 15.60 90 59

17142 257 261 253 -0.06 0.00 0.06 0.00 17.70 0.00 0.00 17.70 92 57

17192 243 253 237 -0.09 0.00 0.09 0.00 26.10 0.00 0.00 26.10 101 48

17242 250 248 243 -0.08 0.00 0.08 0.00 24.60 0.00 -0.00 24.60 99 50

17292 241 249 238 -0.12 0.00 0.12 0.00 36.30 0.00 0.00 36.30 111 38

17342 259 255 234 0.11 0.00 -0.11 0.00 -31.50 0.00 -0.00 -31.50 43 106

17392 270 261 237 0.19 0.00 -0.19 0.00 -57.30 0.00 -0.00 -57.30 17 132

17442 273 269 245 0.15 0.00 -0.15 0.00 -44.70 0.00 0.00 -44.70 30 119

17492 287 282 245 0.30 0.00 -0.30 0.00 -90.60 0.00 -0.00 -90.60 0 165

17542 293 293 256 0.26 0.00 -0.26 0.00 -78.90 0.00 0.00 -78.90 0 153

17592 299 288 260 0.28 0.00 -0.28 0.00 -83.40 0.00 -0.00 -83.40 0 158

17642 302 292 272 0.19 0.00 -0.19 0.00 -57.60 0.00 0.00 -57.60 17 132

17692 300 292 272 0.17 0.00 -0.17 0.00 -51.60 0.00 0.00 -51.60 23 126

17742 302 290 276 0.15 0.00 -0.15 0.00 -45.00 0.00 0.00 -45.00 30 120

17792 288 306 282 -0.03 0.00 0.03 0.00 10.20 0.00 0.00 10.20 85 64

17842 298 305 278 0.11 0.00 -0.11 0.00 -31.50 0.00 -0.00 -31.50 43 106

17892 302 300 281 0.11 0.00 -0.11 0.00 -33.00 0.00 -0.00 -33.00 42 108

17942 304 305 288 0.06 0.00 -0.06 0.00 -19.50 0.00 0.00 -19.50 55 94

17992 302 296 292 -0.00 0.00 0.00 0.00 1.20 0.00 0.00 1.20 76 73

18042 294 307 288 -0.03 0.00 0.03 0.00 9.90 0.00 0.00 9.90 84 65

18092 301 307 285 0.07 0.00 -0.07 0.00 -20.10 0.00 -0.00 -20.10 54 95

18142 291 299 289 -0.08 0.00 0.08 0.00 24.30 0.00 0.00 24.30 99 50

18192 294 297 286 -0.02 0.00 0.02 0.00 6.90 0.00 -0.00 6.90 81 68

18242 283 300 283 -0.10 0.00 0.10 0.00 30.00 0.00 0.00 30.00 105 45

18292 290 274 290 -0.13 0.00 0.13 0.00 37.80 0.00 0.00 37.80 112 37

18342 280 281 277 -0.09 0.00 0.09 0.00 26.70 0.00 -0.00 26.70 101 48

18392 272 279 269 -0.09 0.00 0.09 0.00 27.30 0.00 0.00 27.30 102 47

18442 272 267 260 -0.01 0.00 0.01 0.00 3.90 0.00 -0.00 3.90 78 71

18492 269 275 267 -0.11 0.00 0.11 0.00 31.50 0.00 0.00 31.50 106 43

18542 268 274 264 -0.09 0.00 0.09 0.00 25.80 0.00 -0.00 25.80 100 49

18592 267 271 263 -0.09 0.00 0.09 0.00 26.70 0.00 0.00 26.70 101 48

18642 272 267 257 0.02 0.00 -0.02 0.00 -5.10 0.00 -0.00 -5.10 69 80

18692 269 262 248 0.07 0.00 -0.07 0.00 -21.60 0.00 -0.00 -21.60 53 96

18742 268 267 254 0.01 0.00 -0.01 0.00 -2.10 0.00 0.00 -2.10 72 77

18792 268 276 247 0.09 0.00 -0.09 0.00 -25.80 0.00 -0.00 -25.80 49 100

18842 271 267 258 -0.00 0.00 0.00 0.00 0.90 0.00 0.00 0.90 75 74

18892 264 270 256 -0.05 0.00 0.05 0.00 15.00 0.00 0.00 15.00 90 60

18942 272 268 248 0.11 0.00 -0.11 0.00 -32.40 0.00 -0.00 -32.40 42 107

18992 258 272 256 -0.11 0.00 0.11 0.00 32.40 0.00 0.00 32.40 107 42

19042 266 264 251 0.01 0.00 -0.01 0.00 -4.20 0.00 -0.00 -4.20 70 79

19092 262 265 253 -0.05 0.00 0.05 0.00 13.50 0.00 0.00 13.50 88 61

19142 265 274 253 -0.01 0.00 0.01 0.00 1.80 0.00 -0.00 1.80 76 73

19192 272 266 259 -0.00 0.00 0.00 0.00 1.20 0.00 -0.00 1.20 76 73

19242 268 273 264 -0.09 0.00 0.09 0.00 26.10 0.00 0.00 26.10 101 48

19292 274 279 263 -0.01 0.00 0.01 0.00 3.30 0.00 -0.00 3.30 78 71

19342 272 274 258 0.01 0.00 -0.01 0.00 -4.20 0.00 -0.00 -4.20 70 79

19392 280 276 269 -0.01 0.00 0.01 0.00 4.20 0.00 0.00 4.20 79 70

19442 282 283 264 0.06 0.00 -0.06 0.00 -18.90 0.00 -0.00 -18.90 56 93

19492 275 276 266 -0.03 0.00 0.03 0.00 10.20 0.00 0.00 10.20 85 64

19542 287 285 265 0.10 0.00 -0.10 0.00 -31.50 0.00 -0.00 -31.50 43 106

19592 287 285 265 0.10 0.00 -0.10 0.00 -31.50 0.00 0.00 -31.50 43 106

19642 286 286 267 0.05 0.00 -0.05 0.00 -13.80 0.00 0.00 -13.80 61 88

19692 289 281 267 0.10 0.00 -0.10 0.00 -30.30 0.00 -0.00 -30.30 44 105

19742 283 296 274 -0.01 0.00 0.01 0.00 4.20 0.00 0.00 4.20 79 70

19792 293 290 271 0.11 0.00 -0.11 0.00 -33.00 0.00 -0.00 -33.00 42 108

19842 289 290 284 -0.06 0.00 0.06 0.00 18.00 0.00 0.00 18.00 93 57

19892 293 284 275 0.06 0.00 -0.06 0.00 -19.20 0.00 -0.00 -19.20 55 94

19942 294 283 277 0.05 0.00 -0.05 0.00 -15.90 0.00 0.00 -15.90 59 90

19992 284 285 272 0.00 0.00 -0.00 0.00 -1.50 0.00 0.00 -1.50 73 76

20042 288 284 278 -0.02 0.00 0.02 0.00 4.80 0.00 0.00 4.80 79 70

20092 284 279 278 -0.06 0.00 0.06 0.00 18.30 0.00 0.00 18.30 93 56

20142 291 290 280 0.00 0.00 0.00 0.00 0.00 0.00 -0.00 0.00 75 75

20192 285 284 281 -0.08 0.00 0.08 0.00 22.80 0.00 0.00 22.80 97 52

20242 291 290 276 0.04 0.00 -0.04 0.00 -12.00 0.00 -0.00 -12.00 63 87

20292 285 286 285 -0.11 0.00 0.11 0.00 34.20 0.00 0.00 34.20 109 40

20342 268 268 282 -0.27 0.00 0.27 0.00 81.60 0.00 0.00 81.60 156 0

20392 248 246 276 -0.43 0.00 0.43 0.00 130.20 0.00 0.00 130.20 200 0

20442 248 246 276 -0.43 0.00 0.43 0.00 130.20 0.00 0.00 130.20 200 0

20492 227 236 253 -0.42 0.00 0.42 0.00 127.20 0.00 -0.00 127.20 200 0

20542 215 209 237 -0.41 0.00 0.41 0.00 123.30 0.00 -0.00 123.30 198 0

20592 198 204 222 -0.44 0.00 0.44 0.00 130.80 0.00 0.00 130.80 200 0

20642 185 181 198 -0.35 0.00 0.35 0.00 104.70 0.00 -0.00 104.70 179 0

20692 171 160 190 -0.43 0.00 0.43 0.00 129.00 0.00 0.00 129.00 200 0

20742 161 143 173 -0.38 0.00 0.38 0.00 113.10 0.00 -0.00 113.10 188 0

20792 151 127 158 -0.34 0.00 0.34 0.00 102.90 0.00 -0.00 102.90 177 0

20842 140 126 149 -0.36 0.00 0.36 0.00 109.20 0.00 0.00 109.20 184 0

20892 140 126 141 -0.28 0.00 0.28 0.00 85.20 0.00 -0.00 85.20 160 0

20942 152 129 142 -0.17 0.00 0.17 0.00 51.30 0.00 -0.00 51.30 126 23

20992 152 129 142 -0.17 0.00 0.17 0.00 51.30 0.00 0.00 51.30 126 23

21042 169 136 144 -0.01 0.00 0.01 0.00 4.20 0.00 -0.00 4.20 79 70

21092 186 156 149 0.13 0.00 -0.13 0.00 -37.80 0.00 -0.00 -37.80 37 112

21142 201 176 164 0.15 0.00 -0.15 0.00 -43.80 0.00 -0.00 -43.80 31 118

21192 230 223 181 0.31 0.00 -0.31 0.00 -93.90 0.00 -0.00 -93.90 0 168

21242 255 230 199 0.39 0.00 -0.39 0.00 -117.00 0.00 -0.00 -117.00 0 192

21292 261 254 216 0.30 0.00 -0.30 0.00 -91.20 0.00 0.00 -91.20 0 166

21342 275 260 227 0.34 0.00 -0.34 0.00 -102.00 0.00 -0.00 -102.00 0 177

21392 274 260 227 0.34 0.00 -0.34 0.00 -102.00 0.00 0.00 -102.00 0 177

21442 274 255 236 0.23 0.00 -0.23 0.00 -70.50 0.00 0.00 -70.50 4 145

21492 280 264 241 0.25 0.00 -0.25 0.00 -76.20 0.00 -0.00 -76.20 0 151

21542 275 265 240 0.21 0.00 -0.21 0.00 -64.50 0.00 0.00 -64.50 10 139

21592 275 261 250 0.11 0.00 -0.11 0.00 -33.30 0.00 0.00 -33.30 41 108

21642 273 260 253 0.06 0.00 -0.06 0.00 -18.00 0.00 0.00 -18.00 57 93

21692 275 263 258 0.03 0.00 -0.03 0.00 -9.90 0.00 0.00 -9.90 65 84

21742 274 265 263 -0.03 0.00 0.03 0.00 7.50 0.00 0.00 7.50 82 67

21792 273 261 269 -0.10 0.00 0.10 0.00 29.70 0.00 0.00 29.70 104 45

21842 273 261 269 -0.10 0.00 0.10 0.00 29.70 0.00 0.00 29.70 104 45

21892 272 265 276 -0.18 0.00 0.18 0.00 52.50 0.00 0.00 52.50 127 22

21942 268 263 263 -0.09 0.00 0.09 0.00 26.10 0.00 -0.00 26.10 101 48

21992 265 256 266 -0.15 0.00 0.15 0.00 46.20 0.00 0.00 46.20 121 28

22042 257 255 257 -0.14 0.00 0.14 0.00 43.50 0.00 -0.00 43.50 118 31

22092 256 243 252 -0.12 0.00 0.12 0.00 35.10 0.00 -0.00 35.10 110 39

22142 254 245 247 -0.09 0.00 0.09 0.00 25.50 0.00 -0.00 25.50 100 49

22192 250 237 249 -0.15 0.00 0.15 0.00 45.90 0.00 0.00 45.90 120 29

22242 248 237 243 -0.11 0.00 0.11 0.00 33.90 0.00 -0.00 33.90 108 41

22292 250 245 242 -0.08 0.00 0.08 0.00 22.50 0.00 -0.00 22.50 97 52

22342 258 236 242 -0.00 0.00 0.00 0.00 1.20 0.00 -0.00 1.20 76 73

22392 258 236 242 -0.00 0.00 0.00 0.00 1.20 0.00 0.00 1.20 76 73

22442 251 239 244 -0.09 0.00 0.09 0.00 27.30 0.00 0.00 27.30 102 47

22492 265 245 247 0.03 0.00 -0.03 0.00 -7.50 0.00 -0.00 -7.50 67 82

22542 262 239 241 0.05 0.00 -0.05 0.00 -14.70 0.00 -0.00 -14.70 60 89

22592 272 243 252 0.04 0.00 -0.04 0.00 -12.90 0.00 0.00 -12.90 62 87

22642 271 255 248 0.09 0.00 -0.09 0.00 -25.50 0.00 -0.00 -25.50 49 100

22692 276 255 252 0.09 0.00 -0.09 0.00 -28.50 0.00 -0.00 -28.50 46 103

22742 281 257 258 0.09 0.00 -0.09 0.00 -26.10 0.00 0.00 -26.10 48 101

22792 276 259 259 0.03 0.00 -0.03 0.00 -8.70 0.00 0.00 -8.70 66 83

22842 280 255 259 0.06 0.00 -0.06 0.00 -19.50 0.00 -0.00 -19.50 55 94

22892 279 265 260 0.05 0.00 -0.05 0.00 -16.50 0.00 0.00 -16.50 58 91

22942 279 260 266 -0.01 0.00 0.01 0.00 3.00 0.00 0.00 3.00 78 72

22992 277 259 261 0.02 0.00 -0.02 0.00 -5.70 0.00 -0.00 -5.70 69 80

23042 276 253 260 0.01 0.00 -0.01 0.00 -3.90 0.00 0.00 -3.90 71 78

23092 281 258 265 0.02 0.00 -0.02 0.00 -5.40 0.00 -0.00 -5.40 69 80

23142 280 252 262 0.03 0.00 -0.03 0.00 -9.60 0.00 -0.00 -9.60 65 84

23192 274 249 261 -0.02 0.00 0.02 0.00 6.30 0.00 0.00 6.30 81 68

23242 274 243 260 -0.02 0.00 0.02 0.00 5.10 0.00 -0.00 5.10 80 69

23292 267 244 256 -0.05 0.00 0.05 0.00 13.80 0.00 0.00 13.80 88 61

23342 266 240 252 -0.02 0.00 0.02 0.00 6.00 0.00 -0.00 6.00 81 69

23392 261 243 253 -0.08 0.00 0.08 0.00 23.10 0.00 0.00 23.10 98 51

23442 252 240 249 -0.13 0.00 0.13 0.00 39.00 0.00 0.00 39.00 114 36

23492 254 240 249 -0.13 0.00 0.13 0.00 39.00 0.00 0.00 39.00 114 36

Using steerval only. Note monotonically decreasing distance

Even more confusing, it appears that the earlier (September 2021) trial using the steering value input also used the measured center distance to modulate the steering value so the robot would tend to track the steering value but also trend toward the desired wall offset distance. Here’s the tracking code from FourWD_WallTrackTest_V3:

WallTrackSteerVal = RightSteeringVal + (Lidar_RightCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 250.f;

//update motor speeds, skipping bad values
if (!isnan(WallTrackSteerVal))
{
	//10/12/20 now this executes every time, with interval controlled by timer ISR
	//09/03/21 rev fcn type to double vs void to eliminate output param
	//PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,
	//	kp, ki, kd, WallTrackOutput);
	WallTrackOutput = PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,
		kp, ki, kd);

WallTrackSteerVal = RightSteeringVal + (Lidar_RightCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 250.f;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

//10/12/20 now this executes every time, with interval controlled by timer ISR

//09/03/21 rev fcn type to double vs void to eliminate output param

//PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,

// kp, ki, kd, WallTrackOutput);

WallTrackOutput = PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, TIMER5_INTERVAL_MSEC, lastError, lastInput, lastIval, lastDerror,

kp, ki, kd);

In the above code snippet, ‘Lidar_RightCenter’ is in mm, so WALL_OFFSET_TGTDIST_CM must be multiplied by 10 to match units.

At this point I am thoroughly confused, (but hopeful, since I have evidence from an earlier version of myself that something (actually two somethings) actually work. I believe the next step is to see if I can use my WallE3_WallTrackTuning_V5 code to consolidate everything down to something that works.

12 January 2023 Update:

I went back and loaded up WallE3_WallTrack_V2.ino and ran it on my 4m ‘range’ with two 30º breaks. The robot tracked amazingly well, as shown in the following telemetry output and Excel plot

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	Fdir	Cdir	Rdir	Steer	Set	error	Ival	Kp*err	Ki*Ival	Kd*Din	Output	Lspd	Rspd
15780 	381	411	364 	0.18	0.00	-0.18	0.00 	-54.30	0.00	-0.00 	-54.30	20	129
15830 	387	410	371 	0.17	0.00	-0.17	0.00 	-51.00	0.00	0.00 	-51.00	24	126
15880 	375	413	373 	0.03	0.00	-0.03	0.00 	-9.90	0.00	0.00 	-9.90	65	84
15930 	381	406	376 	0.06	0.00	-0.06	0.00 	-16.80	0.00	-0.00 	-16.80	58	91
15980 	381	418	366 	0.17	0.00	-0.17	0.00 	-50.40	0.00	-0.00 	-50.40	24	125
16030 	382	414	367 	0.16	0.00	-0.16	0.00 	-49.20	0.00	0.00 	-49.20	25	124
16080 	375	413	365 	0.11	0.00	-0.11	0.00 	-33.90	0.00	0.00 	-33.90	41	108
16130 	386	403	381 	0.05	0.00	-0.05	0.00 	-15.90	0.00	0.00 	-15.90	59	90
16180 	383	403	381 	0.05	0.00	-0.05	0.00 	-15.90	0.00	0.00 	-15.90	59	90
16230 	383	408	383 	0.01	0.00	-0.01	0.00 	-2.40	0.00	0.00 	-2.40	72	77
16280 	381	409	383 	-0.01	0.00	0.01	0.00 	3.30	0.00	0.00 	3.30	78	71
16330 	380	417	398 	-0.16	0.00	0.16	0.00 	48.90	0.00	0.00 	48.90	123	26
16380 	386	414	385 	0.02	0.00	-0.02	0.00 	-7.20	0.00	-0.00 	-7.20	67	82
16430 	370	395	373 	-0.04	0.00	0.04	0.00 	10.50	0.00	0.00 	10.50	85	64
16480 	352	387	363 	-0.12	0.00	0.12	0.00 	36.90	0.00	0.00 	36.90	111	38
16530 	350	374	358 	-0.11	0.00	0.11	0.00 	31.80	0.00	-0.00 	31.80	106	43
16580 	339	360	358 	-0.23	0.00	0.23	0.00 	69.00	0.00	0.00 	69.00	144	6
16630 	339	360	358 	-0.23	0.00	0.23	0.00 	69.00	0.00	0.00 	69.00	144	6
16680 	324	348	335 	-0.16	0.00	0.16	0.00 	48.60	0.00	-0.00 	48.60	123	26
16730 	314	345	305 	0.04	0.00	-0.04	0.00 	-10.50	0.00	-0.00 	-10.50	64	85
16780 	307	326	295 	0.05	0.00	-0.05	0.00 	-13.80	0.00	-0.00 	-13.80	61	88
16830 	293	320	298 	-0.13	0.00	0.13	0.00 	39.00	0.00	0.00 	39.00	114	36
16880 	281	303	286 	-0.07	0.00	0.07	0.00 	20.10	0.00	-0.00 	20.10	95	54
16930 	281	300	278 	-0.07	0.00	0.07	0.00 	21.00	0.00	0.00 	21.00	96	54
16980 	279	287	274 	-0.06	0.00	0.06	0.00 	18.90	0.00	-0.00 	18.90	93	56
17030 	267	288	263 	-0.07	0.00	0.07	0.00 	21.60	0.00	0.00 	21.60	96	53
17080 	266	284	250 	0.04	0.00	-0.04	0.00 	-13.20	0.00	-0.00 	-13.20	61	88
17130 	255	270	254 	-0.12	0.00	0.12	0.00 	36.00	0.00	0.00 	36.00	111	39
17180 	257	270	247 	-0.03	0.00	0.03	0.00 	9.00	0.00	-0.00 	9.00	84	66
17230 	250	271	240 	-0.03	0.00	0.03	0.00 	8.70	0.00	-0.00 	8.70	83	66
17280 	241	267	240 	-0.12	0.00	0.12	0.00 	36.90	0.00	0.00 	36.90	111	38
17330 	244	262	231 	-0.01	0.00	0.01	0.00 	2.40	0.00	-0.00 	2.40	77	72
17380 	237	263	235 	-0.12	0.00	0.12	0.00 	35.10	0.00	0.00 	35.10	110	39
17430 	236	262	237 	-0.15	0.00	0.15	0.00 	44.40	0.00	0.00 	44.40	119	30
17480 	240	256	235 	-0.09	0.00	0.09	0.00 	28.20	0.00	-0.00 	28.20	103	46
17530 	242	251	237 	-0.10	0.00	0.10	0.00 	29.70	0.00	0.00 	29.70	104	45
17580 	254	261	231 	0.09	0.00	-0.09	0.00 	-27.30	0.00	-0.00 	-27.30	47	102
17630 	265	267	230 	0.22	0.00	-0.22	0.00 	-65.10	0.00	-0.00 	-65.10	9	140
17680 	274	289	243 	0.20	0.00	-0.20	0.00 	-59.70	0.00	0.00 	-59.70	15	134
17730 	275	279	253 	0.10	0.00	-0.10	0.00 	-29.70	0.00	0.00 	-29.70	45	104
17780 	275	292	258 	0.06	0.00	-0.06	0.00 	-18.60	0.00	0.00 	-18.60	56	93
17830 	277	287	257 	0.09	0.00	-0.09	0.00 	-26.10	0.00	-0.00 	-26.10	48	101
17880 	279	292	261 	0.07	0.00	-0.07	0.00 	-21.60	0.00	0.00 	-21.60	53	96
17930 	286	289	261 	0.06	0.00	-0.06	0.00 	-17.70	0.00	0.00 	-17.70	57	92
17980 	275	303	267 	-0.02	0.00	0.02	0.00 	5.10	0.00	0.00 	5.10	80	69
18030 	283	305	270 	0.03	0.00	-0.03	0.00 	-10.50	0.00	-0.00 	-10.50	64	85
18080 	286	297	271 	0.05	0.00	-0.05	0.00 	-14.10	0.00	-0.00 	-14.10	60	89
18130 	286	303	273 	0.03	0.00	-0.03	0.00 	-9.90	0.00	0.00 	-9.90	65	84
18180 	285	299	267 	0.08	0.00	-0.08	0.00 	-23.70	0.00	-0.00 	-23.70	51	98
18230 	284	302	270 	0.04	0.00	-0.04	0.00 	-12.60	0.00	0.00 	-12.60	62	87
18280 	283	295	275 	-0.02	0.00	0.02	0.00 	7.50	0.00	0.00 	7.50	82	67
18330 	281	311	262 	0.10	0.00	-0.10	0.00 	-30.30	0.00	-0.00 	-30.30	44	105
18380 	283	299	269 	0.04	0.00	-0.04	0.00 	-11.70	0.00	0.00 	-11.70	63	86
18430 	278	304	268 	0.00	0.00	-0.00	0.00 	-1.20	0.00	0.00 	-1.20	73	76
18480 	279	297	271 	-0.02	0.00	0.02	0.00 	6.90	0.00	0.00 	6.90	81	68
18530 	283	303	268 	0.05	0.00	-0.05	0.00 	-15.90	0.00	-0.00 	-15.90	59	90
18580 	280	294	268 	0.01	0.00	-0.01	0.00 	-4.20	0.00	0.00 	-4.20	70	79
18630 	281	292	271 	-0.01	0.00	0.01	0.00 	2.40	0.00	0.00 	2.40	77	72
18680 	275	288	268 	-0.04	0.00	0.04	0.00 	12.60	0.00	0.00 	12.60	87	62
18730 	272	294	266 	-0.05	0.00	0.05	0.00 	13.80	0.00	0.00 	13.80	88	61
18780 	273	282	259 	0.02	0.00	-0.02	0.00 	-6.60	0.00	-0.00 	-6.60	68	81
18830 	264	291	265 	-0.12	0.00	0.12	0.00 	35.70	0.00	0.00 	35.70	110	39
18880 	270	273	266 	-0.09	0.00	0.09	0.00 	26.10	0.00	-0.00 	26.10	101	48
18930 	267	282	259 	-0.04	0.00	0.04	0.00 	11.40	0.00	-0.00 	11.40	86	63
18980 	268	283	258 	-0.02	0.00	0.02	0.00 	5.10	0.00	-0.00 	5.10	80	69
19030 	263	279	256 	-0.05	0.00	0.05	0.00 	15.30	0.00	0.00 	15.30	90	59
19080 	261	269	252 	-0.04	0.00	0.04	0.00 	12.30	0.00	-0.00 	12.30	87	62
19130 	259	280	241 	0.06	0.00	-0.06	0.00 	-18.00	0.00	-0.00 	-18.00	56	93
19180 	257	274	251 	-0.07	0.00	0.07	0.00 	19.80	0.00	0.00 	19.80	94	55
19230 	252	275	246 	-0.06	0.00	0.06	0.00 	19.50	0.00	-0.00 	19.50	94	55
19280 	259	273	241 	0.00	0.00	-0.00	0.00 	-0.90	0.00	-0.00 	-0.90	74	75
19330 	259	282	238 	0.09	0.00	-0.09	0.00 	-27.60	0.00	-0.00 	-27.60	47	102
19380 	250	272	244 	-0.07	0.00	0.07	0.00 	20.40	0.00	0.00 	20.40	95	54
19430 	252	274	249 	-0.10	0.00	0.10	0.00 	28.80	0.00	0.00 	28.80	103	46
19480 	246	270	246 	-0.13	0.00	0.13	0.00 	39.00	0.00	0.00 	39.00	114	36
19530 	248	270	248 	-0.13	0.00	0.13	0.00 	39.00	0.00	0.00 	39.00	114	36
19580 	250	269	244 	-0.07	0.00	0.07	0.00 	21.30	0.00	-0.00 	21.30	96	53
19630 	250	270	241 	-0.04	0.00	0.04	0.00 	12.00	0.00	-0.00 	12.00	87	63
19680 	250	277	235 	0.03	0.00	-0.03	0.00 	-8.10	0.00	-0.00 	-8.10	66	83
19730 	246	274	235 	-0.02	0.00	0.02	0.00 	4.80	0.00	0.00 	4.80	79	70
19780 	246	266	240 	-0.07	0.00	0.07	0.00 	22.20	0.00	0.00 	22.20	97	52
19830 	251	268	239 	-0.01	0.00	0.01	0.00 	3.60	0.00	-0.00 	3.60	78	71
19880 	250	275	241 	-0.03	0.00	0.03	0.00 	10.50	0.00	0.00 	10.50	85	64
19930 	251	274	231 	0.07	0.00	-0.07	0.00 	-22.20	0.00	-0.00 	-22.20	52	97
19980 	255	278	241 	0.02	0.00	-0.02	0.00 	-5.40	0.00	0.00 	-5.40	69	80
20030 	255	278	241 	0.02	0.00	-0.02	0.00 	-5.40	0.00	0.00 	-5.40	69	80
20080 	264	290	240 	0.13	0.00	-0.13	0.00 	-39.00	0.00	-0.00 	-39.00	36	114
20130 	280	289	251 	0.18	0.00	-0.18	0.00 	-53.70	0.00	-0.00 	-53.70	21	128
20180 	285	303	250 	0.25	0.00	-0.25	0.00 	-75.90	0.00	-0.00 	-75.90	0	150
20230 	278	301	266 	0.02	0.00	-0.02	0.00 	-6.30	0.00	0.00 	-6.30	68	81
20280 	269	291	272 	-0.14	0.00	0.14	0.00 	41.70	0.00	0.00 	41.70	116	33
20330 	261	287	275 	-0.25	0.00	0.25	0.00 	75.90	0.00	0.00 	75.90	150	0
20380 	245	285	263 	-0.30	0.00	0.30	0.00 	88.50	0.00	0.00 	88.50	163	0
20430 	245	285	263 	-0.30	0.00	0.30	0.00 	88.50	0.00	0.00 	88.50	163	0
20480 	220	239	232 	-0.28	0.00	0.28	0.00 	84.30	0.00	-0.00 	84.30	159	0
20530 	204	225	226 	-0.39	0.00	0.39	0.00 	118.50	0.00	0.00 	118.50	193	0
20580 	201	217	206 	-0.23	0.00	0.23	0.00 	69.90	0.00	-0.00 	69.90	144	5
20630 	186	203	201 	-0.35	0.00	0.35	0.00 	104.10	0.00	0.00 	104.10	179	0
20680 	173	194	185 	-0.33	0.00	0.33	0.00 	97.80	0.00	-0.00 	97.80	172	0
20730 	163	173	178 	-0.38	0.00	0.38	0.00 	113.10	0.00	0.00 	113.10	188	0
20780 	159	160	169 	-0.34	0.00	0.34	0.00 	102.00	0.00	-0.00 	102.00	177	0
20830 	159	160	169 	-0.34	0.00	0.34	0.00 	102.00	0.00	0.00 	102.00	177	0
20880 	159	155	162 	-0.28	0.00	0.28	0.00 	82.50	0.00	-0.00 	82.50	157	0
20930 	162	154	160 	-0.23	0.00	0.23	0.00 	67.80	0.00	-0.00 	67.80	142	7
20980 	174	170	160 	-0.09	0.00	0.09	0.00 	27.00	0.00	-0.00 	27.00	102	48
21030 	188	192	168 	-0.01	0.00	0.01	0.00 	2.40	0.00	-0.00 	2.40	77	72
21080 	209	221	175 	0.16	0.00	-0.16	0.00 	-48.30	0.00	-0.00 	-48.30	26	123
21130 	227	244	186 	0.25	0.00	-0.25	0.00 	-76.20	0.00	-0.00 	-76.20	0	151
21180 	246	258	205 	0.27	0.00	-0.27	0.00 	-80.40	0.00	-0.00 	-80.40	0	155
21230 	265	278	218 	0.35	0.00	-0.35	0.00 	-104.40	0.00	-0.00 	-104.40	0	179
21280 	265	278	218 	0.35	0.00	-0.35	0.00 	-104.40	0.00	0.00 	-104.40	0	179
21330 	273	282	233 	0.28	0.00	-0.28	0.00 	-84.60	0.00	0.00 	-84.60	0	159
21380 	263	290	241 	0.11	0.00	-0.11	0.00 	-33.00	0.00	0.00 	-33.00	42	108
21430 	269	278	244 	0.13	0.00	-0.13	0.00 	-38.40	0.00	-0.00 	-38.40	36	113
21480 	272	286	252 	0.09	0.00	-0.09	0.00 	-25.80	0.00	0.00 	-25.80	49	100
21530 	272	287	253 	0.08	0.00	-0.08	0.00 	-23.10	0.00	0.00 	-23.10	51	98
21580 	266	285	263 	-0.09	0.00	0.09	0.00 	25.50	0.00	0.00 	25.50	100	49
21630 	277	295	263 	0.04	0.00	-0.04	0.00 	-10.50	0.00	-0.00 	-10.50	64	85
21680 	274	295	263 	0.04	0.00	-0.04	0.00 	-10.50	0.00	0.00 	-10.50	64	85
21730 	274	293	263 	0.00	0.00	-0.00	0.00 	-0.90	0.00	0.00 	-0.90	74	75
21780 	279	299	260 	0.09	0.00	-0.09	0.00 	-26.70	0.00	-0.00 	-26.70	48	101
21830 	280	294	266 	0.03	0.00	-0.03	0.00 	-10.20	0.00	0.00 	-10.20	64	85
21880 	277	302	272 	-0.05	0.00	0.05	0.00 	14.40	0.00	0.00 	14.40	89	60
21930 	279	304	268 	0.01	0.00	-0.01	0.00 	-4.20	0.00	-0.00 	-4.20	70	79
21980 	276	295	277 	-0.11	0.00	0.11	0.00 	34.50	0.00	0.00 	34.50	109	40
22030 	283	298	272 	0.01	0.00	-0.01	0.00 	-2.40	0.00	-0.00 	-2.40	72	77
22080 	278	295	274 	-0.06	0.00	0.06	0.00 	19.50	0.00	0.00 	19.50	94	55
22130 	282	309	270 	0.03	0.00	-0.03	0.00 	-8.70	0.00	-0.00 	-8.70	66	83
22180 	283	301	276 	-0.03	0.00	0.03	0.00 	8.70	0.00	0.00 	8.70	83	66
22230 	286	306	278 	-0.01	0.00	0.01	0.00 	4.20	0.00	-0.00 	4.20	79	70
22280 	293	312	278 	0.06	0.00	-0.06	0.00 	-18.60	0.00	-0.00 	-18.60	56	93
22330 	293	312	278 	0.06	0.00	-0.06	0.00 	-18.60	0.00	0.00 	-18.60	56	93
22380 	300	320	280 	0.12	0.00	-0.12	0.00 	-36.00	0.00	-0.00 	-36.00	39	111
22430 	297	320	284 	0.05	0.00	-0.05	0.00 	-15.00	0.00	0.00 	-15.00	60	90
22480 	306	322	291 	0.07	0.00	-0.07	0.00 	-21.60	0.00	-0.00 	-21.60	53	96
22530 	306	322	291 	0.07	0.00	-0.07	0.00 	-21.60	0.00	0.00 	-21.60	53	96
22580 	300	324	284 	0.08	0.00	-0.08	0.00 	-25.20	0.00	-0.00 	-25.20	49	100
22630 	302	324	292 	0.02	0.00	-0.02	0.00 	-7.20	0.00	0.00 	-7.20	67	82
22680 	302	328	290 	0.05	0.00	-0.05	0.00 	-14.40	0.00	-0.00 	-14.40	60	89
22730 	300	320	295 	-0.03	0.00	0.03	0.00 	9.00	0.00	0.00 	9.00	84	66
22780 	298	330	288 	0.03	0.00	-0.03	0.00 	-9.00	0.00	-0.00 	-9.00	66	84
22830 	294	323	287 	-0.01	0.00	0.01	0.00 	2.10	0.00	0.00 	2.10	77	72
22880 	296	319	292 	-0.04	0.00	0.04	0.00 	12.30	0.00	0.00 	12.30	87	62
22930 	290	319	287 	-0.05	0.00	0.05	0.00 	15.30	0.00	0.00 	15.30	90	59
22980 	293	324	278 	0.07	0.00	-0.07	0.00 	-22.20	0.00	-0.00 	-22.20	52	97

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

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00 0.00 0.00

Msec Fdir Cdir Rdir Steer Set error Ival Kp*err Ki*Ival Kd*Din Output Lspd Rspd

15780 381 411 364 0.18 0.00 -0.18 0.00 -54.30 0.00 -0.00 -54.30 20 129

15830 387 410 371 0.17 0.00 -0.17 0.00 -51.00 0.00 0.00 -51.00 24 126

15880 375 413 373 0.03 0.00 -0.03 0.00 -9.90 0.00 0.00 -9.90 65 84

15930 381 406 376 0.06 0.00 -0.06 0.00 -16.80 0.00 -0.00 -16.80 58 91

15980 381 418 366 0.17 0.00 -0.17 0.00 -50.40 0.00 -0.00 -50.40 24 125

16030 382 414 367 0.16 0.00 -0.16 0.00 -49.20 0.00 0.00 -49.20 25 124

16080 375 413 365 0.11 0.00 -0.11 0.00 -33.90 0.00 0.00 -33.90 41 108

16130 386 403 381 0.05 0.00 -0.05 0.00 -15.90 0.00 0.00 -15.90 59 90

16180 383 403 381 0.05 0.00 -0.05 0.00 -15.90 0.00 0.00 -15.90 59 90

16230 383 408 383 0.01 0.00 -0.01 0.00 -2.40 0.00 0.00 -2.40 72 77

16280 381 409 383 -0.01 0.00 0.01 0.00 3.30 0.00 0.00 3.30 78 71

16330 380 417 398 -0.16 0.00 0.16 0.00 48.90 0.00 0.00 48.90 123 26

16380 386 414 385 0.02 0.00 -0.02 0.00 -7.20 0.00 -0.00 -7.20 67 82

16430 370 395 373 -0.04 0.00 0.04 0.00 10.50 0.00 0.00 10.50 85 64

16480 352 387 363 -0.12 0.00 0.12 0.00 36.90 0.00 0.00 36.90 111 38

16530 350 374 358 -0.11 0.00 0.11 0.00 31.80 0.00 -0.00 31.80 106 43

16580 339 360 358 -0.23 0.00 0.23 0.00 69.00 0.00 0.00 69.00 144 6

16630 339 360 358 -0.23 0.00 0.23 0.00 69.00 0.00 0.00 69.00 144 6

16680 324 348 335 -0.16 0.00 0.16 0.00 48.60 0.00 -0.00 48.60 123 26

16730 314 345 305 0.04 0.00 -0.04 0.00 -10.50 0.00 -0.00 -10.50 64 85

16780 307 326 295 0.05 0.00 -0.05 0.00 -13.80 0.00 -0.00 -13.80 61 88

16830 293 320 298 -0.13 0.00 0.13 0.00 39.00 0.00 0.00 39.00 114 36

16880 281 303 286 -0.07 0.00 0.07 0.00 20.10 0.00 -0.00 20.10 95 54

16930 281 300 278 -0.07 0.00 0.07 0.00 21.00 0.00 0.00 21.00 96 54

16980 279 287 274 -0.06 0.00 0.06 0.00 18.90 0.00 -0.00 18.90 93 56

17030 267 288 263 -0.07 0.00 0.07 0.00 21.60 0.00 0.00 21.60 96 53

17080 266 284 250 0.04 0.00 -0.04 0.00 -13.20 0.00 -0.00 -13.20 61 88

17130 255 270 254 -0.12 0.00 0.12 0.00 36.00 0.00 0.00 36.00 111 39

17180 257 270 247 -0.03 0.00 0.03 0.00 9.00 0.00 -0.00 9.00 84 66

17230 250 271 240 -0.03 0.00 0.03 0.00 8.70 0.00 -0.00 8.70 83 66

17280 241 267 240 -0.12 0.00 0.12 0.00 36.90 0.00 0.00 36.90 111 38

17330 244 262 231 -0.01 0.00 0.01 0.00 2.40 0.00 -0.00 2.40 77 72

17380 237 263 235 -0.12 0.00 0.12 0.00 35.10 0.00 0.00 35.10 110 39

17430 236 262 237 -0.15 0.00 0.15 0.00 44.40 0.00 0.00 44.40 119 30

17480 240 256 235 -0.09 0.00 0.09 0.00 28.20 0.00 -0.00 28.20 103 46

17530 242 251 237 -0.10 0.00 0.10 0.00 29.70 0.00 0.00 29.70 104 45

17580 254 261 231 0.09 0.00 -0.09 0.00 -27.30 0.00 -0.00 -27.30 47 102

17630 265 267 230 0.22 0.00 -0.22 0.00 -65.10 0.00 -0.00 -65.10 9 140

17680 274 289 243 0.20 0.00 -0.20 0.00 -59.70 0.00 0.00 -59.70 15 134

17730 275 279 253 0.10 0.00 -0.10 0.00 -29.70 0.00 0.00 -29.70 45 104

17780 275 292 258 0.06 0.00 -0.06 0.00 -18.60 0.00 0.00 -18.60 56 93

17830 277 287 257 0.09 0.00 -0.09 0.00 -26.10 0.00 -0.00 -26.10 48 101

17880 279 292 261 0.07 0.00 -0.07 0.00 -21.60 0.00 0.00 -21.60 53 96

17930 286 289 261 0.06 0.00 -0.06 0.00 -17.70 0.00 0.00 -17.70 57 92

17980 275 303 267 -0.02 0.00 0.02 0.00 5.10 0.00 0.00 5.10 80 69

18030 283 305 270 0.03 0.00 -0.03 0.00 -10.50 0.00 -0.00 -10.50 64 85

18080 286 297 271 0.05 0.00 -0.05 0.00 -14.10 0.00 -0.00 -14.10 60 89

18130 286 303 273 0.03 0.00 -0.03 0.00 -9.90 0.00 0.00 -9.90 65 84

18180 285 299 267 0.08 0.00 -0.08 0.00 -23.70 0.00 -0.00 -23.70 51 98

18230 284 302 270 0.04 0.00 -0.04 0.00 -12.60 0.00 0.00 -12.60 62 87

18280 283 295 275 -0.02 0.00 0.02 0.00 7.50 0.00 0.00 7.50 82 67

18330 281 311 262 0.10 0.00 -0.10 0.00 -30.30 0.00 -0.00 -30.30 44 105

18380 283 299 269 0.04 0.00 -0.04 0.00 -11.70 0.00 0.00 -11.70 63 86

18430 278 304 268 0.00 0.00 -0.00 0.00 -1.20 0.00 0.00 -1.20 73 76

18480 279 297 271 -0.02 0.00 0.02 0.00 6.90 0.00 0.00 6.90 81 68

18530 283 303 268 0.05 0.00 -0.05 0.00 -15.90 0.00 -0.00 -15.90 59 90

18580 280 294 268 0.01 0.00 -0.01 0.00 -4.20 0.00 0.00 -4.20 70 79

18630 281 292 271 -0.01 0.00 0.01 0.00 2.40 0.00 0.00 2.40 77 72

18680 275 288 268 -0.04 0.00 0.04 0.00 12.60 0.00 0.00 12.60 87 62

18730 272 294 266 -0.05 0.00 0.05 0.00 13.80 0.00 0.00 13.80 88 61

18780 273 282 259 0.02 0.00 -0.02 0.00 -6.60 0.00 -0.00 -6.60 68 81

18830 264 291 265 -0.12 0.00 0.12 0.00 35.70 0.00 0.00 35.70 110 39

18880 270 273 266 -0.09 0.00 0.09 0.00 26.10 0.00 -0.00 26.10 101 48

18930 267 282 259 -0.04 0.00 0.04 0.00 11.40 0.00 -0.00 11.40 86 63

18980 268 283 258 -0.02 0.00 0.02 0.00 5.10 0.00 -0.00 5.10 80 69

19030 263 279 256 -0.05 0.00 0.05 0.00 15.30 0.00 0.00 15.30 90 59

19080 261 269 252 -0.04 0.00 0.04 0.00 12.30 0.00 -0.00 12.30 87 62

19130 259 280 241 0.06 0.00 -0.06 0.00 -18.00 0.00 -0.00 -18.00 56 93

19180 257 274 251 -0.07 0.00 0.07 0.00 19.80 0.00 0.00 19.80 94 55

19230 252 275 246 -0.06 0.00 0.06 0.00 19.50 0.00 -0.00 19.50 94 55

19280 259 273 241 0.00 0.00 -0.00 0.00 -0.90 0.00 -0.00 -0.90 74 75

19330 259 282 238 0.09 0.00 -0.09 0.00 -27.60 0.00 -0.00 -27.60 47 102

19380 250 272 244 -0.07 0.00 0.07 0.00 20.40 0.00 0.00 20.40 95 54

19430 252 274 249 -0.10 0.00 0.10 0.00 28.80 0.00 0.00 28.80 103 46

19480 246 270 246 -0.13 0.00 0.13 0.00 39.00 0.00 0.00 39.00 114 36

19530 248 270 248 -0.13 0.00 0.13 0.00 39.00 0.00 0.00 39.00 114 36

19580 250 269 244 -0.07 0.00 0.07 0.00 21.30 0.00 -0.00 21.30 96 53

19630 250 270 241 -0.04 0.00 0.04 0.00 12.00 0.00 -0.00 12.00 87 63

19680 250 277 235 0.03 0.00 -0.03 0.00 -8.10 0.00 -0.00 -8.10 66 83

19730 246 274 235 -0.02 0.00 0.02 0.00 4.80 0.00 0.00 4.80 79 70

19780 246 266 240 -0.07 0.00 0.07 0.00 22.20 0.00 0.00 22.20 97 52

19830 251 268 239 -0.01 0.00 0.01 0.00 3.60 0.00 -0.00 3.60 78 71

19880 250 275 241 -0.03 0.00 0.03 0.00 10.50 0.00 0.00 10.50 85 64

19930 251 274 231 0.07 0.00 -0.07 0.00 -22.20 0.00 -0.00 -22.20 52 97

19980 255 278 241 0.02 0.00 -0.02 0.00 -5.40 0.00 0.00 -5.40 69 80

20030 255 278 241 0.02 0.00 -0.02 0.00 -5.40 0.00 0.00 -5.40 69 80

20080 264 290 240 0.13 0.00 -0.13 0.00 -39.00 0.00 -0.00 -39.00 36 114

20130 280 289 251 0.18 0.00 -0.18 0.00 -53.70 0.00 -0.00 -53.70 21 128

20180 285 303 250 0.25 0.00 -0.25 0.00 -75.90 0.00 -0.00 -75.90 0 150

20230 278 301 266 0.02 0.00 -0.02 0.00 -6.30 0.00 0.00 -6.30 68 81

20280 269 291 272 -0.14 0.00 0.14 0.00 41.70 0.00 0.00 41.70 116 33

20330 261 287 275 -0.25 0.00 0.25 0.00 75.90 0.00 0.00 75.90 150 0

20380 245 285 263 -0.30 0.00 0.30 0.00 88.50 0.00 0.00 88.50 163 0

20430 245 285 263 -0.30 0.00 0.30 0.00 88.50 0.00 0.00 88.50 163 0

20480 220 239 232 -0.28 0.00 0.28 0.00 84.30 0.00 -0.00 84.30 159 0

20530 204 225 226 -0.39 0.00 0.39 0.00 118.50 0.00 0.00 118.50 193 0

20580 201 217 206 -0.23 0.00 0.23 0.00 69.90 0.00 -0.00 69.90 144 5

20630 186 203 201 -0.35 0.00 0.35 0.00 104.10 0.00 0.00 104.10 179 0

20680 173 194 185 -0.33 0.00 0.33 0.00 97.80 0.00 -0.00 97.80 172 0

20730 163 173 178 -0.38 0.00 0.38 0.00 113.10 0.00 0.00 113.10 188 0

20780 159 160 169 -0.34 0.00 0.34 0.00 102.00 0.00 -0.00 102.00 177 0

20830 159 160 169 -0.34 0.00 0.34 0.00 102.00 0.00 0.00 102.00 177 0

20880 159 155 162 -0.28 0.00 0.28 0.00 82.50 0.00 -0.00 82.50 157 0

20930 162 154 160 -0.23 0.00 0.23 0.00 67.80 0.00 -0.00 67.80 142 7

20980 174 170 160 -0.09 0.00 0.09 0.00 27.00 0.00 -0.00 27.00 102 48

21030 188 192 168 -0.01 0.00 0.01 0.00 2.40 0.00 -0.00 2.40 77 72

21080 209 221 175 0.16 0.00 -0.16 0.00 -48.30 0.00 -0.00 -48.30 26 123

21130 227 244 186 0.25 0.00 -0.25 0.00 -76.20 0.00 -0.00 -76.20 0 151

21180 246 258 205 0.27 0.00 -0.27 0.00 -80.40 0.00 -0.00 -80.40 0 155

21230 265 278 218 0.35 0.00 -0.35 0.00 -104.40 0.00 -0.00 -104.40 0 179

21280 265 278 218 0.35 0.00 -0.35 0.00 -104.40 0.00 0.00 -104.40 0 179

21330 273 282 233 0.28 0.00 -0.28 0.00 -84.60 0.00 0.00 -84.60 0 159

21380 263 290 241 0.11 0.00 -0.11 0.00 -33.00 0.00 0.00 -33.00 42 108

21430 269 278 244 0.13 0.00 -0.13 0.00 -38.40 0.00 -0.00 -38.40 36 113

21480 272 286 252 0.09 0.00 -0.09 0.00 -25.80 0.00 0.00 -25.80 49 100

21530 272 287 253 0.08 0.00 -0.08 0.00 -23.10 0.00 0.00 -23.10 51 98

21580 266 285 263 -0.09 0.00 0.09 0.00 25.50 0.00 0.00 25.50 100 49

21630 277 295 263 0.04 0.00 -0.04 0.00 -10.50 0.00 -0.00 -10.50 64 85

21680 274 295 263 0.04 0.00 -0.04 0.00 -10.50 0.00 0.00 -10.50 64 85

21730 274 293 263 0.00 0.00 -0.00 0.00 -0.90 0.00 0.00 -0.90 74 75

21780 279 299 260 0.09 0.00 -0.09 0.00 -26.70 0.00 -0.00 -26.70 48 101

21830 280 294 266 0.03 0.00 -0.03 0.00 -10.20 0.00 0.00 -10.20 64 85

21880 277 302 272 -0.05 0.00 0.05 0.00 14.40 0.00 0.00 14.40 89 60

21930 279 304 268 0.01 0.00 -0.01 0.00 -4.20 0.00 -0.00 -4.20 70 79

21980 276 295 277 -0.11 0.00 0.11 0.00 34.50 0.00 0.00 34.50 109 40

22030 283 298 272 0.01 0.00 -0.01 0.00 -2.40 0.00 -0.00 -2.40 72 77

22080 278 295 274 -0.06 0.00 0.06 0.00 19.50 0.00 0.00 19.50 94 55

22130 282 309 270 0.03 0.00 -0.03 0.00 -8.70 0.00 -0.00 -8.70 66 83

22180 283 301 276 -0.03 0.00 0.03 0.00 8.70 0.00 0.00 8.70 83 66

22230 286 306 278 -0.01 0.00 0.01 0.00 4.20 0.00 -0.00 4.20 79 70

22280 293 312 278 0.06 0.00 -0.06 0.00 -18.60 0.00 -0.00 -18.60 56 93

22330 293 312 278 0.06 0.00 -0.06 0.00 -18.60 0.00 0.00 -18.60 56 93

22380 300 320 280 0.12 0.00 -0.12 0.00 -36.00 0.00 -0.00 -36.00 39 111

22430 297 320 284 0.05 0.00 -0.05 0.00 -15.00 0.00 0.00 -15.00 60 90

22480 306 322 291 0.07 0.00 -0.07 0.00 -21.60 0.00 -0.00 -21.60 53 96

22530 306 322 291 0.07 0.00 -0.07 0.00 -21.60 0.00 0.00 -21.60 53 96

22580 300 324 284 0.08 0.00 -0.08 0.00 -25.20 0.00 -0.00 -25.20 49 100

22630 302 324 292 0.02 0.00 -0.02 0.00 -7.20 0.00 0.00 -7.20 67 82

22680 302 328 290 0.05 0.00 -0.05 0.00 -14.40 0.00 -0.00 -14.40 60 89

22730 300 320 295 -0.03 0.00 0.03 0.00 9.00 0.00 0.00 9.00 84 66

22780 298 330 288 0.03 0.00 -0.03 0.00 -9.00 0.00 -0.00 -9.00 66 84

22830 294 323 287 -0.01 0.00 0.01 0.00 2.10 0.00 0.00 2.10 77 72

22880 296 319 292 -0.04 0.00 0.04 0.00 12.30 0.00 0.00 12.30 87 62

22930 290 319 287 -0.05 0.00 0.05 0.00 15.30 0.00 0.00 15.30 90 59

22980 293 324 278 0.07 0.00 -0.07 0.00 -22.20 0.00 -0.00 -22.20 52 97

WallE3_WallTrack_V2’s tracking algorithm uses the difference between the desired and measured offset distances to ‘tweak’ the steering value, as discussed above, so clearly this works – or at least doesn’t screw things up too badly. In the above telemetry output, the ‘Steer’ column is the steering value after the offset distance adjustment shown here

//have to weight value by both angle and wall offset
WallTrackSteerVal = glLeftSteeringVal + (glLidar_LeftCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;

1 2	//have to weight value by both angle and wall offset WallTrackSteerVal = glLeftSteeringVal + (glLidar_LeftCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;

So at the point where the robot hit the minimum center distance of about 154mm, the steering value adjustment would be (154-400)/1000 = -0.246. The total steering value term at this point was 0.23, which means the ‘raw’ steering value was +0.016 and the offset distance error term accounted for ~97% of the total. This is good evidence that including the the distance offset term works.

My new new new plan is to focus on my October 2022 post that uses the orientation angle corrected offset distance as the input to the PID engine, and see if I can incorporate this, along with all my recent updates/bugfixes into WallE3_WallTrackTuning_V5

WallE3 Spin Turn, Revisited

1 Reply

Posted 23 December, 2022

Just over a year ago I made a radical change to WallE2’s form factor to improve it’s turning performance. Since then I have been making other changes and improvements to WallE3, including a recent successful effort to improve WallE3’s ‘rolling turn’ capabilities. Based on that, this post describes a similar effort to improve WallE3’s ‘spin turn performance.

To date, WallE3’s ‘spin turn’ performance has been lackluster at best. Rather than turning smoothly, it tends to ‘motorboat’ it’s way through the turn, starting and stopping several times through the turn. After achieving some success with smoothing out the ‘rolling turn’ feature, I decided to take another crack at ‘spin turn’.

I started by creating yet another part-task program that does nothing but support spin turn operations. This program accepts a set of parameters, calls the existing ‘SpinTurn()’ subroutine to execute the specified spin turn, and then returns to the parameter entry state for another go.

The starting point for the effort was a 90º CCW turn at 45º/s, with PID = (1,0,0). As shown below, this produced a nice smooth turn, but way too slow (17º/s vs 45º/s):

After a coupe of dozen trials, I gradually worked my way up to a ‘final’ PID of (0.7,0.3,0). Here are plots and videos for various turn lengths and rates.

SpinTurn(CCW,30,45)PID(0.7,0.3,0)

SpinTurn(CCW,60,45)PID(0.7,0.3,0)

SpinTurn(CCW,90,45)PID(0.7,0.3,0)

After running these tests, I decided to see how WallE3 would perform on carpet with these same settings. As shown below, carpet performance was pretty much identical to the smooth surface runs.

SpinTurn(CCW,30,45)PID(0.7,0.3,0)

SpinTurn(CCW,60,45)PID(0.7,0.3,0)

SpinTurn(CCW,90,45)PID(0.7,0.3,0)

So, with much better performance now for both the ‘rolling turn’ and ‘spin turn’ features, it is now probably time to revisit the whole wall-tracking thing, and see if I can achieve better performance than in past efforts.

Stay tuned,

Frank

Using mklink to centralize Teensy OTA Files

1 Reply

Posted 20 December 2022

This post describes the actions I have taken to centralize the ‘board.txt’ and ‘TeensyOTA1.ttl’ (Tera Term macro) locations that facilitate the Teensy OTA capability, so that all my various firmware projects targeted at my wall-following robot all use the same set of files. This is done by using the Windows ‘mklink’ command to create ‘hard’ links from the various program folders to a single folder called ‘Robot Common Files’.

For the past five or six years I’ve been working on a 4-wheel robot project to autonomously navigate around my house using a (by now) fairly sophisticated wall-following algorithm. As I have worked through the various problems, I’ve gone through at least three different physical form factors, starting with a 3-wheel (two driven wheels, one castering wheel) and ending up (so far) with a custom 4-wheel ‘wide-track’ model.

Also along the way I have gone through any number of software versions. I work mainly with the Arduino ecosystem, but I don’t use Arduino directly. I use Microsoft Visual Studio 2022 Community Edition with the Visual Micro extension, and this is a great platform.

My latest hardware upgrade was to replace my original Arduino Mega 2560 processor with a Teensy 3.5 main processor, with a firmware update to achieve over-the-air (OTA) updates using a Wixel RF pair.

I now have at least twenty different software/firmware projects that I run on the robot for different reasons. I have a ‘main line’ project that incorporates all features required to full execute autonomous wall-following, and then I have lots of smaller projects aimed at exercising some small subset of the full feature set – things like distance calibration testing for the robot’s two 3-element VL53L0X time-of-flight distance sensors, and for testing out different ways of managing ‘rolling’ and ‘spin’ turns, and different algorithms for wall-tracking. All of these projects use the same ‘over-the-air’ (OTA) firmware/hardware configuration for firmware updates. Up until now I have been simply copying the two required files – ‘board.txt’ and ‘TeensyOTA1.ttl’ (a Tera Term macro) from project folder to project folder, but a recent Visual Micro update broke my OTA routine, and I discovered that having these two files copied into multiple project folders didn’t work so well – oops!

So, I decided to create ‘Robot Common Files’ folder in my Documents\Arduino folder tree, place these two files (along with the required FlashTxx.cpp/h files and my ‘enums.h’ file) into this folder, and then use the ‘mklink /H’ command (12/23/22 note: Admin privileges are not required) to create ‘hard’ links from each project’s folder to the single files in m Robot Common Files folder. The commands I used to accomplish this were:

//
//
C:\Users\Frank\Documents\Arduino\WallE3_AnomalyRecovery_V2>mklink /H board.txt "C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt"
Hardlink created for board.txt <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt

C:\Users\Frank\Documents\Arduino\WallE3_AnomalyRecovery_V2>mklink /H TeensyOTA1.ttl "C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl"
Hardlink created for TeensyOTA1.ttl <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl

C:\Users\Frank\Documents\Arduino\WallE3_AnomalyRecovery_V2>mklink /H board.txt "C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt"

Hardlink created for board.txt <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt

C:\Users\Frank\Documents\Arduino\WallE3_AnomalyRecovery_V2>mklink /H TeensyOTA1.ttl "C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl"

Hardlink created for TeensyOTA1.ttl <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl

where ‘WallE3_AnomalyRecovery_V2’ gets replaced each time with the actual project name

Now, when I want to edit either the ‘board.txt’ or ‘TeensyOTA1.ttl’ file, I can simply right-click on the filename in any project folder and select ‘edit with NotePad++’, and the single file in Robot Common Files gets opened for edit, and the changes are immediately available in all projects that use the Teensy OTA feature.

31 January 2023 Update:

After trying this trick a few times, I realized I had left out a step, so this update fixes that. Below is the entire Cmd line session for my latest ‘new project’:

Microsoft Windows [Version 10.0.19044.2486]
(c) Microsoft Corporation. All rights reserved.

C:\Users\Frank>cd C:\Users\Frank\Documents\Arduino\WallE3_Complete_V1

C:\Users\Frank\Documents\Arduino\WallE3_Complete_V1>mklink /H board.txt "C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt"
Hardlink created for board.txt <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt

C:\Users\Frank\Documents\Arduino\WallE3_Complete_V1>mklink /H TeensyOTA1.ttl "C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl"
Hardlink created for TeensyOTA1.ttl <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl

C:\Users\Frank\Documents\Arduino\WallE3_Complete_V1>

Microsoft Windows [Version 10.0.19044.2486]

C:\Users\Frank>cd C:\Users\Frank\Documents\Arduino\WallE3_Complete_V1

C:\Users\Frank\Documents\Arduino\WallE3_Complete_V1>mklink /H board.txt "C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt"

Hardlink created for board.txt <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\board.txt

C:\Users\Frank\Documents\Arduino\WallE3_Complete_V1>mklink /H TeensyOTA1.ttl "C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl"

Hardlink created for TeensyOTA1.ttl <<===>> C:\Users\Frank\Documents\Arduino\Robot Common Files\TeensyOTA1.ttl

C:\Users\Frank\Documents\Arduino\WallE3_Complete_V1>

The first line above shows the ‘cd’ step, which I had left out of the previous post. After that, I just copy/pasted the ‘mklink….’ text for each of the two files, and then everything was wonderful again.

Stay Tuned,

Frank

Wall Parallel Find PID Tuning, Part II

2 Replies

Posted 01 December 2022

I thought I had the ‘parallel find’ problem solved about 18 months ago, back in April of 2021, but it seems this is a problem that refuses to die (well, up until now, anyway). This post describes the ‘new, new!’ solution, based on much improved distance measurement performance from the VL53L0X time-of-flight sensors and associated software. Basically, I discovered that I had been doing VL53L0X distance calibration all wrong, and a big part of the problem was my use of an integer instead of floating point data type to represent distance values. The VL53L0X sensor reports distances in integer millimeters, but when I converted the integer mm values to integer cm (without even rounding – but by truncation – yikes!) the resulting loss of precision caused significant problems with the parallel find algorithm.

So, after converting all my VL53L0X distance variables from integer to float (which turned out to be pretty easy as I had practiced good modularity and low coupling – yay!), I started over again with a two stage strategy. The first part addressed the calibration problem by redoing a series of tests to acquire compensation curves for both the right and left-side sensor arrays which were then programmed into the Teensy 3.5 processor that handles the VL53L0X arrays. The second part was to revisit the ‘parallel find’ algorithm. This post describes the result of the second part – revisiting the ‘parallel find’ algorithm.

The parallel find algorithm implements a two-stage search for the parallel condition, defined as the robot (or, more accurately, the relevant VL53L0X array) orientation that produces identical distance readings from the front and rear sensors of the relevant (i.e. left or right) array. The first ‘coarse’ stage searches for the change in sign of the steering value, and the second ‘fine tune’ stage searches for a steering value < 0.01, meaning that the front and rear distance values are nearly identical.

Here is the code for the parallel find subroutine:

bool RotateToParallelOrientation(WallTrackingCases trkdir, float Kp, float Ki, float Kd)
{
  //Purpose: Rotate to parallel orientation (steering val == 0)
  // Inputs:
  //	trkdir = WallTrackingCases enum denoting left or right-side tracking
  //	ParallelFindKp,Ki,Kd = integers representing PID engine control params
  // Outputs:
  //	Robot rotates CCW or CW to the desired orientation, under PID control
  // Plan:
  //  Step1: rotate to parallel using sign-change criteria
  //  Step2: fine-tune prallel using zero front/back distance criteria
  //Notes:
  //	Created 05/24/22 for new IRHomingNavigateToIAP algorithm
  //  11/15/22 chg side distances to float vs uint16_t
  //  12/02/22 added kp,ki,kd to sig

  float lastError = 0;
  float lastInput = 0;
  float lastIval = 0;
  float lastDerror = 0;
  float front_d = 0;
  float rear_d = 0;
  float ParallelFindOutput = 0; //05/24/22

  //DEBUG!!
  gl_pSerPort->printf("\nIn RotateToParallelOrientation(%s) with Kp/Ki/Kd = %2.1f/%2.1f/%2.1f\n",
    TrkStrArray[trkdir], Kp, Ki, Kd);
  //DEBUG!!

  StopBothMotors();

  AnomalyCode errcode = NO_ANOMALIES;
  float steerval = 0;
  float prev_steerval = 0;

  gl_pSerPort->printf("Msec\tHdg\tFrontD\tRearD\tSteer\tPSteer\terr\tIval\tOut\tspeed\n");

  //Step1: rotate to parallel using sign-change criteria
  elapsedMillis MsecSinceLastAdj = 0; //added 04/29/22
  //while (errcode == NO_ANOMALIES && steerval * prev_steerval >= 0)
  while (steerval * prev_steerval >= 0)
  {
    CheckForUserInput();

    if (MsecSinceLastAdj >= 200)
    {
      MsecSinceLastAdj -= 200;
      prev_steerval = steerval;
      UpdateAllEnvironmentParameters();
      errcode = CheckForAnomalies();

      steerval = (trkdir == TRACKING_LEFT) ? glLeftSteeringVal : glRightSteeringVal;
      front_d = (trkdir == TRACKING_LEFT) ? glLeftFrontCm : glRightFrontCm;
      rear_d = (trkdir == TRACKING_LEFT) ? glLeftRearCm : glRightRearCm;

      ParallelFindOutput = PIDCalcs(steerval, ParallelFindSetpoint, lastError,
        lastInput, lastIval, lastDerror, Kp, Ki, Kd);

      int16_t speed = (int16_t)ParallelFindOutput;

      //PIDCalcs out can be negative, must handle both signs
      speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed;
      speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed;

      UpdateIMUHdgValDeg();

      //DEBUG!!
      gl_pSerPort->printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%d\n",
        millis(), IMUHdgValDeg, front_d, rear_d, steerval, prev_steerval,
        lastError, lastIval, ParallelFindOutput, speed);
      //DEBUG!!

      RunBothMotorsBidirectional(speed, -speed); //CCW for speed < 0, CW for speed > 0
    }
  }

  gl_pSerPort->printf("Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = %2.1f/%2.1f/%2.1f/%s\n",
    IMUHdgValDeg, steerval, prev_steerval, AnomalyStrArray[errcode]);

  StopBothMotors();
  delay(1000);

  //Step2: fine-tune prallel using zero front/back distance criteria
      //at this point, front & rear distances should be very close, so I should be
      //able to make them equal.

  gl_pSerPort->printf("Msec\tHdg\tFrontD\tRearD\tSteer\tPSteer\terr\tIval\tOut\tspeed\n");

  MsecSinceLastAdj = 0; //added 04/29/22
  while (abs(steerval) > 0.01f)
  {
    CheckForUserInput();

    if (MsecSinceLastAdj >= 200)
    {
      MsecSinceLastAdj -= 200;
      prev_steerval = steerval;
      UpdateAllEnvironmentParameters();
      errcode = CheckForAnomalies();

      steerval = (trkdir == TRACKING_LEFT) ? glLeftSteeringVal : glRightSteeringVal;
      front_d = (trkdir == TRACKING_LEFT) ? glLeftFrontCm : glRightFrontCm;
      rear_d = (trkdir == TRACKING_LEFT) ? glLeftRearCm : glRightRearCm;

      ParallelFindOutput = PIDCalcs(steerval, ParallelFindSetpoint, lastError,
        lastInput, lastIval, lastDerror, ParallelFindKp, ParallelFindKi, ParallelFindKd);

      int16_t speed = (int16_t)ParallelFindOutput;

      //PIDCalcs out can be negative, must handle both signs
      speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed;
      speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed;

      //DEBUG!!
      gl_pSerPort->printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%d\n",
        millis(), IMUHdgValDeg, front_d, rear_d, steerval, prev_steerval,
        lastError, lastIval, ParallelFindOutput, speed);
      //DEBUG!!

      RunBothMotorsBidirectional(speed, -speed); //CCW for speed < 0, CW for speed > 0
    }
  }

  gl_pSerPort->printf("Exited RTPO fine tune with hdg/frontD/steerval/prev_steerval = %2.1f/%d/%2.2f/%2.2f\n",
    IMUHdgValDeg, glFrontCm, steerval, prev_steerval);

  return true;
}

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

bool RotateToParallelOrientation(WallTrackingCases trkdir, float Kp, float Ki, float Kd)

{

//Purpose: Rotate to parallel orientation (steering val == 0)

// Inputs:

// trkdir = WallTrackingCases enum denoting left or right-side tracking

// ParallelFindKp,Ki,Kd = integers representing PID engine control params

// Outputs:

// Robot rotates CCW or CW to the desired orientation, under PID control

// Plan:

// Step1: rotate to parallel using sign-change criteria

// Step2: fine-tune prallel using zero front/back distance criteria

//Notes:

// Created 05/24/22 for new IRHomingNavigateToIAP algorithm

// 11/15/22 chg side distances to float vs uint16_t

// 12/02/22 added kp,ki,kd to sig

float lastError = 0;

float lastInput = 0;

float lastIval = 0;

float lastDerror = 0;

float front_d = 0;

float rear_d = 0;

float ParallelFindOutput = 0; //05/24/22

//DEBUG!!

gl_pSerPort->printf("\nIn RotateToParallelOrientation(%s) with Kp/Ki/Kd = %2.1f/%2.1f/%2.1f\n",

TrkStrArray[trkdir], Kp, Ki, Kd);

//DEBUG!!

StopBothMotors();

AnomalyCode errcode = NO_ANOMALIES;

float steerval = 0;

float prev_steerval = 0;

gl_pSerPort->printf("Msec\tHdg\tFrontD\tRearD\tSteer\tPSteer\terr\tIval\tOut\tspeed\n");

//Step1: rotate to parallel using sign-change criteria

elapsedMillis MsecSinceLastAdj = 0; //added 04/29/22

//while (errcode == NO_ANOMALIES && steerval * prev_steerval >= 0)

while (steerval * prev_steerval >= 0)

{

CheckForUserInput();

if (MsecSinceLastAdj >= 200)

{

MsecSinceLastAdj -= 200;

prev_steerval = steerval;

UpdateAllEnvironmentParameters();

errcode = CheckForAnomalies();

steerval = (trkdir == TRACKING_LEFT) ? glLeftSteeringVal : glRightSteeringVal;

front_d = (trkdir == TRACKING_LEFT) ? glLeftFrontCm : glRightFrontCm;

rear_d = (trkdir == TRACKING_LEFT) ? glLeftRearCm : glRightRearCm;

ParallelFindOutput = PIDCalcs(steerval, ParallelFindSetpoint, lastError,

lastInput, lastIval, lastDerror, Kp, Ki, Kd);

int16_t speed = (int16_t)ParallelFindOutput;

//PIDCalcs out can be negative, must handle both signs

speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed;

speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed;

UpdateIMUHdgValDeg();

//DEBUG!!

gl_pSerPort->printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%d\n",

millis(), IMUHdgValDeg, front_d, rear_d, steerval, prev_steerval,

lastError, lastIval, ParallelFindOutput, speed);

//DEBUG!!

RunBothMotorsBidirectional(speed, -speed); //CCW for speed < 0, CW for speed > 0

}

gl_pSerPort->printf("Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = %2.1f/%2.1f/%2.1f/%s\n",

IMUHdgValDeg, steerval, prev_steerval, AnomalyStrArray[errcode]);

StopBothMotors();

delay(1000);

//Step2: fine-tune prallel using zero front/back distance criteria

//at this point, front & rear distances should be very close, so I should be

//able to make them equal.

gl_pSerPort->printf("Msec\tHdg\tFrontD\tRearD\tSteer\tPSteer\terr\tIval\tOut\tspeed\n");

MsecSinceLastAdj = 0; //added 04/29/22

while (abs(steerval) > 0.01f)

{

CheckForUserInput();

if (MsecSinceLastAdj >= 200)

{

MsecSinceLastAdj -= 200;

prev_steerval = steerval;

UpdateAllEnvironmentParameters();

errcode = CheckForAnomalies();

steerval = (trkdir == TRACKING_LEFT) ? glLeftSteeringVal : glRightSteeringVal;

front_d = (trkdir == TRACKING_LEFT) ? glLeftFrontCm : glRightFrontCm;

rear_d = (trkdir == TRACKING_LEFT) ? glLeftRearCm : glRightRearCm;

ParallelFindOutput = PIDCalcs(steerval, ParallelFindSetpoint, lastError,

lastInput, lastIval, lastDerror, ParallelFindKp, ParallelFindKi, ParallelFindKd);

int16_t speed = (int16_t)ParallelFindOutput;

//PIDCalcs out can be negative, must handle both signs

speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed;

speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed;

//DEBUG!!

gl_pSerPort->printf("%lu\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%d\n",

millis(), IMUHdgValDeg, front_d, rear_d, steerval, prev_steerval,

lastError, lastIval, ParallelFindOutput, speed);

//DEBUG!!

RunBothMotorsBidirectional(speed, -speed); //CCW for speed < 0, CW for speed > 0

}

gl_pSerPort->printf("Exited RTPO fine tune with hdg/frontD/steerval/prev_steerval = %2.1f/%d/%2.2f/%2.2f\n",

IMUHdgValDeg, glFrontCm, steerval, prev_steerval);

return true;

}

And here is a short video and the telemetry output for a successful parallel find run

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 100.0/10.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
33033	20.8	28.0	23.0	0.4	0.0	-0.4	-4.3	-47.3	-47
33235	14.1	27.0	24.0	0.3	0.4	-0.3	-7.7	-41.7	-41
33435	1.7	24.0	22.0	0.2	0.3	-0.2	-9.8	-30.8	-30
33644	-7.8	24.0	22.0	0.2	0.2	-0.2	-11.9	-32.9	-32
33824	-13.7	21.0	22.0	-0.1	0.2	0.1	-11.1	-3.1	-3
Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = -13.7/-0.1/0.2/NO_ANOMALIES
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
35057	-15.1	21.0	22.0	-0.1	-0.1	0.1	-9.9	2.1	2
35267	-15.1	20.0	23.0	-0.2	-0.1	0.2	-7.5	16.5	16
35467	-15.0	20.0	23.0	-0.2	-0.2	0.2	-5.1	18.9	18
35656	-14.7	21.0	23.0	-0.2	-0.2	0.2	-3.1	16.9	16
35854	-14.6	21.0	21.0	-0.1	-0.2	0.1	-2.6	2.4	2
36068	-14.6	21.0	21.0	-0.1	-0.1	0.1	-2.1	2.9	2
36258	-14.6	20.0	22.0	-0.1	-0.1	0.1	-1.0	10.0	10
36455	-14.6	20.0	22.0	-0.1	-0.1	0.1	0.1	11.1	11
36655	-14.6	20.0	22.0	-0.1	-0.1	0.1	1.4	14.4	14
36857	-14.6	20.0	22.0	-0.1	-0.1	0.1	2.7	15.7	15
37058	-14.6	20.0	22.0	-0.2	-0.1	0.2	4.6	23.6	23
37258	-14.2	20.0	20.0	0.0	-0.2	-0.0	4.2	0.2	0
37458	-14.0	20.0	20.0	0.0	0.0	-0.0	3.8	-0.2	0
37656	-14.0	21.0	22.0	-0.1	0.0	0.1	4.8	14.8	14
37860	-14.1	20.0	22.0	-0.2	-0.1	0.2	6.5	23.5	23
38060	-13.7	20.0	22.0	-0.2	-0.2	0.2	8.2	25.2	25
38258	-12.7	20.0	22.0	-0.1	-0.2	0.1	9.6	23.6	23
38460	-11.3	20.0	22.0	-0.1	-0.1	0.1	11.0	25.0	24
38659	-10.2	20.0	20.0	0.1	-0.1	-0.1	10.4	4.4	4
38860	-10.1	20.0	22.0	-0.1	0.1	0.1	11.7	24.7	24
39060	-10.1	20.0	22.0	-0.1	-0.1	0.1	13.0	26.0	25
39261	-10.0	20.0	20.0	0.0	-0.1	0.0	13.0	13.0	12
Exited RTPO fine tune with hdg/frontD/steerval/prev_steerval = -10.0/400/0.00/-0.13
39279	20.0	19.0	20.0	0.00
39401	20.0	19.0	20.0	0.00
39513	20.0	20.0	20.0	0.03
39636	20.0	20.0	20.0	0.03

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 100.0/10.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

33033 20.8 28.0 23.0 0.4 0.0 -0.4 -4.3 -47.3 -47

33235 14.1 27.0 24.0 0.3 0.4 -0.3 -7.7 -41.7 -41

33435 1.7 24.0 22.0 0.2 0.3 -0.2 -9.8 -30.8 -30

33644 -7.8 24.0 22.0 0.2 0.2 -0.2 -11.9 -32.9 -32

33824 -13.7 21.0 22.0 -0.1 0.2 0.1 -11.1 -3.1 -3

Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = -13.7/-0.1/0.2/NO_ANOMALIES

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

35057 -15.1 21.0 22.0 -0.1 -0.1 0.1 -9.9 2.1 2

35267 -15.1 20.0 23.0 -0.2 -0.1 0.2 -7.5 16.5 16

35467 -15.0 20.0 23.0 -0.2 -0.2 0.2 -5.1 18.9 18

35656 -14.7 21.0 23.0 -0.2 -0.2 0.2 -3.1 16.9 16

35854 -14.6 21.0 21.0 -0.1 -0.2 0.1 -2.6 2.4 2

36068 -14.6 21.0 21.0 -0.1 -0.1 0.1 -2.1 2.9 2

36258 -14.6 20.0 22.0 -0.1 -0.1 0.1 -1.0 10.0 10

36455 -14.6 20.0 22.0 -0.1 -0.1 0.1 0.1 11.1 11

36655 -14.6 20.0 22.0 -0.1 -0.1 0.1 1.4 14.4 14

36857 -14.6 20.0 22.0 -0.1 -0.1 0.1 2.7 15.7 15

37058 -14.6 20.0 22.0 -0.2 -0.1 0.2 4.6 23.6 23

37258 -14.2 20.0 20.0 0.0 -0.2 -0.0 4.2 0.2 0

37458 -14.0 20.0 20.0 0.0 0.0 -0.0 3.8 -0.2 0

37656 -14.0 21.0 22.0 -0.1 0.0 0.1 4.8 14.8 14

37860 -14.1 20.0 22.0 -0.2 -0.1 0.2 6.5 23.5 23

38060 -13.7 20.0 22.0 -0.2 -0.2 0.2 8.2 25.2 25

38258 -12.7 20.0 22.0 -0.1 -0.2 0.1 9.6 23.6 23

38460 -11.3 20.0 22.0 -0.1 -0.1 0.1 11.0 25.0 24

38659 -10.2 20.0 20.0 0.1 -0.1 -0.1 10.4 4.4 4

38860 -10.1 20.0 22.0 -0.1 0.1 0.1 11.7 24.7 24

39060 -10.1 20.0 22.0 -0.1 -0.1 0.1 13.0 26.0 25

39261 -10.0 20.0 20.0 0.0 -0.1 0.0 13.0 13.0 12

Exited RTPO fine tune with hdg/frontD/steerval/prev_steerval = -10.0/400/0.00/-0.13

39279 20.0 19.0 20.0 0.00

39401 20.0 19.0 20.0 0.00

39513 20.0 20.0 20.0 0.03

39636 20.0 20.0 20.0 0.03

So it appears that my original parallel find algorithm now performs much better, due mainly to the more accurate distance reporting obtained by changing from integer to float data type, and better distance compensation curves for each of the six sensors in the two VL53L0X arrays. Now I need to back-port this improved code into my main robot navigation code, probably by way of the Wall Track part-task program described in my earlier ‘More Wall Track PID Tuning‘ post.

29 December 2022 Update:

After going through some additional ‘part-task’ tune-up exercises, I started the process of integrating all the changes back into my Wall Tracking PID Tuning code. After the usual number of screw-ups I got the left-side offset capture feature working up to the point of turning back parallel to the wall. The code at this point just used ‘SpinTurn()’ to turn 30º in the opposite direction as the first turn away from the wall, with a note that this was done because ‘Parallel Find is fatally flawed’. Well, since I had just gotten through testing this feature I thought I could just drop it in. When I did, the robot promptly turned in the wrong direction and started spinning – yikes! Back to the ‘Parallel Find’ drawing board!

After making some code changes to make the part-task code a bit easier to use, I got everything working again, with basically the same PID values (100,0,0) as before. Here’s the output and a short video from a run:

At top of parameter retrieval section
Wallstr = 0
isRightWall = FALSE
Parallel Find for Left Wall with PID = (100.0,0.0,0.0)
Enter any key to start
Got 48
Got 48

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 100.0/0.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
941942	109.2	36.0	30.0	0.6	0.0	-0.6	0.0	-63.0	-63
942142	97.7	31.0	27.0	0.4	0.6	-0.4	0.0	-38.0	-38
942311: Error in GetFrontDistCm() - 0 replaced with 312
942342	79.0	24.0	22.0	0.2	0.4	-0.2	0.0	-15.0	-15
942533	70.3	22.0	22.0	0.1	0.2	-0.1	0.0	-7.0	-7
942742	70.0	22.0	22.0	0.0	0.1	-0.0	0.0	-2.0	-2
942941	70.1	22.0	22.0	0.0	0.0	-0.0	0.0	-2.0	-2
943153	70.1	22.0	22.0	0.0	0.0	-0.0	0.0	-3.0	-3
943342	70.1	22.0	22.0	-0.0	0.0	0.0	0.0	2.0	2
Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = 70.1/-0.0/0.0/NO_ANOMALIES

At top of parameter retrieval section

Wallstr = 0

isRightWall = FALSE

Parallel Find for Left Wall with PID = (100.0,0.0,0.0)

Enter any key to start

Got 48

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 100.0/0.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

941942 109.2 36.0 30.0 0.6 0.0 -0.6 0.0 -63.0 -63

942142 97.7 31.0 27.0 0.4 0.6 -0.4 0.0 -38.0 -38

942311: Error in GetFrontDistCm() - 0 replaced with 312

942342 79.0 24.0 22.0 0.2 0.4 -0.2 0.0 -15.0 -15

942533 70.3 22.0 22.0 0.1 0.2 -0.1 0.0 -7.0 -7

942742 70.0 22.0 22.0 0.0 0.1 -0.0 0.0 -2.0 -2

942941 70.1 22.0 22.0 0.0 0.0 -0.0 0.0 -2.0 -2

943153 70.1 22.0 22.0 0.0 0.0 -0.0 0.0 -3.0 -3

943342 70.1 22.0 22.0 -0.0 0.0 0.0 0.0 2.0 2

Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = 70.1/-0.0/0.0/NO_ANOMALIES

30 December 2022 Update:

Not so fast, pardner! Well, it seems I was a bit premature about completing this effort – After a few more trials I realized this wasn’t working anywhere near as well as I thought – oops!

So, after lots more trials aimlessly wandering around PID space, I came up with new values that *seem* to work (for the left side case, at least). I also discovered that I really don’t need a two-stage process (‘coarse’ tune followed by ‘fine’ tune) to get a decent result, as long as the robot turns slowly enough to allow the distance measurement changes to keep up. Here’s are a couple of runs (telemetry output and short video) for the new setup.

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
528906	-42.5	44.60	47.40	-0.28	0.00	0.28	1.12	6.72	6
529106	-42.5	44.70	46.20	-0.15	-0.28	0.15	1.72	4.72	4
529305	-42.5	45.30	46.40	-0.11	-0.15	0.11	2.16	4.36	4
529506	-42.5	43.50	47.30	-0.38	-0.11	0.38	3.68	11.28	11
529705	-42.5	43.50	47.00	-0.35	-0.38	0.35	5.08	12.08	12
529905	-42.5	44.10	46.50	-0.19	-0.35	0.19	5.84	9.64	9
530106	-42.5	43.80	45.90	-0.21	-0.19	0.21	6.68	10.88	10
530297	-42.5	44.60	47.30	-0.27	-0.21	0.27	7.76	13.16	13
530506	-42.5	43.70	46.30	-0.26	-0.27	0.26	8.80	14.00	14
530707	-42.5	44.20	47.00	-0.28	-0.26	0.28	9.92	15.52	15
530907	-42.5	44.50	47.70	-0.32	-0.28	0.32	11.20	17.60	17
531108	-42.5	44.30	47.60	-0.33	-0.32	0.33	12.52	19.12	19
531298	-41.7	43.90	46.30	-0.24	-0.33	0.24	13.48	18.28	18
531509	-41.4	43.40	45.70	-0.23	-0.24	0.23	14.40	19.00	19
531708	-41.4	43.60	44.50	-0.09	-0.23	0.09	14.76	16.56	16
531909	-41.4	44.40	46.10	-0.17	-0.09	0.17	15.44	18.84	18
532107	-41.4	44.20	46.20	-0.20	-0.17	0.20	16.24	20.24	20
532298	-41.4	44.00	47.00	-0.30	-0.20	0.30	17.44	23.44	23
532510	-41.3	43.20	46.90	-0.37	-0.30	0.37	18.92	26.32	26
532709	-39.7	43.40	45.40	-0.20	-0.37	0.20	19.72	23.72	23
532910	-37.4	41.60	43.30	-0.17	-0.20	0.17	20.40	23.80	23
533109	-35.3	39.90	41.90	-0.20	-0.17	0.20	21.20	25.20	25
533301	-33.4	39.70	41.90	-0.22	-0.20	0.22	22.08	26.48	26
533510	-31.7	38.60	40.20	-0.16	-0.22	0.16	22.72	25.92	25
533711	-30.1	37.20	37.90	-0.07	-0.16	0.07	23.00	24.40	24
533900	-28.4	37.00	38.40	-0.14	-0.07	0.14	23.56	26.36	26
534111	-26.6	36.10	37.30	-0.12	-0.14	0.12	24.04	26.44	26
534302	-25.2	35.90	36.30	-0.04	-0.12	0.04	24.20	25.00	24
534511	-23.6	35.00	36.40	-0.14	-0.04	0.14	24.76	27.56	27
534710	-21.7	34.10	35.20	-0.11	-0.14	0.11	25.20	27.40	27
534912	-19.6	33.90	34.70	-0.08	-0.11	0.08	25.52	27.12	27
535111	-17.3	33.40	33.30	0.01	-0.08	-0.01	25.48	25.28	25
Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = -17.29/0.01/-0.08/NO_ANOMALIES
536138	33.7	34.6	33.8	-0.01
536247	32.9	33.7	33.6	-0.07
536365	33.4	33.9	33.6	-0.02
536475	33.4	34.5	33.4	0.00
536591	33.5	33.5	33.5	0.00
536702	33.6	33.7	33.6	0.00
536820	33.1	34.2	33.3	-0.02
536928	33.5	34.3	33.6	-0.01
537038	33.0	34.1	33.8	-0.08
537156	32.9	33.6	33.8	-0.08

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

528906 -42.5 44.60 47.40 -0.28 0.00 0.28 1.12 6.72 6

529106 -42.5 44.70 46.20 -0.15 -0.28 0.15 1.72 4.72 4

529305 -42.5 45.30 46.40 -0.11 -0.15 0.11 2.16 4.36 4

529506 -42.5 43.50 47.30 -0.38 -0.11 0.38 3.68 11.28 11

529705 -42.5 43.50 47.00 -0.35 -0.38 0.35 5.08 12.08 12

529905 -42.5 44.10 46.50 -0.19 -0.35 0.19 5.84 9.64 9

530106 -42.5 43.80 45.90 -0.21 -0.19 0.21 6.68 10.88 10

530297 -42.5 44.60 47.30 -0.27 -0.21 0.27 7.76 13.16 13

530506 -42.5 43.70 46.30 -0.26 -0.27 0.26 8.80 14.00 14

530707 -42.5 44.20 47.00 -0.28 -0.26 0.28 9.92 15.52 15

530907 -42.5 44.50 47.70 -0.32 -0.28 0.32 11.20 17.60 17

531108 -42.5 44.30 47.60 -0.33 -0.32 0.33 12.52 19.12 19

531298 -41.7 43.90 46.30 -0.24 -0.33 0.24 13.48 18.28 18

531509 -41.4 43.40 45.70 -0.23 -0.24 0.23 14.40 19.00 19

531708 -41.4 43.60 44.50 -0.09 -0.23 0.09 14.76 16.56 16

531909 -41.4 44.40 46.10 -0.17 -0.09 0.17 15.44 18.84 18

532107 -41.4 44.20 46.20 -0.20 -0.17 0.20 16.24 20.24 20

532298 -41.4 44.00 47.00 -0.30 -0.20 0.30 17.44 23.44 23

532510 -41.3 43.20 46.90 -0.37 -0.30 0.37 18.92 26.32 26

532709 -39.7 43.40 45.40 -0.20 -0.37 0.20 19.72 23.72 23

532910 -37.4 41.60 43.30 -0.17 -0.20 0.17 20.40 23.80 23

533109 -35.3 39.90 41.90 -0.20 -0.17 0.20 21.20 25.20 25

533301 -33.4 39.70 41.90 -0.22 -0.20 0.22 22.08 26.48 26

533510 -31.7 38.60 40.20 -0.16 -0.22 0.16 22.72 25.92 25

533711 -30.1 37.20 37.90 -0.07 -0.16 0.07 23.00 24.40 24

533900 -28.4 37.00 38.40 -0.14 -0.07 0.14 23.56 26.36 26

534111 -26.6 36.10 37.30 -0.12 -0.14 0.12 24.04 26.44 26

534302 -25.2 35.90 36.30 -0.04 -0.12 0.04 24.20 25.00 24

534511 -23.6 35.00 36.40 -0.14 -0.04 0.14 24.76 27.56 27

534710 -21.7 34.10 35.20 -0.11 -0.14 0.11 25.20 27.40 27

534912 -19.6 33.90 34.70 -0.08 -0.11 0.08 25.52 27.12 27

535111 -17.3 33.40 33.30 0.01 -0.08 -0.01 25.48 25.28 25

Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = -17.29/0.01/-0.08/NO_ANOMALIES

536138 33.7 34.6 33.8 -0.01

536247 32.9 33.7 33.6 -0.07

536365 33.4 33.9 33.6 -0.02

536475 33.4 34.5 33.4 0.00

536591 33.5 33.5 33.5 0.00

536702 33.6 33.7 33.6 0.00

536820 33.1 34.2 33.3 -0.02

536928 33.5 34.3 33.6 -0.01

537038 33.0 34.1 33.8 -0.08

537156 32.9 33.6 33.8 -0.08

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
623308	31.3	42.40	37.10	0.53	0.00	-0.53	-2.12	-12.72	-12
623498	31.3	42.80	37.50	0.53	0.53	-0.53	-4.24	-14.84	-14
623697	31.2	42.00	37.30	0.47	0.53	-0.47	-6.12	-15.52	-15
623897	31.1	42.40	36.90	0.55	0.47	-0.55	-8.32	-19.32	-19
624099	31.0	41.30	37.10	0.42	0.55	-0.42	-10.00	-18.40	-18
624297	31.0	41.30	37.40	0.39	0.42	-0.39	-11.56	-19.36	-19
624497	31.0	41.00	37.20	0.38	0.39	-0.38	-13.08	-20.68	-20
624699	30.9	42.80	37.20	0.49	0.38	-0.49	-15.04	-24.84	-24
624899	30.4	42.80	36.20	0.66	0.49	-0.66	-17.68	-30.88	-30
625098	26.4	40.40	36.10	0.43	0.66	-0.43	-19.40	-28.00	-28
625300	20.9	38.10	33.80	0.43	0.43	-0.43	-21.12	-29.72	-29
625510	15.1	36.30	33.30	0.30	0.43	-0.30	-22.32	-28.32	-28
625701	10.6	35.50	32.40	0.31	0.30	-0.31	-23.56	-29.76	-29
625900	5.1	34.10	32.10	0.20	0.31	-0.20	-24.36	-28.36	-28
626100	0.7	34.30	32.10	0.22	0.20	-0.22	-25.24	-29.64	-29
626290	-3.1	31.70	32.20	-0.05	0.22	0.05	-25.04	-24.04	-24
Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = -3.07/-0.05/0.22/NO_ANOMALIES
627317	32.3	35.1	30.8	0.15
627430	32.1	34.8	31.5	0.06
627538	32.1	34.2	31.0	0.11
627654	32.2	33.9	31.1	0.11
627762	32.6	34.7	31.5	0.11
627879	32.0	34.9	30.9	0.11
627987	32.4	35.3	31.1	0.13
628101	32.7	34.6	31.5	0.12
628214	32.9	35.4	31.5	0.14
628328	32.5	34.9	31.6	0.09

In RotateToParallelOrientation(Left) with Kp/Ki/Kd = 20.0/4.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

623308 31.3 42.40 37.10 0.53 0.00 -0.53 -2.12 -12.72 -12

623498 31.3 42.80 37.50 0.53 0.53 -0.53 -4.24 -14.84 -14

623697 31.2 42.00 37.30 0.47 0.53 -0.47 -6.12 -15.52 -15

623897 31.1 42.40 36.90 0.55 0.47 -0.55 -8.32 -19.32 -19

624099 31.0 41.30 37.10 0.42 0.55 -0.42 -10.00 -18.40 -18

624297 31.0 41.30 37.40 0.39 0.42 -0.39 -11.56 -19.36 -19

624497 31.0 41.00 37.20 0.38 0.39 -0.38 -13.08 -20.68 -20

624699 30.9 42.80 37.20 0.49 0.38 -0.49 -15.04 -24.84 -24

624899 30.4 42.80 36.20 0.66 0.49 -0.66 -17.68 -30.88 -30

625098 26.4 40.40 36.10 0.43 0.66 -0.43 -19.40 -28.00 -28

625300 20.9 38.10 33.80 0.43 0.43 -0.43 -21.12 -29.72 -29

625510 15.1 36.30 33.30 0.30 0.43 -0.30 -22.32 -28.32 -28

625701 10.6 35.50 32.40 0.31 0.30 -0.31 -23.56 -29.76 -29

625900 5.1 34.10 32.10 0.20 0.31 -0.20 -24.36 -28.36 -28

626100 0.7 34.30 32.10 0.22 0.20 -0.22 -25.24 -29.64 -29

626290 -3.1 31.70 32.20 -0.05 0.22 0.05 -25.04 -24.04 -24

Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = -3.07/-0.05/0.22/NO_ANOMALIES

627317 32.3 35.1 30.8 0.15

627430 32.1 34.8 31.5 0.06

627538 32.1 34.2 31.0 0.11

627654 32.2 33.9 31.1 0.11

627762 32.6 34.7 31.5 0.11

627879 32.0 34.9 30.9 0.11

627987 32.4 35.3 31.1 0.13

628101 32.7 34.6 31.5 0.12

628214 32.9 35.4 31.5 0.14

628328 32.5 34.9 31.6 0.09

In RotateToParallelOrientation(Right) with Kp/Ki/Kd = 20.0/4.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
134663	55.1	46.10	50.90	-0.48	0.00	0.48	1.92	11.52	11
134875	55.1	45.40	51.20	-0.58	-0.48	0.58	4.24	15.84	15
135064	55.1	45.60	53.00	-0.74	-0.58	0.74	7.20	22.00	22
135266	53.8	44.70	52.00	-0.73	-0.74	0.73	10.12	24.72	24
135465	51.0	43.90	49.10	-0.52	-0.73	0.52	12.20	22.60	22
135666	48.6	41.30	46.20	-0.49	-0.52	0.49	14.16	23.96	23
135864	46.3	40.00	43.90	-0.39	-0.49	0.39	15.72	23.52	23
136066	44.2	39.00	43.70	-0.47	-0.39	0.47	17.60	27.00	26
136266	41.2	37.60	40.60	-0.30	-0.47	0.30	18.80	24.80	24
136466	37.6	36.80	39.30	-0.25	-0.30	0.25	19.80	24.80	24
136677	33.6	35.60	38.20	-0.26	-0.25	0.26	20.84	26.04	26
136867	30.0	34.60	35.50	-0.09	-0.26	0.09	21.20	23.00	23
137066	26.7	33.70	34.10	-0.04	-0.09	0.04	21.36	22.16	22
137267	24.4	33.60	34.50	-0.09	-0.04	0.09	21.72	23.52	23
137467	22.3	33.20	33.10	0.01	-0.09	-0.01	21.68	21.48	21
Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = 22.29/0.01/-0.09/NO_ANOMALIES
138486	33.4	32.7	33.5	-0.01
138600	33.2	32.7	33.6	-0.04
138708	33.2	32.3	33.7	-0.05
138822	33.0	32.6	33.3	-0.03
138926	32.9	32.3	32.5	0.04
139035	33.3	31.9	33.8	-0.05
139146	32.8	32.4	33.5	-0.07
139254	33.1	32.6	33.1	0.00
139367	33.0	32.5	33.8	-0.08
139474	32.7	32.2	33.9	-0.12

In RotateToParallelOrientation(Right) with Kp/Ki/Kd = 20.0/4.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

134663 55.1 46.10 50.90 -0.48 0.00 0.48 1.92 11.52 11

134875 55.1 45.40 51.20 -0.58 -0.48 0.58 4.24 15.84 15

135064 55.1 45.60 53.00 -0.74 -0.58 0.74 7.20 22.00 22

135266 53.8 44.70 52.00 -0.73 -0.74 0.73 10.12 24.72 24

135465 51.0 43.90 49.10 -0.52 -0.73 0.52 12.20 22.60 22

135666 48.6 41.30 46.20 -0.49 -0.52 0.49 14.16 23.96 23

135864 46.3 40.00 43.90 -0.39 -0.49 0.39 15.72 23.52 23

136066 44.2 39.00 43.70 -0.47 -0.39 0.47 17.60 27.00 26

136266 41.2 37.60 40.60 -0.30 -0.47 0.30 18.80 24.80 24

136466 37.6 36.80 39.30 -0.25 -0.30 0.25 19.80 24.80 24

136677 33.6 35.60 38.20 -0.26 -0.25 0.26 20.84 26.04 26

136867 30.0 34.60 35.50 -0.09 -0.26 0.09 21.20 23.00 23

137066 26.7 33.70 34.10 -0.04 -0.09 0.04 21.36 22.16 22

137267 24.4 33.60 34.50 -0.09 -0.04 0.09 21.72 23.52 23

137467 22.3 33.20 33.10 0.01 -0.09 -0.01 21.68 21.48 21

Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = 22.29/0.01/-0.09/NO_ANOMALIES

138486 33.4 32.7 33.5 -0.01

138600 33.2 32.7 33.6 -0.04

138708 33.2 32.3 33.7 -0.05

138822 33.0 32.6 33.3 -0.03

138926 32.9 32.3 32.5 0.04

139035 33.3 31.9 33.8 -0.05

139146 32.8 32.4 33.5 -0.07

139254 33.1 32.6 33.1 0.00

139367 33.0 32.5 33.8 -0.08

139474 32.7 32.2 33.9 -0.12

Starting Parallel Find Test - enter Wall (1 = right/0 = left), PID triplet Kp,Ki,Kd
At top of parameter retrieval section
Wallstr = 1
isRightWall = TRUE
Parallel Find for Right Wall with PID = (20.0,4.0,0.0)
Enter any key to start
Got 44
Got 10

In RotateToParallelOrientation(Right) with Kp/Ki/Kd = 20.0/4.0/0.0
Msec	Hdg	FrontD	RearD	Steer	PSteer	err	Ival	Out	speed
168221	-14.9	47.00	42.10	0.49	0.00	-0.49	-1.96	-11.76	-11
168421	-14.9	47.10	41.50	0.56	0.49	-0.56	-4.20	-15.40	-15
168622	-14.9	46.80	40.30	0.65	0.56	-0.65	-6.80	-19.80	-19
168812	-14.8	47.00	42.30	0.47	0.65	-0.47	-8.68	-18.08	-18
169011	-14.4	46.90	40.30	0.66	0.47	-0.66	-11.32	-24.52	-24
169212	-13.5	46.80	40.40	0.64	0.66	-0.64	-13.88	-26.68	-26
169412	-11.0	45.70	41.00	0.47	0.64	-0.47	-15.76	-25.16	-25
169611	-8.5	44.30	39.60	0.47	0.47	-0.47	-17.64	-27.04	-27
169812	-5.5	43.60	38.90	0.47	0.47	-0.47	-19.52	-28.92	-28
170013	-1.7	42.30	37.60	0.47	0.47	-0.47	-21.40	-30.80	-30
170213	2.7	40.70	38.90	0.23	0.47	-0.23	-22.32	-26.92	-26
170413	6.6	39.60	37.80	0.18	0.23	-0.18	-23.04	-26.64	-26
170613	9.6	38.60	37.80	0.08	0.18	-0.08	-23.36	-24.96	-24
170812	11.8	38.60	36.10	0.25	0.08	-0.25	-24.36	-29.36	-29
171013	15.0	38.60	38.50	0.01	0.25	-0.01	-24.40	-24.60	-24
171214	17.3	38.60	37.90	0.07	0.01	-0.07	-24.68	-26.08	-26
171414	19.0	37.90	38.70	-0.08	0.07	0.08	-24.36	-22.76	-22
Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = 19.01/-0.08/0.07/NO_ANOMALIES
172430	37.9	37.6	38.3	-0.04
172535	38.3	37.0	38.5	-0.02
172642	38.3	37.7	38.3	0.00
172748	38.0	37.7	37.6	0.04
172855	38.3	37.4	38.4	-0.01
172962	38.3	37.7	37.8	0.05
173071	37.8	37.9	37.3	0.05
173175	38.1	37.5	37.5	0.06
173282	37.6	37.8	39.6	-0.20
173391	38.3	37.3	38.7	-0.04

Starting Parallel Find Test - enter Wall (1 = right/0 = left), PID triplet Kp,Ki,Kd

At top of parameter retrieval section

Wallstr = 1

isRightWall = TRUE

Parallel Find for Right Wall with PID = (20.0,4.0,0.0)

Enter any key to start

Got 44

Got 10

In RotateToParallelOrientation(Right) with Kp/Ki/Kd = 20.0/4.0/0.0

Msec Hdg FrontD RearD Steer PSteer err Ival Out speed

168221 -14.9 47.00 42.10 0.49 0.00 -0.49 -1.96 -11.76 -11

168421 -14.9 47.10 41.50 0.56 0.49 -0.56 -4.20 -15.40 -15

168622 -14.9 46.80 40.30 0.65 0.56 -0.65 -6.80 -19.80 -19

168812 -14.8 47.00 42.30 0.47 0.65 -0.47 -8.68 -18.08 -18

169011 -14.4 46.90 40.30 0.66 0.47 -0.66 -11.32 -24.52 -24

169212 -13.5 46.80 40.40 0.64 0.66 -0.64 -13.88 -26.68 -26

169412 -11.0 45.70 41.00 0.47 0.64 -0.47 -15.76 -25.16 -25

169611 -8.5 44.30 39.60 0.47 0.47 -0.47 -17.64 -27.04 -27

169812 -5.5 43.60 38.90 0.47 0.47 -0.47 -19.52 -28.92 -28

170013 -1.7 42.30 37.60 0.47 0.47 -0.47 -21.40 -30.80 -30

170213 2.7 40.70 38.90 0.23 0.47 -0.23 -22.32 -26.92 -26

170413 6.6 39.60 37.80 0.18 0.23 -0.18 -23.04 -26.64 -26

170613 9.6 38.60 37.80 0.08 0.18 -0.08 -23.36 -24.96 -24

170812 11.8 38.60 36.10 0.25 0.08 -0.25 -24.36 -29.36 -29

171013 15.0 38.60 38.50 0.01 0.25 -0.01 -24.40 -24.60 -24

171214 17.3 38.60 37.90 0.07 0.01 -0.07 -24.68 -26.08 -26

171414 19.0 37.90 38.70 -0.08 0.07 0.08 -24.36 -22.76 -22

Exited RTPO coarse tune with hdg/steerval/prev_steerval/errcode = 19.01/-0.08/0.07/NO_ANOMALIES

172430 37.9 37.6 38.3 -0.04

172535 38.3 37.0 38.5 -0.02

172642 38.3 37.7 38.3 0.00

172748 38.0 37.7 37.6 0.04

172855 38.3 37.4 38.4 -0.01

172962 38.3 37.7 37.8 0.05

173071 37.8 37.9 37.3 0.05

173175 38.1 37.5 37.5 0.06

173282 37.6 37.8 39.6 -0.20

173391 38.3 37.3 38.7 -0.04

05 January 2023 Update:

Well, even the above ‘slow as she goes’ idea doesn’t work all the time or that well. I wound up trying to instrument what is going on by doing the following

Turning the robot in 10⁰ steps, followed by a 1500 mSec pause to let the measurements catch up
At 100mSec intervals during the 1500mSec pause, computing and displaying the 5-pt running average of the left front and rear distances, along with the running average computation of the steering value.

When I did this, I got the following plot

1500mSec plot of the steering value computed from a 5-pt running average of the left front and left rear distances. Each line is a different 10deg angle orientation, with the lowest (yellow) line representing a parallel or near-parallel orientation

As can be seen from the above plots, there is significant variation over the 1500mSec interval, even though the robot is stationary. In particular, the last plot shows that at the start of the 1500mSec stationary period, the steering value goes from 0.15 (i.e. not parallel at all) to near zero (i.e. very parallel), even though the robot isn’t moving at all.

I have no idea why this is happening, but it sure screws up any thoughts of rapidly finding a parallel orientation, and even sheds significant doubt on the entire idea of using multiple VL53L0X sensors for wall tracking – UGH!!

This time I tried a ‘parallel find’ run using 5⁰ SpinTurn() steps, with no averaging. Here’s the data, and a short video showing the result

This actually looks pretty good, and the unaveraged distance sensor results, although noisy, behaved reasonably well.

07 January 2023 Update:

As I was drifting off to sleep After yesterday’s ‘successful’ trial runs, I was happy visualizing the robot using the ‘SpinTurn() facility to gradually turn to the (mostly) parallel position (I’m a visual person, so I often turn programming problems into visual ones), it suddenly struck me that I was taking the long way around the barn; here I was sneaking up on the parallel orientation by small SpinTurn increments waiting for the steering value to change signs, when actually all I had to do was use my ‘GetWallOrientDeg(float steerval)’. This function takes the current steering value as the sole parameter and returns the current orientation angle with respect to the nearest wall; with this information I could use SpinTurn() to rotate to the parallel orientation in one shot – cool!

So, I recoded my test program to do just that, with the ‘enhancement’ (I hope) of taking a second or so at the start to acquire a 10-point average of the steering value, so as to, hopefully, reduce errors due to noisy distance sensor outputs. Here’s the output and a short video showing the result:

40.20	36.00
40.60	35.90
40.00	35.40
40.50	35.20
40.10	35.10
40.70	36.00
40.00	34.90
39.60	35.50
40.20	35.00
40.30	35.70
starting front/rear/steer avgs = 40.22/35.47/0.47
GetWallOrientDeg(0.47) returned starting orientation of 27.14 deg
Init hdg = 39.23 deg, Turn = 27.14  deg, tgt = 12.09 deg, timeout = 1.81 sec

32.20	30.30
31.30	29.70
30.90	29.90
31.10	30.20
31.40	30.20
31.40	29.70
31.20	29.90
30.60	29.90
30.80	29.70
31.10	29.20
ending front/rear/steer avgs = 31.20/29.87/0.13
GetWallOrientDeg(0.13) returned ending orientation of 7.60 deg

40.20 36.00

40.60 35.90

40.00 35.40

40.50 35.20

40.10 35.10

40.70 36.00

40.00 34.90

39.60 35.50

40.20 35.00

40.30 35.70

starting front/rear/steer avgs = 40.22/35.47/0.47

GetWallOrientDeg(0.47) returned starting orientation of 27.14 deg

Init hdg = 39.23 deg, Turn = 27.14 deg, tgt = 12.09 deg, timeout = 1.81 sec

32.20 30.30

31.30 29.70

30.90 29.90

31.10 30.20

31.40 30.20

31.40 29.70

31.20 29.90

30.60 29.90

30.80 29.70

31.10 29.20

ending front/rear/steer avgs = 31.20/29.87/0.13

GetWallOrientDeg(0.13) returned ending orientation of 7.60 deg

This seemed to work very well, and is a much simpler algorithm than trying to use an ‘on the fly’ steering value and a PID machine. I believe with this result I can get back to the problem I was originally trying to solve – that of reasonable wall following.

02 February 2023 Update:

Referring to my current quest to incorporate the results from all my previous ‘part-task’ programs into a ‘WallE3_Complete_V1’, I have been going through each ‘part-task’ program to verify results, and then incorporating the results (usually in the form of a single function) into the ‘complete’ program. When I got to WallE3_ParallelFind_V1, I discovered that, although I had made quite a bit of progress, including drastically simplifying the ‘parallel find’ algorithm, it still didn’t work very well.

After some additional work, I now think that ‘parallel find’ is ready for inclusion in the complete program. Here’s the functional code from WallE3_ParallelFind_V1:

//Step 0: set up arrays for 5-point running average calcs
const uint16_t DIST_ARRAY_SIZE = 10;//01/07/23
float frontdistsum = 0;
float reardistsum = 0;

if (isRightWall)
{
  //01/07/22 now using initial average steering value for single-turn solution
  for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)
  {
	GetRequestedVL53l0xValues(VL53L0X_RIGHT);
	frontdistsum += glRightFrontCm;
	reardistsum += glRightRearCm;
	delay(200);
	gl_pSerPort->printf("%2.2f\t%2.2f\n", glRightFrontCm, glRightRearCm);
  }

  float avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;
  float avgreardist = reardistsum / DIST_ARRAY_SIZE;
  float avgsteerval = (avgfrontdist - avgreardist) / 10.f;
  gl_pSerPort->printf("starting front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",
	avgfrontdist, avgreardist, avgsteerval);

  //01/07/23 now use average steering value to determine orientation w/r/t near wall
  float orientdeg = GetWallOrientDeg(avgsteerval);
  gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned starting orientation of %2.2f deg\n", avgsteerval, orientdeg);

  //modify turn rate based on size of rotation angle
  float dps = 15;//default rate for small rotation angles
  if (abs(orientdeg) > 15)
  {
	//dps = orientdeg; //basically a 1 second turn
	dps = abs(orientdeg); //basically a 1 second turn
  }
  orientdeg > 0 ? SpinTurn(!isRightWall, orientdeg, dps) : SpinTurn(isRightWall, abs(orientdeg), dps);


  //01/07/22 do another 10-pt average to see how we did
  frontdistsum = 0;
  reardistsum = 0;
  for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)
  {
	GetRequestedVL53l0xValues(VL53L0X_RIGHT);
	frontdistsum += glRightFrontCm;
	reardistsum += glRightRearCm;
	delay(200);
	gl_pSerPort->printf("%2.2f\t%2.2f\n", glRightFrontCm, glRightRearCm);
  }

  avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;
  avgreardist = reardistsum / DIST_ARRAY_SIZE;
  avgsteerval = (avgfrontdist - avgreardist) / 10.f;
  gl_pSerPort->printf("ending front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",
	avgfrontdist, avgreardist, avgsteerval);


  orientdeg = GetWallOrientDeg(avgsteerval);
  gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned ending orientation of %2.2f deg\n", avgsteerval, orientdeg);

  //01/07/23 try another turn if necessary
  if (abs(orientdeg) > 5)
  {
	orientdeg > 0 ? SpinTurn(!isRightWall, orientdeg, 30) : SpinTurn(isRightWall, abs(orientdeg), dps);
  }


  //01/07/22 do another 10-pt average to see how we did
  frontdistsum = 0;
  reardistsum = 0;
  for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)
  {
	GetRequestedVL53l0xValues(VL53L0X_RIGHT);
	frontdistsum += glRightFrontCm;
	reardistsum += glRightRearCm;
	delay(200);
	gl_pSerPort->printf("%2.2f\t%2.2f\n", glRightFrontCm, glRightRearCm);
  }

  avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;
  avgreardist = reardistsum / DIST_ARRAY_SIZE;
  avgsteerval = (avgfrontdist - avgreardist) / 10.f;
  gl_pSerPort->printf("after second turn: front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",
	avgfrontdist, avgreardist, avgsteerval);
}
else //wall must be on left side
{
  //01/07/22 now using initial average steering value for single-turn solution
  for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)
  {
	GetRequestedVL53l0xValues(VL53L0X_LEFT);
	frontdistsum += glLeftFrontCm;
	reardistsum += glLeftRearCm;
	delay(200);
	gl_pSerPort->printf("%2.2f\t%2.2f\n", glLeftFrontCm, glLeftRearCm);
  }

  float avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;
  float avgreardist = reardistsum / DIST_ARRAY_SIZE;
  float avgsteerval = (avgfrontdist - avgreardist) / 10.f;
  gl_pSerPort->printf("starting front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",
	avgfrontdist, avgreardist, avgsteerval);

  //01/07/23 now use average steering value to determine orientation w/r/t near wall
  float orientdeg = GetWallOrientDeg(avgsteerval);
  gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned starting orientation of %2.2f deg\n", avgsteerval, orientdeg);

  //modify turn rate based on size of rotation angle
  float dps = 15;//default rate for small rotation angles
  if (abs(orientdeg) > 15)
  {
	//dps = orientdeg; //basically a 1 second turn
	dps = abs(orientdeg); //basically a 1 second turn
  }
  orientdeg > 0? SpinTurn(!isRightWall, orientdeg, dps): SpinTurn(isRightWall, abs(orientdeg), dps);


  //01/07/22 do another 10-pt average to see how we did
  frontdistsum = 0;
  reardistsum = 0;
  for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)
  {
	GetRequestedVL53l0xValues(VL53L0X_LEFT);
	frontdistsum += glLeftFrontCm;
	reardistsum += glLeftRearCm;
	delay(200);
	gl_pSerPort->printf("%2.2f\t%2.2f\n", glLeftFrontCm, glLeftRearCm);
  }

  avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;
  avgreardist = reardistsum / DIST_ARRAY_SIZE;
  avgsteerval = (avgfrontdist - avgreardist) / 10.f;
  gl_pSerPort->printf("ending front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",
	avgfrontdist, avgreardist, avgsteerval);


  orientdeg = GetWallOrientDeg(avgsteerval);
  gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned ending orientation of %2.2f deg\n", avgsteerval, orientdeg);

 //01/07/23 try another turn if necessary
  if (abs(orientdeg) > 5)
  {
	orientdeg > 0 ? SpinTurn(!isRightWall, orientdeg, 30) : SpinTurn(isRightWall, abs(orientdeg), dps);
  }


  //01/07/22 do another 10-pt average to see how we did
  frontdistsum = 0;
  reardistsum = 0;
  for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)
  {
	GetRequestedVL53l0xValues(VL53L0X_LEFT);
	frontdistsum += glLeftFrontCm;
	reardistsum += glLeftRearCm;
	delay(200);
	gl_pSerPort->printf("%2.2f\t%2.2f\n", glLeftFrontCm, glLeftRearCm);
  }

  avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;
  avgreardist = reardistsum / DIST_ARRAY_SIZE;
  avgsteerval = (avgfrontdist - avgreardist) / 10.f;
  gl_pSerPort->printf("after second turn: front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",
	avgfrontdist, avgreardist, avgsteerval);
}

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

//Step 0: set up arrays for 5-point running average calcs

const uint16_t DIST_ARRAY_SIZE = 10;//01/07/23

float frontdistsum = 0;

float reardistsum = 0;

if (isRightWall)

{

//01/07/22 now using initial average steering value for single-turn solution

for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)

{

GetRequestedVL53l0xValues(VL53L0X_RIGHT);

frontdistsum += glRightFrontCm;

reardistsum += glRightRearCm;

delay(200);

gl_pSerPort->printf("%2.2f\t%2.2f\n", glRightFrontCm, glRightRearCm);

}

float avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;

float avgreardist = reardistsum / DIST_ARRAY_SIZE;

float avgsteerval = (avgfrontdist - avgreardist) / 10.f;

gl_pSerPort->printf("starting front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",

avgfrontdist, avgreardist, avgsteerval);

//01/07/23 now use average steering value to determine orientation w/r/t near wall

float orientdeg = GetWallOrientDeg(avgsteerval);

gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned starting orientation of %2.2f deg\n", avgsteerval, orientdeg);

//modify turn rate based on size of rotation angle

float dps = 15;//default rate for small rotation angles

if (abs(orientdeg) > 15)

{

//dps = orientdeg; //basically a 1 second turn

dps = abs(orientdeg); //basically a 1 second turn

}

orientdeg > 0 ? SpinTurn(!isRightWall, orientdeg, dps) : SpinTurn(isRightWall, abs(orientdeg), dps);

//01/07/22 do another 10-pt average to see how we did

frontdistsum = 0;

reardistsum = 0;

for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)

{

GetRequestedVL53l0xValues(VL53L0X_RIGHT);

frontdistsum += glRightFrontCm;

reardistsum += glRightRearCm;

delay(200);

gl_pSerPort->printf("%2.2f\t%2.2f\n", glRightFrontCm, glRightRearCm);

}

avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;

avgreardist = reardistsum / DIST_ARRAY_SIZE;

avgsteerval = (avgfrontdist - avgreardist) / 10.f;

gl_pSerPort->printf("ending front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",

avgfrontdist, avgreardist, avgsteerval);

orientdeg = GetWallOrientDeg(avgsteerval);

gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned ending orientation of %2.2f deg\n", avgsteerval, orientdeg);

//01/07/23 try another turn if necessary

if (abs(orientdeg) > 5)

{

orientdeg > 0 ? SpinTurn(!isRightWall, orientdeg, 30) : SpinTurn(isRightWall, abs(orientdeg), dps);

}

//01/07/22 do another 10-pt average to see how we did

frontdistsum = 0;

reardistsum = 0;

for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)

{

GetRequestedVL53l0xValues(VL53L0X_RIGHT);

frontdistsum += glRightFrontCm;

reardistsum += glRightRearCm;

delay(200);

gl_pSerPort->printf("%2.2f\t%2.2f\n", glRightFrontCm, glRightRearCm);

}

avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;

avgreardist = reardistsum / DIST_ARRAY_SIZE;

avgsteerval = (avgfrontdist - avgreardist) / 10.f;

gl_pSerPort->printf("after second turn: front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",

avgfrontdist, avgreardist, avgsteerval);

}

else //wall must be on left side

{

//01/07/22 now using initial average steering value for single-turn solution

for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)

{

GetRequestedVL53l0xValues(VL53L0X_LEFT);

frontdistsum += glLeftFrontCm;

reardistsum += glLeftRearCm;

delay(200);

gl_pSerPort->printf("%2.2f\t%2.2f\n", glLeftFrontCm, glLeftRearCm);

}

float avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;

float avgreardist = reardistsum / DIST_ARRAY_SIZE;

float avgsteerval = (avgfrontdist - avgreardist) / 10.f;

gl_pSerPort->printf("starting front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",

avgfrontdist, avgreardist, avgsteerval);

//01/07/23 now use average steering value to determine orientation w/r/t near wall

float orientdeg = GetWallOrientDeg(avgsteerval);

gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned starting orientation of %2.2f deg\n", avgsteerval, orientdeg);

//modify turn rate based on size of rotation angle

float dps = 15;//default rate for small rotation angles

if (abs(orientdeg) > 15)

{

//dps = orientdeg; //basically a 1 second turn

dps = abs(orientdeg); //basically a 1 second turn

}

orientdeg > 0? SpinTurn(!isRightWall, orientdeg, dps): SpinTurn(isRightWall, abs(orientdeg), dps);

//01/07/22 do another 10-pt average to see how we did

frontdistsum = 0;

reardistsum = 0;

for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)

{

GetRequestedVL53l0xValues(VL53L0X_LEFT);

frontdistsum += glLeftFrontCm;

reardistsum += glLeftRearCm;

delay(200);

gl_pSerPort->printf("%2.2f\t%2.2f\n", glLeftFrontCm, glLeftRearCm);

}

avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;

avgreardist = reardistsum / DIST_ARRAY_SIZE;

avgsteerval = (avgfrontdist - avgreardist) / 10.f;

gl_pSerPort->printf("ending front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",

avgfrontdist, avgreardist, avgsteerval);

orientdeg = GetWallOrientDeg(avgsteerval);

gl_pSerPort->printf("GetWallOrientDeg(%2.2f) returned ending orientation of %2.2f deg\n", avgsteerval, orientdeg);

//01/07/23 try another turn if necessary

if (abs(orientdeg) > 5)

{

orientdeg > 0 ? SpinTurn(!isRightWall, orientdeg, 30) : SpinTurn(isRightWall, abs(orientdeg), dps);

}

//01/07/22 do another 10-pt average to see how we did

frontdistsum = 0;

reardistsum = 0;

for (size_t i = 0; i < DIST_ARRAY_SIZE; i++)

{

GetRequestedVL53l0xValues(VL53L0X_LEFT);

frontdistsum += glLeftFrontCm;

reardistsum += glLeftRearCm;

delay(200);

gl_pSerPort->printf("%2.2f\t%2.2f\n", glLeftFrontCm, glLeftRearCm);

}

avgfrontdist = frontdistsum / DIST_ARRAY_SIZE;

avgreardist = reardistsum / DIST_ARRAY_SIZE;

avgsteerval = (avgfrontdist - avgreardist) / 10.f;

gl_pSerPort->printf("after second turn: front/rear/steer avgs = %2.2f/%2.2f/%2.2f\n",

avgfrontdist, avgreardist, avgsteerval);

}

The above code will replace the code in ‘RotateToParallelOrientation()’. See my post on consolidating everything into a ‘Complete’ program for more details

Stay tuned,

Frank

Improving VL53L0X Measurement Accuracy/Precision

2 Replies

Posted 15 November 2022

Last April I described how I determined that individual VL53L0X ‘time-of-flight’ distances sensors exhibited measurement errors, and also described my method for minimizing those errors. This has worked well up to now, but I recently realized that there was another error term I hadn’t accounted for; the error associated with using integer variables to hold VL53L0X distance measurements.

The left & right 3-element VL53L0X arrays (plus a single rear distance sensor) are managed by a dedicated Teensy 3.5 processor, which retrieves distance measurements from all seven sensors and then calibrates them using the method described in my April post. When requested by the main processor, these measurements (in integer MM) are provided via a I2C link. After receipt from the VL53L0X processor, distance measurements are converted from MM to CM by dividing by 10, ignoring integer truncation.

However, I have recently discovered that this integer truncation may well be more significant that I originally thought, and may be leading to performance issues, particularly with my ‘RotateToParallelOrientation()’ function and wall offset tracking in general. The linear distance between the front and rear VL53L0X sensor on each side is about 8.5Cm. Assuming that all constant errors are calibrated out, the robot will be parallel to the wall when the front and rear sensors return the same value. However, because the distance values only change in 1Cm increments, the actual distances measured by the front and rear sensors can be as much as 1Cm different. 1Cm difference over a length of 8.5Cm is about 7∘ – small, but not necessarily insignificant.

It turned out to be relatively painless to change all VL53L0X distance variables from ‘uint16_t’ to ‘float’, and get everything going again. After making this change I tried some more ‘rotate to parallel’ experiments but the change didn’t seem to make much of an improvement. Poking around a bit more I found out why – the raw measurement data coming from the VL53L0X sensors exhibited a lot of ‘noise’, even with the robot stationary and only a few cm from the nearby ‘wall’. The following Excel plot shows one sensor’s data with the robot approximately 14cm from the wall.

As can be seen from the above the reported distance varies from 13 to about 14.5cm. Assuming that all three right-hand sensors behave similarly, it would be possible for the front sensor to report 13cm while the rear sensor was reporting 14.5cm. Moreover, my parallel find algorithm defines ‘parallel’ as RF-RR ~= 0, so it can (and does) terminate well before or after the actual physical parallel orientation occurs.

Looking through (again) the VL53L0X documentation, I came across the ‘measurement budget’ parameter, which is set by default to ‘30000’ (30msec). In my application I had it set to ‘20000’ (20msec) because I thought at the time that with 7 total VL53L0X sensors, I couldn’t afford 30msec delay for each and still hold to a 200msec system loop period. I later changed the system design to use a separate Teensy 3.5 to continuously poll the sensors and report the latest measurement to the main processor when asked, which essentially eliminated the sensor delay from the overall loop (not entirely, as longer sensor measurement times may mean that physical dynamics aren’t followed quite as faithfully, but fortunately my robot doesn’t do anything quickly).

To test the effect of longer measurement budgets, I placed my robot in a cardboard box with its right-side sensor array about 7cm from the wall of the box, and then took measurements with measurement budgets of20, 30, 40, 50, and 60msec. For each value I plotted the distance output and also calculated the variance for each sensor, as shown in the plots below:

20msec budget – the current configuration

30mse budge with 20 & 30msec variances shown

Showing the effect of 40, 50 & 60 msec budgets

As can be seen from the chart immediately above, a measurement budget of 50msec is noticeably better than that for 40msec (which is itself better than the 20 or 30msec budgets), but the 60msec budget plot shows little improvement over 50msec. Looking The ‘RF’ variance starts at 0.0566 for 20msec, drops to less than half that at 30msec, drops by half again at 40msec, and drops by another 50% or so at 50msec. From there to 60msec is only a change from 0.011074 to 0.01046 (this all assumes that I can draw conclusions like this when not only going up with measurement budget, but going across sensors as well). In any case, I settled on a new measurement budge value of 50msec, as shown below.

New value of 50msec for measurement budget

Note that while the motionless measurement variation has been significantly improved, I still have a problem with different nominal measurements from each sensor; the right-front (RF) sensor insists the wall is about 9.2cm away, while the center (RC) and rear (RR) ones think the wall is about 8 and 7.7cm away, respectively (as a side note, before I changed reported measurement variables from ‘uint_16’ to ‘float’, these values would have been reported as 9,8, and 7cm respectively). I thought I had fixed this problem earlier with a set of correction functions as described here, but I obviously have some more work to do (see this post for more on distance correction)

Stay tuned,

Frank

VL53L0X Distance Measurement Compensation

4 Replies

Posted 20 November 2022

To study the issue of VL53L0X sensor calibration, I set up an experiment where ten measurements from each of the three right-side sensors were collected at distances from 15 – 30cm, as shown below. As can be seen, the ‘raw’ values (no correction) are pretty linear. I used Excel’s ‘trendline’ tool to display the ‘best fit’ linear expression for each line, then used these expressions to calculate a correction expression, (dashed lines)

The actual correction expressions were (cm units):

RF: RFCorr = (RF-0.4297)/1.0808
RC: RCCorr = (RC-5.0603)/1.0438
RR: RRCorr = =(RR-5.6746)/1.0353

Next, I edited my ‘lidar_XX_Correction()’ subroutines in my Teensy_7VL53L0X_I2C_Slave_V4 project to implement the above expressions, and made another run of distances from 15 to 30cm, as shown below.

Before (solid lines) and After (dashed lines) Correction

The above plot shows that the correction algorithm is effective and repeatable, at least on the right side sensors. Now I have to perform the same corrections on the left side and I’ll be all set – at least for this particular part of the ongoing Sisyphean task of educating WallE3, my somewhat retarded autonomous wall-following robot.

Applying the same methodology to the left side sensors, I first captured left-side reported distances for measured values from 15 to 30 cm, same as for the left side. Then, using Excel’s ‘trendline’ calculation feature to derive a correction expression, I added simulated correction lines to the plot, as shown below:

The actual correction expressions were (cm units):

LF: LFCorr = (LF – 0.5478)/1.0918
LC: LCCorr = (LC – 1.989)/0.9913)
LR: LRCorr = (LR + 0.9676)/1.1616

Not much correction is needed for the left-side sensors. However, since I already have the corrections, I might as well put them in; I modified the VL53L0X sensor manager firmware to include the above corrections, and then re-did the calibration plot -this time plotting the pre-correction reported data along with the post-correction reported data, as shown below:

As can be seen from the above plot, left-side correction is pretty good over the entire 15-30cm range – nice!

Stay tuned,

Frank

Wall-E3 Charging Station Integration, Part II

21 June 2022 Update:

Progress has been a bit slow over the last month, as I’ve been doing other things, including recovering from an eye/scalp injury sustained at the Senior Games in Florida (those old guys can still pack a wallop!). However, I now think I have the charging station homing operation working, at least for the right-wall-tracking (the most common and probably only) case.

As it turns out, I didn’t really need to worry about the problem of the robot picking up the IR homing beacon while tracking the far wall (i.e. the wall perpendicular to the one associated with the charging station). As I noted above, the robot can sometimes ‘see’ a beacon signal above the threshold, but when it does, the steering signal (goes from -1 to +1) is always off scale on one side or another. So, by adding the requirement that the steering signal be within the range of -0.8 to +0.8, this case is entirely eliminated, leaving only the ‘same-wall’ homing case – yay!

I made a number of improvements to the IR homing code, in particular the ‘initial approach’ code that places the robot optimally with respect to the IR beacon beam, so that the subsequent ‘home-to-charging-station’ procedure is almost always successful.

In the following short video, the robot tracks the opposite wall, makes the turn to track the adjacent wall, and then detects the IR homing beacon at the conclusion of the offset capture phase for the adjacent wall. Then the robot transitions to ‘I’m hungry – feed me’ mode.

In ‘feed me’ mode, the robot first aligns itself with the beacon in a two-stage (coarse and fine) procedure. Once this is accomplished, the robot determines its distance from the beacon, and then uses this information to determine how far off the adjacent wall it should be to perfectly line up with the center of the IR beam, makes a turn to be perpendicular to the adjacent wall, and then moves forward or backward to achieve the desired offset measurement. Once this is accomplished the robot re-aligns itself with the beacon again, using the same two-stage procedure. Once all this is done, the robot then homes on the beacon to connect to the charger.

The above algorithm is shown in action in the following short video. When viewing the video, keep the following in mind:

The homing beacon is only recognized at the conclusion of the wall offset capture maneuver for the adjacent wall, even though the IR beacon signal is above the threshold as soon as the robot make the turn to the adjacent wall. This is done to place the robot at a known location to start the process.
While aligning itself to the beacon, the robot turns ON its red laser pointer to allow us humans to follow the action. Although the ‘red’ laser looks mostly white in the video, it is indeed red.
The beacon alignment takes place in two stages – coarse, then fine. This can be see by the speed at which the red laser dot moves back and forth.
In this particular video, the robot is already at the proper wall offset, so the ‘move to desired rear distance’ operation is truncated – the robot makes the turn to be perpendicular to the wall, determines it is at the proper offset, and so immediately turns back to align to the beacon signal.

Successful ‘track and then home to charger’ run

Wall-E3 Charging Station Integration

24 April 2022 Update:

The above data and video was collected using the ‘NoPing’ version of the IR Homing algorithm, so I went back and did this again using the version that monitors the front distance, both for detecting a ‘stuck’ condition and for the ability to navigate around the charging station if a charge isn’t needed. Of course, this didn’t work right away for various reasons, but eventually I got it working and made another test run. Here’s a short slo-mo video, and the accompanying Excel chart showing homing performance.

IR Homing run in slo-mo with tracking status shown on rear panel LEDs

Steering value and front distance vs time

01 May 2022 Update:

Wall-E3 has made some significant progress in the last week, and is now capable of switching from right-wall tracking to IR Homing to the charging station, as shown in the following short video and telemetry output:

automatic switching from right-wall tracking to charging station homing

In TrackRightWallOffset with Kp/Ki/Kd = 400.00	0.00	0.00

In TrackRightWallOffset with RR/RC/RF = 20	18	19
straight window = 8 cm
GetWallOrientDeg(-0.0900): slopeval = 0.02, offparalleldeg -5.14
R/C/F dists = 20	18	19 Steerval = -0.090, WallAngle = -5.14, Inside
GetWallOrientDeg(0.4400): slopeval = 0.02, offparalleldeg 25.14
R/C/F dists = 20	20	24 Steerval = 0.440, WallOrientDeg = 25.14

approach start: orient angle = 25.143, tgt_distCm = 35
At start: meas_dist = 20, corr_dist = 18, err = prev_err = -17
GetWallOrientDeg(0.4800): slopeval = 0.02, offparalleldeg 27.43
R/C/F dists = 20	21	25 Steerval = 0.480, OrientDeg = 27.43, err/prev_err = -17
Msec	RFront	RCtr	RRear	ODeg	ORad	c_dist	Err	P_err
11065	26	22	20	27.4	0.5	18	-17	-17
11179	30	26	22	27.4	0.5	23	-12	-17
11293	33	28	25	27.4	0.5	24	-11	-12
11407	36	32	29	27.4	0.5	28	-7	-11
11521	40	35	31	27.4	0.5	31	-4	-7
11635	44	40	35	27.4	0.5	35	0	-4
At end of offset capture - prev_res*res = 0
correct back to parallel (Right side): RF/RC/RR/Orient = 44	40	35	27.43

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 400.00	0.00	0.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
12235	728	112	816		41	39	38		104	11326	49	508	0.30	0.25	-20	94	55
12335	728	111	816		40	39	40		103	11536	56	479	0.01	0.25	96	0	127
12439	728	111	127		41	40	39		103	11730	57	452	0.23	0.25	 8	67	83
12539	728	111	816		41	39	40		103	11891	54	428	0.14	0.25	44	31	119
12637	728	111	816		42	39	40		101	12011	57	405	0.12	0.25	52	23	127
12736	728	110	816		43	42	39		100	12071	68	385	0.42	0.25	-68	127	7
12839	728	115	816		46	43	42		99	12052	75	370	0.47	0.25	-88	127	0
12936	728	109	816		47	45	42		91	11963	71	353	0.59	0.25	-136	127	0
13039	728	105	816		47	45	44		86	11772	79	342	0.45	0.25	-80	127	0
13138	111	113	123		48	45	43		80	11461	83	335	0.52	0.25	-108	127	0
13237	728	112	121		47	46	45		77	10997	76	323	0.40	0.25	-60	127	14
13336	728	117	126		48	46	46		76	10351	84	318	0.17	0.25	32	43	107
13437	728	794	126		50	49	49		76	9497	100	329	0.21	0.25	16	59	91
13537	728	794	816		47	48	49		71	9628	104	346	0.02	0.25	92	0	127
13635	113	111	816		47	46	47		67	9771	97	352	0.16	0.25	36	39	111
13738	107	794	816		46	44	46		64	9918	95	356	0.12	0.25	52	23	127
13836	728	794	816		44	43	43		61	10073	108	376	0.12	0.25	52	23	127
13935	728	794	125		44	42	43		57	10213	98	382	0.17	0.25	32	42	107
14036	728	112	816		44	42	42		55	10343	104	395	0.22	0.25	12	63	87
14134	728	119	816		44	41	41		50	10483	116	426	0.40	0.25	-60	127	14
14237	728	794	816		45	43	41		44	10637	121	463	0.50	0.25	-100	127	0
14338	106	98	125		46	43	41		38	10797	109	480	0.51	0.25	-104	127	0
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Right
IRHomingValTotalAvg = 33711 
IR Beam Detected with Battery Voltage = 6.87
Low battery - homing to Charger
OK we have a IR Beacon, with front distance = 121!
In IRHomingNavigateToIAP with LF/LR/LS/RF/RR/RS = 728	816	-8.86	29	28	0.16
In IRHomingNavigateToIAP(Right) with LF/LR/LS/RF/RR/RS = 728	816	-8.86	30	27	0.26
IP is at 60 cm offset, 135 cm away from charging station, a ratio of 0.44
Robot is at 121 from charger, so we need a 53 cm offset
Current wall offset is 28 cm or 52% of required offset
offset is too small or too large - turn away from wall to adjust
After SpinTurn(), rear dist (cm) is 28
In MoveToDesiredRearDistCm(53) with Kp/Ki/Kd = 2.0/0.5/0.0
Msec	Rdist	TgtD	err	Ival	Out
19015	25	53	28.0	14.0	70.0
19215	32	53	21.0	24.5	66.5
19415	34	53	19.0	34.0	72.0
19615	47	53	6.0	37.0	49.0
19815	50	53	3.0	38.5	44.5
20015	62	53	-9.0	34.0	16.0
20215	60	53	-7.0	30.5	16.5
20415	70	53	-17.0	22.0	-12.0
20615	70	53	-17.0	13.5	-20.5
20815	63	53	-10.0	8.5	-11.5
21015	70	53	-17.0	0.0	-34.0
21215	62	53	-9.0	-4.5	-22.5
21415	66	53	-13.0	-11.0	-37.0
21615	57	53	-4.0	-13.0	-21.0
21815	62	53	-9.0	-17.5	-35.5
22015	51	53	2.0	-16.5	-12.5
22215	57	53	-4.0	-18.5	-26.5
22415	48	53	5.0	-16.0	-6.0
22615	55	53	-2.0	-17.0	-21.0
22815	48	53	5.0	-14.5	-4.5
23015	48	53	5.0	-12.0	-2.0
23215	53	53	0.0	-12.0	-12.0
Wall offset now 53 cm, with IRHomingValTotalAvg = 32824
TurnToHomingBeacon with IRAvg = 32705, Steering = 0.39 & IRRotate Kp,Ki,Kd = (40,10, 0)
TurnToHomingBeacon(RIGHT)


Msec	Signal	Steer	Val
25506	174.74	0.94	75
25627	-173.51	0.57	92
25746	-162.99	-0.03	94
25867	-152.65	-0.43	92
25986	-142.47	-0.67	54
26137	-129.94	-0.50	51
26256	-119.93	-1.00	7821
26377	-109.97	-0.99	19453
26496	-99.64	-0.90	33325
26617	-88.96	-0.83	40602
26738	-78.32	-0.72	47367
26857	-67.75	-0.46	55756
26978	-57.49	-0.01	67318
Coarse adjustment finished
Rotate back and forth as necessary to maximize received signal strength.
Start with CW rotation for trkside == TRACKING_RIGHT, and CCW for TRACKING_LEFT
Got 67510, 11448, 75232, 0.71 from IR Detector module


Msec	IRVal1	IRVal2	ValAvg	Steer	speed
28098	67460	11364	79224	0.71	-35
28196	66514	11494	78598	0.71	-42
28299	63398	12996	77744	0.66	-47
28397	60976	16642	77342	0.57	-49
28501	57878	23998	78631	0.41	-47
28598	46500	35428	80475	0.14	-37
28702	31442	45008	80086	-0.18	-23
28800	18736	49560	75560	-0.45	-7
28897	13184	51822	69920	-0.59	4
28999	12084	52470	65955	-0.63	11
29102	11924	52404	64633	-0.63	17
29198	11878	52354	64375	-0.63	24
29300	11922	52172	64222	-0.63	30
29399	13192	51606	64378	-0.59	35
29498	16370	50314	65195	-0.51	36
29601	21518	48806	67271	-0.39	35
29698	29220	46094	70776	-0.22	31
29803	37744	42130	75172	-0.05	25
TurnToHomingBeacon exited with Steering = -0.05
Should be close to beacon centerline val avg = 75172, SteeringVal = -0.055
IRHomeToChgStn with detection value = 80398, Steering = 0.09 & IRHOMING Kp,Ki,Kd = (75, 0,40)
front dist = 122, avoidance dist = 0

Time	BattV	Fin1	Fin2	Steer	PID_Out		LSpd	RSpd	FrontD	RearD
31939	6.86	47052	39000	0.09	-3.27		71	78	122	43
32036	6.77	47008	39042	0.09	-6.98		68	81	123	49
32137	6.83	47626	40062	0.09	-6.72		68	81	116	44
32237	6.75	48380	43116	0.06	-5.46		69	80	114	48
32337	6.82	50606	46486	0.04	-3.79		71	78	114	57
32438	6.85	53028	50760	0.02	-2.46		72	77	111	61
32537	6.85	55452	56032	-0.01	-0.69		74	75	104	60
32638	6.86	57998	61800	-0.03	1.32		76	73	100	64
32739	6.72	60542	68338	-0.06	3.39		78	71	97	75
32840	6.70	64942	74526	-0.07	4.82		79	70	95	80
32940	6.78	71884	79754	-0.05	4.57		79	70	90	75
33036	6.67	81310	84894	-0.02	2.83		77	72	86	81
33141	6.84	90606	92696	-0.01	1.26		76	73	83	89
33236	6.86	98940	101460	-0.01	0.90		75	74	79	89
33338	6.84	108390	112432	-0.02	1.14		76	73	75	90
33436	6.86	117900	124044	-0.03	1.62		76	73	69	104
33539	6.86	133920	136456	-0.01	1.34		76	73	66	110
33640	6.86	157150	150632	0.02	-0.37		74	75	61	103
33736	6.86	185900	168138	0.05	-2.60		72	77	58	109
33837	6.86	221118	193390	0.07	-4.35		70	79	52	123
33937	6.68	262698	222510	0.08	-5.57		69	80	48	123
34039	6.86	296898	256092	0.07	-5.90		69	80	40	750
34136	6.73	343506	290618	0.08	-5.87		69	80	35	750
34239	6.85	481186	345974	0.16	-9.06		65	84	26	819
34336	6.85	851810	480074	0.28	-16.31		58	91	20	819
34438	6.86	1434172	684000	0.35	-23.56		51	98	13	750
34538	6.77	2083914	1004848	0.35	-26.39		48	101	11	750
34638	6.85	2424412	1658876	0.19	-20.54		54	95	9	750
34737	6.84	2468376	2381848	0.02	-8.12		66	83	4	819
34838	6.94	2444718	2432648	0.00	-0.80		74	75	4	819
Charger Connected at 34839
Robot has successfully connected to charger!
Reset VL53L0X Teensy to reduce current consumption
ChgSec	BattV	TotalI	RunI	ChgI	RearD

0.0	6.9568	1.2419	0.4290	0.8129
0.1	6.9697	1.2355	0.3968	0.8387

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

In TrackRightWallOffset with Kp/Ki/Kd = 400.00 0.00 0.00

In TrackRightWallOffset with RR/RC/RF = 20 18 19

straight window = 8 cm

GetWallOrientDeg(-0.0900): slopeval = 0.02, offparalleldeg -5.14

R/C/F dists = 20 18 19 Steerval = -0.090, WallAngle = -5.14, Inside

GetWallOrientDeg(0.4400): slopeval = 0.02, offparalleldeg 25.14

R/C/F dists = 20 20 24 Steerval = 0.440, WallOrientDeg = 25.14

approach start: orient angle = 25.143, tgt_distCm = 35

At start: meas_dist = 20, corr_dist = 18, err = prev_err = -17

GetWallOrientDeg(0.4800): slopeval = 0.02, offparalleldeg 27.43

R/C/F dists = 20 21 25 Steerval = 0.480, OrientDeg = 27.43, err/prev_err = -17

Msec RFront RCtr RRear ODeg ORad c_dist Err P_err

11065 26 22 20 27.4 0.5 18 -17 -17

11179 30 26 22 27.4 0.5 23 -12 -17

11293 33 28 25 27.4 0.5 24 -11 -12

11407 36 32 29 27.4 0.5 28 -7 -11

11521 40 35 31 27.4 0.5 31 -4 -7

11635 44 40 35 27.4 0.5 35 0 -4

At end of offset capture - prev_res*res = 0

correct back to parallel (Right side): RF/RC/RR/Orient = 44 40 35 27.43

TrackRightWallOffset: Start tracking offset of 40cm with Kp/Ki/Kd = 400.00 0.00 0.00

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

12235 728 112 816 41 39 38 104 11326 49 508 0.30 0.25 -20 94 55

12335 728 111 816 40 39 40 103 11536 56 479 0.01 0.25 96 0 127

12439 728 111 127 41 40 39 103 11730 57 452 0.23 0.25 8 67 83

12539 728 111 816 41 39 40 103 11891 54 428 0.14 0.25 44 31 119

12637 728 111 816 42 39 40 101 12011 57 405 0.12 0.25 52 23 127

12736 728 110 816 43 42 39 100 12071 68 385 0.42 0.25 -68 127 7

12839 728 115 816 46 43 42 99 12052 75 370 0.47 0.25 -88 127 0

12936 728 109 816 47 45 42 91 11963 71 353 0.59 0.25 -136 127 0

13039 728 105 816 47 45 44 86 11772 79 342 0.45 0.25 -80 127 0

13138 111 113 123 48 45 43 80 11461 83 335 0.52 0.25 -108 127 0

13237 728 112 121 47 46 45 77 10997 76 323 0.40 0.25 -60 127 14

13336 728 117 126 48 46 46 76 10351 84 318 0.17 0.25 32 43 107

13437 728 794 126 50 49 49 76 9497 100 329 0.21 0.25 16 59 91

13537 728 794 816 47 48 49 71 9628 104 346 0.02 0.25 92 0 127

13635 113 111 816 47 46 47 67 9771 97 352 0.16 0.25 36 39 111

13738 107 794 816 46 44 46 64 9918 95 356 0.12 0.25 52 23 127

13836 728 794 816 44 43 43 61 10073 108 376 0.12 0.25 52 23 127

13935 728 794 125 44 42 43 57 10213 98 382 0.17 0.25 32 42 107

14036 728 112 816 44 42 42 55 10343 104 395 0.22 0.25 12 63 87

14134 728 119 816 44 41 41 50 10483 116 426 0.40 0.25 -60 127 14

14237 728 794 816 45 43 41 44 10637 121 463 0.50 0.25 -100 127 0

14338 106 98 125 46 43 41 38 10797 109 480 0.51 0.25 -104 127 0

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Right

IRHomingValTotalAvg = 33711

IR Beam Detected with Battery Voltage = 6.87

Low battery - homing to Charger

OK we have a IR Beacon, with front distance = 121!

In IRHomingNavigateToIAP with LF/LR/LS/RF/RR/RS = 728 816 -8.86 29 28 0.16

In IRHomingNavigateToIAP(Right) with LF/LR/LS/RF/RR/RS = 728 816 -8.86 30 27 0.26

IP is at 60 cm offset, 135 cm away from charging station, a ratio of 0.44

Robot is at 121 from charger, so we need a 53 cm offset

Current wall offset is 28 cm or 52% of required offset

offset is too small or too large - turn away from wall to adjust

After SpinTurn(), rear dist (cm) is 28

In MoveToDesiredRearDistCm(53) with Kp/Ki/Kd = 2.0/0.5/0.0

Msec Rdist TgtD err Ival Out

19015 25 53 28.0 14.0 70.0

19215 32 53 21.0 24.5 66.5

19415 34 53 19.0 34.0 72.0

19615 47 53 6.0 37.0 49.0

19815 50 53 3.0 38.5 44.5

20015 62 53 -9.0 34.0 16.0

20215 60 53 -7.0 30.5 16.5

20415 70 53 -17.0 22.0 -12.0

20615 70 53 -17.0 13.5 -20.5

20815 63 53 -10.0 8.5 -11.5

21015 70 53 -17.0 0.0 -34.0

21215 62 53 -9.0 -4.5 -22.5

21415 66 53 -13.0 -11.0 -37.0

21615 57 53 -4.0 -13.0 -21.0

21815 62 53 -9.0 -17.5 -35.5

22015 51 53 2.0 -16.5 -12.5

22215 57 53 -4.0 -18.5 -26.5

22415 48 53 5.0 -16.0 -6.0

22615 55 53 -2.0 -17.0 -21.0

22815 48 53 5.0 -14.5 -4.5

23015 48 53 5.0 -12.0 -2.0

23215 53 53 0.0 -12.0 -12.0

Wall offset now 53 cm, with IRHomingValTotalAvg = 32824

TurnToHomingBeacon with IRAvg = 32705, Steering = 0.39 & IRRotate Kp,Ki,Kd = (40,10, 0)

TurnToHomingBeacon(RIGHT)

Msec Signal Steer Val

25506 174.74 0.94 75

25627 -173.51 0.57 92

25746 -162.99 -0.03 94

25867 -152.65 -0.43 92

25986 -142.47 -0.67 54

26137 -129.94 -0.50 51

26256 -119.93 -1.00 7821

26377 -109.97 -0.99 19453

26496 -99.64 -0.90 33325

26617 -88.96 -0.83 40602

26738 -78.32 -0.72 47367

26857 -67.75 -0.46 55756

26978 -57.49 -0.01 67318

Coarse adjustment finished

Rotate back and forth as necessary to maximize received signal strength.

Start with CW rotation for trkside == TRACKING_RIGHT, and CCW for TRACKING_LEFT

Got 67510, 11448, 75232, 0.71 from IR Detector module

Msec IRVal1 IRVal2 ValAvg Steer speed

28098 67460 11364 79224 0.71 -35

28196 66514 11494 78598 0.71 -42

28299 63398 12996 77744 0.66 -47

28397 60976 16642 77342 0.57 -49

28501 57878 23998 78631 0.41 -47

28598 46500 35428 80475 0.14 -37

28702 31442 45008 80086 -0.18 -23

28800 18736 49560 75560 -0.45 -7

28897 13184 51822 69920 -0.59 4

28999 12084 52470 65955 -0.63 11

29102 11924 52404 64633 -0.63 17

29198 11878 52354 64375 -0.63 24

29300 11922 52172 64222 -0.63 30

29399 13192 51606 64378 -0.59 35

29498 16370 50314 65195 -0.51 36

29601 21518 48806 67271 -0.39 35

29698 29220 46094 70776 -0.22 31

29803 37744 42130 75172 -0.05 25

TurnToHomingBeacon exited with Steering = -0.05

Should be close to beacon centerline val avg = 75172, SteeringVal = -0.055

IRHomeToChgStn with detection value = 80398, Steering = 0.09 & IRHOMING Kp,Ki,Kd = (75, 0,40)

front dist = 122, avoidance dist = 0

Time BattV Fin1 Fin2 Steer PID_Out LSpd RSpd FrontD RearD

31939 6.86 47052 39000 0.09 -3.27 71 78 122 43

32036 6.77 47008 39042 0.09 -6.98 68 81 123 49

32137 6.83 47626 40062 0.09 -6.72 68 81 116 44

32237 6.75 48380 43116 0.06 -5.46 69 80 114 48

32337 6.82 50606 46486 0.04 -3.79 71 78 114 57

32438 6.85 53028 50760 0.02 -2.46 72 77 111 61

32537 6.85 55452 56032 -0.01 -0.69 74 75 104 60

32638 6.86 57998 61800 -0.03 1.32 76 73 100 64

32739 6.72 60542 68338 -0.06 3.39 78 71 97 75

32840 6.70 64942 74526 -0.07 4.82 79 70 95 80

32940 6.78 71884 79754 -0.05 4.57 79 70 90 75

33036 6.67 81310 84894 -0.02 2.83 77 72 86 81

33141 6.84 90606 92696 -0.01 1.26 76 73 83 89

33236 6.86 98940 101460 -0.01 0.90 75 74 79 89

33338 6.84 108390 112432 -0.02 1.14 76 73 75 90

33436 6.86 117900 124044 -0.03 1.62 76 73 69 104

33539 6.86 133920 136456 -0.01 1.34 76 73 66 110

33640 6.86 157150 150632 0.02 -0.37 74 75 61 103

33736 6.86 185900 168138 0.05 -2.60 72 77 58 109

33837 6.86 221118 193390 0.07 -4.35 70 79 52 123

33937 6.68 262698 222510 0.08 -5.57 69 80 48 123

34039 6.86 296898 256092 0.07 -5.90 69 80 40 750

34136 6.73 343506 290618 0.08 -5.87 69 80 35 750

34239 6.85 481186 345974 0.16 -9.06 65 84 26 819

34336 6.85 851810 480074 0.28 -16.31 58 91 20 819

34438 6.86 1434172 684000 0.35 -23.56 51 98 13 750

34538 6.77 2083914 1004848 0.35 -26.39 48 101 11 750

34638 6.85 2424412 1658876 0.19 -20.54 54 95 9 750

34737 6.84 2468376 2381848 0.02 -8.12 66 83 4 819

34838 6.94 2444718 2432648 0.00 -0.80 74 75 4 819

Charger Connected at 34839

Robot has successfully connected to charger!

Reset VL53L0X Teensy to reduce current consumption

ChgSec BattV TotalI RunI ChgI RearD

0.0 6.9568 1.2419 0.4290 0.8129

0.1 6.9697 1.2355 0.3968 0.8387

Stay tuned,

Frank