Tag Archives: Wall Tracking

New Wall-Following Capability For Wall-E3

Posted 28 March, 2022

I’ve been working with Wall-E3, my new Teensy 3.5-powered autonomous wall-following robot. I’ve gotten left-wall and right-wall tracking working pretty well, but the transition from one wall to the next (typically right-angle) wall was pretty awkward. The robot basically ran right up to the next wall, stopped, backed up, and then made a right-angle turn to follow the next wall. So, I am trying to make that transition a bit smoother.

After trying a few different ideas, the one I settled on was to use my current very successful ‘SpinTurn()’ function to do the transition. I modified my ‘CheckForAnomalies()’ function to add a check for forward distance less than twice the desired offset distance. When this distance is detected, the robot stops and then makes a right-angle ‘spin’ turn (one sides wheels go forward, the other sides wheels go backward) in the direction away from the currently tracked wall, and then re-enters ‘Track’ mode causing it to track the next wall normally. Here’s a short video showing the process:

robot tracking left wall, then making a ‘spin’ turn to follow the next wall

Now that I have it working for left-wall tracking, it should be easy to port it to the right-wall tracking condition.

07 April 2022 Update:

Well, as usual, what I thought would be easy has turned out to be anything but. I was able to port the left wall tracking algorithm to the right side, but as I was testing the result, I noticed that Wall-E3 doesn’t really track the right (or left, for that matter) wall. After it ‘captures’ the desired wall offset and turns back to the parallel orientation, it basically goes straight ahead (same speed applied to both side’s motors). If the initial orientation is close to parallel, it looks like it is tracking, but it isn’t.

So, I tried a number of ideas to actually get it to track the desired offset, but they all resulted in poor-to-catastrophic tracking. After working the problem, I began to see that, as always, the issue is the errors associated with the VL53L0X sensor distance measurements. There are two distinct types of errors – an initial ‘calibration’ error associated with sensor-to-sensor variation, and the measurement error that occurs when the robot isn’t oriented parallel to the measured surface.

Calibration Errors:

Each individual VL53L0X sensor gets a slightly different value for the distance to the target, and sometimes ‘slightly’ can be pretty big – 2-3cm at 20cm, for instance. Up until now I had been ignoring these errors, but the time had come to do something about. So, as I always do when troubleshooting an issue, I started taking data. I ran a bunch of trials for all seven VL53L0X sensors at various distances. After gathering the data, I used Excel’s curve-fitting capability to fit a linear equation to the points, as shown below:

The linear-fit equations gave me a starting point, but they still had to be tweaked a bit to provide the best possible match between what the VL53L0X sensor reported and the actual measurement. Again I used Excel to tweak the equations to give the best match as shown below:

Left-side ‘tweaked’ correction expression

Right-side ‘tweaked’ correction expression

The expressions shown in red are the ones used to correct the VL53L0X-measured distances to be as close as possible to the actual distances (10cm, 20cm, 30cm).

The above corrections were coded into a set of seven ‘correction’ functions for the Teensy 3.5 program that manages the two VL53L0X arrays and the single VL53L0X rear distance sensor.

Correction functions in Teensy_7VL53L0X_I2C_Slave_V4.ino

While this did, indeed, solve a lot of problems – especially with the calculations for wall offset capture initial approach angle, it still didn’t entirely address Wall-E3’s inconsistent offset tracking performance.

Orientation Angle Induced Errors:

Wall-E3 tracks walls by offset by comparing the center VL53L0X measurement to the desired offset, and adjusting the left/right motor speeds to turn the robot in the desired direction. Unfortunately, the turn also throws off the measurements as now the sensors are pointing off-perpendicular, and return a different distance than the actual robot-to-wall perpendicular distance. I tried adjusting the PID controller algorithm to control the robot’s steering angle rather than the offset distance, and then calculating a new steering angle each time – this worked, but not very well.

So, the solution (I think) is to come up with a distance correction factor for off-perpendicular orientations. Going through the trigonometry, I came up with this expression:

corrdist=measdist*cos⁡(steeringAngle)

I programmed this into the following function:

uint16_t OrientCorr(uint16_t meas_dist, float steerval)
{
  //double corr_ang_rad = atan2(100*steerval,83.f); //83mm between front & rear sensors
  double corr_ang_rad = atan2(100*steerval,83.f); //83mm between front & rear sensors
  double corr_dist = meas_dist * cos(corr_ang_rad);

  myTeePrint.printf("OrientCorr: meas = %d, steer = %2.2f, corr_ang_rad = %2.2f, corr_dist = %2.2f\n", meas_dist, steerval, corr_ang_rad, corr_dist);

  return (uint16_t)round(corr_dist);
}

uint16_t OrientCorr(uint16_t meas_dist, float steerval)

{

//double corr_ang_rad = atan2(100*steerval,83.f); //83mm between front & rear sensors

double corr_ang_rad = atan2(100*steerval,83.f); //83mm between front & rear sensors

double corr_dist = meas_dist * cos(corr_ang_rad);

myTeePrint.printf("OrientCorr: meas = %d, steer = %2.2f, corr_ang_rad = %2.2f, corr_dist = %2.2f\n", meas_dist, steerval, corr_ang_rad, corr_dist);

return (uint16_t)round(corr_dist);

}

and then ran some tests to verify that the correction algorithm was having the desired effect. Here’s the setup:

and here are some Excel plots showing the results

Distance correction for off-perpendicular angles

As can be seen from the above plot, the corrected distance (gray curve) is pretty constant for angles of -30, 0 and +30 degrees.

09 April 2022 Update:

I have been thinking about the above orientation angle induced errors issue for a couple of days. I wasn’t really happy with that correction as shown in the above Excel plot, and it occurred to me that I didn’t really have to strictly abide by the above correction expression derived from the actual geometry. What I really wanted was a correction that would be accurate at low (or zero) offset angles, but would slightly over-correct for orientation angles in the +/- 30 deg range. In this way, when the PID engine adjusts the motor speeds to correct for an offset error, the system doesn’t try to run away. In fact, for a slight overcorrection algorithm, the center distance reported by the robot might actually go down rather than up for off-perpendicular angles. This would tend to make the PID think that it was over-correcting instead of under-correcting as it does with uncorrected distance reporting.

So, I went back to my test setup, and made some more measurements of corrected vs uncorrected center distances for -30, 0, and 30 degree orientations, for varying values of ‘tweak’ values in he correction expression, as shown in the Excel plots below:

Out of the above correction values, I like the “cos(1.1*corr_ang_rad)” configuration the best. The correction doesn’t modify the center distance at all for the parallel case, and produces a very slight over-correction at the +/- 30 degree orientation cases.

I added the ‘1.1*’ correction to the ‘OrientCorr()’ function and performed another right wall tracking test in my office ‘sandbox’ as shown in the short video below:

Right wall tracking with sensor calibration and orientation correction applied

Here is the telemetry output for this run:

In TrackRightWallOffset with Kp/Ki/Kd = 300.00	0.00	10.00

In TrackRightWallOffset with RR/RC/RF = 30	30	30
straight window = 8 cm
R/C/F dists = 30	30	30 Steerval = 0.020, WallAngle = 1.14, Inside
In SpinTurn(CCW, 28.86, 45.00)
R/C/F dists = 27	30	30 Steerval = 0.350, WallOrientDeg = 20.00

approach start: orient angle = 20.000, tgt_distCm = 40
At start - err = prev_err = -10
R/C/F dists = 27	30	30 Steerval = 0.350, OrientDeg = 20.00, err/prev_err = -10
R/C/F dists = 27	30	30 Steerval = 0.350, OrientDeg = 20.00, err/prev_err = -10
Msec	RFront	RCtr	RRear	Orient	Front	Rear	Err	P_err
68368: Error in GetFrontDistCm() - 0 replaced with 60
68384	31	30	28	17.1	118	46	-10	-10
68496	34	33	30	22.3	125	56	-7	-10
68609	36	36	32	24.0	119	60	-4	-7
68720	39	38	34	28.0	115	66	-2	-4
68831	44	41	36	44.0	108	68	1	-2
At end of offset capture - prev_res*res = -2
correct back to parallel (Right side): RF/RC/RR/Orient = 44	41	36	44.00
In SpinTurn(CW, 44.00, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	10.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
69557	728	794	816		49	47	50		95	10899	73	39363	-0.10	-0.07	8.70	66	83
69657	728	794	816		50	51	52		99	10901	80	39332	-0.18	-0.11	20.60	54	95
69756	728	794	816		50	48	50		93	10910	86	39318	0.00	-0.08	-22.50	97	52
69858	728	794	816		49	46	48		90	10917	94	39277	0.09	-0.06	-44.30	119	30
69958	728	794	816		46	44	47		86	10927	87	39265	-0.10	-0.04	15.90	59	90
70055	728	114	816		44	45	45		76	10956	102	39252	-0.11	-0.05	18.00	57	93
70157	728	108	816		43	41	43		77	10976	819	48383	-0.02	-0.01	3.50	71	78
70255	728	109	816		39	38	40		74	10994	123	48334	-0.09	0.02	32.00	43	107
70357	109	99	816		36	36	38		70	11023	750	55303	-0.19	0.04	67.80	7	127
70455	112	103	816		34	33	34		65	11056	104	55318	-0.02	0.07	28.40	46	103
70557	728	116	816		29	30	31		55	11101	819	63702	-0.15	0.10	73.40	1	127
70655	728	120	816		27	27	28		48	11170	106	63708	-0.08	0.13	63.40	11	127
70757	728	794	816		24	24	25		43	11246	110	63715	-0.11	0.15	77.50	0	127
70854	728	794	816		22	22	24		37	11329	118	55334	-0.19	0.15	101.20	0	127
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither
In SpinTurn(CCW, 90.00, 45.00)
71682: glLeftCenterCm = 794 glRightCenterCm = 25
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
71686: TrackRightWallOffset(), glRightFrontCm = 26

In TrackRightWallOffset with Kp/Ki/Kd = 300.00	0.00	10.00

In TrackRightWallOffset with RR/RC/RF = 26	25	26
straight window = 8 cm
R/C/F dists = 26	25	26 Steerval = -0.060, WallAngle = -3.43, Inside
In SpinTurn(CCW, 33.43, 45.00)
R/C/F dists = 26	30	32 Steerval = 0.610, WallOrientDeg = 34.86

approach start: orient angle = 34.857, tgt_distCm = 40
At start - err = prev_err = -8
R/C/F dists = 26	30	32 Steerval = 0.610, OrientDeg = 34.86, err/prev_err = -8
R/C/F dists = 27	33	33 Steerval = 0.580, OrientDeg = 33.14, err/prev_err = -8
Msec	RFront	RCtr	RRear	Orient	Front	Rear	Err	P_err
72138	33	33	27	33.1	219	25	-7	-8
72254	40	38	31	40.0	195	29	-3	-7
72365	44	43	34	43.4	187	32	3	-3
At end of offset capture - prev_res*res = -9
correct back to parallel (Right side): RF/RC/RR/Orient = 44	43	34	43.43
In SpinTurn(CW, 43.43, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	10.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
73091	110	112	122		40	38	39		151	11228	29	55621	0.09	0.02	-20.30	95	54
73189	728	110	816		39	40	41		153	11220	35	55688	-0.14	0.00	39.90	35	114
73291	728	112	816		40	38	39		150	11210	37	55762	0.18	0.02	-45.00	120	29
73394	728	111	816		40	38	39		146	11195	36	55854	0.06	0.02	-13.20	88	61
73492	114	115	120		38	37	38		141	11171	41	55917	0.07	0.03	-12.00	87	63
73589	728	113	816		38	38	38		134	11147	50	55921	-0.03	0.02	14.10	60	89
73692	728	116	816		36	37	36		129	11102	56	55913	0.05	0.03	-5.30	80	69
73789	112	115	816		36	35	36		124	11068	55	55971	0.00	0.05	14.30	60	89
73890	728	117	816		35	33	34		118	11028	64	56027	0.14	0.07	-19.80	94	55
73991	728	120	816		34	33	34		112	10988	69	56080	0.00	0.07	19.60	55	94
74090	728	120	816		34	33	32		109	10942	67	56165	0.11	0.07	-10.90	85	64
74189	728	122	816		33	32	32		103	10880	72	56246	0.06	0.08	5.40	69	80
74290	728	122	816		32	31	32		100	10825	85	56296	0.03	0.09	17.60	57	92
74393	728	794	816		32	30	31		92	10778	91	56321	0.10	0.10	0.60	74	75
74489	728	120	816		31	30	31		85	10736	86	56382	-0.01	0.10	31.90	43	106
74589	728	794	816		30	30	30		81	10687	92	56196	-0.01	0.10	33.00	42	108
74689	728	119	816		30	29	30		78	10611	104	56033	-0.01	0.11	35.90	39	110
74793	728	123	816		30	28	29		74	10519	107	55890	0.11	0.12	4.10	70	79
74890	728	122	816		30	29	29		71	10414	107	55749	0.12	0.11	-2.80	77	72
74990	728	794	816		31	29	29		67	10296	119	55629	0.20	0.11	-26.20	101	48
75089	728	115	816		31	30	29		63	10341	122	55523	0.15	0.10	-15.40	90	59
75189	728	119	816		31	31	30		59	10388	111	55452	0.09	0.09	-0.50	75	74
75290	728	115	816		32	31	30		52	10501	750	62042	0.20	0.09	-31.90	106	43
75392	728	119	816		33	31	30		47	10633	819	69966	0.30	0.09	-62.00	127	12
75490	728	123	816		32	32	31		40	10775	819	77492	0.12	0.08	-13.70	88	61
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither
In SpinTurn(CCW, 90.00, 45.00)
76435: glLeftCenterCm = 794 glRightCenterCm = 26
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
76440: TrackRightWallOffset(), glRightFrontCm = 27

In TrackRightWallOffset with Kp/Ki/Kd = 300.00	0.00	10.00

In TrackRightWallOffset with RR/RC/RF = 27	25	26
straight window = 8 cm
R/C/F dists = 27	25	26 Steerval = 0.010, WallAngle = 0.57, Inside
In SpinTurn(CCW, 29.43, 45.00)
R/C/F dists = 26	28	31 Steerval = 0.440, WallOrientDeg = 25.14

approach start: orient angle = 25.143, tgt_distCm = 40
At start - err = prev_err = -9
R/C/F dists = 26	28	31 Steerval = 0.440, OrientDeg = 25.14, err/prev_err = -9
R/C/F dists = 26	28	31 Steerval = 0.440, OrientDeg = 25.14, err/prev_err = -9
Msec	RFront	RCtr	RRear	Orient	Front	Rear	Err	P_err
76821: Error in GetFrontDistCm() - 0 replaced with 71
76832	32	31	26	33.7	159	40	-9	-9
76944	37	35	30	38.3	171	42	-5	-9
77059	41	39	33	42.3	167	46	-1	-5
77171	43	42	36	42.9	165	50	2	-1
At end of offset capture - prev_res*res = -2
correct back to parallel (Right side): RF/RC/RR/Orient = 43	42	36	42.86
In SpinTurn(CW, 42.86, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	10.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
77927	728	794	816		42	41	41		116	10841	49	56047	0.08	-0.01	-26.10	101	48
78023	728	794	816		43	41	42		116	10846	46	56107	0.10	-0.01	-32.80	107	42
78126	728	794	816		42	41	45		119	10845	53	56159	-0.26	-0.01	71.40	3	127
78224	728	794	816		42	41	44		117	10841	58	56198	-0.21	-0.01	60.50	14	127
78326	728	794	816		39	39	38		109	10841	54	56270	0.11	0.01	-27.00	102	48
78427	728	794	816		38	36	37		101	10844	56	56357	0.08	0.04	-12.60	87	62
78525	728	794	816		37	35	36		96	10850	65	56427	0.12	0.05	-20.70	95	54
78623	728	794	816		36	36	34		91	10852	72	56493	0.21	0.04	-50.00	125	25
78726	728	794	816		35	35	35		87	10848	71	56543	0.01	0.05	9.90	65	84
78828	728	794	816		35	34	36		84	10841	78	56607	-0.13	0.06	55.50	19	127
78927	728	794	816		34	33	34		80	10836	90	47712	-0.07	0.07	42.50	32	117
79025	728	794	816		33	31	33		74	10836	94	47751	0.04	0.09	15.90	59	90
79124	728	794	816		32	31	31		71	10836	85	40235	0.04	0.09	15.00	60	90
79223	728	794	816		32	31	32		66	10839	103	40236	-0.03	0.09	35.30	39	110
79324	728	794	816		30	29	29		61	10845	110	30500	0.11	0.11	1.20	73	76
79424	728	794	816		30	30	29		56	10855	102	30502	0.18	0.10	-23.20	98	51
79523	728	794	816		31	30	29		50	10862	108	30502	0.27	0.10	-50.10	125	24
79625	728	794	816		31	30	29		43	10877	126	30505	0.21	0.10	-33.60	108	41
79725	728	794	816		31	30	29		38	10884	126	30344	0.18	0.10	-24.30	99	50
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither
In SpinTurn(CCW, 90.00, 45.00)
80675: glLeftCenterCm = 794 glRightCenterCm = 24
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
80680: TrackRightWallOffset(), glRightFrontCm = 25

In TrackRightWallOffset with Kp/Ki/Kd = 300.00	0.00	10.00

In TrackRightWallOffset with RR/RC/RF = 25	24	25
straight window = 8 cm
R/C/F dists = 25	24	25 Steerval = 0.050, WallAngle = 2.86, Inside
In SpinTurn(CCW, 27.14, 45.00)
R/C/F dists = 25	26	28 Steerval = 0.300, WallOrientDeg = 17.14

approach start: orient angle = 17.143, tgt_distCm = 40
At start - err = prev_err = -12
R/C/F dists = 25	26	28 Steerval = 0.300, OrientDeg = 17.14, err/prev_err = -12
R/C/F dists = 25	26	28 Steerval = 0.300, OrientDeg = 17.14, err/prev_err = -12
Msec	RFront	RCtr	RRear	Orient	Front	Rear	Err	P_err
81023: Error in GetFrontDistCm() - 0 replaced with 85
81031: Error in GetFrontDistCm() - 0 replaced with 85
81035	28	27	24	24.0	85	31	-13	-12
81151	32	32	27	31.4	205	40	-8	-13
81265	35	33	29	34.3	201	45	-7	-8
81378	38	36	31	38.3	196	49	-4	-7
81496	42	38	35	32.0	191	50	-2	-4
81611	45	44	38	17.7	189	55	2	-2
At end of offset capture - prev_res*res = -4
correct back to parallel (Right side): RF/RC/RR/Orient = 45	44	38	17.71
In SpinTurn(CW, 17.71, 30.00)

TrackRightWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	10.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar	Steer	Set	Output	LSpd	RSpd
82121	113	117	126		44	46	42		134	11189	51	30097	0.19	-0.06	-72.50	127	2
82221	109	111	127		46	45	43		129	11171	57	30079	0.25	-0.05	-89.50	127	0
82321	106	110	119		45	44	43		127	11143	58	30075	0.20	-0.04	-72.60	127	2
82419	108	111	115		46	46	47		127	11105	55	30075	-0.07	-0.06	0.50	74	75
82522	728	114	120		50	49	49		128	11055	61	30081	0.02	-0.09	-31.80	106	43
82621	112	116	816		50	47	50		109	11015	71	30077	-0.03	-0.07	-12.70	87	62
82721	728	794	816		48	46	52		104	10975	69	30073	-0.32	-0.06	75.00	0	127
82820	728	794	126		44	44	46		101	10930	73	30072	-0.18	-0.04	43.20	31	118
82919	728	794	816		43	42	43		110	10854	83	30074	0.05	-0.02	-18.90	93	56
83024	728	121	816		38	37	41		264	11010	82	30086	-0.29	0.03	92.10	0	127
83124	728	794	816		35	35	37		263	11116	83	30090	-0.18	0.05	69.90	5	127
83222	728	794	816		32	32	32		163	10944	85	30098	0.01	0.08	22.60	52	97
83319	728	794	816		30	30	32		87	10840	97	30103	-0.14	0.10	70.30	4	127
83421	728	794	128		30	29	30		82	10725	101	30106	-0.01	0.11	37.20	37	112
83520	728	124	816		29	27	27		81	10581	99	30110	0.18	0.13	-13.30	88	61
83621	728	122	816		28	27	28		77	10425	102	30115	-0.04	0.13	48.80	26	123
83720	728	123	816		28	27	26		73	10555	117	30114	0.17	0.13	-9.90	84	65
83819	728	117	816		28	28	28		68	10656	108	30115	0.05	0.12	19.90	55	94
83919	728	120	816		30	29	28		66	10746	750	30115	0.29	0.11	-51.50	126	23
84018	114	123	816		31	29	27		63	10839	819	30115	0.36	0.11	-74.30	127	0
84119	112	120	816		31	30	28		57	10950	819	30115	0.29	0.10	-57.60	127	17
84217	112	118	126		31	30	30		51	11065	750	37818	0.10	0.10	-1.90	76	73
84317	728	794	816		31	30	31		45	11196	750	45125	0.00	0.10	29.00	46	104
84418	728	122	127		31	29	29		39	11339	819	53775	0.18	0.11	-19.30	94	55

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

In TrackRightWallOffset with Kp/Ki/Kd = 300.00 0.00 10.00

In TrackRightWallOffset with RR/RC/RF = 30 30 30

straight window = 8 cm

R/C/F dists = 30 30 30 Steerval = 0.020, WallAngle = 1.14, Inside

In SpinTurn(CCW, 28.86, 45.00)

R/C/F dists = 27 30 30 Steerval = 0.350, WallOrientDeg = 20.00

approach start: orient angle = 20.000, tgt_distCm = 40

At start - err = prev_err = -10

R/C/F dists = 27 30 30 Steerval = 0.350, OrientDeg = 20.00, err/prev_err = -10

Msec RFront RCtr RRear Orient Front Rear Err P_err

68368: Error in GetFrontDistCm() - 0 replaced with 60

68384 31 30 28 17.1 118 46 -10 -10

68496 34 33 30 22.3 125 56 -7 -10

68609 36 36 32 24.0 119 60 -4 -7

68720 39 38 34 28.0 115 66 -2 -4

68831 44 41 36 44.0 108 68 1 -2

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

correct back to parallel (Right side): RF/RC/RR/Orient = 44 41 36 44.00

In SpinTurn(CW, 44.00, 30.00)

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

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

69557 728 794 816 49 47 50 95 10899 73 39363 -0.10 -0.07 8.70 66 83

69657 728 794 816 50 51 52 99 10901 80 39332 -0.18 -0.11 20.60 54 95

69756 728 794 816 50 48 50 93 10910 86 39318 0.00 -0.08 -22.50 97 52

69858 728 794 816 49 46 48 90 10917 94 39277 0.09 -0.06 -44.30 119 30

69958 728 794 816 46 44 47 86 10927 87 39265 -0.10 -0.04 15.90 59 90

70055 728 114 816 44 45 45 76 10956 102 39252 -0.11 -0.05 18.00 57 93

70157 728 108 816 43 41 43 77 10976 819 48383 -0.02 -0.01 3.50 71 78

70255 728 109 816 39 38 40 74 10994 123 48334 -0.09 0.02 32.00 43 107

70357 109 99 816 36 36 38 70 11023 750 55303 -0.19 0.04 67.80 7 127

70455 112 103 816 34 33 34 65 11056 104 55318 -0.02 0.07 28.40 46 103

70557 728 116 816 29 30 31 55 11101 819 63702 -0.15 0.10 73.40 1 127

70655 728 120 816 27 27 28 48 11170 106 63708 -0.08 0.13 63.40 11 127

70757 728 794 816 24 24 25 43 11246 110 63715 -0.11 0.15 77.50 0 127

70854 728 794 816 22 22 24 37 11329 118 55334 -0.19 0.15 101.20 0 127

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither

In SpinTurn(CCW, 90.00, 45.00)

71682: glLeftCenterCm = 794 glRightCenterCm = 25

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

71686: TrackRightWallOffset(), glRightFrontCm = 26

In TrackRightWallOffset with Kp/Ki/Kd = 300.00 0.00 10.00

In TrackRightWallOffset with RR/RC/RF = 26 25 26

straight window = 8 cm

R/C/F dists = 26 25 26 Steerval = -0.060, WallAngle = -3.43, Inside

In SpinTurn(CCW, 33.43, 45.00)

R/C/F dists = 26 30 32 Steerval = 0.610, WallOrientDeg = 34.86

approach start: orient angle = 34.857, tgt_distCm = 40

At start - err = prev_err = -8

R/C/F dists = 26 30 32 Steerval = 0.610, OrientDeg = 34.86, err/prev_err = -8

R/C/F dists = 27 33 33 Steerval = 0.580, OrientDeg = 33.14, err/prev_err = -8

Msec RFront RCtr RRear Orient Front Rear Err P_err

72138 33 33 27 33.1 219 25 -7 -8

72254 40 38 31 40.0 195 29 -3 -7

72365 44 43 34 43.4 187 32 3 -3

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

correct back to parallel (Right side): RF/RC/RR/Orient = 44 43 34 43.43

In SpinTurn(CW, 43.43, 30.00)

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

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

73091 110 112 122 40 38 39 151 11228 29 55621 0.09 0.02 -20.30 95 54

73189 728 110 816 39 40 41 153 11220 35 55688 -0.14 0.00 39.90 35 114

73291 728 112 816 40 38 39 150 11210 37 55762 0.18 0.02 -45.00 120 29

73394 728 111 816 40 38 39 146 11195 36 55854 0.06 0.02 -13.20 88 61

73492 114 115 120 38 37 38 141 11171 41 55917 0.07 0.03 -12.00 87 63

73589 728 113 816 38 38 38 134 11147 50 55921 -0.03 0.02 14.10 60 89

73692 728 116 816 36 37 36 129 11102 56 55913 0.05 0.03 -5.30 80 69

73789 112 115 816 36 35 36 124 11068 55 55971 0.00 0.05 14.30 60 89

73890 728 117 816 35 33 34 118 11028 64 56027 0.14 0.07 -19.80 94 55

73991 728 120 816 34 33 34 112 10988 69 56080 0.00 0.07 19.60 55 94

74090 728 120 816 34 33 32 109 10942 67 56165 0.11 0.07 -10.90 85 64

74189 728 122 816 33 32 32 103 10880 72 56246 0.06 0.08 5.40 69 80

74290 728 122 816 32 31 32 100 10825 85 56296 0.03 0.09 17.60 57 92

74393 728 794 816 32 30 31 92 10778 91 56321 0.10 0.10 0.60 74 75

74489 728 120 816 31 30 31 85 10736 86 56382 -0.01 0.10 31.90 43 106

74589 728 794 816 30 30 30 81 10687 92 56196 -0.01 0.10 33.00 42 108

74689 728 119 816 30 29 30 78 10611 104 56033 -0.01 0.11 35.90 39 110

74793 728 123 816 30 28 29 74 10519 107 55890 0.11 0.12 4.10 70 79

74890 728 122 816 30 29 29 71 10414 107 55749 0.12 0.11 -2.80 77 72

74990 728 794 816 31 29 29 67 10296 119 55629 0.20 0.11 -26.20 101 48

75089 728 115 816 31 30 29 63 10341 122 55523 0.15 0.10 -15.40 90 59

75189 728 119 816 31 31 30 59 10388 111 55452 0.09 0.09 -0.50 75 74

75290 728 115 816 32 31 30 52 10501 750 62042 0.20 0.09 -31.90 106 43

75392 728 119 816 33 31 30 47 10633 819 69966 0.30 0.09 -62.00 127 12

75490 728 123 816 32 32 31 40 10775 819 77492 0.12 0.08 -13.70 88 61

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither

In SpinTurn(CCW, 90.00, 45.00)

76435: glLeftCenterCm = 794 glRightCenterCm = 26

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

76440: TrackRightWallOffset(), glRightFrontCm = 27

In TrackRightWallOffset with Kp/Ki/Kd = 300.00 0.00 10.00

In TrackRightWallOffset with RR/RC/RF = 27 25 26

straight window = 8 cm

R/C/F dists = 27 25 26 Steerval = 0.010, WallAngle = 0.57, Inside

In SpinTurn(CCW, 29.43, 45.00)

R/C/F dists = 26 28 31 Steerval = 0.440, WallOrientDeg = 25.14

approach start: orient angle = 25.143, tgt_distCm = 40

At start - err = prev_err = -9

R/C/F dists = 26 28 31 Steerval = 0.440, OrientDeg = 25.14, err/prev_err = -9

Msec RFront RCtr RRear Orient Front Rear Err P_err

76821: Error in GetFrontDistCm() - 0 replaced with 71

76832 32 31 26 33.7 159 40 -9 -9

76944 37 35 30 38.3 171 42 -5 -9

77059 41 39 33 42.3 167 46 -1 -5

77171 43 42 36 42.9 165 50 2 -1

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

correct back to parallel (Right side): RF/RC/RR/Orient = 43 42 36 42.86

In SpinTurn(CW, 42.86, 30.00)

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

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

77927 728 794 816 42 41 41 116 10841 49 56047 0.08 -0.01 -26.10 101 48

78023 728 794 816 43 41 42 116 10846 46 56107 0.10 -0.01 -32.80 107 42

78126 728 794 816 42 41 45 119 10845 53 56159 -0.26 -0.01 71.40 3 127

78224 728 794 816 42 41 44 117 10841 58 56198 -0.21 -0.01 60.50 14 127

78326 728 794 816 39 39 38 109 10841 54 56270 0.11 0.01 -27.00 102 48

78427 728 794 816 38 36 37 101 10844 56 56357 0.08 0.04 -12.60 87 62

78525 728 794 816 37 35 36 96 10850 65 56427 0.12 0.05 -20.70 95 54

78623 728 794 816 36 36 34 91 10852 72 56493 0.21 0.04 -50.00 125 25

78726 728 794 816 35 35 35 87 10848 71 56543 0.01 0.05 9.90 65 84

78828 728 794 816 35 34 36 84 10841 78 56607 -0.13 0.06 55.50 19 127

78927 728 794 816 34 33 34 80 10836 90 47712 -0.07 0.07 42.50 32 117

79025 728 794 816 33 31 33 74 10836 94 47751 0.04 0.09 15.90 59 90

79124 728 794 816 32 31 31 71 10836 85 40235 0.04 0.09 15.00 60 90

79223 728 794 816 32 31 32 66 10839 103 40236 -0.03 0.09 35.30 39 110

79324 728 794 816 30 29 29 61 10845 110 30500 0.11 0.11 1.20 73 76

79424 728 794 816 30 30 29 56 10855 102 30502 0.18 0.10 -23.20 98 51

79523 728 794 816 31 30 29 50 10862 108 30502 0.27 0.10 -50.10 125 24

79625 728 794 816 31 30 29 43 10877 126 30505 0.21 0.10 -33.60 108 41

79725 728 794 816 31 30 29 38 10884 126 30344 0.18 0.10 -24.30 99 50

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Neither

In SpinTurn(CCW, 90.00, 45.00)

80675: glLeftCenterCm = 794 glRightCenterCm = 24

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

80680: TrackRightWallOffset(), glRightFrontCm = 25

In TrackRightWallOffset with Kp/Ki/Kd = 300.00 0.00 10.00

In TrackRightWallOffset with RR/RC/RF = 25 24 25

straight window = 8 cm

R/C/F dists = 25 24 25 Steerval = 0.050, WallAngle = 2.86, Inside

In SpinTurn(CCW, 27.14, 45.00)

R/C/F dists = 25 26 28 Steerval = 0.300, WallOrientDeg = 17.14

approach start: orient angle = 17.143, tgt_distCm = 40

At start - err = prev_err = -12

R/C/F dists = 25 26 28 Steerval = 0.300, OrientDeg = 17.14, err/prev_err = -12

Msec RFront RCtr RRear Orient Front Rear Err P_err

81023: Error in GetFrontDistCm() - 0 replaced with 85

81031: Error in GetFrontDistCm() - 0 replaced with 85

81035 28 27 24 24.0 85 31 -13 -12

81151 32 32 27 31.4 205 40 -8 -13

81265 35 33 29 34.3 201 45 -7 -8

81378 38 36 31 38.3 196 49 -4 -7

81496 42 38 35 32.0 191 50 -2 -4

81611 45 44 38 17.7 189 55 2 -2

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

correct back to parallel (Right side): RF/RC/RR/Orient = 45 44 38 17.71

In SpinTurn(CW, 17.71, 30.00)

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

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output LSpd RSpd

82121 113 117 126 44 46 42 134 11189 51 30097 0.19 -0.06 -72.50 127 2

82221 109 111 127 46 45 43 129 11171 57 30079 0.25 -0.05 -89.50 127 0

82321 106 110 119 45 44 43 127 11143 58 30075 0.20 -0.04 -72.60 127 2

82419 108 111 115 46 46 47 127 11105 55 30075 -0.07 -0.06 0.50 74 75

82522 728 114 120 50 49 49 128 11055 61 30081 0.02 -0.09 -31.80 106 43

82621 112 116 816 50 47 50 109 11015 71 30077 -0.03 -0.07 -12.70 87 62

82721 728 794 816 48 46 52 104 10975 69 30073 -0.32 -0.06 75.00 0 127

82820 728 794 126 44 44 46 101 10930 73 30072 -0.18 -0.04 43.20 31 118

82919 728 794 816 43 42 43 110 10854 83 30074 0.05 -0.02 -18.90 93 56

83024 728 121 816 38 37 41 264 11010 82 30086 -0.29 0.03 92.10 0 127

83124 728 794 816 35 35 37 263 11116 83 30090 -0.18 0.05 69.90 5 127

83222 728 794 816 32 32 32 163 10944 85 30098 0.01 0.08 22.60 52 97

83319 728 794 816 30 30 32 87 10840 97 30103 -0.14 0.10 70.30 4 127

83421 728 794 128 30 29 30 82 10725 101 30106 -0.01 0.11 37.20 37 112

83520 728 124 816 29 27 27 81 10581 99 30110 0.18 0.13 -13.30 88 61

83621 728 122 816 28 27 28 77 10425 102 30115 -0.04 0.13 48.80 26 123

83720 728 123 816 28 27 26 73 10555 117 30114 0.17 0.13 -9.90 84 65

83819 728 117 816 28 28 28 68 10656 108 30115 0.05 0.12 19.90 55 94

83919 728 120 816 30 29 28 66 10746 750 30115 0.29 0.11 -51.50 126 23

84018 114 123 816 31 29 27 63 10839 819 30115 0.36 0.11 -74.30 127 0

84119 112 120 816 31 30 28 57 10950 819 30115 0.29 0.10 -57.60 127 17

84217 112 118 126 31 30 30 51 11065 750 37818 0.10 0.10 -1.90 76 73

84317 728 794 816 31 30 31 45 11196 750 45125 0.00 0.10 29.00 46 104

84418 728 122 127 31 29 29 39 11339 819 53775 0.18 0.11 -19.30 94 55

Looking at the video and the telemetry, the first leg starts with the normal offset capture maneuver, which ends with the robot about 44cm from the wall. Then it makes a pretty distinct correction toward the wall, overshoots the desired offset, and winds up the leg at about 22cm from the wall.

The second leg again starts with a capture maneuver to about 43cm. Then it stabilizes at about 32cm from the wall – nice.

The third leg maneuvers to about 43cm, and then again stabilizes at about 30cm.

The fourth leg was a bit anomalous, as it appeared to way overcorrect after capturing the desired 40cm offset, but I couldn’t find anything in the telemetry to explain it. It’s a mystery!

It’s clear from the above that I no longer need to correct for orientation angle induced errors during the offset maneuver, as these are now handled by my recent ‘global’ correction code. This will probably help with subsequent offset tracking, as the initial offset should be closer to the offset target at the start of the tracking phase. We’ll see…

After a number of trial runs, I finally settled (as much as anything is ‘settled’ in the Wall-E world) on PID = (400,5,40). Here’s a short video showing performance in this configuration:

And, once again, I still have to port this configuration and code back to the left-side wall tracking configuration. Here’s a short video of left-side wall tracking. Interestingly, my ‘random walk’ PID tuning technique resulted in significantly different PID values (300,0,200) vs (400,5,40) than the right side. No clue why.

At this point, I believe I have gone about as far as I can at the moment for wall tracking. WallE3 now can consistently track the walls in my office ‘sandbox’ using either the left-side or the right-side wall for reference. My plan going forward is to ‘archive’ this version (WallE3_WallTrack_V5) by copying it to a new project. The new project will have the goal of integrating charging station homing/connection into the system.

In preparation, I recently modified the charging station lead-in structure to accommodate the wider wheelbase on WallE3, as shown in the following photo:

17 April 2022 Update:

Well, I spoke too soon when I said above that wall-tracking was “settled”. I ran into a couple of significant problems; first, when the robot is already close to the proper offset, it is supposed to just turn to parallel the wall and then go into tracking mode, but on a number of occasions Wall-E3 ran out of control into the next wall. Secondly, wall tracking was anything but smooth, and I couldn’t get it to reliably track the desired offset. So, back to the drawing board (again).

The ‘close enough’ failures were being caused by a flaw in the ‘RotateToParallelOrientation() routine; as the robot approached the parallel orientation, the PID controller also started slowing the rotation speed, to the point where the robot wasn’t rotating anymore – just going straight ahead. If the actual parallel orientation wasn’t reached, the robot just kept going straight ahead forever – oops! The fix for this was to abandon the RotateToParallelOrientation() subroutine entirely, and just use WallOrientDeg() to get the current angular offset from parallel, and SpinTurn() to turn that angular amount back to parallel. RotateToParallelOrientation() is only used in two places (TrackRight/LeftWallOffset()), so the entire function can be removed as well.

The issue with offset tracking continues to bedevil me. When the robot is turned to approach the offset, the measured distances go the wrong way, so the PID tends to ‘wind up’ and drive the robot toward or away from the wall, rather than smoothly approaching the offset. I thought I had the answer to this by ‘tweaking’ the distance corrections due to off-parallel angles, but sadly, this did not help.

So, I removed the off-angle distance correction and went back to just tracking the steering angle – a value proportional to the difference between the front and rear side distance measurements. Now tracking was much more stable, but the robot traveled in a straight line slightly toward the wall. After a few trials, I realized that the robot was doing exactly what I told it to do – drive the front/back measurement error to zero, but unfortunately ‘zero’ did not equate to ‘parallel’. After scratching my head for a while, I realized that rather than using zero as the setpoint, I should use the value that causes the robot to travel parallel to the wall – which turned out to be about 0.25. Using this value I could increase the Kp value back up to 400 or so, and this resulted in very good tracking of whatever offset resulted from the ‘offset approach’ phase of the tracking algorithm. Just this step was a huge improvement in tracking performance, but it wasn’t quite ‘offset tracking’ yet as it didn’t pay any attention to the actual offset – just the difference between the front and back wall offset measurements.

Once I had this working, I was able to re-incorporate my earlier idea of biasing the actual steering value with a term that is proportional to the actual offset, i.e.

WallTrackSteerVal = glRightSteeringVal + (float)(glRightCenterCm – offsetCm) / 50.f;

This, coupled with the empirically determined steering value setpoint of 0.25 resulted in a very stable, very precise tracking performance, as shown in the short video below and the associated telemetry and Excel plots.

PID (400,0,0), SetPoint = +0.25

approach start: orient angle = 16.571, tgt_distCm = 35
At start: meas_dist = 15, corr_dist = 14, err = prev_err = -21
R/C/F dists = 13	15	15 Steerval = 0.290, OrientDeg = 16.57, err/prev_err = -21
Msec	RFront	RCtr	RRear	ODeg	ORad	c_dist	Err	P_err
11024	16	15	12	16.6	0.3	14	-21	-21
11135	18	17	14	16.6	0.3	16	-19	-21
11249	20	18	16	16.6	0.3	17	-18	-19
11365	22	22	18	16.6	0.3	21	-14	-18
11480	26	24	21	16.6	0.3	23	-12	-14
11591	29	27	23	16.6	0.3	25	-10	-12
11702	31	30	26	16.6	0.3	28	-7	-10
11814	34	32	29	16.6	0.3	30	-5	-7
11927	36	34	31	16.6	0.3	32	-3	-5
12037	41	38	35	16.6	0.3	36	1	-3
At end of offset capture - prev_res*res = -3
correct back to parallel (Right side): RF/RC/RR/Orient = 41	38	35	16.57
In SpinTurn(CW, 16.57, 30.00)

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
12519	728	794	816		41	39	39		90	11753	60	11683	0.21	0.25	16	59	91
12619	728	794	816		41	39	38		90	11797	70	11628	0.31	0.25	-24	99	51
12719	728	794	816		41	40	38		87	11752	69	11576	0.39	0.25	-56	127	19
12817	728	794	816		41	40	38		87	11588	63	11529	0.37	0.25	-48	123	27
12917	728	113	816		42	40	38		80	11311	70	11482	0.40	0.25	-60	127	14
13016	728	794	816		42	40	40		75	10891	78	11436	0.20	0.25	20	55	95
13118	728	794	816		42	41	40		71	10302	84	11392	0.23	0.25	 8	67	83
13220	728	794	816		43	40	39		66	9523	79	11350	0.36	0.25	-44	119	30
13321	728	794	816		42	40	39		62	9693	84	11309	0.22	0.25	12	63	87
13416	728	794	816		42	40	39		59	9870	93	11273	0.27	0.25	-8	83	67
13519	728	794	816		40	39	40		57	10068	93	11237	-0.02	0.25	108	0	127
13618	728	794	816		40	39	38		54	10261	91	11202	0.14	0.25	44	31	119
13716	728	794	816		39	38	38		51	10455	107	11177	0.09	0.25	64	11	127
13817	728	794	816		40	37	37		45	10663	112	11156	0.20	0.25	20	54	95
13918	728	794	816		39	38	36		41	10879	109	11132	0.29	0.25	-16	91	58
14017	728	794	816		40	39	36		35	11109	115	11114	0.38	0.25	-52	127	23
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Right
In SpinTurn(CCW, 90.00, 45.00)
IRHomingValTotalAvg = 199 
15351: glLeftCenterCm = 794 glRightCenterCm = 24
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
15355: TrackRightWallOffset(), glRightFrontCm = 26

In TrackRightWallOffset with Kp/Ki/Kd = 400.00	0.00	0.00

In TrackRightWallOffset with RR/RC/RF = 24	24	26
straight window = 8 cm
R/C/F dists = 24	24	26 Steerval = 0.170, WallAngle = 9.71, Inside
In SpinTurn(CCW, 20.29, 45.00)
R/C/F dists = 26	31	32 Steerval = 0.590, WallOrientDeg = 33.71

approach start: orient angle = 33.714, tgt_distCm = 35
At start: meas_dist = 31, corr_dist = 26, err = prev_err = -9
R/C/F dists = 26	31	32 Steerval = 0.590, OrientDeg = 33.71, err/prev_err = -9
Msec	RFront	RCtr	RRear	ODeg	ORad	c_dist	Err	P_err
15712	33	31	27	33.7	0.6	25	-10	-9
15826	37	34	29	33.7	0.6	28	-7	-10
15940	38	37	32	33.7	0.6	30	-5	-7
16053	42	41	36	33.7	0.6	34	-1	-5
16165	46	44	39	33.7	0.6	36	1	-1
At end of offset capture - prev_res*res = -1
correct back to parallel (Right side): RF/RC/RR/Orient = 46	44	39	33.71
In SpinTurn(CW, 33.71, 30.00)

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
16862	112	110	125		43	43	42		108	11196	64	11178	0.25	0.25	 0	75	75
16964	728	110	816		45	43	42		108	11162	65	11179	0.27	0.25	-8	82	67
17060	728	111	127		44	42	40		104	11135	61	11186	0.39	0.25	-56	127	19
17161	110	114	124		42	42	41		102	11105	70	11186	0.19	0.25	24	51	99
17263	728	114	126		44	42	41		100	11077	75	11184	0.30	0.25	-20	94	55
17362	728	115	816		43	42	41		97	11038	71	11186	0.23	0.25	 8	67	83
17461	728	116	126		43	41	41		95	10989	77	11185	0.24	0.25	 4	71	79
17562	728	113	816		44	41	41		90	10938	86	11183	0.36	0.25	-44	119	30
17664	728	109	124		43	41	41		84	10886	91	11180	0.24	0.25	 4	71	79
17763	728	118	816		41	41	41		82	10825	90	11179	0.07	0.25	72	3	127
17860	728	794	127		42	39	39		79	10762	90	11178	0.29	0.25	-16	91	59
17961	728	115	816		41	40	39		73	10708	106	11182	0.23	0.25	 8	67	83
18059	728	120	816		42	39	39		69	10656	106	11186	0.21	0.25	16	59	91
18161	728	116	816		41	39	39		64	10608	105	11189	0.15	0.25	40	35	115
18262	728	121	816		40	39	36		60	10550	113	11198	0.35	0.25	-40	115	35
18360	728	121	816		40	38	38		58	10482	124	11217	0.17	0.25	32	42	107
18459	728	121	816		40	38	37		53	10405	117	11228	0.22	0.25	12	63	87
18561	728	121	816		40	38	39		49	10600	750	19677	0.07	0.25	72	3	127
18659	728	794	816		40	39	37		43	10806	819	29443	0.24	0.25	 4	71	79
18762	728	120	816		40	39	36		36	11021	819	38752	0.37	0.25	-48	122	27
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Right
In SpinTurn(CCW, 90.00, 45.00)
IRHomingValTotalAvg = 273 
19919: glLeftCenterCm = 794 glRightCenterCm = 25
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
19923: TrackRightWallOffset(), glRightFrontCm = 28

In TrackRightWallOffset with Kp/Ki/Kd = 400.00	0.00	0.00

In TrackRightWallOffset with RR/RC/RF = 26	25	28
straight window = 8 cm
R/C/F dists = 26	25	28 Steerval = 0.180, WallAngle = 10.29, Inside
In SpinTurn(CCW, 19.71, 45.00)
R/C/F dists = 25	28	30 Steerval = 0.490, WallOrientDeg = 28.00

approach start: orient angle = 28.000, tgt_distCm = 35
At start: meas_dist = 28, corr_dist = 25, err = prev_err = -10
R/C/F dists = 25	28	30 Steerval = 0.520, OrientDeg = 29.71, err/prev_err = -10
Msec	RFront	RCtr	RRear	ODeg	ORad	c_dist	Err	P_err
20308	30	28	25	29.7	0.5	24	-11	-10
20420	32	30	26	29.7	0.5	26	-9	-11
20533	33	32	29	29.7	0.5	26	-9	-9
20647	36	33	30	29.7	0.5	28	-7	-9
20761	38	36	32	29.7	0.5	31	-4	-7
20876	41	38	36	29.7	0.5	33	-2	-4
20988	42	42	38	29.7	0.5	36	1	-2
At end of offset capture - prev_res*res = -2
correct back to parallel (Right side): RF/RC/RR/Orient = 42	42	38	29.71
In SpinTurn(CW, 29.71, 30.00)

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
21657	728	794	816		45	42	41		97	10859	68	29659	0.40	0.25	-60	127	14
21758	728	794	816		46	42	43		99	10841	69	29645	0.26	0.25	-4	79	71
21856	728	794	816		44	44	45		98	10811	65	29634	0.04	0.25	84	0	127
21956	728	794	816		45	43	44		95	10782	68	29638	0.14	0.25	44	31	119
22055	728	794	816		44	41	42		91	10754	77	29623	0.21	0.25	16	59	91
22154	728	794	816		42	40	40		85	10722	73	29601	0.21	0.25	16	59	91
22258	728	123	816		42	40	41		82	10686	77	29588	0.14	0.25	44	31	119
22356	728	794	816		42	39	39		77	10648	87	29574	0.24	0.25	 4	71	79
22459	728	794	816		41	39	40		74	10604	90	29566	0.16	0.25	36	39	111
22555	728	794	816		40	38	38		70	10564	89	29551	0.19	0.25	24	51	99
22656	728	794	816		39	37	37		68	10513	93	29539	0.08	0.25	68	7	127
22758	728	794	816		40	38	37		64	10459	107	29526	0.22	0.25	12	63	87
22855	728	123	816		42	39	37		60	10395	111	29512	0.49	0.25	-96	127	0
22957	728	121	816		43	42	39		56	10314	106	29497	0.43	0.25	-72	127	3
23057	728	794	816		42	42	39		52	10491	112	29495	0.46	0.25	-84	127	0
23156	728	794	816		43	41	40		49	10673	121	29492	0.32	0.25	-28	103	46
23256	728	794	816		44	41	41		43	10870	126	29488	0.34	0.25	-36	111	39
23355	728	794	816		43	41	40		40	11071	750	37177	0.39	0.25	-56	127	18
In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected
WALL_OFFSET_DIST_AHEAD case detected with tracking case Right
In SpinTurn(CCW, 90.00, 45.00)
IRHomingValTotalAvg = 193 
24722: glLeftCenterCm = 794 glRightCenterCm = 30
glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()
24726: TrackRightWallOffset(), glRightFrontCm = 31

In TrackRightWallOffset with Kp/Ki/Kd = 400.00	0.00	0.00

In TrackRightWallOffset with RR/RC/RF = 30	30	31
straight window = 8 cm
R/C/F dists = 30	30	31 Steerval = 0.140, WallAngle = 8.00, Inside
In SpinTurn(CCW, 22.00, 45.00)
R/C/F dists = 27	29	30 Steerval = 0.300, WallOrientDeg = 17.14

approach start: orient angle = 17.143, tgt_distCm = 35
At start: meas_dist = 29, corr_dist = 28, err = prev_err = -7
R/C/F dists = 27	29	30 Steerval = 0.300, OrientDeg = 17.14, err/prev_err = -7
Msec	RFront	RCtr	RRear	ODeg	ORad	c_dist	Err	P_err
25109	31	29	27	17.1	0.3	27	-8	-7
25220	32	30	29	17.1	0.3	28	-7	-8
25331	33	31	28	17.1	0.3	29	-6	-7
25446	34	31	30	17.1	0.3	29	-6	-6
25559	35	33	31	17.1	0.3	31	-4	-6
25674	35	34	32	17.1	0.3	32	-3	-4
25787	37	35	32	17.1	0.3	33	-2	-3
25899	39	37	33	17.1	0.3	35	0	-2
At end of offset capture - prev_res*res = 0
correct back to parallel (Right side): RF/RC/RR/Orient = 39	37	33	17.14
In SpinTurn(CW, 17.14, 30.00)

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
26414	111	113	816		41	39	39		99	10861	74	37154	0.21	0.25	16	59	91
26512	113	117	123		41	40	38		100	10811	70	37167	0.27	0.25	-8	83	67
26612	109	113	118		42	38	39		96	10759	70	37170	0.29	0.25	-16	91	58
26714	111	115	124		41	39	38		94	10698	80	37163	0.34	0.25	-36	111	39
26811	109	117	119		40	38	37		88	10645	88	37159	0.27	0.25	-8	83	67
26912	111	113	124		39	38	37		84	10588	84	37172	0.24	0.25	 4	71	79
27012	113	117	816		40	38	36		80	10527	87	37178	0.23	0.25	 8	67	82
27111	728	120	124		38	37	36		77	10460	98	37165	0.12	0.25	52	23	127
27212	110	116	127		37	35	35		75	10384	101	37171	0.09	0.25	64	11	127
27314	728	114	122		36	36	36		72	10304	101	37178	-0.01	0.25	104	0	127
27412	112	117	816		35	34	33		66	10218	106	37177	0.16	0.25	36	39	111
27511	112	112	816		36	34	33		64	10126	116	37174	0.26	0.25	-4	79	71
27610	728	118	127		38	36	34		62	10261	750	44549	0.28	0.25	-12	87	62
27711	728	117	125		38	38	36		60	10406	750	51599	0.24	0.25	 4	71	79
27813	728	112	126		40	37	35		57	10556	819	53316	0.38	0.25	-52	127	23
27911	113	116	816		41	38	35		53	10713	819	53316	0.54	0.25	-116	127	0
28012	728	116	128		41	40	37		46	10888	819	53316	0.36	0.25	-44	119	30
28113	113	114	119		42	40	38		41	11075	750	59737	0.40	0.25	-60	127	14
28212	104	108	120		41	39	39		36	11273	750	65748	0.26	0.25	-4	79	71

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

approach start: orient angle = 16.571, tgt_distCm = 35

At start: meas_dist = 15, corr_dist = 14, err = prev_err = -21

R/C/F dists = 13 15 15 Steerval = 0.290, OrientDeg = 16.57, err/prev_err = -21

Msec RFront RCtr RRear ODeg ORad c_dist Err P_err

11024 16 15 12 16.6 0.3 14 -21 -21

11135 18 17 14 16.6 0.3 16 -19 -21

11249 20 18 16 16.6 0.3 17 -18 -19

11365 22 22 18 16.6 0.3 21 -14 -18

11480 26 24 21 16.6 0.3 23 -12 -14

11591 29 27 23 16.6 0.3 25 -10 -12

11702 31 30 26 16.6 0.3 28 -7 -10

11814 34 32 29 16.6 0.3 30 -5 -7

11927 36 34 31 16.6 0.3 32 -3 -5

12037 41 38 35 16.6 0.3 36 1 -3

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

correct back to parallel (Right side): RF/RC/RR/Orient = 41 38 35 16.57

In SpinTurn(CW, 16.57, 30.00)

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

12519 728 794 816 41 39 39 90 11753 60 11683 0.21 0.25 16 59 91

12619 728 794 816 41 39 38 90 11797 70 11628 0.31 0.25 -24 99 51

12719 728 794 816 41 40 38 87 11752 69 11576 0.39 0.25 -56 127 19

12817 728 794 816 41 40 38 87 11588 63 11529 0.37 0.25 -48 123 27

12917 728 113 816 42 40 38 80 11311 70 11482 0.40 0.25 -60 127 14

13016 728 794 816 42 40 40 75 10891 78 11436 0.20 0.25 20 55 95

13118 728 794 816 42 41 40 71 10302 84 11392 0.23 0.25 8 67 83

13220 728 794 816 43 40 39 66 9523 79 11350 0.36 0.25 -44 119 30

13321 728 794 816 42 40 39 62 9693 84 11309 0.22 0.25 12 63 87

13416 728 794 816 42 40 39 59 9870 93 11273 0.27 0.25 -8 83 67

13519 728 794 816 40 39 40 57 10068 93 11237 -0.02 0.25 108 0 127

13618 728 794 816 40 39 38 54 10261 91 11202 0.14 0.25 44 31 119

13716 728 794 816 39 38 38 51 10455 107 11177 0.09 0.25 64 11 127

13817 728 794 816 40 37 37 45 10663 112 11156 0.20 0.25 20 54 95

13918 728 794 816 39 38 36 41 10879 109 11132 0.29 0.25 -16 91 58

14017 728 794 816 40 39 36 35 11109 115 11114 0.38 0.25 -52 127 23

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Right

In SpinTurn(CCW, 90.00, 45.00)

IRHomingValTotalAvg = 199

15351: glLeftCenterCm = 794 glRightCenterCm = 24

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

15355: TrackRightWallOffset(), glRightFrontCm = 26

In TrackRightWallOffset with Kp/Ki/Kd = 400.00 0.00 0.00

In TrackRightWallOffset with RR/RC/RF = 24 24 26

straight window = 8 cm

R/C/F dists = 24 24 26 Steerval = 0.170, WallAngle = 9.71, Inside

In SpinTurn(CCW, 20.29, 45.00)

R/C/F dists = 26 31 32 Steerval = 0.590, WallOrientDeg = 33.71

approach start: orient angle = 33.714, tgt_distCm = 35

At start: meas_dist = 31, corr_dist = 26, err = prev_err = -9

R/C/F dists = 26 31 32 Steerval = 0.590, OrientDeg = 33.71, err/prev_err = -9

Msec RFront RCtr RRear ODeg ORad c_dist Err P_err

15712 33 31 27 33.7 0.6 25 -10 -9

15826 37 34 29 33.7 0.6 28 -7 -10

15940 38 37 32 33.7 0.6 30 -5 -7

16053 42 41 36 33.7 0.6 34 -1 -5

16165 46 44 39 33.7 0.6 36 1 -1

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

correct back to parallel (Right side): RF/RC/RR/Orient = 46 44 39 33.71

In SpinTurn(CW, 33.71, 30.00)

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

16862 112 110 125 43 43 42 108 11196 64 11178 0.25 0.25 0 75 75

16964 728 110 816 45 43 42 108 11162 65 11179 0.27 0.25 -8 82 67

17060 728 111 127 44 42 40 104 11135 61 11186 0.39 0.25 -56 127 19

17161 110 114 124 42 42 41 102 11105 70 11186 0.19 0.25 24 51 99

17263 728 114 126 44 42 41 100 11077 75 11184 0.30 0.25 -20 94 55

17362 728 115 816 43 42 41 97 11038 71 11186 0.23 0.25 8 67 83

17461 728 116 126 43 41 41 95 10989 77 11185 0.24 0.25 4 71 79

17562 728 113 816 44 41 41 90 10938 86 11183 0.36 0.25 -44 119 30

17664 728 109 124 43 41 41 84 10886 91 11180 0.24 0.25 4 71 79

17763 728 118 816 41 41 41 82 10825 90 11179 0.07 0.25 72 3 127

17860 728 794 127 42 39 39 79 10762 90 11178 0.29 0.25 -16 91 59

17961 728 115 816 41 40 39 73 10708 106 11182 0.23 0.25 8 67 83

18059 728 120 816 42 39 39 69 10656 106 11186 0.21 0.25 16 59 91

18161 728 116 816 41 39 39 64 10608 105 11189 0.15 0.25 40 35 115

18262 728 121 816 40 39 36 60 10550 113 11198 0.35 0.25 -40 115 35

18360 728 121 816 40 38 38 58 10482 124 11217 0.17 0.25 32 42 107

18459 728 121 816 40 38 37 53 10405 117 11228 0.22 0.25 12 63 87

18561 728 121 816 40 38 39 49 10600 750 19677 0.07 0.25 72 3 127

18659 728 794 816 40 39 37 43 10806 819 29443 0.24 0.25 4 71 79

18762 728 120 816 40 39 36 36 11021 819 38752 0.37 0.25 -48 122 27

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Right

In SpinTurn(CCW, 90.00, 45.00)

IRHomingValTotalAvg = 273

19919: glLeftCenterCm = 794 glRightCenterCm = 25

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

19923: TrackRightWallOffset(), glRightFrontCm = 28

In TrackRightWallOffset with Kp/Ki/Kd = 400.00 0.00 0.00

In TrackRightWallOffset with RR/RC/RF = 26 25 28

straight window = 8 cm

R/C/F dists = 26 25 28 Steerval = 0.180, WallAngle = 10.29, Inside

In SpinTurn(CCW, 19.71, 45.00)

R/C/F dists = 25 28 30 Steerval = 0.490, WallOrientDeg = 28.00

approach start: orient angle = 28.000, tgt_distCm = 35

At start: meas_dist = 28, corr_dist = 25, err = prev_err = -10

R/C/F dists = 25 28 30 Steerval = 0.520, OrientDeg = 29.71, err/prev_err = -10

Msec RFront RCtr RRear ODeg ORad c_dist Err P_err

20308 30 28 25 29.7 0.5 24 -11 -10

20420 32 30 26 29.7 0.5 26 -9 -11

20533 33 32 29 29.7 0.5 26 -9 -9

20647 36 33 30 29.7 0.5 28 -7 -9

20761 38 36 32 29.7 0.5 31 -4 -7

20876 41 38 36 29.7 0.5 33 -2 -4

20988 42 42 38 29.7 0.5 36 1 -2

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

correct back to parallel (Right side): RF/RC/RR/Orient = 42 42 38 29.71

In SpinTurn(CW, 29.71, 30.00)

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

21657 728 794 816 45 42 41 97 10859 68 29659 0.40 0.25 -60 127 14

21758 728 794 816 46 42 43 99 10841 69 29645 0.26 0.25 -4 79 71

21856 728 794 816 44 44 45 98 10811 65 29634 0.04 0.25 84 0 127

21956 728 794 816 45 43 44 95 10782 68 29638 0.14 0.25 44 31 119

22055 728 794 816 44 41 42 91 10754 77 29623 0.21 0.25 16 59 91

22154 728 794 816 42 40 40 85 10722 73 29601 0.21 0.25 16 59 91

22258 728 123 816 42 40 41 82 10686 77 29588 0.14 0.25 44 31 119

22356 728 794 816 42 39 39 77 10648 87 29574 0.24 0.25 4 71 79

22459 728 794 816 41 39 40 74 10604 90 29566 0.16 0.25 36 39 111

22555 728 794 816 40 38 38 70 10564 89 29551 0.19 0.25 24 51 99

22656 728 794 816 39 37 37 68 10513 93 29539 0.08 0.25 68 7 127

22758 728 794 816 40 38 37 64 10459 107 29526 0.22 0.25 12 63 87

22855 728 123 816 42 39 37 60 10395 111 29512 0.49 0.25 -96 127 0

22957 728 121 816 43 42 39 56 10314 106 29497 0.43 0.25 -72 127 3

23057 728 794 816 42 42 39 52 10491 112 29495 0.46 0.25 -84 127 0

23156 728 794 816 43 41 40 49 10673 121 29492 0.32 0.25 -28 103 46

23256 728 794 816 44 41 41 43 10870 126 29488 0.34 0.25 -36 111 39

23355 728 794 816 43 41 40 40 11071 750 37177 0.39 0.25 -56 127 18

In HandleAnomalousConditions with WALL_OFFSET_DIST_AHEAD error code detected

WALL_OFFSET_DIST_AHEAD case detected with tracking case Right

In SpinTurn(CCW, 90.00, 45.00)

IRHomingValTotalAvg = 193

24722: glLeftCenterCm = 794 glRightCenterCm = 30

glLeftCenterCm > glRightCenterCm --> Calling TrackRightWallOffset()

24726: TrackRightWallOffset(), glRightFrontCm = 31

In TrackRightWallOffset with Kp/Ki/Kd = 400.00 0.00 0.00

In TrackRightWallOffset with RR/RC/RF = 30 30 31

straight window = 8 cm

R/C/F dists = 30 30 31 Steerval = 0.140, WallAngle = 8.00, Inside

In SpinTurn(CCW, 22.00, 45.00)

R/C/F dists = 27 29 30 Steerval = 0.300, WallOrientDeg = 17.14

approach start: orient angle = 17.143, tgt_distCm = 35

At start: meas_dist = 29, corr_dist = 28, err = prev_err = -7

R/C/F dists = 27 29 30 Steerval = 0.300, OrientDeg = 17.14, err/prev_err = -7

Msec RFront RCtr RRear ODeg ORad c_dist Err P_err

25109 31 29 27 17.1 0.3 27 -8 -7

25220 32 30 29 17.1 0.3 28 -7 -8

25331 33 31 28 17.1 0.3 29 -6 -7

25446 34 31 30 17.1 0.3 29 -6 -6

25559 35 33 31 17.1 0.3 31 -4 -6

25674 35 34 32 17.1 0.3 32 -3 -4

25787 37 35 32 17.1 0.3 33 -2 -3

25899 39 37 33 17.1 0.3 35 0 -2

At end of offset capture - prev_res*res = 0

correct back to parallel (Right side): RF/RC/RR/Orient = 39 37 33 17.14

In SpinTurn(CW, 17.14, 30.00)

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

26414 111 113 816 41 39 39 99 10861 74 37154 0.21 0.25 16 59 91

26512 113 117 123 41 40 38 100 10811 70 37167 0.27 0.25 -8 83 67

26612 109 113 118 42 38 39 96 10759 70 37170 0.29 0.25 -16 91 58

26714 111 115 124 41 39 38 94 10698 80 37163 0.34 0.25 -36 111 39

26811 109 117 119 40 38 37 88 10645 88 37159 0.27 0.25 -8 83 67

26912 111 113 124 39 38 37 84 10588 84 37172 0.24 0.25 4 71 79

27012 113 117 816 40 38 36 80 10527 87 37178 0.23 0.25 8 67 82

27111 728 120 124 38 37 36 77 10460 98 37165 0.12 0.25 52 23 127

27212 110 116 127 37 35 35 75 10384 101 37171 0.09 0.25 64 11 127

27314 728 114 122 36 36 36 72 10304 101 37178 -0.01 0.25 104 0 127

27412 112 117 816 35 34 33 66 10218 106 37177 0.16 0.25 36 39 111

27511 112 112 816 36 34 33 64 10126 116 37174 0.26 0.25 -4 79 71

27610 728 118 127 38 36 34 62 10261 750 44549 0.28 0.25 -12 87 62

27711 728 117 125 38 38 36 60 10406 750 51599 0.24 0.25 4 71 79

27813 728 112 126 40 37 35 57 10556 819 53316 0.38 0.25 -52 127 23

27911 113 116 816 41 38 35 53 10713 819 53316 0.54 0.25 -116 127 0

28012 728 116 128 41 40 37 46 10888 819 53316 0.36 0.25 -44 119 30

28113 113 114 119 42 40 38 41 11075 750 59737 0.40 0.25 -60 127 14

28212 104 108 120 41 39 39 36 11273 750 65748 0.26 0.25 -4 79 71

All four wall sections – note straight lines are due to gaps between wall sections

So now I think I finally (I’ve only been working on this for the last three years!) have a wall tracking algorithm that actually makes sense and does what it is supposed to do – track the wall at a constant offset – yay!!

After getting the right side working, I ported everything back to the left side, with some differences; for the left side approach phase, I wound up using a fudge factor of 10cm vice 5cm to get the approach to stop near the desired offset. Also, the base steering value setpoint was -0.35 instead of +0.25, and the input (WallTrackSteerVal) wound up being

  WallTrackSteerVal = glLeftSteeringVal + ((float)glLeftCenterCm - (float)offsetCm) / 25.f;

With these settings, Wall-E3 seemed pretty comfortable navigating around my office ‘sandbox’, as shown in the following short video:

Stay tuned,

Frank

Wall-E3 Right Wall Following Trial

Leave a reply

Posted 23 March 2022,

Earlier this month I was able to demonstrate a multi-lap left-side wall tracking run by Wall-E3 in my office ‘sandbox’. This post describes my efforts to extend this capability to right-side wall tracking.

Since I already had the left-side wall tracking algorithm “in the can”, I thought it would be a piece of cake to extend this capability to right-side tracking. Little did I know that this would turn into yet another adventure in Wonderland – but at least when I finally made it back out of the rabbit-hole, the result was a distinct improvement over the left-side algorithm I started with. Here’s the left-side code:

void TrackLeftWallOffset(float kp, float ki, float kd, float offsetCm)
{
  //Purpose: Track left wall at specified offset
  // Inputs:
  //  kp,ki,kd = float objects denoting PID parameters for PIDCalcs()
  //  offsetCm = float denoting desired wall offset, in Cm
  // Plan:
  //  Step1: If robot is already close to desired offset, just turn parallel and go
  //  Step1: Otherwise, get current robot orientation and offset wrt wall
  //  Step2: Move to offset distance and parallel to wall
  //  Step3: Track wall at desired offset 
  //Notes:
  //	06/21/21 modified to do a turn, then straight, then track 0 steer val

  mSecSinceLastWallTrackUpdate = 0;
  float lastError = 0;
  float lastInput = 0;
  float lastIval = 0;
  float lastDerror = 0;
  float spinRateDPS = 30;

  AnomalyCode errcode = NO_ANOMALIES;//03/06/22 mvd here to make global to function

  GetRequestedVL53l0xValues(VL53L0X_LEFT); //update glLeftSteeringVal

  myTeePrint.printf("\nIn TrackLeftWallOffset with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);
  myTeePrint.printf("\nIn TrackLeftWallOffset with LR/LC/LF = %d\t%d\t%d\n", glLeftRearCm, glLeftCenterCm, glLeftFrontCm);

//Step1: If robot is already close to desired offset, just turn parallel and go
  if (abs(WALL_OFFSET_TGTDIST_CM - glLeftCenterCm) <= (uint16_t)((float)WALL_OFFSET_TGTDIST_CM/10.f))
  {
    myTeePrint.printf("At %d with desired offset = %d: close enough  - just parallel and go\n", 
      glLeftCenterCm, WALL_OFFSET_TGTDIST_CM);

    RotateToParallelOrientation(TRACKING_LEFT);
  }
  else //OK, have to move to offset distance before rotating back to parallel
  {
    //Step1: Get current robot orientation and offset wrt wall
    float cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(glLeftCenterCm);//positive inside, negative outside desired offset
    //myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, CutAngle = %2.2f\n",
    //  glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, cutAngleDeg);

    //07/05/21 implement min cut angle
    if (cutAngleDeg < 0 && abs(cutAngleDeg) < WALL_OFFSET_APPR_ANGLE_MINDEG)
    {
      //myTeePrint.printf("(cutAngleDeg < 0 && abs(cutAngleDeg) < %d -- setting to -%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);
      cutAngleDeg = -WALL_OFFSET_APPR_ANGLE_MINDEG;
    }
    else if (cutAngleDeg > 0 && cutAngleDeg < WALL_OFFSET_APPR_ANGLE_MINDEG)
    {
      //myTeePrint.printf("(cutAngleDeg > 0 && abs(cutAngleDeg) < %d -- setting to +%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);
      cutAngleDeg = WALL_OFFSET_APPR_ANGLE_MINDEG;
    }

    //Step2: get the steering angle from the steering value, for comparison to the computed cut angle
    //positive value means robot is oriented away from wall
    float WallOrientDeg = GetWallOrientDeg(glLeftSteeringVal);

    //myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f\n",
    //  glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, WallOrientDeg, cutAngleDeg);

    //Step3: now decide which way to turn.
    int16_t fudgeFactorCm = 0; //07/05/21 added so can change for inside vs outside starting condx
    if (cutAngleDeg > 0) //robot inside desired offset distance
    {
      if (WallOrientDeg > cutAngleDeg)  // --> WallOrientDeg also > 0, have to turn CCW
      {
        //myTeePrint.printf("CutAngDeg > 0, WallOrientDeg > cutAngleDeg - Turn CCW to cut angle\n");
        SpinTurn(true, WallOrientDeg - cutAngleDeg, spinRateDPS); //
      }
      else //(WallOrientDeg <= cutAngleDeg)// --> WallOrientDeg could be < 0
      {
        //myTeePrint.printf("CutAngDeg > 0, WallOrientDeg < cutAngleDeg\n");
        SpinTurn(false, cutAngleDeg - WallOrientDeg, spinRateDPS); //turn diff between WallOrientDeg & cutAngleDeg
      }

      fudgeFactorCm = -10; //07/05/21 don't need fudge factor for inside start condx
    }
    else // cutAngleDeg < 0 --> robot outside desired offset distance
    {
      if (WallOrientDeg > cutAngleDeg)  // --> WallOrientDeg may also be > 0
      {
        //robot turned too far toward wall
        myTeePrint.printf("CutAngDeg < 0, WallOrientDeg > cutAngleDeg\n");
        SpinTurn(true, WallOrientDeg - cutAngleDeg, spinRateDPS); //
      }
      else// (WallOrientDeg < cutAngleDeg)// --> WallOrientDeg must also be < 0
      {
        //robot turned to far away from wall
        myTeePrint.printf("CutAngDeg < 0, WallOrientDeg <= 0\n");
        SpinTurn(false, cutAngleDeg - WallOrientDeg, spinRateDPS); //null out WallOrientDeg & then turn addnl cutAngleDeg
      }

      fudgeFactorCm = 10; //07/05/21 need fudge factor for outside start condx
    }

    //myTeePrint.printf("fudgeFactorCm = %d\n", fudgeFactorCm);
    //delay(1000);

    //Step4: Figure out how far to travel to get to desired offset
    //adjust so offset capture occurs at correct perpendicular offset
    float adjfactor = cos(PI * cutAngleDeg / 180.f);
    float numerator = (float)WALL_OFFSET_TGTDIST_CM;
    float adjOffsetCm = (numerator / adjfactor);
    adjOffsetCm += fudgeFactorCm; //fudge factor for distance measurements lagging behind robot's travel. 
    //myTeePrint.printf("\nat approach start: cut angle = %2.3f, adjfactor = %2.3f, num = %2.2f, adjOffsetCm = %2.2f\n",
      //cutAngleDeg, adjfactor, numerator, adjOffsetCm);

    GetRequestedVL53l0xValues(VL53L0X_ALL); //added 09/05/21
    int16_t err = glLeftFrontCm - (uint16_t)adjOffsetCm; //neg for inside going out, pos for outside going in
    int16_t prev_err = err;
    //myTeePrint.printf("At start - err = prev_err = %ld\n", err, prev_err);

    WallOrientDeg = GetWallOrientDeg(glLeftSteeringVal);
    myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f, err/prev_err = %li\n",
      glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, WallOrientDeg, cutAngleDeg, err);


    //myTeePrint.printf("Msec\tLFront\tLCtr\tLRear\tFront\tRear\tErr\tP_err\n");

    //Step5: Move to desired offset
    //02/25/22 added check for error conditions (stuck, obstacle, dead battery, etc)
    AnomalyCode errcode = NO_ANOMALIES;
    while (((cutAngleDeg > 0 && err < 0) || (cutAngleDeg <= 0 && err >= 0)) && prev_err * err > 0 && errcode == NO_ANOMALIES) //sign change makes this < 0
    {
      prev_err = err;

      //MoveAhead(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);
      MoveAhead(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
      delay(100);
      glFrontdistval = GetFrontDistCm();
      GetRequestedVL53l0xValues(VL53L0X_ALL);
      err = glLeftFrontCm - adjOffsetCm; //06/29/21 now adj for slant dist vs perp dist
      //myTeePrint.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%ld\t%ld\n",
      //  millis(),
      //  glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glFrontdistval, glRearCm, err, prev_err);

      CheckForUserInput();
      errcode = CheckForErrorCondx();
    }

    //Step6: now turn back the same (unadjusted) amount
    //myTeePrint.printf("At end of offset capture - prev_res*res = %ld\n", prev_err * err);
    //myTeePrint.printf("correct back to parallel (left side)\n");
    SpinTurn(!(cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg
  }
  WallTrackSetPoint = 0; //moved here 6/22/21

  myTeePrint.printf("\nTrackLeftWallOffset: Start tracking offset of %2.2fcm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", offsetCm, kp, ki, kd);
  //myTeePrint.printf("Msec\tFdir\tCdir\tRdir\tRearD\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");
  myTeePrint.printf("Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\t\tR\tRvar\t\tSteer\tSet\tOutput\n");

  //Step7: Track the wall using PID algorithm
  mSecSinceLastWallTrackUpdate = 0; //added 09/04/21
  while (errcode == NO_ANOMALIES)
  {
    CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

    //09/04/21 back to elapsedMillis timing
    if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)
    {
      digitalToggle(DURATION_MEASUREMENT_PIN2); //03/08/22 toggle duration measurement pulse

      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 = CheckForErrorCondx();

      //have to weight value by both angle and wall offset
      WallTrackSteerVal = glLeftSteeringVal + (glLeftCenterCm - WALL_OFFSET_TGTDIST_CM) / 1000.f;

      //update motor speeds, skipping bad values
      if (!isnan(WallTrackSteerVal))
      {
        WallTrackOutput = PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror,
          kp, ki, kd);

        int glLeftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;
        int glRightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

        glRightspeednum = (glRightspeednum <= MOTOR_SPEED_HALF) ? glRightspeednum : MOTOR_SPEED_HALF;
        glRightspeednum = (glRightspeednum > 0) ? glRightspeednum : 0;
        glLeftspeednum = (glLeftspeednum <= MOTOR_SPEED_HALF) ? glLeftspeednum : MOTOR_SPEED_HALF;
        glLeftspeednum = (glLeftspeednum > 0) ? glLeftspeednum : 0;

        MoveAhead(glLeftspeednum, glRightspeednum);

        //myTeePrint.printf("Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\tR\tRvar\t\tSteer\tSet\tOutput\n");
        myTeePrint.printf("%lu\t%d\t%d\t%d\t\t%d\t%d\t%d\t\t%d\t%d\t%2.2f\t%2.2f\t\t%2.2f\t%2.2f\t%2.2f\n",
          millis(),
          glLeftFrontCm, glLeftCenterCm, glLeftRearCm,
          glRightFrontCm, glRightCenterCm, glRightRearCm, 
          glFrontdistval, glFrontvar, glRearCm, glRearvar,
          WallTrackSteerVal, WallTrackSetPoint, WallTrackOutput);
      }
      digitalToggle(DURATION_MEASUREMENT_PIN2); //03/08/22 toggle duration measurement pulse

    }
  }

  //Step8: Handle any anomalous conditions
  HandleAnomalousConditions(errcode, TRACKING_LEFT);
}

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

void TrackLeftWallOffset(float kp, float ki, float kd, float offsetCm)

{

//Purpose: Track left wall at specified offset

// Inputs:

// kp,ki,kd = float objects denoting PID parameters for PIDCalcs()

// offsetCm = float denoting desired wall offset, in Cm

// Plan:

// Step1: If robot is already close to desired offset, just turn parallel and go

// Step1: Otherwise, get current robot orientation and offset wrt wall

// Step2: Move to offset distance and parallel to wall

// Step3: Track wall at desired offset

//Notes:

// 06/21/21 modified to do a turn, then straight, then track 0 steer val

mSecSinceLastWallTrackUpdate = 0;

float lastError = 0;

float lastInput = 0;

float lastIval = 0;

float lastDerror = 0;

float spinRateDPS = 30;

AnomalyCode errcode = NO_ANOMALIES;//03/06/22 mvd here to make global to function

GetRequestedVL53l0xValues(VL53L0X_LEFT); //update glLeftSteeringVal

myTeePrint.printf("\nIn TrackLeftWallOffset with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);

myTeePrint.printf("\nIn TrackLeftWallOffset with LR/LC/LF = %d\t%d\t%d\n", glLeftRearCm, glLeftCenterCm, glLeftFrontCm);

//Step1: If robot is already close to desired offset, just turn parallel and go

if (abs(WALL_OFFSET_TGTDIST_CM - glLeftCenterCm) <= (uint16_t)((float)WALL_OFFSET_TGTDIST_CM/10.f))

{

myTeePrint.printf("At %d with desired offset = %d: close enough - just parallel and go\n",

glLeftCenterCm, WALL_OFFSET_TGTDIST_CM);

RotateToParallelOrientation(TRACKING_LEFT);

}

else //OK, have to move to offset distance before rotating back to parallel

{

//Step1: Get current robot orientation and offset wrt wall

float cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(glLeftCenterCm);//positive inside, negative outside desired offset

//myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, CutAngle = %2.2f\n",

// glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, cutAngleDeg);

//07/05/21 implement min cut angle

if (cutAngleDeg < 0 && abs(cutAngleDeg) < WALL_OFFSET_APPR_ANGLE_MINDEG)

{

//myTeePrint.printf("(cutAngleDeg < 0 && abs(cutAngleDeg) < %d -- setting to -%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);

cutAngleDeg = -WALL_OFFSET_APPR_ANGLE_MINDEG;

}

else if (cutAngleDeg > 0 && cutAngleDeg < WALL_OFFSET_APPR_ANGLE_MINDEG)

{

//myTeePrint.printf("(cutAngleDeg > 0 && abs(cutAngleDeg) < %d -- setting to +%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);

cutAngleDeg = WALL_OFFSET_APPR_ANGLE_MINDEG;

}

//Step2: get the steering angle from the steering value, for comparison to the computed cut angle

//positive value means robot is oriented away from wall

float WallOrientDeg = GetWallOrientDeg(glLeftSteeringVal);

//myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f\n",

// glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, WallOrientDeg, cutAngleDeg);

//Step3: now decide which way to turn.

int16_t fudgeFactorCm = 0; //07/05/21 added so can change for inside vs outside starting condx

if (cutAngleDeg > 0) //robot inside desired offset distance

{

if (WallOrientDeg > cutAngleDeg) // --> WallOrientDeg also > 0, have to turn CCW

{

//myTeePrint.printf("CutAngDeg > 0, WallOrientDeg > cutAngleDeg - Turn CCW to cut angle\n");

SpinTurn(true, WallOrientDeg - cutAngleDeg, spinRateDPS); //

}

else //(WallOrientDeg <= cutAngleDeg)// --> WallOrientDeg could be < 0

{

//myTeePrint.printf("CutAngDeg > 0, WallOrientDeg < cutAngleDeg\n");

SpinTurn(false, cutAngleDeg - WallOrientDeg, spinRateDPS); //turn diff between WallOrientDeg & cutAngleDeg

}

fudgeFactorCm = -10; //07/05/21 don't need fudge factor for inside start condx

}

else // cutAngleDeg < 0 --> robot outside desired offset distance

{

if (WallOrientDeg > cutAngleDeg) // --> WallOrientDeg may also be > 0

{

//robot turned too far toward wall

myTeePrint.printf("CutAngDeg < 0, WallOrientDeg > cutAngleDeg\n");

SpinTurn(true, WallOrientDeg - cutAngleDeg, spinRateDPS); //

}

else// (WallOrientDeg < cutAngleDeg)// --> WallOrientDeg must also be < 0

{

//robot turned to far away from wall

myTeePrint.printf("CutAngDeg < 0, WallOrientDeg <= 0\n");

SpinTurn(false, cutAngleDeg - WallOrientDeg, spinRateDPS); //null out WallOrientDeg & then turn addnl cutAngleDeg

}

fudgeFactorCm = 10; //07/05/21 need fudge factor for outside start condx

}

//myTeePrint.printf("fudgeFactorCm = %d\n", fudgeFactorCm);

//delay(1000);

//Step4: Figure out how far to travel to get to desired offset

//adjust so offset capture occurs at correct perpendicular offset

float adjfactor = cos(PI * cutAngleDeg / 180.f);

float numerator = (float)WALL_OFFSET_TGTDIST_CM;

float adjOffsetCm = (numerator / adjfactor);

adjOffsetCm += fudgeFactorCm; //fudge factor for distance measurements lagging behind robot's travel.

//myTeePrint.printf("\nat approach start: cut angle = %2.3f, adjfactor = %2.3f, num = %2.2f, adjOffsetCm = %2.2f\n",

//cutAngleDeg, adjfactor, numerator, adjOffsetCm);

GetRequestedVL53l0xValues(VL53L0X_ALL); //added 09/05/21

int16_t err = glLeftFrontCm - (uint16_t)adjOffsetCm; //neg for inside going out, pos for outside going in

int16_t prev_err = err;

//myTeePrint.printf("At start - err = prev_err = %ld\n", err, prev_err);

WallOrientDeg = GetWallOrientDeg(glLeftSteeringVal);

myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f, err/prev_err = %li\n",

glLeftRearCm, glLeftCenterCm, glLeftFrontCm, glLeftSteeringVal, WallOrientDeg, cutAngleDeg, err);

//myTeePrint.printf("Msec\tLFront\tLCtr\tLRear\tFront\tRear\tErr\tP_err\n");

//Step5: Move to desired offset

//02/25/22 added check for error conditions (stuck, obstacle, dead battery, etc)

AnomalyCode errcode = NO_ANOMALIES;

while (((cutAngleDeg > 0 && err < 0) || (cutAngleDeg <= 0 && err >= 0)) && prev_err * err > 0 && errcode == NO_ANOMALIES) //sign change makes this < 0

{

prev_err = err;

//MoveAhead(MOTOR_SPEED_FULL, MOTOR_SPEED_FULL);

MoveAhead(MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

delay(100);

glFrontdistval = GetFrontDistCm();

GetRequestedVL53l0xValues(VL53L0X_ALL);

err = glLeftFrontCm - adjOffsetCm; //06/29/21 now adj for slant dist vs perp dist

//myTeePrint.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%ld\t%ld\n",

// millis(),

// glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glFrontdistval, glRearCm, err, prev_err);

CheckForUserInput();

errcode = CheckForErrorCondx();

}

//Step6: now turn back the same (unadjusted) amount

//myTeePrint.printf("At end of offset capture - prev_res*res = %ld\n", prev_err * err);

//myTeePrint.printf("correct back to parallel (left side)\n");

SpinTurn(!(cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg

}

WallTrackSetPoint = 0; //moved here 6/22/21

myTeePrint.printf("\nTrackLeftWallOffset: Start tracking offset of %2.2fcm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", offsetCm, kp, ki, kd);

//myTeePrint.printf("Msec\tFdir\tCdir\tRdir\tRearD\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

myTeePrint.printf("Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\t\tR\tRvar\t\tSteer\tSet\tOutput\n");

//Step7: Track the wall using PID algorithm

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

while (errcode == NO_ANOMALIES)

{

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

//09/04/21 back to elapsedMillis timing

if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)

{

digitalToggle(DURATION_MEASUREMENT_PIN2); //03/08/22 toggle duration measurement pulse

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 = CheckForErrorCondx();

//have to weight value by both angle and wall offset

WallTrackSteerVal = glLeftSteeringVal + (glLeftCenterCm - WALL_OFFSET_TGTDIST_CM) / 1000.f;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

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

kp, ki, kd);

int glLeftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

int glRightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

glRightspeednum = (glRightspeednum <= MOTOR_SPEED_HALF) ? glRightspeednum : MOTOR_SPEED_HALF;

glRightspeednum = (glRightspeednum > 0) ? glRightspeednum : 0;

glLeftspeednum = (glLeftspeednum <= MOTOR_SPEED_HALF) ? glLeftspeednum : MOTOR_SPEED_HALF;

glLeftspeednum = (glLeftspeednum > 0) ? glLeftspeednum : 0;

MoveAhead(glLeftspeednum, glRightspeednum);

//myTeePrint.printf("Msec\tLF\tLC\tLR\t\tRF\tRC\tRR\t\tF\tFvar\tR\tRvar\t\tSteer\tSet\tOutput\n");

myTeePrint.printf("%lu\t%d\t%d\t%d\t\t%d\t%d\t%d\t\t%d\t%d\t%2.2f\t%2.2f\t\t%2.2f\t%2.2f\t%2.2f\n",

millis(),

glLeftFrontCm, glLeftCenterCm, glLeftRearCm,

glRightFrontCm, glRightCenterCm, glRightRearCm,

glFrontdistval, glFrontvar, glRearCm, glRearvar,

WallTrackSteerVal, WallTrackSetPoint, WallTrackOutput);

}

digitalToggle(DURATION_MEASUREMENT_PIN2); //03/08/22 toggle duration measurement pulse

}

//Step8: Handle any anomalous conditions

HandleAnomalousConditions(errcode, TRACKING_LEFT);

}

The above code works, in the sense that it allows Wall-E3 to successfully track the left-side wall of my ‘sandbox’. However, as I worked on porting the left-side tracking code to the right side, I kept thinking – this is awful code – surely there is a better way?

After letting this problem percolate for few days, I decided to see if I could approach the problem a little more logically. I realized there were two major conditions associated with the problem – namely is the robot’s initial position inside or outside the desired offset distance K? In addition, the robot can start out parallel to the wall, or pointed toward or away from the wall. Ignoring the ‘started out parallel’ degenerate case, this reminded me of a 3-parameter Karnaugh map configuration, so I started sketching it out in my notebook, and then later in a Word document, as shown below:

As shown above, I broke the 3-parameter into two 2-parameter Karnaugh maps, and the output is denoted by α_T. After a few minutes it became obvious that the formula for α_T is pretty simple – its either α_R – α_A1 or α_R – α_A2depending on whether the robot starts out outside or inside the desired offset distance. In code, this boils down to one line, as shown at the bottom of the Karnaugh map above, using the C++ ‘?’ trinary operator, and choosing CW vs CCW is easy too, as a negative result implies CCW, and a positive one implies CW. The actual code block is shown below:

    //Step3: now decide which way to turn.
    float turndeg = 0; 
    float appAngle = 0;
    if (bInsideOffset) //approach angle = +30
    {
      appAngle = 30;
      turndeg = WallOrientDeg - appAngle;
      fudgeFactorCm = -10;
    }
    else
    {
      appAngle = -30;
      turndeg = WallOrientDeg - appAngle;
      fudgeFactorCm = 10; 
    }

    SpinTurn((turndeg < 0), abs(turndeg));

//Step3: now decide which way to turn.

float turndeg = 0;

float appAngle = 0;

if (bInsideOffset) //approach angle = +30

{

appAngle = 30;

turndeg = WallOrientDeg - appAngle;

fudgeFactorCm = -10;

}

else

{

appAngle = -30;

turndeg = WallOrientDeg - appAngle;

fudgeFactorCm = 10;

}

SpinTurn((turndeg < 0), abs(turndeg));

Here’s a short video of Wall-E3 navigating the office ‘sandbox’ while tracking the right-side wall.

So, it looks like Wall-E3 now has tracking ability for both left-side and right-side walls, although I still have to clean things up and port the simpler right-side code into TrackLeftWallOffset().

25 March 2022 Update:

Well, that was easy! I just got through porting the new right-side wall tracking algorithm over to TrackLeftWallOffset(), and right out of the box was able to demonstrate successful left-wall tracking in my office ‘sandbox’.

At this point I believe I’m going to consider the ‘WallE3_WallTrack_V3’ project ‘finished’ (in the sense that most, if not all, my wall tracking goals have been met with this version), and move on to V4, thereby limiting the possible damage from my next inevitable descent through the rabbit hole into wonderland.

Stay tuned,

Frank

Wall-E3 Multi-Lap Wall-Following Trial

Leave a reply

Posted 06 March 2022

I’ve pretty much finished with transitioning my autonomous wall-following robot from the old Arduino MEGA 2560 main controller to the new Teensy 3.5 main controller, and now I am moving on to actually getting Wall-E3 to do the job it was created for – namely, to follow walls autonomously. This post describes the result of a multi-lap run through my little testing ‘sandbox’, consisting of a set of barriers forming a 2m X 2m rectangular ‘room’. Here’s a short video showing the run:

I captured the telemetry output from the run and went back through it pretty much line-by-line, trying to make sure I understood all the actions displayed in the video – particularly the little off-piste excursion at about 1:00 on the second lap, just before the end of the run (the run was cancelled when a portion of the wall fell over on Wall-E3).

First Leg: 36.0- 38.9 Sec (5-8 sec in video)

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	Fdir	Cdir	Rdir	RearD	Steer	Set	error	Ival	Kp*err	Ki*Ival	Kd*Din	Output	Lspd	Rspd
36017 	46	46	46	17	-0.01	0.00	0.01	0.00 	4.20	0.00	0.00 	4.20	79	70
36114 	45	45	50	18	-0.44	0.00	0.44	0.00 	130.50	0.00	0.00 	130.50	127	0
36215 	45	46	46	20	-0.12	0.00	0.12	0.00 	37.20	0.00	-0.00 	37.20	112	37
36313 	45	45	45	24	-0.00	0.00	0.00	0.00 	1.50	0.00	-0.00 	1.50	76	73
36415 	46	46	46	28	0.05	0.00	-0.05	0.00 	-13.80	0.00	-0.00 	-13.80	61	88
36514 	47	47	45	33	0.16	0.00	-0.16	0.00 	-47.10	0.00	-0.00 	-47.10	27	122
36614 	49	48	48	38	0.16	0.00	-0.16	0.00 	-47.40	0.00	-0.00 	-47.40	27	122
36717 	48	48	48	44	-0.03	0.00	0.03	0.00 	9.60	0.00	0.00 	9.60	84	65
36817 	49	48	48	47	0.08	0.00	-0.08	0.00 	-23.40	0.00	-0.00 	-23.40	51	98
36914 	47	47	48	52	-0.08	0.00	0.08	0.00 	24.90	0.00	0.00 	24.90	99	50
37013 	47	48	48	58	-0.18	0.00	0.18	0.00 	54.60	0.00	0.00 	54.60	127	20
37113 	45	47	48	65	-0.31	0.00	0.31	0.00 	93.90	0.00	0.00 	93.90	127	0
37215 	45	46	45	70	0.05	0.00	-0.05	0.00 	-13.80	0.00	-0.00 	-13.80	61	88
37313 	45	46	46	77	-0.10	0.00	0.10	0.00 	31.20	0.00	0.00 	31.20	106	43
37415 	45	46	46	79	0.01	0.00	-0.01	0.00 	-1.80	0.00	-0.00 	-1.80	73	76
37515 	48	47	46	87	0.22	0.00	-0.22	0.00 	-65.10	0.00	-0.00 	-65.10	9	127
37614 	46	48	47	92	-0.07	0.00	0.07	0.00 	21.60	0.00	0.00 	21.60	96	53
37713 	48	48	49	99	-0.06	0.00	0.06	0.00 	18.60	0.00	-0.00 	18.60	93	56
37814 	49	49	47	103	0.14	0.00	-0.14	0.00 	-41.70	0.00	-0.00 	-41.70	33	116
37912 	48	48	47	105	0.09	0.00	-0.09	0.00 	-26.40	0.00	0.00 	-26.40	48	101
38013 	49	48	48	114	0.11	0.00	-0.11	0.00 	-32.40	0.00	-0.00 	-32.40	42	107
38114 	47	50	50	118	-0.25	0.00	0.25	0.00 	75.00	0.00	0.00 	75.00	127	0
38212 	48	48	50	119	-0.14	0.00	0.14	0.00 	42.60	0.00	-0.00 	42.60	117	32
38311 	48	48	48	819	0.03	0.00	-0.03	0.00 	-8.40	0.00	-0.00 	-8.40	66	83
38411 	49	48	48	819	0.12	0.00	-0.12	0.00 	-35.40	0.00	-0.00 	-35.40	39	110
38512 	49	49	47	819	0.21	0.00	-0.21	0.00 	-62.70	0.00	-0.00 	-62.70	12	127
38612 	48	48	49	819	-0.09	0.00	0.09	0.00 	27.60	0.00	0.00 	27.60	102	47
38710 	48	49	48	819	-0.07	0.00	0.07	0.00 	21.30	0.00	-0.00 	21.30	96	53
38812 	48	49	47	819	0.07	0.00	-0.07	0.00 	-20.70	0.00	-0.00 	-20.70	54	95
CheckForErrorCondx returning error code OBSTACLE_AHEAD
38914 	50	50	47	819	0.14	0.00	-0.14	0.00 	-42.00	0.00	-0.00 	-42.00	33	117

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

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

36017 46 46 46 17 -0.01 0.00 0.01 0.00 4.20 0.00 0.00 4.20 79 70

36114 45 45 50 18 -0.44 0.00 0.44 0.00 130.50 0.00 0.00 130.50 127 0

36215 45 46 46 20 -0.12 0.00 0.12 0.00 37.20 0.00 -0.00 37.20 112 37

36313 45 45 45 24 -0.00 0.00 0.00 0.00 1.50 0.00 -0.00 1.50 76 73

36415 46 46 46 28 0.05 0.00 -0.05 0.00 -13.80 0.00 -0.00 -13.80 61 88

36514 47 47 45 33 0.16 0.00 -0.16 0.00 -47.10 0.00 -0.00 -47.10 27 122

36614 49 48 48 38 0.16 0.00 -0.16 0.00 -47.40 0.00 -0.00 -47.40 27 122

36717 48 48 48 44 -0.03 0.00 0.03 0.00 9.60 0.00 0.00 9.60 84 65

36817 49 48 48 47 0.08 0.00 -0.08 0.00 -23.40 0.00 -0.00 -23.40 51 98

36914 47 47 48 52 -0.08 0.00 0.08 0.00 24.90 0.00 0.00 24.90 99 50

37013 47 48 48 58 -0.18 0.00 0.18 0.00 54.60 0.00 0.00 54.60 127 20

37113 45 47 48 65 -0.31 0.00 0.31 0.00 93.90 0.00 0.00 93.90 127 0

37215 45 46 45 70 0.05 0.00 -0.05 0.00 -13.80 0.00 -0.00 -13.80 61 88

37313 45 46 46 77 -0.10 0.00 0.10 0.00 31.20 0.00 0.00 31.20 106 43

37415 45 46 46 79 0.01 0.00 -0.01 0.00 -1.80 0.00 -0.00 -1.80 73 76

37515 48 47 46 87 0.22 0.00 -0.22 0.00 -65.10 0.00 -0.00 -65.10 9 127

37614 46 48 47 92 -0.07 0.00 0.07 0.00 21.60 0.00 0.00 21.60 96 53

37713 48 48 49 99 -0.06 0.00 0.06 0.00 18.60 0.00 -0.00 18.60 93 56

37814 49 49 47 103 0.14 0.00 -0.14 0.00 -41.70 0.00 -0.00 -41.70 33 116

37912 48 48 47 105 0.09 0.00 -0.09 0.00 -26.40 0.00 0.00 -26.40 48 101

38013 49 48 48 114 0.11 0.00 -0.11 0.00 -32.40 0.00 -0.00 -32.40 42 107

38114 47 50 50 118 -0.25 0.00 0.25 0.00 75.00 0.00 0.00 75.00 127 0

38212 48 48 50 119 -0.14 0.00 0.14 0.00 42.60 0.00 -0.00 42.60 117 32

38311 48 48 48 819 0.03 0.00 -0.03 0.00 -8.40 0.00 -0.00 -8.40 66 83

38411 49 48 48 819 0.12 0.00 -0.12 0.00 -35.40 0.00 -0.00 -35.40 39 110

38512 49 49 47 819 0.21 0.00 -0.21 0.00 -62.70 0.00 -0.00 -62.70 12 127

38612 48 48 49 819 -0.09 0.00 0.09 0.00 27.60 0.00 0.00 27.60 102 47

38710 48 49 48 819 -0.07 0.00 0.07 0.00 21.30 0.00 -0.00 21.30 96 53

38812 48 49 47 819 0.07 0.00 -0.07 0.00 -20.70 0.00 -0.00 -20.70 54 95

CheckForErrorCondx returning error code OBSTACLE_AHEAD

38914 50 50 47 819 0.14 0.00 -0.14 0.00 -42.00 0.00 -0.00 -42.00 33 117

In the first leg, the robot starts off parallel to the wall on the left, but too close (16cm instead of 40cm). It makes a 27.1º CW turn away from the wall to achieve a cut angle of 30º, moves forward, and then turns back 30º CCW to end up parallel to the wall, and about 36cm away – almost perfectly spaced 40cm off the wall.

Next it tracks down the wall from 14.992 sec to 17.390sec , trying (and generally succeeding) to maintain the 40cm standoff distance.

At 17.390 sec it detects an upcoming obstacle (the obstacle detection distance was set to 20cm for this run), and stops (not quite getting all the way stopped before running into the wall – oops!).

First-to-Second Leg Transition:

The handling procedure for the ‘OBSTACLE_AHEAD’ case is to stop, back straight up to achieve the nominal wall offset distance (40cm here), then make a 90º turn (CW in this case) away from the wall to orient itself parallel to the wall again.

In HandleAnomalousConditions with 3 error code detected
OBSTACLE_AHEAD case detected
In BackupAndTurn90Deg()with front distance = 3
MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0	4
Msec	Dist	SetD	speed
40135	4.00	40	-75
40337	5.00	40	-77
40535	19.00	40	-51
40738	27.00	40	-36
40938	37.00	40	-16
Stopped with front dist = 39
In SpinTurn(CW, 90.00, 45.00)
Init hdg = 174.59 deg, Turn = 90.00  deg, tgt = -95.41 deg, timeout = 4.00 sec

Completed turn with yaw = -94.62, tgt = -95.41, and match = 0.996
BackupAndTurn90Deg finished
In TrackLeftWallOffset with Kp/Ki/Kd = 300.00	0.00	0.00

In TrackLeftWallOffset with LR/LC/LF = 50	50	53
CutAngDeg < 0, SteerAngleDeg > cutAngleDeg
In SpinTurn(CCW, 47.14, 30.00)
Init hdg = -82.72 deg, Turn = 47.14  deg, tgt = -129.87 deg, timeout = 3.14 sec

Completed turn with yaw = -130.81, tgt = -129.87, and match = 0.995
fudgeFactorCm = 10

at approach start: cut angle = -30.000, adjfactor = 0.866, num = 40.00, adjOffsetCm = 56.19
At start - err = prev_err = 12
R/C/F dists = 66	67	68 Steerval = 0.160, SteerAngle = 9.14, CutAngle = -30.00, err/prev_err = 12
Msec	LFront	LCtr	LRear	Front	Rear	Err	P_err
At end of offset capture - prev_res*res = -12
correct back to parallel (left side)
In SpinTurn(CW, 30.00, 30.00)
Init hdg = -143.65 deg, Turn = 30.00  deg, tgt = -113.65 deg, timeout = 2.00 sec

Completed turn with yaw = -112.75, tgt = -113.65, and match = 0.995

In HandleAnomalousConditions with 3 error code detected

OBSTACLE_AHEAD case detected

In BackupAndTurn90Deg()with front distance = 3

MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0 4

Msec Dist SetD speed

40135 4.00 40 -75

40337 5.00 40 -77

40535 19.00 40 -51

40738 27.00 40 -36

40938 37.00 40 -16

Stopped with front dist = 39

In SpinTurn(CW, 90.00, 45.00)

Init hdg = 174.59 deg, Turn = 90.00 deg, tgt = -95.41 deg, timeout = 4.00 sec

Completed turn with yaw = -94.62, tgt = -95.41, and match = 0.996

BackupAndTurn90Deg finished

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

In TrackLeftWallOffset with LR/LC/LF = 50 50 53

CutAngDeg < 0, SteerAngleDeg > cutAngleDeg

In SpinTurn(CCW, 47.14, 30.00)

Init hdg = -82.72 deg, Turn = 47.14 deg, tgt = -129.87 deg, timeout = 3.14 sec

Completed turn with yaw = -130.81, tgt = -129.87, and match = 0.995

fudgeFactorCm = 10

at approach start: cut angle = -30.000, adjfactor = 0.866, num = 40.00, adjOffsetCm = 56.19

At start - err = prev_err = 12

R/C/F dists = 66 67 68 Steerval = 0.160, SteerAngle = 9.14, CutAngle = -30.00, err/prev_err = 12

Msec LFront LCtr LRear Front Rear Err P_err

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

correct back to parallel (left side)

In SpinTurn(CW, 30.00, 30.00)

Init hdg = -143.65 deg, Turn = 30.00 deg, tgt = -113.65 deg, timeout = 2.00 sec

Completed turn with yaw = -112.75, tgt = -113.65, and match = 0.995

As can be seen from the above telemetry, that is exactly what happens, backing up to the point where the front LIDAR sensor shows 38cm and then making the required 90º turn with SpinTurn(CW, 90.00, 45.00). The ‘backup and turn’ evolution is completed in approximately 1.5 sec.

Second Leg: 47.4 – 49.0 Sec (16-19 sec in video)

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	Fdir	Cdir	Rdir	RearD	Steer	Set	error	Ival	Kp*err	Ki*Ival	Kd*Din	Output	Lspd	Rspd
47399 	24	23	23	73	0.13	0.00	-0.13	0.00 	-39.90	0.00	-0.00 	-39.90	35	114
47497 	24	23	23	71	0.05	0.00	-0.05	0.00 	-15.90	0.00	0.00 	-15.90	59	90
47597 	24	23	23	77	0.02	0.00	-0.02	0.00 	-6.90	0.00	0.00 	-6.90	68	81
47699 	23	22	23	79	0.01	0.00	-0.01	0.00 	-3.60	0.00	0.00 	-3.60	71	78
47801 	23	22	23	86	0.00	0.00	-0.00	0.00 	-0.60	0.00	0.00 	-0.60	74	75
47898 	21	22	24	89	-0.26	0.00	0.26	0.00 	77.40	0.00	0.00 	77.40	127	0
47996 	20	20	22	95	-0.19	0.00	0.19	0.00 	57.00	0.00	-0.00 	57.00	127	18
48097 	20	20	20	97	-0.07	0.00	0.07	0.00 	21.00	0.00	-0.00 	21.00	96	54
48195 	20	18	20	102	0.02	0.00	-0.02	0.00 	-5.40	0.00	-0.00 	-5.40	69	80
48295 	21	19	21	108	0.03	0.00	-0.03	0.00 	-8.70	0.00	-0.00 	-8.70	66	83
48397 	22	21	21	114	-0.07	0.00	0.07	0.00 	20.70	0.00	0.00 	20.70	95	54
48497 	22	21	21	117	0.09	0.00	-0.09	0.00 	-27.30	0.00	-0.00 	-27.30	47	102
48596 	23	21	21	122	0.18	0.00	-0.18	0.00 	-54.30	0.00	-0.00 	-54.30	20	127
48695 	24	22	22	819	0.19	0.00	-0.19	0.00 	-57.60	0.00	-0.00 	-57.60	17	127
48796 	24	23	23	819	0.10	0.00	-0.10	0.00 	-30.90	0.00	0.00 	-30.90	44	105
48894 	24	23	24	819	-0.06	0.00	0.06	0.00 	17.10	0.00	0.00 	17.10	92	57
CheckForErrorCondx returning error code OBSTACLE_AHEAD
48997 	22	23	24	819	-0.16	0.00	0.16	0.00 	47.10	0.00	0.00 	47.10	122	27

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

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

47399 24 23 23 73 0.13 0.00 -0.13 0.00 -39.90 0.00 -0.00 -39.90 35 114

47497 24 23 23 71 0.05 0.00 -0.05 0.00 -15.90 0.00 0.00 -15.90 59 90

47597 24 23 23 77 0.02 0.00 -0.02 0.00 -6.90 0.00 0.00 -6.90 68 81

47699 23 22 23 79 0.01 0.00 -0.01 0.00 -3.60 0.00 0.00 -3.60 71 78

47801 23 22 23 86 0.00 0.00 -0.00 0.00 -0.60 0.00 0.00 -0.60 74 75

47898 21 22 24 89 -0.26 0.00 0.26 0.00 77.40 0.00 0.00 77.40 127 0

47996 20 20 22 95 -0.19 0.00 0.19 0.00 57.00 0.00 -0.00 57.00 127 18

48097 20 20 20 97 -0.07 0.00 0.07 0.00 21.00 0.00 -0.00 21.00 96 54

48195 20 18 20 102 0.02 0.00 -0.02 0.00 -5.40 0.00 -0.00 -5.40 69 80

48295 21 19 21 108 0.03 0.00 -0.03 0.00 -8.70 0.00 -0.00 -8.70 66 83

48397 22 21 21 114 -0.07 0.00 0.07 0.00 20.70 0.00 0.00 20.70 95 54

48497 22 21 21 117 0.09 0.00 -0.09 0.00 -27.30 0.00 -0.00 -27.30 47 102

48596 23 21 21 122 0.18 0.00 -0.18 0.00 -54.30 0.00 -0.00 -54.30 20 127

48695 24 22 22 819 0.19 0.00 -0.19 0.00 -57.60 0.00 -0.00 -57.60 17 127

48796 24 23 23 819 0.10 0.00 -0.10 0.00 -30.90 0.00 0.00 -30.90 44 105

48894 24 23 24 819 -0.06 0.00 0.06 0.00 17.10 0.00 0.00 17.10 92 57

CheckForErrorCondx returning error code OBSTACLE_AHEAD

48997 22 23 24 819 -0.16 0.00 0.16 0.00 47.10 0.00 0.00 47.10 122 27

Second-to-Third Leg Transition:

In HandleAnomalousConditions with 3 error code detected
OBSTACLE_AHEAD case detected
In BackupAndTurn90Deg()with front distance = 4
MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0	4
Msec	Dist	SetD	speed
50223	4.00	40	-75
50423	4.00	40	-79
50622	19.00	40	-51
50823	27.00	40	-36
51022	34.00	40	-23
Stopped with front dist = 38
In SpinTurn(CW, 90.00, 45.00)
Init hdg = -95.98 deg, Turn = 90.00  deg, tgt = -5.98 deg, timeout = 4.00 sec

Completed turn with yaw = -4.66, tgt = -5.98, and match = 0.993
BackupAndTurn90Deg finished
In TrackLeftWallOffset with Kp/Ki/Kd = 300.00	0.00	0.00

In TrackLeftWallOffset with LR/LC/LF = 49	50	48
CutAngDeg < 0, SteerAngleDeg > cutAngleDeg
In SpinTurn(CCW, 39.71, 30.00)
Init hdg = 7.90 deg, Turn = 39.71  deg, tgt = -31.81 deg, timeout = 2.65 sec

Completed turn with yaw = -31.90, tgt = -31.81, and match = 1.000
fudgeFactorCm = 10

at approach start: cut angle = -30.000, adjfactor = 0.866, num = 40.00, adjOffsetCm = 56.19
At start - err = prev_err = -13
R/C/F dists = 32	41	43 Steerval = 1.040, SteerAngle = 59.43, CutAngle = -30.00, err/prev_err = -13
Msec	LFront	LCtr	LRear	Front	Rear	Err	P_err
At end of offset capture - prev_res*res = 169
correct back to parallel (left side)
In SpinTurn(CW, 30.00, 30.00)
Init hdg = -43.89 deg, Turn = 30.00  deg, tgt = -13.89 deg, timeout = 2.00 sec

Completed turn with yaw = -13.84, tgt = -13.89, and match = 1.000

In HandleAnomalousConditions with 3 error code detected

OBSTACLE_AHEAD case detected

In BackupAndTurn90Deg()with front distance = 4

MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0 4

Msec Dist SetD speed

50223 4.00 40 -75

50423 4.00 40 -79

50622 19.00 40 -51

50823 27.00 40 -36

51022 34.00 40 -23

Stopped with front dist = 38

In SpinTurn(CW, 90.00, 45.00)

Init hdg = -95.98 deg, Turn = 90.00 deg, tgt = -5.98 deg, timeout = 4.00 sec

Completed turn with yaw = -4.66, tgt = -5.98, and match = 0.993

BackupAndTurn90Deg finished

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

In TrackLeftWallOffset with LR/LC/LF = 49 50 48

CutAngDeg < 0, SteerAngleDeg > cutAngleDeg

In SpinTurn(CCW, 39.71, 30.00)

Init hdg = 7.90 deg, Turn = 39.71 deg, tgt = -31.81 deg, timeout = 2.65 sec

Completed turn with yaw = -31.90, tgt = -31.81, and match = 1.000

fudgeFactorCm = 10

at approach start: cut angle = -30.000, adjfactor = 0.866, num = 40.00, adjOffsetCm = 56.19

At start - err = prev_err = -13

R/C/F dists = 32 41 43 Steerval = 1.040, SteerAngle = 59.43, CutAngle = -30.00, err/prev_err = -13

Msec LFront LCtr LRear Front Rear Err P_err

At end of offset capture - prev_res*res = 169

correct back to parallel (left side)

In SpinTurn(CW, 30.00, 30.00)

Init hdg = -43.89 deg, Turn = 30.00 deg, tgt = -13.89 deg, timeout = 2.00 sec

Completed turn with yaw = -13.84, tgt = -13.89, and match = 1.000

Third Leg: 57.0 – 59.8 Sec (25-29 sec in video)

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	Fdir	Cdir	Rdir	RearD	Steer	Set	error	Ival	Kp*err	Ki*Ival	Kd*Din	Output	Lspd	Rspd
57020 	47	47	48	20	-0.10	0.00	0.10	0.00 	30.90	0.00	0.00 	30.90	105	44
57117 	46	48	45	20	0.09	0.00	-0.09	0.00 	-26.40	0.00	-0.00 	-26.40	48	101
57218 	47	47	48	24	-0.07	0.00	0.07	0.00 	21.90	0.00	0.00 	21.90	96	53
57320 	47	48	48	29	-0.07	0.00	0.07	0.00 	21.60	0.00	-0.00 	21.60	96	53
57420 	47	49	47	33	0.01	0.00	-0.01	0.00 	-2.70	0.00	-0.00 	-2.70	72	77
57514 	50	50	48	38	0.24	0.00	-0.24	0.00 	-72.00	0.00	-0.00 	-72.00	3	127
57618 	50	50	49	43	0.10	0.00	-0.10	0.00 	-30.00	0.00	0.00 	-30.00	45	105
57718 	51	48	49	49	0.16	0.00	-0.16	0.00 	-47.40	0.00	-0.00 	-47.40	27	122
57818 	50	49	52	55	-0.20	0.00	0.20	0.00 	60.30	0.00	0.00 	60.30	127	14
57915 	50	49	52	60	-0.08	0.00	0.08	0.00 	24.30	0.00	-0.00 	24.30	99	50
58015 	50	50	51	64	-0.16	0.00	0.16	0.00 	48.00	0.00	0.00 	48.00	123	27
58115 	51	50	50	70	-0.03	0.00	0.03	0.00 	9.00	0.00	-0.00 	9.00	84	66
58215 	50	51	49	74	0.14	0.00	-0.14	0.00 	-42.30	0.00	-0.00 	-42.30	32	117
58318 	50	52	50	80	0.15	0.00	-0.15	0.00 	-45.60	0.00	-0.00 	-45.60	29	120
58417 	52	52	52	83	-0.02	0.00	0.02	0.00 	5.40	0.00	0.00 	5.40	80	69
58517 	51	53	50	90	0.05	0.00	-0.05	0.00 	-15.90	0.00	-0.00 	-15.90	59	90
58615 	51	53	51	91	0.07	0.00	-0.07	0.00 	-21.90	0.00	-0.00 	-21.90	53	96
58715 	50	52	51	99	-0.07	0.00	0.07	0.00 	20.40	0.00	0.00 	20.40	95	54
58816 	52	51	52	104	-0.05	0.00	0.05	0.00 	14.70	0.00	-0.00 	14.70	89	60
58918 	52	53	52	114	-0.03	0.00	0.03	0.00 	8.10	0.00	-0.00 	8.10	83	66
59015 	54	54	52	116	0.13	0.00	-0.13	0.00 	-40.20	0.00	-0.00 	-40.20	34	115
59115 	54	54	55	117	-0.03	0.00	0.03	0.00 	7.80	0.00	0.00 	7.80	82	67
59215 	55	54	54	125	0.18	0.00	-0.18	0.00 	-55.20	0.00	-0.00 	-55.20	19	127
59316 	55	56	55	819	-0.01	0.00	0.01	0.00 	4.20	0.00	0.00 	4.20	79	70
59415 	55	54	55	819	-0.04	0.00	0.04	0.00 	10.80	0.00	0.00 	10.80	85	64
59515 	54	55	56	819	-0.14	0.00	0.14	0.00 	40.50	0.00	0.00 	40.50	115	34
59615 	56	55	54	819	0.17	0.00	-0.17	0.00 	-49.50	0.00	-0.00 	-49.50	25	124
59716 	57	55	56	819	0.09	0.00	-0.09	0.00 	-25.50	0.00	0.00 	-25.50	49	100
CheckForErrorCondx returning error code OBSTACLE_AHEAD
59816 	57	57	58	819	-0.12	0.00	0.12	0.00 	36.90	0.00	0.00 	36.90	111	38

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

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

57020 47 47 48 20 -0.10 0.00 0.10 0.00 30.90 0.00 0.00 30.90 105 44

57117 46 48 45 20 0.09 0.00 -0.09 0.00 -26.40 0.00 -0.00 -26.40 48 101

57218 47 47 48 24 -0.07 0.00 0.07 0.00 21.90 0.00 0.00 21.90 96 53

57320 47 48 48 29 -0.07 0.00 0.07 0.00 21.60 0.00 -0.00 21.60 96 53

57420 47 49 47 33 0.01 0.00 -0.01 0.00 -2.70 0.00 -0.00 -2.70 72 77

57514 50 50 48 38 0.24 0.00 -0.24 0.00 -72.00 0.00 -0.00 -72.00 3 127

57618 50 50 49 43 0.10 0.00 -0.10 0.00 -30.00 0.00 0.00 -30.00 45 105

57718 51 48 49 49 0.16 0.00 -0.16 0.00 -47.40 0.00 -0.00 -47.40 27 122

57818 50 49 52 55 -0.20 0.00 0.20 0.00 60.30 0.00 0.00 60.30 127 14

57915 50 49 52 60 -0.08 0.00 0.08 0.00 24.30 0.00 -0.00 24.30 99 50

58015 50 50 51 64 -0.16 0.00 0.16 0.00 48.00 0.00 0.00 48.00 123 27

58115 51 50 50 70 -0.03 0.00 0.03 0.00 9.00 0.00 -0.00 9.00 84 66

58215 50 51 49 74 0.14 0.00 -0.14 0.00 -42.30 0.00 -0.00 -42.30 32 117

58318 50 52 50 80 0.15 0.00 -0.15 0.00 -45.60 0.00 -0.00 -45.60 29 120

58417 52 52 52 83 -0.02 0.00 0.02 0.00 5.40 0.00 0.00 5.40 80 69

58517 51 53 50 90 0.05 0.00 -0.05 0.00 -15.90 0.00 -0.00 -15.90 59 90

58615 51 53 51 91 0.07 0.00 -0.07 0.00 -21.90 0.00 -0.00 -21.90 53 96

58715 50 52 51 99 -0.07 0.00 0.07 0.00 20.40 0.00 0.00 20.40 95 54

58816 52 51 52 104 -0.05 0.00 0.05 0.00 14.70 0.00 -0.00 14.70 89 60

58918 52 53 52 114 -0.03 0.00 0.03 0.00 8.10 0.00 -0.00 8.10 83 66

59015 54 54 52 116 0.13 0.00 -0.13 0.00 -40.20 0.00 -0.00 -40.20 34 115

59115 54 54 55 117 -0.03 0.00 0.03 0.00 7.80 0.00 0.00 7.80 82 67

59215 55 54 54 125 0.18 0.00 -0.18 0.00 -55.20 0.00 -0.00 -55.20 19 127

59316 55 56 55 819 -0.01 0.00 0.01 0.00 4.20 0.00 0.00 4.20 79 70

59415 55 54 55 819 -0.04 0.00 0.04 0.00 10.80 0.00 0.00 10.80 85 64

59515 54 55 56 819 -0.14 0.00 0.14 0.00 40.50 0.00 0.00 40.50 115 34

59615 56 55 54 819 0.17 0.00 -0.17 0.00 -49.50 0.00 -0.00 -49.50 25 124

59716 57 55 56 819 0.09 0.00 -0.09 0.00 -25.50 0.00 0.00 -25.50 49 100

CheckForErrorCondx returning error code OBSTACLE_AHEAD

59816 57 57 58 819 -0.12 0.00 0.12 0.00 36.90 0.00 0.00 36.90 111 38

Transition and Fourth Leg: 61.0 – 70.4 Sec (30-40 sec in video)

In HandleAnomalousConditions with 3 error code detected
OBSTACLE_AHEAD case detected
In BackupAndTurn90Deg()with front distance = 4
MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0	4
Msec	Dist	SetD	speed
61040	4.00	40	-75
61242	4.00	40	-79
61440	15.00	40	-59
61640	25.00	40	-41
61840	32.00	40	-28
Stopped with front dist = 39
In SpinTurn(CW, 90.00, 45.00)
Init hdg = -5.67 deg, Turn = 90.00  deg, tgt = 84.33 deg, timeout = 4.00 sec

Completed turn with yaw = 82.99, tgt = 84.33, and match = 0.993
BackupAndTurn90Deg finished
In TrackLeftWallOffset with Kp/Ki/Kd = 300.00	0.00	0.00

In TrackLeftWallOffset with LR/LC/LF = 46	45	47
CutAngDeg < 0, SteerAngleDeg > cutAngleDeg
In SpinTurn(CCW, 36.86, 30.00)
Init hdg = 96.48 deg, Turn = 36.86  deg, tgt = 59.62 deg, timeout = 2.46 sec

Completed turn with yaw = 60.53, tgt = 59.62, and match = 0.995
fudgeFactorCm = 10

at approach start: cut angle = -30.000, adjfactor = 0.866, num = 40.00, adjOffsetCm = 56.19
At start - err = prev_err = -4
R/C/F dists = 58	54	52 Steerval = -0.600, SteerAngle = -34.29, CutAngle = -30.00, err/prev_err = -4
Msec	LFront	LCtr	LRear	Front	Rear	Err	P_err
At end of offset capture - prev_res*res = 16
correct back to parallel (left side)
In SpinTurn(CW, 30.00, 30.00)
Init hdg = 56.06 deg, Turn = 30.00  deg, tgt = 86.06 deg, timeout = 2.00 sec

Completed turn with yaw = 85.46, tgt = 86.06, and match = 0.997

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	Fdir	Cdir	Rdir	RearD	Steer	Set	error	Ival	Kp*err	Ki*Ival	Kd*Din	Output	Lspd	Rspd
68020 	45	45	44	48	0.13	0.00	-0.13	0.00 	-40.50	0.00	-0.00 	-40.50	34	115
68118 	46	46	45	48	0.09	0.00	-0.09	0.00 	-25.80	0.00	0.00 	-25.80	49	100
68220 	46	45	44	51	0.20	0.00	-0.20	0.00 	-61.50	0.00	-0.00 	-61.50	13	127
68321 	45	46	44	54	0.07	0.00	-0.07	0.00 	-19.80	0.00	0.00 	-19.80	55	94
68416 	43	44	45	57	-0.21	0.00	0.21	0.00 	61.80	0.00	0.00 	61.80	127	13
68517 	43	43	44	61	-0.08	0.00	0.08	0.00 	23.10	0.00	-0.00 	23.10	98	51
68619 	42	43	42	67	0.10	0.00	-0.10	0.00 	-30.90	0.00	-0.00 	-30.90	44	105
68720 	42	41	46	72	-0.39	0.00	0.39	0.00 	116.70	0.00	0.00 	116.70	127	0
68819 	43	43	42	78	0.10	0.00	-0.10	0.00 	-30.90	0.00	-0.00 	-30.90	44	105
68916 	44	45	43	80	0.16	0.00	-0.16	0.00 	-49.50	0.00	-0.00 	-49.50	25	124
69016 	45	45	44	86	0.10	0.00	-0.10	0.00 	-28.50	0.00	0.00 	-28.50	46	103
69120 	44	45	43	89	0.04	0.00	-0.04	0.00 	-10.50	0.00	0.00 	-10.50	64	85
69216 	43	45	44	96	-0.09	0.00	0.09	0.00 	28.50	0.00	0.00 	28.50	103	46
69319 	43	44	44	100	-0.12	0.00	0.12	0.00 	34.80	0.00	0.00 	34.80	109	40
69415 	43	44	42	106	0.06	0.00	-0.06	0.00 	-19.20	0.00	-0.00 	-19.20	55	94
69518 	44	43	43	109	0.13	0.00	-0.13	0.00 	-39.90	0.00	-0.00 	-39.90	35	114
69617 	43	45	44	119	-0.05	0.00	0.05	0.00 	13.50	0.00	0.00 	13.50	88	61
69718 	44	44	43	120	0.00	0.00	-0.00	0.00 	-1.20	0.00	-0.00 	-1.20	73	76
69815 	45	44	45	128	-0.03	0.00	0.03	0.00 	7.80	0.00	0.00 	7.80	82	67
69916 	44	44	42	819	0.18	0.00	-0.18	0.00 	-55.20	0.00	-0.00 	-55.20	19	127
70016 	44	45	44	819	-0.05	0.00	0.05	0.00 	13.50	0.00	0.00 	13.50	88	61
70118 	44	44	44	819	-0.07	0.00	0.07	0.00 	19.80	0.00	0.00 	19.80	94	55
70217 	45	44	44	819	0.04	0.00	-0.04	0.00 	-13.20	0.00	-0.00 	-13.20	61	88
70314 	44	45	44	819	0.00	0.00	-0.00	0.00 	-1.50	0.00	0.00 	-1.50	73	76
CheckForErrorCondx returning error code OBSTACLE_AHEAD
70415 	44	44	45	819	-0.14	0.00	0.14	0.00 	40.80	0.00	0.00 	40.80	115	34

In HandleAnomalousConditions with 3 error code detected

OBSTACLE_AHEAD case detected

In BackupAndTurn90Deg()with front distance = 4

MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0 4

Msec Dist SetD speed

61040 4.00 40 -75

61242 4.00 40 -79

61440 15.00 40 -59

61640 25.00 40 -41

61840 32.00 40 -28

Stopped with front dist = 39

In SpinTurn(CW, 90.00, 45.00)

Init hdg = -5.67 deg, Turn = 90.00 deg, tgt = 84.33 deg, timeout = 4.00 sec

Completed turn with yaw = 82.99, tgt = 84.33, and match = 0.993

BackupAndTurn90Deg finished

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

In TrackLeftWallOffset with LR/LC/LF = 46 45 47

CutAngDeg < 0, SteerAngleDeg > cutAngleDeg

In SpinTurn(CCW, 36.86, 30.00)

Init hdg = 96.48 deg, Turn = 36.86 deg, tgt = 59.62 deg, timeout = 2.46 sec

Completed turn with yaw = 60.53, tgt = 59.62, and match = 0.995

fudgeFactorCm = 10

at approach start: cut angle = -30.000, adjfactor = 0.866, num = 40.00, adjOffsetCm = 56.19

At start - err = prev_err = -4

R/C/F dists = 58 54 52 Steerval = -0.600, SteerAngle = -34.29, CutAngle = -30.00, err/prev_err = -4

Msec LFront LCtr LRear Front Rear Err P_err

At end of offset capture - prev_res*res = 16

correct back to parallel (left side)

In SpinTurn(CW, 30.00, 30.00)

Init hdg = 56.06 deg, Turn = 30.00 deg, tgt = 86.06 deg, timeout = 2.00 sec

Completed turn with yaw = 85.46, tgt = 86.06, and match = 0.997

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

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

68020 45 45 44 48 0.13 0.00 -0.13 0.00 -40.50 0.00 -0.00 -40.50 34 115

68118 46 46 45 48 0.09 0.00 -0.09 0.00 -25.80 0.00 0.00 -25.80 49 100

68220 46 45 44 51 0.20 0.00 -0.20 0.00 -61.50 0.00 -0.00 -61.50 13 127

68321 45 46 44 54 0.07 0.00 -0.07 0.00 -19.80 0.00 0.00 -19.80 55 94

68416 43 44 45 57 -0.21 0.00 0.21 0.00 61.80 0.00 0.00 61.80 127 13

68517 43 43 44 61 -0.08 0.00 0.08 0.00 23.10 0.00 -0.00 23.10 98 51

68619 42 43 42 67 0.10 0.00 -0.10 0.00 -30.90 0.00 -0.00 -30.90 44 105

68720 42 41 46 72 -0.39 0.00 0.39 0.00 116.70 0.00 0.00 116.70 127 0

68819 43 43 42 78 0.10 0.00 -0.10 0.00 -30.90 0.00 -0.00 -30.90 44 105

68916 44 45 43 80 0.16 0.00 -0.16 0.00 -49.50 0.00 -0.00 -49.50 25 124

69016 45 45 44 86 0.10 0.00 -0.10 0.00 -28.50 0.00 0.00 -28.50 46 103

69120 44 45 43 89 0.04 0.00 -0.04 0.00 -10.50 0.00 0.00 -10.50 64 85

69216 43 45 44 96 -0.09 0.00 0.09 0.00 28.50 0.00 0.00 28.50 103 46

69319 43 44 44 100 -0.12 0.00 0.12 0.00 34.80 0.00 0.00 34.80 109 40

69415 43 44 42 106 0.06 0.00 -0.06 0.00 -19.20 0.00 -0.00 -19.20 55 94

69518 44 43 43 109 0.13 0.00 -0.13 0.00 -39.90 0.00 -0.00 -39.90 35 114

69617 43 45 44 119 -0.05 0.00 0.05 0.00 13.50 0.00 0.00 13.50 88 61

69718 44 44 43 120 0.00 0.00 -0.00 0.00 -1.20 0.00 -0.00 -1.20 73 76

69815 45 44 45 128 -0.03 0.00 0.03 0.00 7.80 0.00 0.00 7.80 82 67

69916 44 44 42 819 0.18 0.00 -0.18 0.00 -55.20 0.00 -0.00 -55.20 19 127

70016 44 45 44 819 -0.05 0.00 0.05 0.00 13.50 0.00 0.00 13.50 88 61

70118 44 44 44 819 -0.07 0.00 0.07 0.00 19.80 0.00 0.00 19.80 94 55

70217 45 44 44 819 0.04 0.00 -0.04 0.00 -13.20 0.00 -0.00 -13.20 61 88

70314 44 45 44 819 0.00 0.00 -0.00 0.00 -1.50 0.00 0.00 -1.50 73 76

CheckForErrorCondx returning error code OBSTACLE_AHEAD

70415 44 44 45 819 -0.14 0.00 0.14 0.00 40.80 0.00 0.00 40.80 115 34

Transition and Fifth Leg: 71.6 – 80.9 Sec (40-50 sec in video)

In HandleAnomalousConditions with 3 error code detected
OBSTACLE_AHEAD case detected
In BackupAndTurn90Deg()with front distance = 3
MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0	3
Msec	Dist	SetD	speed
71639	4.00	40	-75
71839	3.00	40	-81
72039	17.00	40	-55
72238	30.00	40	-30
72439	36.00	40	-19
Stopped with front dist = 40
In SpinTurn(CW, 90.00, 45.00)
Init hdg = 82.59 deg, Turn = 90.00  deg, tgt = 172.59 deg, timeout = 4.00 sec

Completed turn with yaw = 171.54, tgt = 172.59, and match = 0.994
BackupAndTurn90Deg finished
In TrackLeftWallOffset with Kp/Ki/Kd = 300.00	0.00	0.00

In TrackLeftWallOffset with LR/LC/LF = 46	49	46
CutAngDeg < 0, SteerAngleDeg > cutAngleDeg
In SpinTurn(CCW, 28.29, 30.00)
Init hdg = -175.88 deg, Turn = 28.29  deg, tgt = 155.83 deg, timeout = 1.89 sec

Completed turn with yaw = 154.75, tgt = 155.83, and match = 0.994
fudgeFactorCm = 10

at approach start: cut angle = -30.000, adjfactor = 0.866, num = 40.00, adjOffsetCm = 56.19
At start - err = prev_err = -6
R/C/F dists = 53	52	50 Steerval = -0.330, SteerAngle = -18.86, CutAngle = -30.00, err/prev_err = -6
Msec	LFront	LCtr	LRear	Front	Rear	Err	P_err
At end of offset capture - prev_res*res = 36
correct back to parallel (left side)
In SpinTurn(CW, 30.00, 30.00)
Init hdg = 144.58 deg, Turn = 30.00  deg, tgt = 174.58 deg, timeout = 2.00 sec

Completed turn with yaw = 173.97, tgt = 174.58, and match = 0.997

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	Fdir	Cdir	Rdir	RearD	Steer	Set	error	Ival	Kp*err	Ki*Ival	Kd*Din	Output	Lspd	Rspd
78389 	45	47	45	42	0.23	0.00	-0.23	0.00 	-68.10	0.00	-0.00 	-68.10	6	127
78490 	45	47	45	42	0.05	0.00	-0.05	0.00 	-14.10	0.00	0.00 	-14.10	60	89
78591 	45	47	44	43	0.11	0.00	-0.11	0.00 	-32.10	0.00	-0.00 	-32.10	42	107
78693 	44	46	45	48	-0.05	0.00	0.05	0.00 	16.20	0.00	0.00 	16.20	91	58
78790 	43	46	44	52	-0.03	0.00	0.03	0.00 	10.20	0.00	-0.00 	10.20	85	64
78890 	43	48	44	55	-0.08	0.00	0.08	0.00 	24.60	0.00	0.00 	24.60	99	50
78989 	42	45	44	61	-0.23	0.00	0.23	0.00 	70.50	0.00	0.00 	70.50	127	4
79093 	41	42	45	65	-0.36	0.00	0.36	0.00 	107.40	0.00	0.00 	107.40	127	0
79192 	41	42	41	72	0.03	0.00	-0.03	0.00 	-9.60	0.00	-0.00 	-9.60	65	84
79292 	42	41	40	79	0.17	0.00	-0.17	0.00 	-51.30	0.00	-0.00 	-51.30	23	126
79390 	42	41	41	83	0.12	0.00	-0.12	0.00 	-36.30	0.00	0.00 	-36.30	38	111
79493 	42	42	41	85	0.08	0.00	-0.08	0.00 	-24.60	0.00	0.00 	-24.60	50	99
79590 	40	43	43	92	-0.23	0.00	0.23	0.00 	68.10	0.00	0.00 	68.10	127	6
79690 	40	41	42	99	0.03	0.00	-0.03	0.00 	-9.30	0.00	-0.00 	-9.30	65	84
79793 	41	42	40	103	0.13	0.00	-0.13	0.00 	-39.60	0.00	-0.00 	-39.60	35	114
79892 	42	41	42	110	0.01	0.00	-0.01	0.00 	-3.30	0.00	0.00 	-3.30	71	78
79989 	40	41	42	112	-0.14	0.00	0.14	0.00 	41.70	0.00	0.00 	41.70	116	33
80089 	40	41	39	121	0.11	0.00	-0.11	0.00 	-33.30	0.00	-0.00 	-33.30	41	108
80189 	40	41	39	124	0.17	0.00	-0.17	0.00 	-51.30	0.00	-0.00 	-51.30	23	126
80289 	39	38	40	819	-0.15	0.00	0.15	0.00 	45.60	0.00	0.00 	45.60	120	29
80388 	39	39	40	819	-0.08	0.00	0.08	0.00 	24.30	0.00	-0.00 	24.30	99	50
80489 	38	38	39	819	-0.10	0.00	0.10	0.00 	30.60	0.00	0.00 	30.60	105	44
80589 	37	37	37	819	-0.02	0.00	0.02	0.00 	6.90	0.00	-0.00 	6.90	81	68
80690 	37	37	37	819	-0.01	0.00	0.01	0.00 	3.90	0.00	-0.00 	3.90	78	71
80787 	38	38	37	819	0.10	0.00	-0.10	0.00 	-29.40	0.00	-0.00 	-29.40	45	104
CheckForErrorCondx returning error code OBSTACLE_AHEAD
80889 	38	38	39	819	-0.03	0.00	0.03	0.00 	9.60	0.00	0.00 	9.60	84	65

In HandleAnomalousConditions with 3 error code detected

OBSTACLE_AHEAD case detected

In BackupAndTurn90Deg()with front distance = 3

MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0 3

Msec Dist SetD speed

71639 4.00 40 -75

71839 3.00 40 -81

72039 17.00 40 -55

72238 30.00 40 -30

72439 36.00 40 -19

Stopped with front dist = 40

In SpinTurn(CW, 90.00, 45.00)

Init hdg = 82.59 deg, Turn = 90.00 deg, tgt = 172.59 deg, timeout = 4.00 sec

Completed turn with yaw = 171.54, tgt = 172.59, and match = 0.994

BackupAndTurn90Deg finished

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

In TrackLeftWallOffset with LR/LC/LF = 46 49 46

CutAngDeg < 0, SteerAngleDeg > cutAngleDeg

In SpinTurn(CCW, 28.29, 30.00)

Init hdg = -175.88 deg, Turn = 28.29 deg, tgt = 155.83 deg, timeout = 1.89 sec

Completed turn with yaw = 154.75, tgt = 155.83, and match = 0.994

fudgeFactorCm = 10

at approach start: cut angle = -30.000, adjfactor = 0.866, num = 40.00, adjOffsetCm = 56.19

At start - err = prev_err = -6

R/C/F dists = 53 52 50 Steerval = -0.330, SteerAngle = -18.86, CutAngle = -30.00, err/prev_err = -6

Msec LFront LCtr LRear Front Rear Err P_err

At end of offset capture - prev_res*res = 36

correct back to parallel (left side)

In SpinTurn(CW, 30.00, 30.00)

Init hdg = 144.58 deg, Turn = 30.00 deg, tgt = 174.58 deg, timeout = 2.00 sec

Completed turn with yaw = 173.97, tgt = 174.58, and match = 0.997

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

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

78389 45 47 45 42 0.23 0.00 -0.23 0.00 -68.10 0.00 -0.00 -68.10 6 127

78490 45 47 45 42 0.05 0.00 -0.05 0.00 -14.10 0.00 0.00 -14.10 60 89

78591 45 47 44 43 0.11 0.00 -0.11 0.00 -32.10 0.00 -0.00 -32.10 42 107

78693 44 46 45 48 -0.05 0.00 0.05 0.00 16.20 0.00 0.00 16.20 91 58

78790 43 46 44 52 -0.03 0.00 0.03 0.00 10.20 0.00 -0.00 10.20 85 64

78890 43 48 44 55 -0.08 0.00 0.08 0.00 24.60 0.00 0.00 24.60 99 50

78989 42 45 44 61 -0.23 0.00 0.23 0.00 70.50 0.00 0.00 70.50 127 4

79093 41 42 45 65 -0.36 0.00 0.36 0.00 107.40 0.00 0.00 107.40 127 0

79192 41 42 41 72 0.03 0.00 -0.03 0.00 -9.60 0.00 -0.00 -9.60 65 84

79292 42 41 40 79 0.17 0.00 -0.17 0.00 -51.30 0.00 -0.00 -51.30 23 126

79390 42 41 41 83 0.12 0.00 -0.12 0.00 -36.30 0.00 0.00 -36.30 38 111

79493 42 42 41 85 0.08 0.00 -0.08 0.00 -24.60 0.00 0.00 -24.60 50 99

79590 40 43 43 92 -0.23 0.00 0.23 0.00 68.10 0.00 0.00 68.10 127 6

79690 40 41 42 99 0.03 0.00 -0.03 0.00 -9.30 0.00 -0.00 -9.30 65 84

79793 41 42 40 103 0.13 0.00 -0.13 0.00 -39.60 0.00 -0.00 -39.60 35 114

79892 42 41 42 110 0.01 0.00 -0.01 0.00 -3.30 0.00 0.00 -3.30 71 78

79989 40 41 42 112 -0.14 0.00 0.14 0.00 41.70 0.00 0.00 41.70 116 33

80089 40 41 39 121 0.11 0.00 -0.11 0.00 -33.30 0.00 -0.00 -33.30 41 108

80189 40 41 39 124 0.17 0.00 -0.17 0.00 -51.30 0.00 -0.00 -51.30 23 126

80289 39 38 40 819 -0.15 0.00 0.15 0.00 45.60 0.00 0.00 45.60 120 29

80388 39 39 40 819 -0.08 0.00 0.08 0.00 24.30 0.00 -0.00 24.30 99 50

80489 38 38 39 819 -0.10 0.00 0.10 0.00 30.60 0.00 0.00 30.60 105 44

80589 37 37 37 819 -0.02 0.00 0.02 0.00 6.90 0.00 -0.00 6.90 81 68

80690 37 37 37 819 -0.01 0.00 0.01 0.00 3.90 0.00 -0.00 3.90 78 71

80787 38 38 37 819 0.10 0.00 -0.10 0.00 -29.40 0.00 -0.00 -29.40 45 104

CheckForErrorCondx returning error code OBSTACLE_AHEAD

80889 38 38 39 819 -0.03 0.00 0.03 0.00 9.60 0.00 0.00 9.60 84 65

Fifth-to-Sixth Leg Transition: 82.1 – 88.0 Sec (50-58 sec in video)

In HandleAnomalousConditions with 3 error code detected
OBSTACLE_AHEAD case detected
In BackupAndTurn90Deg()with front distance = 4
MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0	3
Msec	Dist	SetD	speed
82114	4.00	40	-75
82314	6.00	40	-75
82515	18.00	40	-53
82715	29.00	40	-32
82914	37.00	40	-16
Stopped with front dist = 38
In SpinTurn(CW, 90.00, 45.00)
Init hdg = 170.44 deg, Turn = 90.00  deg, tgt = -99.56 deg, timeout = 4.00 sec

Completed turn with yaw = -100.81, tgt = -99.56, and match = 0.993
BackupAndTurn90Deg finished
In TrackLeftWallOffset with Kp/Ki/Kd = 300.00	0.00	0.00

In TrackLeftWallOffset with LR/LC/LF = 47	48	47
CutAngDeg < 0, SteerAngleDeg > cutAngleDeg
In SpinTurn(CCW, 33.43, 30.00)
Init hdg = -89.13 deg, Turn = 33.43  deg, tgt = -122.56 deg, timeout = 2.23 sec

Completed turn with yaw = -122.88, tgt = -122.56, and match = 0.998
fudgeFactorCm = 10

at approach start: cut angle = -30.000, adjfactor = 0.866, num = 40.00, adjOffsetCm = 56.19
At start - err = prev_err = 3
R/C/F dists = 62	59	59 Steerval = -0.300, SteerAngle = -17.14, CutAngle = -30.00, err/prev_err = 3
Msec	LFront	LCtr	LRear	Front	Rear	Err	P_err
At end of offset capture - prev_res*res = 0
correct back to parallel (left side)
In SpinTurn(CW, 30.00, 30.00)
Init hdg = -132.07 deg, Turn = 30.00  deg, tgt = -102.07 deg, timeout = 2.00 sec

Completed turn with yaw = -102.32, tgt = -102.07, and match = 0.999

In HandleAnomalousConditions with 3 error code detected

OBSTACLE_AHEAD case detected

In BackupAndTurn90Deg()with front distance = 4

MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0 3

Msec Dist SetD speed

82114 4.00 40 -75

82314 6.00 40 -75

82515 18.00 40 -53

82715 29.00 40 -32

82914 37.00 40 -16

Stopped with front dist = 38

In SpinTurn(CW, 90.00, 45.00)

Init hdg = 170.44 deg, Turn = 90.00 deg, tgt = -99.56 deg, timeout = 4.00 sec

Completed turn with yaw = -100.81, tgt = -99.56, and match = 0.993

BackupAndTurn90Deg finished

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

In TrackLeftWallOffset with LR/LC/LF = 47 48 47

CutAngDeg < 0, SteerAngleDeg > cutAngleDeg

In SpinTurn(CCW, 33.43, 30.00)

Init hdg = -89.13 deg, Turn = 33.43 deg, tgt = -122.56 deg, timeout = 2.23 sec

Completed turn with yaw = -122.88, tgt = -122.56, and match = 0.998

fudgeFactorCm = 10

at approach start: cut angle = -30.000, adjfactor = 0.866, num = 40.00, adjOffsetCm = 56.19

At start - err = prev_err = 3

R/C/F dists = 62 59 59 Steerval = -0.300, SteerAngle = -17.14, CutAngle = -30.00, err/prev_err = 3

Msec LFront LCtr LRear Front Rear Err P_err

At end of offset capture - prev_res*res = 0

correct back to parallel (left side)

In SpinTurn(CW, 30.00, 30.00)

Init hdg = -132.07 deg, Turn = 30.00 deg, tgt = -102.07 deg, timeout = 2.00 sec

Completed turn with yaw = -102.32, tgt = -102.07, and match = 0.999

Sixth Leg up to Anomaly 88.9 – 89.8 Sec (57-59 sec in video)

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	Fdir	Cdir	Rdir	RearD	Steer	Set	error	Ival	Kp*err	Ki*Ival	Kd*Din	Output	Lspd	Rspd
88946 	43	43	41	48	0.22	0.00	-0.22	0.00 	-66.90	0.00	-0.00 	-66.90	8	127
89044 	42	41	42	49	0.04	0.00	-0.04	0.00 	-12.30	0.00	0.00 	-12.30	62	87
89144 	42	43	40	51	0.18	0.00	-0.18	0.00 	-54.90	0.00	-0.00 	-54.90	20	127
89245 	41	42	41	55	-0.01	0.00	0.01	0.00 	2.40	0.00	0.00 	2.40	77	72
89343 	41	42	43	59	-0.21	0.00	0.21	0.00 	62.40	0.00	0.00 	62.40	127	12
89442 	41	41	41	65	-0.04	0.00	0.04	0.00 	11.70	0.00	-0.00 	11.70	86	63
89540 	40	40	39	70	0.07	0.00	-0.07	0.00 	-21.00	0.00	-0.00 	-21.00	54	96
89640 	39	39	39	75	-0.04	0.00	0.04	0.00 	12.30	0.00	0.00 	12.30	87	62
89743 	37	39	39	80	-0.19	0.00	0.19	0.00 	57.30	0.00	0.00 	57.30	127	17
CheckForErrorCondx returning error code STUCK_AHEAD
89842 	37	37	37	84	0.11	0.00	-0.11	0.00 	-32.10	0.00	-0.00 	-32.10	42	107
In HandleAnomalousConditions with 1 error code detected
STUCK_AHEAD case detected
89854: aFrontDist Init: glFront_Dist_PrevMean = 196.00, glFront_Dist_PrevVar = 13328.00
In SpinTurn(CCW, 90.00, 45.00)
Init hdg = -100.27 deg, Turn = 90.00  deg, tgt = 169.73 deg, timeout = 4.00 sec

Completed turn with yaw = 168.40, tgt = 169.73, and match = 0.993
37/37/0
91902	37	29	1
92101	37	20	1
20/0
In SpinTurn(CW, 90.00, 45.00)
Init hdg = 153.01 deg, Turn = 90.00  deg, tgt = -116.99 deg, timeout = 4.00 sec

Completed turn with yaw = -118.25, tgt = -116.99, and match = 0.993

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

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

88946 43 43 41 48 0.22 0.00 -0.22 0.00 -66.90 0.00 -0.00 -66.90 8 127

89044 42 41 42 49 0.04 0.00 -0.04 0.00 -12.30 0.00 0.00 -12.30 62 87

89144 42 43 40 51 0.18 0.00 -0.18 0.00 -54.90 0.00 -0.00 -54.90 20 127

89245 41 42 41 55 -0.01 0.00 0.01 0.00 2.40 0.00 0.00 2.40 77 72

89343 41 42 43 59 -0.21 0.00 0.21 0.00 62.40 0.00 0.00 62.40 127 12

89442 41 41 41 65 -0.04 0.00 0.04 0.00 11.70 0.00 -0.00 11.70 86 63

89540 40 40 39 70 0.07 0.00 -0.07 0.00 -21.00 0.00 -0.00 -21.00 54 96

89640 39 39 39 75 -0.04 0.00 0.04 0.00 12.30 0.00 0.00 12.30 87 62

89743 37 39 39 80 -0.19 0.00 0.19 0.00 57.30 0.00 0.00 57.30 127 17

CheckForErrorCondx returning error code STUCK_AHEAD

89842 37 37 37 84 0.11 0.00 -0.11 0.00 -32.10 0.00 -0.00 -32.10 42 107

In HandleAnomalousConditions with 1 error code detected

STUCK_AHEAD case detected

89854: aFrontDist Init: glFront_Dist_PrevMean = 196.00, glFront_Dist_PrevVar = 13328.00

In SpinTurn(CCW, 90.00, 45.00)

Init hdg = -100.27 deg, Turn = 90.00 deg, tgt = 169.73 deg, timeout = 4.00 sec

Completed turn with yaw = 168.40, tgt = 169.73, and match = 0.993

37/37/0

91902 37 29 1

92101 37 20 1

20/0

In SpinTurn(CW, 90.00, 45.00)

Init hdg = 153.01 deg, Turn = 90.00 deg, tgt = -116.99 deg, timeout = 4.00 sec

Completed turn with yaw = -118.25, tgt = -116.99, and match = 0.993

On this leg an anomaly occurred. The robot detected a ‘Stuck Ahead’ condition, defined by the condition where the mathematical variance of the last N front LIDAR distance readings falls below a set threshold. This should never happen while the robot is actually moving, but it clearly did happen in this case (unfortunately I wasn’t recording the front distance measurement or the distance measurements to the other wall, so I can’t go back and see exactly what happened). The recovery procedure for a ‘stuck ahead’ condition is to make a 90 deg turn away from the nearest wall, move forward for 1 second, and then make another 90 deg turn to return to a parallel course, but offset 10-20 cm from the previous track. In this case, the robot turned toward the nearest wall, clearly a mistake (it was a mistake in TrackLeftWallOffset() – since fixed). However, it was not a disaster, as the robot made the second turn before hitting the wall, and from there on it returned to normal left wall tracking mode.

Summary:

This first test of left wall tracking performance was very encouraging. The robot was able to continue tracking operations over several laps, including an instance where it recovered from an inadvertent ‘Stuck’ detection. The test could easily have continued until the batteries died, but had to be aborted when one section of the foam-core wall fell in on top of the robot – oops!

Also, this run pointed out the need for more focused telemetry. For this test I was only reporting the left-side and rear distances, but now I know I need to add the right-side measurements as well as the front and rear variance numbers.

09 March 2023 Update:

After cleaning up some messy initialization code, and improving telemetry readouts, I ran another complete left-side wall-tracking lap in my sandbox, as shown in the following short video:

09 March 22 Left-side wall-tracking lap

The telemetry for this run is shown below:

Checking for MPU6050 IMU at I2C Addr 0x68
MPU6050 connection successful
Initializing DMP...
Enabling DMP...
DMP ready! Waiting for MPU6050 drift rate to settle...
Calibrating...Retrieving Calibration Values


Msec	Hdg
6930	0.073	0.076
MPU6050 Ready at 6.93 Sec with delta = -0.004
Checking for Teensy 3.5 VL53L0X Controller at I2C addr 0x20
Teensy available at 8031 with bVL53L0X_TeensyReady = 1. Waiting for Teensy setup() to finish
8033: got 1 from VL53L0X Teensy
Teensy setup() finished at Msec	LFront	LCtr	LRear	RFront	RCtr	RRear	Rear
8138: 15	14	15	819	121	819	14
VL53L0X Teensy Ready at 8138

Initializing Left, Right, Front Distance Arrays...
Initializing Front Distance Array
InitFrontDistArray() top: prev_mean = 0, prev_var = 0
8153: aFrontDist Init: glFront_Dist_PrevMean = 196.00, glFront_Dist_PrevVar = 13328.00
Updating Rear Distance Array
8163: aRearDistCM Init: glRear_Dist_PrevMean = 49.00, glRear_Dist_PrevVar = 833.00

Msec	LF	LC	LR		RF	RC	RR		LSteer	RSteer		F	Fvar	R	Rvar
8185	15	14	15		819	124	819		0.000	0.000		164	13328	14	833

Checking for Teensy 3.2 IRDET Controller at I2C addr 0x8
8194: IRDET Teensy Not Avail...
IRDET Teensy Ready at 8296
Fin1/Fin2/SteeringVal = 22	40	0.0000

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

In TrackLeftWallOffset with LR/LC/LF = 15	15	15
In SpinTurn(CW, 28.29, 30.00)
R/C/F dists = 16	20	22 Steerval = 0.620, SteerAngle = 35.43, CutAngle = 30.00, err/prev_err = -14
In SpinTurn(CCW, 30.00, 30.00)

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar		Steer	Set	Output
12901	38	39	39		123	819	819		132	13328	0.00	-0.07		0.00	21.30	0.00
13001	38	40	40		819	819	819		133	13328	0.00	-0.21		0.00	63.00	0.00
13098	39	40	39		114	128	819		127	13328	0.00	0.03		0.00	-9.00	0.00
13199	38	38	38		111	819	819		122	13328	0.00	0.01		0.00	-2.40	0.00
13299	38	39	38		111	819	819		118	13328	0.00	-0.03		0.00	9.30	0.00
13398	38	37	38		81	819	819		113	13328	0.00	0.04		0.00	-11.10	0.00
13501	38	37	39		121	819	819		109	13328	0.00	-0.07		0.00	21.90	0.00
13599	37	38	38		112	819	819		104	13328	0.00	-0.05		0.00	15.60	0.00
13699	37	39	38		105	819	819		97	13328	0.00	-0.04		0.00	12.30	0.00
13798	38	39	38		105	819	819		92	13328	0.00	-0.06		0.00	18.30	0.00
13900	37	38	38		108	819	819		86	13328	0.00	-0.10		0.00	30.60	0.00
14001	38	39	37		107	819	819		83	13328	0.00	0.18		0.00	-53.70	0.00
14100	39	37	39		113	819	819		79	13328	0.00	0.06		0.00	-17.10	0.00
14201	37	40	39		124	819	819		76	13328	0.00	-0.21		0.00	63.00	0.00
14299	39	40	39		112	819	819		73	13328	0.00	-0.01		0.00	3.00	0.00
14400	40	41	38		111	819	819		67	13328	0.00	0.25		0.00	-75.30	0.00
14498	41	41	39		124	819	819		62	13328	0.00	0.19		0.00	-57.30	0.00
14599	41	41	41		104	819	819		57	13328	0.00	0.00		0.00	-0.30	0.00
14699	41	40	41		819	819	819		51	13328	0.00	-0.04		0.00	12.00	0.00
14798	41	41	41		124	819	819		46	13328	0.00	-0.07		0.00	20.70	0.00
14899	38	41	41		119	819	819		39	13328	0.00	-0.12		0.00	35.70	0.00
14998	39	39	39		819	819	819		33	13328	0.00	-0.05		0.00	15.30	0.00
CheckForErrorCondx returning error code OBSTACLE_AHEAD
15098	38	39	38		819	819	819		29	13328	0.00	-0.03		0.00	9.30	0.00
In HandleAnomalousConditions with 3 error code detected
OBSTACLE_AHEAD case detected
In BackupAndTurn90Deg()with front distance = 14
MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0	14
Msec	Dist	SetD	speed
16323	13.00	40	-56
16520	18.00	40	-48
16721	23.00	40	-40
16920	28.00	40	-31
17121	33.00	40	-22
Stopped with front dist = 38
In SpinTurn(CW, 90.00, 45.00)
BackupAndTurn90Deg finished
In TrackLeftWallOffset with Kp/Ki/Kd = 300.00	0.00	0.00

In TrackLeftWallOffset with LR/LC/LF = 39	40	38
At 40 with desired offset = 40: close enough  - just parallel and go
In RotateToParallelOrientation(Left)
Just before while() loop: steerval/prv_val = -0.03	-0.03

Msec	Steer	Set	lastErr	lastIn	lastI	Kp*err	Ki*err	Kd*Din	Output	Lspeed	Rspeed
Parallel Orientation Achieved at 18285 with SteeringVal = -0.03 & bIRBeamAvail = FALSE

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar		Steer	Set	Output
18413	40	38	39		118	124	819		143	13328	0.00	0.14		0.00	-41.40	0.00
18514	41	39	39		123	819	819		141	13328	0.00	0.20		0.00	-59.70	0.00
18616	39	40	38		819	819	819		138	13328	0.00	0.10		0.00	-30.00	0.00
18711	39	39	39		115	819	819		136	13328	0.00	-0.07		0.00	21.30	0.00
18812	40	40	40		120	819	819		133	13328	0.00	0.04		0.00	-12.00	0.00
18915	38	39	41		105	819	819		127	13328	0.00	-0.23		0.00	69.30	0.00
19014	38	37	38		108	819	819		122	13328	0.00	-0.06		0.00	18.90	0.00
19112	38	37	38		105	819	819		118	13328	0.00	0.04		0.00	-11.10	0.00
19211	37	36	38		119	819	819		116	13328	0.00	-0.04		0.00	13.20	0.00
19316	38	37	37		111	819	819		108	13328	0.00	0.10		0.00	-29.10	0.00
19414	38	39	37		114	819	819		104	13328	0.00	0.09		0.00	-26.70	0.00
19510	38	39	37		819	819	819		99	13328	0.00	0.07		0.00	-20.70	0.00
19613	38	38	37		115	819	819		93	13328	0.00	0.13		0.00	-38.40	0.00
19711	39	39	38		127	819	819		89	13328	0.00	0.09		0.00	-26.70	0.00
19814	38	39	39		106	819	819		85	13328	0.00	-0.06		0.00	18.30	0.00
19911	37	38	38		819	819	819		81	13328	0.00	-0.05		0.00	15.60	0.00
20013	38	38	38		117	819	819		77	13328	0.00	0.02		0.00	-5.40	0.00
20112	37	38	37		819	819	819		75	13328	0.00	0.01		0.00	-2.40	0.00
20213	36	38	36		116	819	819		71	13328	0.00	-0.02		0.00	6.60	0.00
20311	37	37	36		128	819	819		68	13328	0.00	-0.02		0.00	6.90	0.00
20413	37	37	35		126	819	819		63	13328	0.00	0.17		0.00	-50.10	0.00
20511	35	39	36		127	819	819		56	13328	0.00	-0.05		0.00	15.30	0.00
20614	36	38	36		125	819	819		50	13328	0.00	0.01		0.00	-2.40	0.00
20713	35	37	36		819	819	819		46	13328	0.00	-0.04		0.00	12.90	0.00
20811	36	37	36		128	819	819		41	13328	0.00	-0.06		0.00	18.90	0.00
20910	35	36	36		121	819	819		37	13328	0.00	-0.07		0.00	22.20	0.00
21010	35	35	36		127	819	819		32	13328	0.00	-0.05		0.00	16.50	0.00
CheckForErrorCondx returning error code OBSTACLE_AHEAD
21111	36	35	36		93	819	819		27	13328	0.00	-0.12		0.00	37.50	0.00
In HandleAnomalousConditions with 3 error code detected
OBSTACLE_AHEAD case detected
In BackupAndTurn90Deg()with front distance = 5
MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0	4
Msec	Dist	SetD	speed
22335	6.00	40	-71
22536	13.00	40	-60
22733	21.00	40	-46
22934	29.00	40	-31
Stopped with front dist = 38
In SpinTurn(CW, 90.00, 45.00)
BackupAndTurn90Deg finished
In TrackLeftWallOffset with Kp/Ki/Kd = 300.00	0.00	0.00

In TrackLeftWallOffset with LR/LC/LF = 41	43	40
At 43 with desired offset = 40: close enough  - just parallel and go
In RotateToParallelOrientation(Left)
Just before while() loop: steerval/prv_val = -0.18	-0.18

Msec	Steer	Set	lastErr	lastIn	lastI	Kp*err	Ki*err	Kd*Din	Output	Lspeed	Rspeed
Parallel Orientation Achieved at 24164 with SteeringVal = -0.18 & bIRBeamAvail = FALSE

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	LF	LC	LR		RF	RC	RR		F	Fvar	R	Rvar		Steer	Set	Output
24290	41	41	40		128	125	819		149	13328	0.00	0.04		0.00	-12.30	0.00
24393	42	42	40		103	819	819		146	13328	0.00	0.17		0.00	-51.60	0.00
24494	40	42	39		114	124	819		145	13328	0.00	0.11		0.00	-33.60	0.00
24591	40	42	40		95	819	819		142	13328	0.00	-0.03		0.00	8.40	0.00
24692	40	41	40		93	819	819		136	13328	0.00	-0.04		0.00	11.70	0.00
24791	40	41	41		91	819	819		133	13328	0.00	-0.09		0.00	26.70	0.00
24891	39	40	40		86	819	819		126	13328	0.00	-0.09		0.00	27.00	0.00
24992	39	39	39		79	819	819		122	13328	0.00	0.04		0.00	-11.70	0.00
25094	38	39	40		88	819	819		118	13328	0.00	-0.11		0.00	33.30	0.00
25190	39	40	39		90	819	819		113	13328	0.00	-0.03		0.00	9.00	0.00
25293	40	39	38		84	819	819		107	13328	0.00	0.14		0.00	-41.70	0.00
25392	38	40	39		76	819	819		103	13328	0.00	-0.06		0.00	18.00	0.00
25493	39	39	39		53	819	819		99	13328	0.00	0.01		0.00	-2.70	0.00
25592	39	40	40		71	819	819		95	13328	0.00	-0.04		0.00	12.00	0.00
25693	39	39	38		65	819	819		90	13328	0.00	0.06		0.00	-17.70	0.00
25792	38	38	39		81	819	819		83	13328	0.00	-0.12		0.00	36.60	0.00
25889	38	38	39		80	819	819		79	13328	0.00	-0.10		0.00	30.60	0.00
25990	38	38	39		83	819	819		74	13328	0.00	-0.09		0.00	27.60	0.00
26089	37	38	37		92	819	819		71	13328	0.00	0.02		0.00	-5.40	0.00
26192	38	38	37		96	819	819		67	13328	0.00	0.05		0.00	-14.40	0.00
26291	38	37	38		819	819	819		62	13328	0.00	0.04		0.00	-11.10	0.00
26392	38	37	37		117	819	819		53	13328	0.00	0.10		0.00	-29.10	0.00
26491	37	36	36		113	819	819		49	13328	0.00	0.11		0.00	-31.80	0.00
26589	37	36	37		109	819	819		47	13328	0.00	-0.07		0.00	22.20	0.00
26690	35	35	37		123	819	819		44	13328	0.00	-0.09		0.00	25.50	0.00
26789	35	35	36		819	819	819		39	13328	0.00	-0.10		0.00	28.50	0.00
26889	36	35	34		118	819	819		33	13328	0.00	0.15		0.00	-43.50	0.00
CheckForErrorCondx returning error code OBSTACLE_AHEAD
26989	35	34	35		819	819	819		28	13328	0.00	0.05		0.00	-16.20	0.00
In HandleAnomalousConditions with 3 error code detected
OBSTACLE_AHEAD case detected
In BackupAndTurn90Deg()with front distance = 7
MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0	6
Msec	Dist	SetD	speed
28211	7.00	40	-69
28409	15.00	40	-55
28611	20.00	40	-47
28813	28.00	40	-33
Stopped with front dist = 37
In SpinTurn(CW, 90.00, 45.00)
BackupAndTurn90Deg finished
In TrackLeftWallOffset with Kp/Ki/Kd = 300.00	0.00	0.00

In TrackLeftWallOffset with LR/LC/LF = 42	41	42
At 41 with desired offset = 40: close enough  - just parallel and go
In RotateToParallelOrientation(Left)
Just before while() loop: steerval/prv_val = -0.02	-0.02

Msec	Steer	Set	lastErr	lastIn	lastI	Kp*err	Ki*err	Kd*Din	Output	Lspeed	Rspeed
Parallel Orientation Achieved at 30049 with SteeringVal = -0.02 & bIRBeamAvail = FALSE

TrackLeftWallOffset: Start tracking offset of 40.00cm with Kp/Ki/Kd = 300.00	0.00	0.00
Msec	LF	LC	LR		30178	43	42	42		819	819	819		138	13328	0.00	0.04		0.00	-12.60	0.00
30276	41	42	42		92	819	819		139	13328	0.00	-0.08		0.00	23.40	0.00
30375	43	42	43		116	819	819		135	13328	0.00	-0.03		0.00	8.40	0.00
30476	43	42	42		109	819	819		130	13328	0.00	0.12		0.00	-36.60	0.00
30577	42	43	43		121	819	819		123	13328	0.00	0.07		0.00	-21.90	0.00
30678	42	43	43		102	819	819		119	13328	0.00	-0.10		0.00	29.10	0.00
30775	43	43	42		112	819	819		113	13328	0.00	0.04		0.00	-12.90	0.00
30878	43	42	42		91	819	819		109	13328	0.00	0.11		0.00	-33.60	0.00
30975	42	42	42		79	819	819		104	13328	0.00	0.07		0.00	-21.60	0.00
31077	42	42	43		87	127	819		102	13328	0.00	-0.09		0.00	26.40	0.00
31174	41	42	41		94	819	819		96	13328	0.00	0.06		0.00	-18.60	0.00
31275	42	41	41		77	127	819		92	13328	0.00	0.07		0.00	-21.30	0.00
31376	42	41	43		71	127	819		87	13328	0.00	-0.07		0.00	20.70	0.00
31476	42	42	41		73	819	819		82	13328	0.00	0.13		0.00	-39.60	0.00
31578	41	41	40		89	819	819		76	13328	0.00	0.03		0.00	-9.30	0.00
31677	41	41	41		92	819	819		73	13328	0.00	0.00		0.00	-0.30	0.00
31776	41	40	40		84	819	819		67	13328	0.00	0.03		0.00	-9.00	0.00
31876	41	40	40		112	819	819		63	13328	0.00	0.13		0.00	-39.00	0.00
31975	40	39	40		819	819	819		57	13328	0.00	0.02		0.00	-5.70	0.00
32074	40	40	39		103	819	819		52	13328	0.00	0.13		0.00	-39.00	0.00
32175	38	39	39		819	819	819		47	13328	0.00	-0.01		0.00	3.30	0.00
32277	39	38	39		819	819	819		42	13328	0.00	-0.05		0.00	15.60	0.00
32373	38	37	40		121	819	819		35	13328	0.00	-0.14		0.00	42.90	0.00
32474	38	38	36		98	819	819		31	13328	0.00	0.16		0.00	-47.40	0.00
CheckForErrorCondx returning error code OBSTACLE_AHEAD
32574	37	37	38		115	819	819		26	13328	0.00	-0.10		0.00	30.90	0.00
In HandleAnomalousConditions with 3 error code detected
OBSTACLE_AHEAD case detected
In BackupAndTurn90Deg()with front distance = 7
MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0	6
Msec	Dist	SetD	speed
I received c on BT Serial1
ENTERING COMMAND MODE:
0 = 180 deg CCW Turn
1 = 180 deg CW Turn
A = Back to Auto Mode
S = Stop
F = Forward
R = Reverse

       Faster
	8
Left 4	5  6 Right
	2
       Slower

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

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

Checking for MPU6050 IMU at I2C Addr 0x68

MPU6050 connection successful

Initializing DMP...

Enabling DMP...

DMP ready! Waiting for MPU6050 drift rate to settle...

Calibrating...Retrieving Calibration Values

Msec Hdg

6930 0.073 0.076

MPU6050 Ready at 6.93 Sec with delta = -0.004

Checking for Teensy 3.5 VL53L0X Controller at I2C addr 0x20

Teensy available at 8031 with bVL53L0X_TeensyReady = 1. Waiting for Teensy setup() to finish

8033: got 1 from VL53L0X Teensy

Teensy setup() finished at Msec LFront LCtr LRear RFront RCtr RRear Rear

8138: 15 14 15 819 121 819 14

VL53L0X Teensy Ready at 8138

Initializing Left, Right, Front Distance Arrays...

Initializing Front Distance Array

InitFrontDistArray() top: prev_mean = 0, prev_var = 0

8153: aFrontDist Init: glFront_Dist_PrevMean = 196.00, glFront_Dist_PrevVar = 13328.00

Updating Rear Distance Array

8163: aRearDistCM Init: glRear_Dist_PrevMean = 49.00, glRear_Dist_PrevVar = 833.00

Msec LF LC LR RF RC RR LSteer RSteer F Fvar R Rvar

8185 15 14 15 819 124 819 0.000 0.000 164 13328 14 833

Checking for Teensy 3.2 IRDET Controller at I2C addr 0x8

8194: IRDET Teensy Not Avail...

IRDET Teensy Ready at 8296

Fin1/Fin2/SteeringVal = 22 40 0.0000

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

In TrackLeftWallOffset with LR/LC/LF = 15 15 15

In SpinTurn(CW, 28.29, 30.00)

R/C/F dists = 16 20 22 Steerval = 0.620, SteerAngle = 35.43, CutAngle = 30.00, err/prev_err = -14

In SpinTurn(CCW, 30.00, 30.00)

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

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output

12901 38 39 39 123 819 819 132 13328 0.00 -0.07 0.00 21.30 0.00

13001 38 40 40 819 819 819 133 13328 0.00 -0.21 0.00 63.00 0.00

13098 39 40 39 114 128 819 127 13328 0.00 0.03 0.00 -9.00 0.00

13199 38 38 38 111 819 819 122 13328 0.00 0.01 0.00 -2.40 0.00

13299 38 39 38 111 819 819 118 13328 0.00 -0.03 0.00 9.30 0.00

13398 38 37 38 81 819 819 113 13328 0.00 0.04 0.00 -11.10 0.00

13501 38 37 39 121 819 819 109 13328 0.00 -0.07 0.00 21.90 0.00

13599 37 38 38 112 819 819 104 13328 0.00 -0.05 0.00 15.60 0.00

13699 37 39 38 105 819 819 97 13328 0.00 -0.04 0.00 12.30 0.00

13798 38 39 38 105 819 819 92 13328 0.00 -0.06 0.00 18.30 0.00

13900 37 38 38 108 819 819 86 13328 0.00 -0.10 0.00 30.60 0.00

14001 38 39 37 107 819 819 83 13328 0.00 0.18 0.00 -53.70 0.00

14100 39 37 39 113 819 819 79 13328 0.00 0.06 0.00 -17.10 0.00

14201 37 40 39 124 819 819 76 13328 0.00 -0.21 0.00 63.00 0.00

14299 39 40 39 112 819 819 73 13328 0.00 -0.01 0.00 3.00 0.00

14400 40 41 38 111 819 819 67 13328 0.00 0.25 0.00 -75.30 0.00

14498 41 41 39 124 819 819 62 13328 0.00 0.19 0.00 -57.30 0.00

14599 41 41 41 104 819 819 57 13328 0.00 0.00 0.00 -0.30 0.00

14699 41 40 41 819 819 819 51 13328 0.00 -0.04 0.00 12.00 0.00

14798 41 41 41 124 819 819 46 13328 0.00 -0.07 0.00 20.70 0.00

14899 38 41 41 119 819 819 39 13328 0.00 -0.12 0.00 35.70 0.00

14998 39 39 39 819 819 819 33 13328 0.00 -0.05 0.00 15.30 0.00

CheckForErrorCondx returning error code OBSTACLE_AHEAD

15098 38 39 38 819 819 819 29 13328 0.00 -0.03 0.00 9.30 0.00

In HandleAnomalousConditions with 3 error code detected

OBSTACLE_AHEAD case detected

In BackupAndTurn90Deg()with front distance = 14

MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0 14

Msec Dist SetD speed

16323 13.00 40 -56

16520 18.00 40 -48

16721 23.00 40 -40

16920 28.00 40 -31

17121 33.00 40 -22

Stopped with front dist = 38

In SpinTurn(CW, 90.00, 45.00)

BackupAndTurn90Deg finished

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

In TrackLeftWallOffset with LR/LC/LF = 39 40 38

At 40 with desired offset = 40: close enough - just parallel and go

In RotateToParallelOrientation(Left)

Just before while() loop: steerval/prv_val = -0.03 -0.03

Msec Steer Set lastErr lastIn lastI Kp*err Ki*err Kd*Din Output Lspeed Rspeed

Parallel Orientation Achieved at 18285 with SteeringVal = -0.03 & bIRBeamAvail = FALSE

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

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output

18413 40 38 39 118 124 819 143 13328 0.00 0.14 0.00 -41.40 0.00

18514 41 39 39 123 819 819 141 13328 0.00 0.20 0.00 -59.70 0.00

18616 39 40 38 819 819 819 138 13328 0.00 0.10 0.00 -30.00 0.00

18711 39 39 39 115 819 819 136 13328 0.00 -0.07 0.00 21.30 0.00

18812 40 40 40 120 819 819 133 13328 0.00 0.04 0.00 -12.00 0.00

18915 38 39 41 105 819 819 127 13328 0.00 -0.23 0.00 69.30 0.00

19014 38 37 38 108 819 819 122 13328 0.00 -0.06 0.00 18.90 0.00

19112 38 37 38 105 819 819 118 13328 0.00 0.04 0.00 -11.10 0.00

19211 37 36 38 119 819 819 116 13328 0.00 -0.04 0.00 13.20 0.00

19316 38 37 37 111 819 819 108 13328 0.00 0.10 0.00 -29.10 0.00

19414 38 39 37 114 819 819 104 13328 0.00 0.09 0.00 -26.70 0.00

19510 38 39 37 819 819 819 99 13328 0.00 0.07 0.00 -20.70 0.00

19613 38 38 37 115 819 819 93 13328 0.00 0.13 0.00 -38.40 0.00

19711 39 39 38 127 819 819 89 13328 0.00 0.09 0.00 -26.70 0.00

19814 38 39 39 106 819 819 85 13328 0.00 -0.06 0.00 18.30 0.00

19911 37 38 38 819 819 819 81 13328 0.00 -0.05 0.00 15.60 0.00

20013 38 38 38 117 819 819 77 13328 0.00 0.02 0.00 -5.40 0.00

20112 37 38 37 819 819 819 75 13328 0.00 0.01 0.00 -2.40 0.00

20213 36 38 36 116 819 819 71 13328 0.00 -0.02 0.00 6.60 0.00

20311 37 37 36 128 819 819 68 13328 0.00 -0.02 0.00 6.90 0.00

20413 37 37 35 126 819 819 63 13328 0.00 0.17 0.00 -50.10 0.00

20511 35 39 36 127 819 819 56 13328 0.00 -0.05 0.00 15.30 0.00

20614 36 38 36 125 819 819 50 13328 0.00 0.01 0.00 -2.40 0.00

20713 35 37 36 819 819 819 46 13328 0.00 -0.04 0.00 12.90 0.00

20811 36 37 36 128 819 819 41 13328 0.00 -0.06 0.00 18.90 0.00

20910 35 36 36 121 819 819 37 13328 0.00 -0.07 0.00 22.20 0.00

21010 35 35 36 127 819 819 32 13328 0.00 -0.05 0.00 16.50 0.00

CheckForErrorCondx returning error code OBSTACLE_AHEAD

21111 36 35 36 93 819 819 27 13328 0.00 -0.12 0.00 37.50 0.00

In HandleAnomalousConditions with 3 error code detected

OBSTACLE_AHEAD case detected

In BackupAndTurn90Deg()with front distance = 5

MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0 4

Msec Dist SetD speed

22335 6.00 40 -71

22536 13.00 40 -60

22733 21.00 40 -46

22934 29.00 40 -31

Stopped with front dist = 38

In SpinTurn(CW, 90.00, 45.00)

BackupAndTurn90Deg finished

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

In TrackLeftWallOffset with LR/LC/LF = 41 43 40

At 43 with desired offset = 40: close enough - just parallel and go

In RotateToParallelOrientation(Left)

Just before while() loop: steerval/prv_val = -0.18 -0.18

Msec Steer Set lastErr lastIn lastI Kp*err Ki*err Kd*Din Output Lspeed Rspeed

Parallel Orientation Achieved at 24164 with SteeringVal = -0.18 & bIRBeamAvail = FALSE

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

Msec LF LC LR RF RC RR F Fvar R Rvar Steer Set Output

24290 41 41 40 128 125 819 149 13328 0.00 0.04 0.00 -12.30 0.00

24393 42 42 40 103 819 819 146 13328 0.00 0.17 0.00 -51.60 0.00

24494 40 42 39 114 124 819 145 13328 0.00 0.11 0.00 -33.60 0.00

24591 40 42 40 95 819 819 142 13328 0.00 -0.03 0.00 8.40 0.00

24692 40 41 40 93 819 819 136 13328 0.00 -0.04 0.00 11.70 0.00

24791 40 41 41 91 819 819 133 13328 0.00 -0.09 0.00 26.70 0.00

24891 39 40 40 86 819 819 126 13328 0.00 -0.09 0.00 27.00 0.00

24992 39 39 39 79 819 819 122 13328 0.00 0.04 0.00 -11.70 0.00

25094 38 39 40 88 819 819 118 13328 0.00 -0.11 0.00 33.30 0.00

25190 39 40 39 90 819 819 113 13328 0.00 -0.03 0.00 9.00 0.00

25293 40 39 38 84 819 819 107 13328 0.00 0.14 0.00 -41.70 0.00

25392 38 40 39 76 819 819 103 13328 0.00 -0.06 0.00 18.00 0.00

25493 39 39 39 53 819 819 99 13328 0.00 0.01 0.00 -2.70 0.00

25592 39 40 40 71 819 819 95 13328 0.00 -0.04 0.00 12.00 0.00

25693 39 39 38 65 819 819 90 13328 0.00 0.06 0.00 -17.70 0.00

25792 38 38 39 81 819 819 83 13328 0.00 -0.12 0.00 36.60 0.00

25889 38 38 39 80 819 819 79 13328 0.00 -0.10 0.00 30.60 0.00

25990 38 38 39 83 819 819 74 13328 0.00 -0.09 0.00 27.60 0.00

26089 37 38 37 92 819 819 71 13328 0.00 0.02 0.00 -5.40 0.00

26192 38 38 37 96 819 819 67 13328 0.00 0.05 0.00 -14.40 0.00

26291 38 37 38 819 819 819 62 13328 0.00 0.04 0.00 -11.10 0.00

26392 38 37 37 117 819 819 53 13328 0.00 0.10 0.00 -29.10 0.00

26491 37 36 36 113 819 819 49 13328 0.00 0.11 0.00 -31.80 0.00

26589 37 36 37 109 819 819 47 13328 0.00 -0.07 0.00 22.20 0.00

26690 35 35 37 123 819 819 44 13328 0.00 -0.09 0.00 25.50 0.00

26789 35 35 36 819 819 819 39 13328 0.00 -0.10 0.00 28.50 0.00

26889 36 35 34 118 819 819 33 13328 0.00 0.15 0.00 -43.50 0.00

CheckForErrorCondx returning error code OBSTACLE_AHEAD

26989 35 34 35 819 819 819 28 13328 0.00 0.05 0.00 -16.20 0.00

In HandleAnomalousConditions with 3 error code detected

OBSTACLE_AHEAD case detected

In BackupAndTurn90Deg()with front distance = 7

MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0 6

Msec Dist SetD speed

28211 7.00 40 -69

28409 15.00 40 -55

28611 20.00 40 -47

28813 28.00 40 -33

Stopped with front dist = 37

In SpinTurn(CW, 90.00, 45.00)

BackupAndTurn90Deg finished

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

In TrackLeftWallOffset with LR/LC/LF = 42 41 42

At 41 with desired offset = 40: close enough - just parallel and go

In RotateToParallelOrientation(Left)

Just before while() loop: steerval/prv_val = -0.02 -0.02

Msec Steer Set lastErr lastIn lastI Kp*err Ki*err Kd*Din Output Lspeed Rspeed

Parallel Orientation Achieved at 30049 with SteeringVal = -0.02 & bIRBeamAvail = FALSE

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

Msec LF LC LR 30178 43 42 42 819 819 819 138 13328 0.00 0.04 0.00 -12.60 0.00

30276 41 42 42 92 819 819 139 13328 0.00 -0.08 0.00 23.40 0.00

30375 43 42 43 116 819 819 135 13328 0.00 -0.03 0.00 8.40 0.00

30476 43 42 42 109 819 819 130 13328 0.00 0.12 0.00 -36.60 0.00

30577 42 43 43 121 819 819 123 13328 0.00 0.07 0.00 -21.90 0.00

30678 42 43 43 102 819 819 119 13328 0.00 -0.10 0.00 29.10 0.00

30775 43 43 42 112 819 819 113 13328 0.00 0.04 0.00 -12.90 0.00

30878 43 42 42 91 819 819 109 13328 0.00 0.11 0.00 -33.60 0.00

30975 42 42 42 79 819 819 104 13328 0.00 0.07 0.00 -21.60 0.00

31077 42 42 43 87 127 819 102 13328 0.00 -0.09 0.00 26.40 0.00

31174 41 42 41 94 819 819 96 13328 0.00 0.06 0.00 -18.60 0.00

31275 42 41 41 77 127 819 92 13328 0.00 0.07 0.00 -21.30 0.00

31376 42 41 43 71 127 819 87 13328 0.00 -0.07 0.00 20.70 0.00

31476 42 42 41 73 819 819 82 13328 0.00 0.13 0.00 -39.60 0.00

31578 41 41 40 89 819 819 76 13328 0.00 0.03 0.00 -9.30 0.00

31677 41 41 41 92 819 819 73 13328 0.00 0.00 0.00 -0.30 0.00

31776 41 40 40 84 819 819 67 13328 0.00 0.03 0.00 -9.00 0.00

31876 41 40 40 112 819 819 63 13328 0.00 0.13 0.00 -39.00 0.00

31975 40 39 40 819 819 819 57 13328 0.00 0.02 0.00 -5.70 0.00

32074 40 40 39 103 819 819 52 13328 0.00 0.13 0.00 -39.00 0.00

32175 38 39 39 819 819 819 47 13328 0.00 -0.01 0.00 3.30 0.00

32277 39 38 39 819 819 819 42 13328 0.00 -0.05 0.00 15.60 0.00

32373 38 37 40 121 819 819 35 13328 0.00 -0.14 0.00 42.90 0.00

32474 38 38 36 98 819 819 31 13328 0.00 0.16 0.00 -47.40 0.00

CheckForErrorCondx returning error code OBSTACLE_AHEAD

32574 37 37 38 115 819 819 26 13328 0.00 -0.10 0.00 30.90 0.00

In HandleAnomalousConditions with 3 error code detected

OBSTACLE_AHEAD case detected

In BackupAndTurn90Deg()with front distance = 7

MoveToDesiredFrontDistCm: Kp/Ki/Kd/dist = -2.0/-0.1/0.0 6

Msec Dist SetD speed

I received c on BT Serial1

ENTERING COMMAND MODE:

0 = 180 deg CCW Turn

1 = 180 deg CW Turn

A = Back to Auto Mode

S = Stop

F = Forward

R = Reverse

Faster

Left 4 5 6 Right

Slower

From the telemetry, it takes about 8 sec for all sensor hardware initialization. After that the left/right/front/rear distance arrays are initialized, and the initial front/rear variances are calculated. All this is summarized on line 26-27.

First leg: 12.866 – 15sec (3-5 sec in video)

Left-side tracking starts on line 34. First (line 37) the robot turns to its initial offset capture heading, moves to the desired offset distance (not shown) and then turns back to parallel the wall (line 39). Actual tracking starts on line 43 at 12.866 sec elapsed time (this corresponds to about 3 sec into the video)

At line 65 (about 15 sec elapsed time) the robot ‘sees’ the upcoming wall, stops and then backs up to 38cm (16.4 – 17.3 sec, 5-6 sec in video). At line 78 it makes a 90deg CW turn to line up with the current wall section, and then navigates down the wall (18.6-21.3sec, 8-11sec in video)

Second leg: 18.6 – 21.3sec (8-11 sec in video)

The robot ‘sees’ the upcoming wall on line 119 (21.010 sec, 11sec in video), backs up (lines 123-129, 11-12 sec in video) and makes another 90deg CW turn to follow the 3rd wall

The third and 4th legs are very similar to the first two, with the robot ending back where it started at 32.574 sec (23 sec on video), ‘seeing’ the upcoming wall at line 221. At this point I transmitted the ‘C’ character over the wireless link to enter manual control to terminate the run.

Summary:

This 4-leg run was pretty much perfect. I adjusted the MIN_FRONT_OBSTACLE_DIST_CM from 20 to 30cm, and this stopped the robot from banging its head against the walls – yay! Also, the telemetry readout changes made for a much more understandable output. I was happy to see that the front variance stayed well above 10,000 the whole time, but unhappy to see that the rear variance was essentially zero the entire time. The low rear variance is due to the fact that the rear VL53L0X sensor range is only about 100cm, and after that it always reports ‘819’. This is not a real problem – it just means that I can’t use the rear variance number to detect a ‘rear stuck’ condition unless it happens within a meter or so from a wall. Hmm, maybe I could use the information from both the front & rear variance numbers to create a more robust detection system.

Stay tuned,

Frank

Wall-E3 Replacing Mega 2560 With Teensy 3.5 Part VIII

Leave a reply

Posted 19 February 2022,

At this point in the evolution of Wall-E3, all the hardware seems to be working, so it’s time to get serious about wall tracking. Last fall I made another run at wall tracking with Wall-E2, and wound up with an algorithm that would first capture the desired wall offset, and then track it ‘forever’. This worked great, but the approach is at odds with the general processing architecture. The current tracking architecture is set up as a loop, where all pertinent parameters are updated every loop period, and the appropriate action is taken. In the case of wall tracking, the ‘action’ was one left/right motor speed update. This allows rapid recognition of, and adaptation to, various ‘error’ conditions, like being stuck or about to run into something. The algorithm developed last fall does none of this, so it can’t react properly (or at all, for that matter) to things like an upcoming wall.

I’m starting to think I can use a hybrid approach – use the current capture/tracking algorithm pretty much as it stands from last fall, but have it check for ‘error’ conditions each time through its own internal loop. If any unusual conditions are detected, then force an exit from the tracking routine and another pass through the main ‘traffic director’ function ‘GetOpMode()’. The updated mode assignment will then percolate back down through loop() and cause the appropriate handling function to be called.

22 February 2022 Update:

As a start, I ported the ‘TrackLeft/RightWallOffset() functions to Wall-E3 and, after the normal number of screwups and mistakes, I got the left side tracking algorithm working, as shown in the Excel plot and short movie clip below:

Here’s the complete code for ‘TrackLeftWallOffset()’:

void TrackRightWallOffset(float kp, float ki, float kd, float offsetCm)
{
  //Notes:
  //	06/21/21 modified to do a turn, then straight, then track 0 steer val
  //	09/12/21 updated from TrackLeftWallOffset
  //  02/22/22 cleaned up

  mSecSinceLastWallTrackUpdate = 0;
  float lastError = 0;
  float lastInput = 0;
  float lastIval = 0;
  float lastDerror = 0;
  float spinRateDPS = 30;

  GetRequestedVL53l0xValues(VL53L0X_RIGHT); //update glRightSteeringVal = (Front - Rear)/100

  //09/12/21 ported from TrackLeftWallOffset
  myTeePrint.printf("\nIn TrackRightWallOffset with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);

  //06/21/21 modified to do a turn, then straight, then track 0 steer val
  float cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(glLidar_RightCenter / 10.f);//positive inside, negative outside desired offset
  myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, CutAngle = %2.2f\n",
    glLidar_RightRear, glLidar_RightCenter, glLidar_RightFront, glRightSteeringVal, cutAngleDeg);

  //07/05/21 implement min cut angle
  if (cutAngleDeg < 0 && abs(cutAngleDeg) < WALL_OFFSET_APPR_ANGLE_MINDEG)
  {
    myTeePrint.printf("(cutAngleDeg < 0 && abs(cutAngleDeg) < %d -- setting to -%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);
    cutAngleDeg = -WALL_OFFSET_APPR_ANGLE_MINDEG;
  }
  else if (cutAngleDeg > 0 && cutAngleDeg < WALL_OFFSET_APPR_ANGLE_MINDEG)
  {
    myTeePrint.printf("(cutAngleDeg > 0 && abs(cutAngleDeg) < %d -- setting to +%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);
    cutAngleDeg = WALL_OFFSET_APPR_ANGLE_MINDEG;
  }

  //get the steering angle from the steering value, for comparison to the computed cut angle
  //positive value means robot is oriented away from wall
  float SteerAngleDeg = GetSteeringAngle(glRightSteeringVal);

  myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f\n",
    glLidar_RightRear, glLidar_RightCenter, glLidar_RightFront, glRightSteeringVal, SteerAngleDeg, cutAngleDeg);

  //now decide which way to turn.
  //double adjCutAngleDeg = 0;
  uint16_t fudgeFactorMM = 0; //07/05/21 added so can change for inside vs outside starting condx
  if (cutAngleDeg > 0) //robot inside desired offset distance
  {
    if (SteerAngleDeg > cutAngleDeg)  // --> implies SteerAngleDeg also > 0
    {
      //robot pointed away from wall more than it should
      myTeePrint.printf("CutAngDeg > 0, SteerAngleDeg > cutAngleDeg\n");
      SpinTurn(false, SteerAngleDeg - cutAngleDeg, spinRateDPS); //turn back toward wall to achieve cutAngle
    }
    else if (SteerAngleDeg < cutAngleDeg)// --> SteerAngleDeg could be < 0
    {
      //robot not turned far enough away from wall (could actually be turned toward wall if SteerAngDeg < 0)
      myTeePrint.printf("CutAngDeg > 0, SteerAngleDeg < cutAngleDeg\n");
      if (SteerAngleDeg <= 0) //cutAngleDeg > 0, SteerAngleDeg <= 0
      {
        //robot turned toward wall
        myTeePrint.printf("CutAngDeg > 0, SteerAngleDeg < cutAngleDeg, SteerAngleDeg <= 0\n");
        SpinTurn(true, cutAngleDeg + SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg
      }
      else //SteerAngleDeg < cutAngleDeg but still > 0
      {
        //robot turned away from wall, but not far enough
        myTeePrint.printf("CutAngDeg > 0,  0 < SteerAngleDeg < cutAngleDeg\n");
        SpinTurn(true, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg
      }
    }

    fudgeFactorMM = -100; //07/05/21 don't need fudge factor for inside start condx
  }
  else // cutAngleDeg < 0 --> robot outside desired offset distance
  {
    if (SteerAngleDeg < cutAngleDeg)  // --> SteerAngleDeg must also be < 0
    {
      //robot turned too far toward wall
      myTeePrint.printf("CutAngDeg < 0, SteerAngleDeg < cutAngleDeg\n");
      SpinTurn(true, SteerAngleDeg - cutAngleDeg, spinRateDPS); //turn away from wall to achieve cutAngleDeg
    }
    else if (SteerAngleDeg >= cutAngleDeg)// --> SteerAngleDeg could be >=0
    {
      if (SteerAngleDeg >= 0) //cutAngleDeg < 0, SteerAngleDeg >= 0
      {
        //robot turned to far away from wall
        myTeePrint.printf("CutAngDeg < 0, SteerAngleDeg <= 0\n");
        SpinTurn(false, cutAngleDeg + SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg
      }
      else //SteerAngleDeg > cutAngleDeg but still < 0
      {
        //robot turned toward wall, but not enough
        myTeePrint.printf("CutAngDeg < 0, 0 < SteerAngleDeg < cutAngleDeg\n");
        SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg
      }
    }

    fudgeFactorMM = 100; //07/05/21 need fudge factor for outside start condx
  }

  delay(1000);

  //adjust so offset capture occurs at correct perpendicular offset
  float adjfactor = cos(PI * cutAngleDeg / 180.f);
  int adjOffsetMM = (int)(10 * (float)WALL_OFFSET_TGTDIST_CM / adjfactor);
  adjOffsetMM += fudgeFactorMM; //fudge factor for distance measurements lagging behind robot's travel. 
  myTeePrint.printf("\nat approach start: cut angle = %2.3f, adjfactor = %2.3f, tgt dist = %d\n", cutAngleDeg, adjfactor, adjOffsetMM);

  GetRequestedVL53l0xValues(VL53L0X_ALL); //added 09/05/21

  long int err = glLidar_RightFront - adjOffsetMM; //neg for inside going out, pos for outside going in
  long int prev_err = err;
  myTeePrint.printf("At start - err = prev_err = %ld\n", err, prev_err);

  SteerAngleDeg = GetSteeringAngle(glRightSteeringVal);
  myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f, err/prev_err = %li\n",
    glLidar_RightRear, glLidar_RightCenter, glLidar_RightFront, glRightSteeringVal, SteerAngleDeg, cutAngleDeg, err);

  myTeePrint.printf("Msec\tRFront\tRCtr\tRRear\tFront\tRear\tErr\tP_err\n");

  //09/05/21 rev to doublecheck robot position - might not need the forward motion section
  while (((cutAngleDeg > 0 && err < 0) || (cutAngleDeg <= 0 && err >= 0)) && prev_err * err > 0) //sign change makes this < 0
  {
    prev_err = err;

    MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);
    delay(100);
    glFrontdistval = GetFrontDistCm();
    GetRequestedVL53l0xValues(VL53L0X_RIGHT);
    err = glLidar_RightFront - adjOffsetMM; //06/29/21 now adj for slant dist vs perp dist
    myTeePrint.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%ld\t%ld\n",
      millis(),
      glLidar_RightFront, glLidar_RightCenter, glLidar_RightRear, glFrontdistval, glLidar_Rear, err, prev_err);
    CheckForUserInput();
  }

  //now turn back the same (unadjusted) amount
  myTeePrint.printf("At end of offset capture - prev_res*res = %ld\n", prev_err * err);
  myTeePrint.printf("correct back to parallel\n");
  SpinTurn((cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg

  WallTrackSetPoint = 0; //moved here 6/22/21

  myTeePrint.printf("\nTrackRightWallOffset: Start tracking offset of %2.2fcm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", offsetCm, kp, ki, kd);
  myTeePrint.printf("Msec\tFdir\tCdir\tRdir\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

  mSecSinceLastWallTrackUpdate = 0; //added 09/04/21
  while (true)
  {
    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;

      GetRequestedVL53l0xValues(VL53L0X_RIGHT); //now done in TIMER5 ISR

      //have to weight value by both angle and wall offset
      WallTrackSteerVal = glRightSteeringVal + (glLidar_RightCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 250.f;

      //update motor speeds, skipping bad values
      if (!isnan(WallTrackSteerVal))
      {
        WallTrackOutput = PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror,
          kp, ki, kd);

        int glLeftspeednum = MOTOR_SPEED_QTR - WallTrackOutput;
        int glRightspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

        glRightspeednum = (glRightspeednum <= MOTOR_SPEED_FULL) ? glRightspeednum : MOTOR_SPEED_FULL;
        glRightspeednum = (glRightspeednum > 0) ? glRightspeednum : 0;
        glLeftspeednum = (glLeftspeednum <= MOTOR_SPEED_FULL) ? glLeftspeednum : MOTOR_SPEED_FULL;
        glLeftspeednum = (glLeftspeednum > 0) ? glLeftspeednum : 0;

        MoveAhead(glLeftspeednum, glRightspeednum);

        myTeePrint.printf("%lu \t%d\t%d\t%d \t%2.2f\t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%d\t%d\n",
          millis(),
          glLidar_RightFront, glLidar_RightCenter, glLidar_RightRear,
          WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,
          kp * lastError, ki * lastIval, kd * lastDerror,
          WallTrackOutput, glLeftspeednum, glRightspeednum);
      }
    }
  }
}

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

void TrackRightWallOffset(float kp, float ki, float kd, float offsetCm)

{

//Notes:

// 06/21/21 modified to do a turn, then straight, then track 0 steer val

// 09/12/21 updated from TrackLeftWallOffset

// 02/22/22 cleaned up

mSecSinceLastWallTrackUpdate = 0;

float lastError = 0;

float lastInput = 0;

float lastIval = 0;

float lastDerror = 0;

float spinRateDPS = 30;

GetRequestedVL53l0xValues(VL53L0X_RIGHT); //update glRightSteeringVal = (Front - Rear)/100

//09/12/21 ported from TrackLeftWallOffset

myTeePrint.printf("\nIn TrackRightWallOffset with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", kp, ki, kd);

//06/21/21 modified to do a turn, then straight, then track 0 steer val

float cutAngleDeg = WALL_OFFSET_TGTDIST_CM - (int)(glLidar_RightCenter / 10.f);//positive inside, negative outside desired offset

myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, CutAngle = %2.2f\n",

glLidar_RightRear, glLidar_RightCenter, glLidar_RightFront, glRightSteeringVal, cutAngleDeg);

//07/05/21 implement min cut angle

if (cutAngleDeg < 0 && abs(cutAngleDeg) < WALL_OFFSET_APPR_ANGLE_MINDEG)

{

myTeePrint.printf("(cutAngleDeg < 0 && abs(cutAngleDeg) < %d -- setting to -%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);

cutAngleDeg = -WALL_OFFSET_APPR_ANGLE_MINDEG;

}

else if (cutAngleDeg > 0 && cutAngleDeg < WALL_OFFSET_APPR_ANGLE_MINDEG)

{

myTeePrint.printf("(cutAngleDeg > 0 && abs(cutAngleDeg) < %d -- setting to +%d\n", WALL_OFFSET_APPR_ANGLE_MINDEG, WALL_OFFSET_APPR_ANGLE_MINDEG);

cutAngleDeg = WALL_OFFSET_APPR_ANGLE_MINDEG;

}

//get the steering angle from the steering value, for comparison to the computed cut angle

//positive value means robot is oriented away from wall

float SteerAngleDeg = GetSteeringAngle(glRightSteeringVal);

myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f\n",

glLidar_RightRear, glLidar_RightCenter, glLidar_RightFront, glRightSteeringVal, SteerAngleDeg, cutAngleDeg);

//now decide which way to turn.

//double adjCutAngleDeg = 0;

uint16_t fudgeFactorMM = 0; //07/05/21 added so can change for inside vs outside starting condx

if (cutAngleDeg > 0) //robot inside desired offset distance

{

if (SteerAngleDeg > cutAngleDeg) // --> implies SteerAngleDeg also > 0

{

//robot pointed away from wall more than it should

myTeePrint.printf("CutAngDeg > 0, SteerAngleDeg > cutAngleDeg\n");

SpinTurn(false, SteerAngleDeg - cutAngleDeg, spinRateDPS); //turn back toward wall to achieve cutAngle

}

else if (SteerAngleDeg < cutAngleDeg)// --> SteerAngleDeg could be < 0

{

//robot not turned far enough away from wall (could actually be turned toward wall if SteerAngDeg < 0)

myTeePrint.printf("CutAngDeg > 0, SteerAngleDeg < cutAngleDeg\n");

if (SteerAngleDeg <= 0) //cutAngleDeg > 0, SteerAngleDeg <= 0

{

//robot turned toward wall

myTeePrint.printf("CutAngDeg > 0, SteerAngleDeg < cutAngleDeg, SteerAngleDeg <= 0\n");

SpinTurn(true, cutAngleDeg + SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg

}

else //SteerAngleDeg < cutAngleDeg but still > 0

{

//robot turned away from wall, but not far enough

myTeePrint.printf("CutAngDeg > 0, 0 < SteerAngleDeg < cutAngleDeg\n");

SpinTurn(true, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg

}

fudgeFactorMM = -100; //07/05/21 don't need fudge factor for inside start condx

}

else // cutAngleDeg < 0 --> robot outside desired offset distance

{

if (SteerAngleDeg < cutAngleDeg) // --> SteerAngleDeg must also be < 0

{

//robot turned too far toward wall

myTeePrint.printf("CutAngDeg < 0, SteerAngleDeg < cutAngleDeg\n");

SpinTurn(true, SteerAngleDeg - cutAngleDeg, spinRateDPS); //turn away from wall to achieve cutAngleDeg

}

else if (SteerAngleDeg >= cutAngleDeg)// --> SteerAngleDeg could be >=0

{

if (SteerAngleDeg >= 0) //cutAngleDeg < 0, SteerAngleDeg >= 0

{

//robot turned to far away from wall

myTeePrint.printf("CutAngDeg < 0, SteerAngleDeg <= 0\n");

SpinTurn(false, cutAngleDeg + SteerAngleDeg, spinRateDPS); //null out SteerAngleDeg & then turn addnl cutAngleDeg

}

else //SteerAngleDeg > cutAngleDeg but still < 0

{

//robot turned toward wall, but not enough

myTeePrint.printf("CutAngDeg < 0, 0 < SteerAngleDeg < cutAngleDeg\n");

SpinTurn(false, cutAngleDeg - SteerAngleDeg, spinRateDPS); //turn diff between SteerAngleDeg & cutAngleDeg

}

fudgeFactorMM = 100; //07/05/21 need fudge factor for outside start condx

}

delay(1000);

//adjust so offset capture occurs at correct perpendicular offset

float adjfactor = cos(PI * cutAngleDeg / 180.f);

int adjOffsetMM = (int)(10 * (float)WALL_OFFSET_TGTDIST_CM / adjfactor);

adjOffsetMM += fudgeFactorMM; //fudge factor for distance measurements lagging behind robot's travel.

myTeePrint.printf("\nat approach start: cut angle = %2.3f, adjfactor = %2.3f, tgt dist = %d\n", cutAngleDeg, adjfactor, adjOffsetMM);

GetRequestedVL53l0xValues(VL53L0X_ALL); //added 09/05/21

long int err = glLidar_RightFront - adjOffsetMM; //neg for inside going out, pos for outside going in

long int prev_err = err;

myTeePrint.printf("At start - err = prev_err = %ld\n", err, prev_err);

SteerAngleDeg = GetSteeringAngle(glRightSteeringVal);

myTeePrint.printf("R/C/F dists = %d\t%d\t%d Steerval = %2.3f, SteerAngle = %2.2f, CutAngle = %2.2f, err/prev_err = %li\n",

glLidar_RightRear, glLidar_RightCenter, glLidar_RightFront, glRightSteeringVal, SteerAngleDeg, cutAngleDeg, err);

myTeePrint.printf("Msec\tRFront\tRCtr\tRRear\tFront\tRear\tErr\tP_err\n");

//09/05/21 rev to doublecheck robot position - might not need the forward motion section

while (((cutAngleDeg > 0 && err < 0) || (cutAngleDeg <= 0 && err >= 0)) && prev_err * err > 0) //sign change makes this < 0

{

prev_err = err;

MoveAhead(MOTOR_SPEED_QTR, MOTOR_SPEED_QTR);

delay(100);

glFrontdistval = GetFrontDistCm();

GetRequestedVL53l0xValues(VL53L0X_RIGHT);

err = glLidar_RightFront - adjOffsetMM; //06/29/21 now adj for slant dist vs perp dist

myTeePrint.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%ld\t%ld\n",

millis(),

glLidar_RightFront, glLidar_RightCenter, glLidar_RightRear, glFrontdistval, glLidar_Rear, err, prev_err);

CheckForUserInput();

}

//now turn back the same (unadjusted) amount

myTeePrint.printf("At end of offset capture - prev_res*res = %ld\n", prev_err * err);

myTeePrint.printf("correct back to parallel\n");

SpinTurn((cutAngleDeg < 0), abs(cutAngleDeg), spinRateDPS); //have to use abs() here, as cutAngleDeg can be neg

WallTrackSetPoint = 0; //moved here 6/22/21

myTeePrint.printf("\nTrackRightWallOffset: Start tracking offset of %2.2fcm with Kp/Ki/Kd = %2.2f\t%2.2f\t%2.2f\n", offsetCm, kp, ki, kd);

myTeePrint.printf("Msec\tFdir\tCdir\tRdir\tSteer\tSet\terror\tIval\tKp*err\tKi*Ival\tKd*Din\tOutput\tLspd\tRspd\n");

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

while (true)

{

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;

GetRequestedVL53l0xValues(VL53L0X_RIGHT); //now done in TIMER5 ISR

//have to weight value by both angle and wall offset

WallTrackSteerVal = glRightSteeringVal + (glLidar_RightCenter - 10 * WALL_OFFSET_TGTDIST_CM) / 250.f;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

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

kp, ki, kd);

int glLeftspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

int glRightspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

glRightspeednum = (glRightspeednum <= MOTOR_SPEED_FULL) ? glRightspeednum : MOTOR_SPEED_FULL;

glRightspeednum = (glRightspeednum > 0) ? glRightspeednum : 0;

glLeftspeednum = (glLeftspeednum <= MOTOR_SPEED_FULL) ? glLeftspeednum : MOTOR_SPEED_FULL;

glLeftspeednum = (glLeftspeednum > 0) ? glLeftspeednum : 0;

MoveAhead(glLeftspeednum, glRightspeednum);

myTeePrint.printf("%lu \t%d\t%d\t%d \t%2.2f\t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%d\t%d\n",

millis(),

glLidar_RightFront, glLidar_RightCenter, glLidar_RightRear,

WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,

kp * lastError, ki * lastIval, kd * lastDerror,

WallTrackOutput, glLeftspeednum, glRightspeednum);

}

The above algorithm works great, but it runs in an infinite loop once it has captured the wall offset. Based on my above comments about a hybrid approach, I could add tests for stuck and/or front or back obstacles to this loop (instead of the current ‘while(true)’). This would cause TrackLeftWallOffset() to exit, and the GetOpMode() function could assign the appropriate mode, which would then cause the proper function to execute.

Or, I could eliminate GetOpMode() entirely and put it’s logic in ‘loop()’? Actually, looking at the loop() function in FourWD_WallE2_V12.ino, my last iteration with the Arduino Mega2560, I see that GetOpMode() is called at the start of loop(), and then the OpMode switch statement comes pretty much immediately afterwards. Here’s the code:

void loop()
{
	CheckForUserInput();  //allows remote manual robot control

	//03/08/17 Revised to introduce three top-level op modes (wall-follow, IR home, charging)
	//Purpose:  Follow nearest wall, or home on IR beam, or monitor charging as necessary
	//Plan:
	//	Step1: Determine current operating mode
	//	Step2: Switch to appropriate operating case block
	//	Step3: Check for FRAM and/or MPU interrupt flags
	//Notes:
	//	03/19/17 IRHomeToChgStn sig rev to include init l/r speeds in IRHOMING case block
	//	09/19/18 added section to write telemetry to FRAM
	//	08/15/20 new plan: use 'bTimeForNavUpdate' set to true in Timer ISR, cleared here

	//	08/15/20 new plan: use 'bTimeForNavUpdate' set to true in Timer1 ISR, cleared here
	if (bTimeForNavUpdate)
	{
		//mySerial.printf("%lu: Top of Loop()\n", millis());
		bTimeForNavUpdate = false; //set in Timer1 ISR

		digitalWrite(RED_LASER_DIODE_PIN, HIGH); //fire the laser pointer

		//03/10/19 rev so NO_STUCK #define can prevent 'stuck' detection while still keeping LIDAR active
#ifndef NO_LIDAR
		//frontdistval = GetFrontDistCm(); //11/02/20 frontdistval updated in ISR
#else
		frontdistval = MAX_FRONT_DISTANCE_CM; //05/03/17 bugfix
#endif //NO_LIDAR

		UpdateLRDistanceArrays(Lidar_LeftCenter, Lidar_RightCenter);
		float batVoltage = GetBattVoltage(); //04/28/19 mvd here so can be used in GetOpMode()

		CurrentOpMode = (OpModes)GetOpMode(batVoltage); //04/28/19 bat volts added to sig

	//Step1: Determine current operating mode
#pragma region OP_MODE SWITCH

		//Step2: Switch to appropriate operating case block
		String trackstr = "Unknown"; //used for telemetry printouts

		switch (CurrentOpMode)
		{
#pragma region MODE_CHARGING
		case MODE_CHARGING:
			digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer
			chgStartMsec = millis(); //used for charge duration telemetry
			Serial.print(F("Starting Charge Mode at ")); Serial.println(chgStartMsec);
			StopBothMotors(); //added 03/13/17

			//04/27/21
			mySerial.printf("Reset VL53L0X Teensy to reduce current consumption\n");
			digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW); //hold Teensy reset to reduce current consumption
			MonitorChargeUntilDone();
			mySerial.printf("Restart VL53L0X Teensy...\n");
			digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH); //restart Teensy
			WaitForVL53L0XTeensy(); //wait for it to come back up.
			ExecDisconManeuver();
			PrevOpMode = MODE_NONE; //04/27/21: this will force wall offset recapture
			//mySerial.printf("%lu: just after ExecDisconManeuver() in TRACKING_RIGHT case block\n", millis());
			break;
#pragma endregion MODE_CHARGING

#pragma region MODE_IRHOMING
		case MODE_IRHOMING:
			digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer
			mySerial.printf("IR Beam Detected with Battery Voltage = %1.2f\n", batVoltage);
			//PrevOpMode = MODE_IRHOMING;

			if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry
			{
				Serial.println("Not Hungry - Avoiding Charger");

				//04/27/21 too complicated - just have robot reverse course
				int leftdist = GetAvgLeftDistCm();
				int rightdist = GetAvgRightDistCm();
				SpinTurn(leftdist > rightdist, 180);
				PrevOpMode = MODE_NONE;
			}
			else //hungry - try to connect
			{
				Serial.println("Low battery - homing to Charger");
				IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
			}
			break;
#pragma endregion IR Homing Case

#pragma region MODE_WALLFOLLOW
		case MODE_WALLFOLLOW:
			//Purpose:  Follow nearest wall, responding to obstacles as necessary
			//Provenance: G. Frank Paynter and Danny Frank 12/20/2015
			//Inputs:  Distances from left/right/front sensors
			//Outputs: updated left/right motor speeds in wall-follow case, or appropriate avoidance maneuver
			//Plan:
			//	Step 1: Update motor speeds & NAV mode based on current TRACK/NAV states and front distance
			//	Step 2: Print out telemetry info
			//Notes:
			//	03/30/14 Step1 code moved to top of loop() - compiler directives cause errors here
			//	09/11/18 revised plan to reflect reality
			//	04/11/20 revised to incorporate heading-based wall offset tracking

			// Update motor speeds & NAV mode based on current TRACK/NAV states and front distance
			// See https://fpaynter.com/2016/01/making-wall-e2-smarter-using-karnaugh-maps/ for details

			//11/13/20 rev to use a sliding obstacle detection range during tracking ops
			//detection range is WALL_OFFSET_TGTDIST_CM/2 at max cut, increasing to WALL_OFFSET_TGTDIST_CM with zero cut angle
			float adjustedFrontObstacleDetectionRangeCm = (WallTrackSetPoint <= 0) ?
				WALL_OFFSET_TGTDIST_CM * ((WallTrackSetPoint / (2 * WALL_OFFSET_TRACK_SETPOINT_LIMIT)) + 1) : WALL_OFFSET_TGTDIST_CM;

			TrackingCase = (WallTrackingCases)GetTrackingDir();

			switch (TrackingCase)
			{
			case TRACKING_LEFT:
#pragma region TRACKLEFT
				//mySerial.printf("At top of TRACKING_LEFT case block\n");
				//07/22/2020 updated to use VL53L0X array vs 'ping' sensors
				trackstr = "Left"; //used for telemetry printouts

				//10/08/20 hack to prevent immediate obstacle detection
				if (PrevOpMode != MODE_WALLFOLLOW) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

				if (bIsStuck)//bIsStuck updated in timer1 ISR
				{
					mySerial.printf("bIsStuck TRUE in TRACKLEFT at %lu with frontdist %d, variance = %6.2f\n",
						millis(), frontdistval, frontvar);
					InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

					ExecuteStuckRecoveryManeuver(TRACKING_LEFT); //10/24/20
					PrevOpMode = MODE_NONE; //this will force wall offset recapture
					WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init
					break; //don't execute the rest - just go back to GetOpMode()
				}

				//11/11/20 added separate section for 'Obstacle Ahead'
				if (bObstacleAhead)//bObstacleAhead updated in timer1 ISR
				{
					mySerial.printf("bObstacleAhead TRUE in TRACKLEFT at %lu with frontdist %d, variance = %6.2f\n",
						millis(), frontdistval, frontvar);
					InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

					//11/13/20 added code to back up to WALL_OFFSET_TGTDIST_CM before beginning the spin turn
					mySerial.printf("backing up to %d using front distance sensor\n", WALL_OFFSET_TGTDIST_CM);

					MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM);

					mySerial.printf("backed up successfully to %d using front distance sensor\n", frontdistval);

					SpinTurn(false, 90);//turn 90 deg right
					PrevOpMode = MODE_NONE; //this will force wall offset recapture
					WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init
					break; //don't execute the rest - just go back to GetOpMode()
				}

				//11/08/20 added section for too close behind
				//the idea is to move ahead to a position about WALL_OFFSET_TGTDIST_CM away from wall, and turn 45 deg
				//one way or other so wall tracking can continue from that point
				else if (bObstacleBehind)
				{
					mySerial.printf("Obstacle behind with reardist, frontdist =  %d, %d cm\n", Lidar_Rear / 10, frontdistval);

					ExecuteRearObstacleRecoveryManeuver(); //doesn't need tracking case info

					PrevOpMode = MODE_NONE;
					WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init
					break;
				}

				//11/13/20 rev to use a sliding obstacle detection range during tracking ops
				//detection range is WALL_OFFSET_TGTDIST_CM/2 at max cut, increasing to WALL_OFFSET_TGTDIST_CM with zero cut angle
				else if (frontdistval < adjustedFrontObstacleDetectionRangeCm)
				{
					//DEBUG!!
					mySerial.printf("Hit adjusted wall offset limit (%2.2f cm) at %lu with frontdist = %d, Setpoint = %2.2f\n",
						adjustedFrontObstacleDetectionRangeCm, millis(), frontdistval, WallTrackSetPoint);
					//DEBUG!!

					//06/06/21 rev to use code from 'if (bObstacleAhead)' block 
					////StepTurn(TRACKING_LEFT);
					//PrevOpMode = MODE_NONE; //this will force wall offset recapture
					//WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

					InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

					//11/13/20 added code to back up to WALL_OFFSET_TGTDIST_CM before beginning the spin turn
					mySerial.printf("backing up to %d using front distance sensor\n", WALL_OFFSET_TGTDIST_CM);

					MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM);

					mySerial.printf("backed up successfully to %d using front distance sensor\n", frontdistval);

					SpinTurn(false, 90);//turn 90 deg right
					PrevOpMode = MODE_NONE; //this will force wall offset recapture
					WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init
					break; //don't execute the rest - just go back to GetOpMode()
				}

				//if we get to here, not stuck and no nearby obstacle issues
				else if (PrevOpMode != MODE_WALLFOLLOW)//02/26/20 first time through capture offset
				{
					PrevOpMode = MODE_WALLFOLLOW; //added 04/27/21

					//04/18/21 experiment with IRHomingNavigateToIAP() here
					RotateToParallelOrientation(TRACKING_LEFT);

					if (bIRBeamAvail)
					{
						IRHomingNavigateToIAP(TRACKING_LEFT);
						if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry
						{
							Serial.println("Not Hungry - Avoiding Charger");
							curMinObstacleDistance = CHG_STN_AVOIDANCE_DIST_CM; //hopefully this will cause avoidance
							IRHomeToChgStn(CHG_STN_AVOIDANCE_DIST_CM, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
						}
						else //hungry - try to connect
						{
							Serial.println("Low battery - homing to Charger");
							IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
							StopBothMotors();
							digitalWrite(RED_LASER_DIODE_PIN, LOW); //turn off the laser
							mySerial.printf("Reset VL53L0X Teensy to reduce current consumption\n");
							digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW); //hold Teensy reset to reduce current consumption
							MonitorChargeUntilDone();
							mySerial.printf("Restart VL53L0X Teensy...\n");
							digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH); //restart Teensy
							WaitForVL53L0XTeensy(); //wait for it to come back up.
							ExecDisconManeuver();
							PrevOpMode = MODE_NONE; //this will force wall offset recapture
							mySerial.printf("%lu: just after ExecDisconManeuver() in TRACKING_LEFT case block\n", millis());
							break;
						}
					}
				}

				//if we get here, we are continuing tracking operation
				PrevOpMode = MODE_WALLFOLLOW; //added 04/27/21
				WallTrackSteerVal = LeftSteeringVal; //computed by Teensy 3.5

				//at 30mm from tgt offset, setpoint will be +/-0.3
				WallTrackSetPoint = (double)(10 * WALL_OFFSET_TGTDIST_CM - Lidar_LeftCenter) / 100.f; //10/04/20 positive value drives robot toward wall

				//limit approach angle to reasonable values
				if (WallTrackSetPoint > WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = WALL_OFFSET_TRACK_SETPOINT_LIMIT;
				if (WallTrackSetPoint < -WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

				//update motor speeds, skipping bad values
				if (!isnan(WallTrackSteerVal))
				{
					//10/12/20 now this executes every time, with interval controlled by timer ISR
					WallTrackPID.Compute();
					leftspeednum = MOTOR_SPEED_QTR - WallTrackOutput;
					rightspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

					rightspeednum = (rightspeednum <= MOTOR_SPEED_FULL) ? rightspeednum : MOTOR_SPEED_FULL;
					rightspeednum = (rightspeednum > 0) ? rightspeednum : 0;
					leftspeednum = (leftspeednum <= MOTOR_SPEED_FULL) ? leftspeednum : MOTOR_SPEED_FULL;
					leftspeednum = (leftspeednum > 0) ? leftspeednum : 0;

					MoveAhead(leftspeednum, rightspeednum);

					mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%8.2f\t%d\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\t%d\t%lu\t%2.3f\n", millis(),
						Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, frontdistval, frontvar, Lidar_Rear, WallTrackSteerVal, WallTrackOutput,
						WallTrackSetPoint, adjustedFrontObstacleDetectionRangeCm, leftspeednum, rightspeednum, IRHomingValTotalAvg, IRHomingLRSteeringVal);
				}
#pragma endregion Tracking Left Wall Case
				//mySerial.printf("At bottom of TRACKING_LEFT case block\n");
				break;
			case TRACKING_RIGHT:
#pragma region TRACKRIGHT
				mySerial.printf("At top of TRACKING_RIGHT case block\n");
				//07/22/2020 updated to use VL53L0X array vs 'ping' sensors
				trackstr = "Right"; //used for telemetry printouts

				//10/08/20 hack to prevent immediate obstacle detection
				if (PrevOpMode != MODE_WALLFOLLOW) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

				if (bIsStuck)//bIsStuck updated in timer1 ISR
				{
					mySerial.printf("bIsStuck TRUE in TRACKRIGHT at %lu with frontdist %d, variance = %6.2f\n",
						millis(), frontdistval, frontvar);
					InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

					ExecuteStuckRecoveryManeuver(TRACKING_RIGHT); //10/24/20
					PrevOpMode = MODE_NONE; //this will force wall offset recapture
					WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init
					break; //don't execute the rest - just go back to GetOpMode()
				}

				//11/11/20 added separate section for 'Obstacle Ahead'
				if (bObstacleAhead)//bObstacleAhead updated in timer1 ISR
				{
					mySerial.printf("bObstacleAhead TRUE in TRACKRIGHT at %lu with frontdist %d, variance = %6.2f\n",
						millis(), frontdistval, frontvar);
					InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

					//11/13/20 added code to back up to WALL_OFFSET_TGTDIST_CM before beginning the spin turn
					mySerial.printf("backing up to %d using front distance sensor\n", WALL_OFFSET_TGTDIST_CM);

					MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM);

					mySerial.printf("backed up successfully to %d using front distance sensor\n", frontdistval);

					SpinTurn(true, 90);//turn 90 deg left
					PrevOpMode = MODE_NONE; //this will force wall offset recapture
					WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

					break; //don't execute the rest - just go back to GetOpMode()
				}

				//11/08/20 added section for too close behind
				//the idea is to move ahead to a position about WALL_OFFSET_TGTDIST_CM away from wall, and turn 45 deg
				//one way or other so wall tracking can continue from that point
				else if (bObstacleBehind)
				{
					mySerial.printf("Obstacle behind with reardist, frontdist =  %d, %d cm\n", Lidar_Rear / 10, frontdistval);

					ExecuteRearObstacleRecoveryManeuver(); //doesn't need tracking case info

					PrevOpMode = MODE_NONE;
					WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init
					break;
				}

				//11/13/20 rev to use a sliding obstacle detection range during tracking ops
				//detection range is WALL_OFFSET_TGTDIST_CM/2 at max cut, increasing to WALL_OFFSET_TGTDIST_CM with zero cut angle
				else if (frontdistval < adjustedFrontObstacleDetectionRangeCm)
				{
					//DEBUG!!
					mySerial.printf("Hit adjusted wall offset limit (%2.2f cm) at %lu with frontdist = %d, Setpoint = %2.2f\n",
						adjustedFrontObstacleDetectionRangeCm, millis(), frontdistval, WallTrackSetPoint);
					//DEBUG!!

					//06/06/21 rev to use code from 'if (bObstacleAhead)' block 
					//StepTurn(TRACKING_RIGHT);
					//PrevOpMode = MODE_NONE; //this will force wall offset recapture
					//WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

					InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

					//11/13/20 added code to back up to WALL_OFFSET_TGTDIST_CM before beginning the spin turn
					mySerial.printf("backing up to %d using front distance sensor\n", WALL_OFFSET_TGTDIST_CM);

					MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM);

					mySerial.printf("backed up successfully to %d using front distance sensor\n", frontdistval);

					SpinTurn(true, 90);//turn 90 deg left
					PrevOpMode = MODE_NONE; //this will force wall offset recapture
					WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init
					break; //don't execute the rest - just go back to GetOpMode()
				}

				//if we get to here, not stuck and no nearby obstacle issues
				else if (PrevOpMode != MODE_WALLFOLLOW)//02/26/20 first time through capture offset
				{
					//04/18/21 experiment with IRHomingNavigateToIAP() here
					RotateToParallelOrientation(TRACKING_RIGHT);
					if (bIRBeamAvail)
					{
						IRHomingNavigateToIAP(TRACKING_RIGHT);
						if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry
						{
							Serial.println("Not Hungry - Avoiding Charger");
							curMinObstacleDistance = CHG_STN_AVOIDANCE_DIST_CM; //hopefully this will cause avoidance
							IRHomeToChgStn(CHG_STN_AVOIDANCE_DIST_CM, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
						}
						else //hungry - try to connect
						{
							Serial.println("Low battery - homing to Charger");
							IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
							StopBothMotors();
							digitalWrite(RED_LASER_DIODE_PIN, LOW); //turn off the laser
							mySerial.printf("Reset VL53L0X Teensy to reduce current consumption\n");
							digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW); //hold Teensy reset to reduce current consumption
							MonitorChargeUntilDone();
							mySerial.printf("Restart VL53L0X Teensy...\n");
							digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH); //restart Teensy
							WaitForVL53L0XTeensy(); //wait for it to come back up.
							ExecDisconManeuver();
							PrevOpMode = MODE_NONE; //04/27/21: this will force wall offset recapture
							mySerial.printf("%lu: just after ExecDisconManeuver() in TRACKING_RIGHT case block\n", millis());
							break;
						}
					}
				}

				mySerial.printf("After else if (PrevOpMode != MODE_WALLFOLLOW) block\n");
				//mySerial.printf("%lu: just after else if (PrevOpMode != MODE_WALLFOLLOW)\n");

				//if we get here, we are continuing tracking operation
				PrevOpMode = MODE_WALLFOLLOW; //added 04/27/21
				WallTrackSteerVal = RightSteeringVal; //computed by Teensy 3.5

				//at 30mm from tgt offset, setpoint will be +/-0.3
				WallTrackSetPoint = (double)(10 * WALL_OFFSET_TGTDIST_CM - Lidar_RightCenter) / 100.f; //10/04/20 positive value drives robot toward wall

				//limit approach angle to reasonable values
				if (WallTrackSetPoint > WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = WALL_OFFSET_TRACK_SETPOINT_LIMIT;
				if (WallTrackSetPoint < -WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

				//update motor speeds, skipping bad values
				if (!isnan(WallTrackSteerVal))
				{
					//10/12/20 now this executes every time, with interval controlled by timer ISR
					WallTrackPID.Compute();
					leftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;
					rightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

					rightspeednum = (rightspeednum <= MOTOR_SPEED_FULL) ? rightspeednum : MOTOR_SPEED_FULL;
					rightspeednum = (rightspeednum > 0) ? rightspeednum : 0;
					leftspeednum = (leftspeednum <= MOTOR_SPEED_FULL) ? leftspeednum : MOTOR_SPEED_FULL;
					leftspeednum = (leftspeednum > 0) ? leftspeednum : 0;

					MoveAhead(leftspeednum, rightspeednum);

					mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%6.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\t%d\t%lu\t%2.3f\n", millis(),
						Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear,
						frontdistval, frontvar, WallTrackSteerVal, WallTrackOutput,
						WallTrackSetPoint, adjustedFrontObstacleDetectionRangeCm, leftspeednum, rightspeednum, IRHomingValTotalAvg, IRHomingLRSteeringVal);
				}
#pragma endregion Tracking Right Wall Case
				//mySerial.printf("End of Tracking Right Wall Case\n");
				break;
			case TRACKING_NEITHER:
#pragma region TRACKNEITHER
				trackstr = "Neither"; //used for telemetry printouts
				mySerial.printf("time(ms) in TRACKNEITHER = %lu\n", millis());

				//added 01/20/16: turn direction arbitrary
				if (bObstacleAhead || bIsStuck) //08/10/20 now bObstacleAhead & bIsStuck updated in timer1 ISR
				{
					if (bIsStuck)
					{
						InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections
					}

					//DEBUG!!
					Serial.println(F("---- Backup & Turn to the Left"));
					//DEBUG!!
					BackupAndTurn90Deg(true, true, MOTOR_SPEED_HALF);

					//go back to normal tracking mode after B&T completes
					NavCase = NAV_WALLTRK;
				}

				//Open Corner case
				else if (frontdistval > STEPTURN_DIST_CM)
				{
					//OK, this is the Open Corner case - turn toward the last-tracked wall

#ifndef NO_MOTORS
					delay(1000); //01/20/16 - delay to give Wall-E2 time to clear the corner a bit
#endif

					if (PrevTrackingCase == TRACKING_LEFT)
					{
						//RotateCWDeg(false, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);
					}
					else if (PrevTrackingCase == TRACKING_RIGHT)
					{
						//RotateCWDeg(true, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);
					}
					else //oops!  not tracking either wall - out in middle of room somewhere?
					{
						//not open corner, but not tracking a wall, either - go straight
						leftspeednum = rightspeednum = MOTOR_SPEED_HALF;
						MoveAhead(leftspeednum, rightspeednum);
					}
				}
#pragma endregion Not Tracking Either Wall
				break;
			default:
				break;
				//mySerial.printf("Bottom of opMode switch\n");
			} //switch (TrackingCase)
#pragma endregion MODE_WALLFOLLOW
			//mySerial.printf("%lu: at end of MODE_WALLFOLLOW\n", millis());

			//07/16/21 added to manage offset capture operation
#pragma region MODE_WALLCAPTURE
		case MODE_WALLCAPTURE: 
			break;
#pragma endregion MODE_WALLCAPTURE
		}


		digitalWrite(RED_LASER_DIODE_PIN, LOW);
		//mySerial.printf("%lu: Bottom of loop()\n", millis());
	}//if (bTimeForNavUpdate)
}//loop()

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

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

void loop()

{

CheckForUserInput(); //allows remote manual robot control

//03/08/17 Revised to introduce three top-level op modes (wall-follow, IR home, charging)

//Purpose: Follow nearest wall, or home on IR beam, or monitor charging as necessary

//Plan:

// Step1: Determine current operating mode

// Step2: Switch to appropriate operating case block

// Step3: Check for FRAM and/or MPU interrupt flags

//Notes:

// 03/19/17 IRHomeToChgStn sig rev to include init l/r speeds in IRHOMING case block

// 09/19/18 added section to write telemetry to FRAM

// 08/15/20 new plan: use 'bTimeForNavUpdate' set to true in Timer ISR, cleared here

// 08/15/20 new plan: use 'bTimeForNavUpdate' set to true in Timer1 ISR, cleared here

if (bTimeForNavUpdate)

{

//mySerial.printf("%lu: Top of Loop()\n", millis());

bTimeForNavUpdate = false; //set in Timer1 ISR

digitalWrite(RED_LASER_DIODE_PIN, HIGH); //fire the laser pointer

//03/10/19 rev so NO_STUCK #define can prevent 'stuck' detection while still keeping LIDAR active

#ifndef NO_LIDAR

//frontdistval = GetFrontDistCm(); //11/02/20 frontdistval updated in ISR

#else

frontdistval = MAX_FRONT_DISTANCE_CM; //05/03/17 bugfix

#endif //NO_LIDAR

UpdateLRDistanceArrays(Lidar_LeftCenter, Lidar_RightCenter);

float batVoltage = GetBattVoltage(); //04/28/19 mvd here so can be used in GetOpMode()

CurrentOpMode = (OpModes)GetOpMode(batVoltage); //04/28/19 bat volts added to sig

//Step1: Determine current operating mode

#pragma region OP_MODE SWITCH

//Step2: Switch to appropriate operating case block

String trackstr = "Unknown"; //used for telemetry printouts

switch (CurrentOpMode)

{

#pragma region MODE_CHARGING

case MODE_CHARGING:

digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer

chgStartMsec = millis(); //used for charge duration telemetry

Serial.print(F("Starting Charge Mode at ")); Serial.println(chgStartMsec);

StopBothMotors(); //added 03/13/17

//04/27/21

mySerial.printf("Reset VL53L0X Teensy to reduce current consumption\n");

digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW); //hold Teensy reset to reduce current consumption

MonitorChargeUntilDone();

mySerial.printf("Restart VL53L0X Teensy...\n");

digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH); //restart Teensy

WaitForVL53L0XTeensy(); //wait for it to come back up.

ExecDisconManeuver();

PrevOpMode = MODE_NONE; //04/27/21: this will force wall offset recapture

//mySerial.printf("%lu: just after ExecDisconManeuver() in TRACKING_RIGHT case block\n", millis());

break;

#pragma endregion MODE_CHARGING

#pragma region MODE_IRHOMING

case MODE_IRHOMING:

digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer

mySerial.printf("IR Beam Detected with Battery Voltage = %1.2f\n", batVoltage);

//PrevOpMode = MODE_IRHOMING;

if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry

{

Serial.println("Not Hungry - Avoiding Charger");

//04/27/21 too complicated - just have robot reverse course

int leftdist = GetAvgLeftDistCm();

int rightdist = GetAvgRightDistCm();

SpinTurn(leftdist > rightdist, 180);

PrevOpMode = MODE_NONE;

}

else //hungry - try to connect

{

Serial.println("Low battery - homing to Charger");

IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

}

break;

#pragma endregion IR Homing Case

#pragma region MODE_WALLFOLLOW

case MODE_WALLFOLLOW:

//Purpose: Follow nearest wall, responding to obstacles as necessary

//Provenance: G. Frank Paynter and Danny Frank 12/20/2015

//Inputs: Distances from left/right/front sensors

//Outputs: updated left/right motor speeds in wall-follow case, or appropriate avoidance maneuver

//Plan:

// Step 1: Update motor speeds & NAV mode based on current TRACK/NAV states and front distance

// Step 2: Print out telemetry info

//Notes:

// 03/30/14 Step1 code moved to top of loop() - compiler directives cause errors here

// 09/11/18 revised plan to reflect reality

// 04/11/20 revised to incorporate heading-based wall offset tracking

// Update motor speeds & NAV mode based on current TRACK/NAV states and front distance

// See https://fpaynter.com/2016/01/making-wall-e2-smarter-using-karnaugh-maps/ for details

//11/13/20 rev to use a sliding obstacle detection range during tracking ops

//detection range is WALL_OFFSET_TGTDIST_CM/2 at max cut, increasing to WALL_OFFSET_TGTDIST_CM with zero cut angle

float adjustedFrontObstacleDetectionRangeCm = (WallTrackSetPoint <= 0) ?

WALL_OFFSET_TGTDIST_CM * ((WallTrackSetPoint / (2 * WALL_OFFSET_TRACK_SETPOINT_LIMIT)) + 1) : WALL_OFFSET_TGTDIST_CM;

TrackingCase = (WallTrackingCases)GetTrackingDir();

switch (TrackingCase)

{

case TRACKING_LEFT:

#pragma region TRACKLEFT

//mySerial.printf("At top of TRACKING_LEFT case block\n");

//07/22/2020 updated to use VL53L0X array vs 'ping' sensors

trackstr = "Left"; //used for telemetry printouts

//10/08/20 hack to prevent immediate obstacle detection

if (PrevOpMode != MODE_WALLFOLLOW) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

if (bIsStuck)//bIsStuck updated in timer1 ISR

{

mySerial.printf("bIsStuck TRUE in TRACKLEFT at %lu with frontdist %d, variance = %6.2f\n",

millis(), frontdistval, frontvar);

InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

ExecuteStuckRecoveryManeuver(TRACKING_LEFT); //10/24/20

PrevOpMode = MODE_NONE; //this will force wall offset recapture

WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

break; //don't execute the rest - just go back to GetOpMode()

}

//11/11/20 added separate section for 'Obstacle Ahead'

if (bObstacleAhead)//bObstacleAhead updated in timer1 ISR

{

mySerial.printf("bObstacleAhead TRUE in TRACKLEFT at %lu with frontdist %d, variance = %6.2f\n",

millis(), frontdistval, frontvar);

InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

//11/13/20 added code to back up to WALL_OFFSET_TGTDIST_CM before beginning the spin turn

mySerial.printf("backing up to %d using front distance sensor\n", WALL_OFFSET_TGTDIST_CM);

MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM);

mySerial.printf("backed up successfully to %d using front distance sensor\n", frontdistval);

SpinTurn(false, 90);//turn 90 deg right

PrevOpMode = MODE_NONE; //this will force wall offset recapture

WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

break; //don't execute the rest - just go back to GetOpMode()

}

//11/08/20 added section for too close behind

//the idea is to move ahead to a position about WALL_OFFSET_TGTDIST_CM away from wall, and turn 45 deg

//one way or other so wall tracking can continue from that point

else if (bObstacleBehind)

{

mySerial.printf("Obstacle behind with reardist, frontdist = %d, %d cm\n", Lidar_Rear / 10, frontdistval);

ExecuteRearObstacleRecoveryManeuver(); //doesn't need tracking case info

PrevOpMode = MODE_NONE;

WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

break;

}

//11/13/20 rev to use a sliding obstacle detection range during tracking ops

//detection range is WALL_OFFSET_TGTDIST_CM/2 at max cut, increasing to WALL_OFFSET_TGTDIST_CM with zero cut angle

else if (frontdistval < adjustedFrontObstacleDetectionRangeCm)

{

//DEBUG!!

mySerial.printf("Hit adjusted wall offset limit (%2.2f cm) at %lu with frontdist = %d, Setpoint = %2.2f\n",

adjustedFrontObstacleDetectionRangeCm, millis(), frontdistval, WallTrackSetPoint);

//DEBUG!!

//06/06/21 rev to use code from 'if (bObstacleAhead)' block

////StepTurn(TRACKING_LEFT);

//PrevOpMode = MODE_NONE; //this will force wall offset recapture

//WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

//11/13/20 added code to back up to WALL_OFFSET_TGTDIST_CM before beginning the spin turn

mySerial.printf("backing up to %d using front distance sensor\n", WALL_OFFSET_TGTDIST_CM);

MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM);

mySerial.printf("backed up successfully to %d using front distance sensor\n", frontdistval);

SpinTurn(false, 90);//turn 90 deg right

PrevOpMode = MODE_NONE; //this will force wall offset recapture

WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

break; //don't execute the rest - just go back to GetOpMode()

}

//if we get to here, not stuck and no nearby obstacle issues

else if (PrevOpMode != MODE_WALLFOLLOW)//02/26/20 first time through capture offset

{

PrevOpMode = MODE_WALLFOLLOW; //added 04/27/21

//04/18/21 experiment with IRHomingNavigateToIAP() here

RotateToParallelOrientation(TRACKING_LEFT);

if (bIRBeamAvail)

{

IRHomingNavigateToIAP(TRACKING_LEFT);

if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry

{

Serial.println("Not Hungry - Avoiding Charger");

curMinObstacleDistance = CHG_STN_AVOIDANCE_DIST_CM; //hopefully this will cause avoidance

IRHomeToChgStn(CHG_STN_AVOIDANCE_DIST_CM, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

}

else //hungry - try to connect

{

Serial.println("Low battery - homing to Charger");

IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

StopBothMotors();

digitalWrite(RED_LASER_DIODE_PIN, LOW); //turn off the laser

mySerial.printf("Reset VL53L0X Teensy to reduce current consumption\n");

digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW); //hold Teensy reset to reduce current consumption

MonitorChargeUntilDone();

mySerial.printf("Restart VL53L0X Teensy...\n");

digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH); //restart Teensy

WaitForVL53L0XTeensy(); //wait for it to come back up.

ExecDisconManeuver();

PrevOpMode = MODE_NONE; //this will force wall offset recapture

mySerial.printf("%lu: just after ExecDisconManeuver() in TRACKING_LEFT case block\n", millis());

break;

}

//if we get here, we are continuing tracking operation

PrevOpMode = MODE_WALLFOLLOW; //added 04/27/21

WallTrackSteerVal = LeftSteeringVal; //computed by Teensy 3.5

//at 30mm from tgt offset, setpoint will be +/-0.3

WallTrackSetPoint = (double)(10 * WALL_OFFSET_TGTDIST_CM - Lidar_LeftCenter) / 100.f; //10/04/20 positive value drives robot toward wall

//limit approach angle to reasonable values

if (WallTrackSetPoint > WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = WALL_OFFSET_TRACK_SETPOINT_LIMIT;

if (WallTrackSetPoint < -WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

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

WallTrackPID.Compute();

leftspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

rightspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

rightspeednum = (rightspeednum <= MOTOR_SPEED_FULL) ? rightspeednum : MOTOR_SPEED_FULL;

rightspeednum = (rightspeednum > 0) ? rightspeednum : 0;

leftspeednum = (leftspeednum <= MOTOR_SPEED_FULL) ? leftspeednum : MOTOR_SPEED_FULL;

leftspeednum = (leftspeednum > 0) ? leftspeednum : 0;

MoveAhead(leftspeednum, rightspeednum);

mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%8.2f\t%d\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\t%d\t%lu\t%2.3f\n", millis(),

Lidar_LeftFront, Lidar_LeftCenter, Lidar_LeftRear, frontdistval, frontvar, Lidar_Rear, WallTrackSteerVal, WallTrackOutput,

WallTrackSetPoint, adjustedFrontObstacleDetectionRangeCm, leftspeednum, rightspeednum, IRHomingValTotalAvg, IRHomingLRSteeringVal);

}

#pragma endregion Tracking Left Wall Case

//mySerial.printf("At bottom of TRACKING_LEFT case block\n");

break;

case TRACKING_RIGHT:

#pragma region TRACKRIGHT

mySerial.printf("At top of TRACKING_RIGHT case block\n");

//07/22/2020 updated to use VL53L0X array vs 'ping' sensors

trackstr = "Right"; //used for telemetry printouts

//10/08/20 hack to prevent immediate obstacle detection

if (PrevOpMode != MODE_WALLFOLLOW) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

if (bIsStuck)//bIsStuck updated in timer1 ISR

{

mySerial.printf("bIsStuck TRUE in TRACKRIGHT at %lu with frontdist %d, variance = %6.2f\n",

millis(), frontdistval, frontvar);

InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

ExecuteStuckRecoveryManeuver(TRACKING_RIGHT); //10/24/20

PrevOpMode = MODE_NONE; //this will force wall offset recapture

WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

break; //don't execute the rest - just go back to GetOpMode()

}

//11/11/20 added separate section for 'Obstacle Ahead'

if (bObstacleAhead)//bObstacleAhead updated in timer1 ISR

{

mySerial.printf("bObstacleAhead TRUE in TRACKRIGHT at %lu with frontdist %d, variance = %6.2f\n",

millis(), frontdistval, frontvar);

InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

//11/13/20 added code to back up to WALL_OFFSET_TGTDIST_CM before beginning the spin turn

mySerial.printf("backing up to %d using front distance sensor\n", WALL_OFFSET_TGTDIST_CM);

MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM);

mySerial.printf("backed up successfully to %d using front distance sensor\n", frontdistval);

SpinTurn(true, 90);//turn 90 deg left

PrevOpMode = MODE_NONE; //this will force wall offset recapture

WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

break; //don't execute the rest - just go back to GetOpMode()

}

//11/08/20 added section for too close behind

//the idea is to move ahead to a position about WALL_OFFSET_TGTDIST_CM away from wall, and turn 45 deg

//one way or other so wall tracking can continue from that point

else if (bObstacleBehind)

{

mySerial.printf("Obstacle behind with reardist, frontdist = %d, %d cm\n", Lidar_Rear / 10, frontdistval);

ExecuteRearObstacleRecoveryManeuver(); //doesn't need tracking case info

PrevOpMode = MODE_NONE;

WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

break;

}

//11/13/20 rev to use a sliding obstacle detection range during tracking ops

//detection range is WALL_OFFSET_TGTDIST_CM/2 at max cut, increasing to WALL_OFFSET_TGTDIST_CM with zero cut angle

else if (frontdistval < adjustedFrontObstacleDetectionRangeCm)

{

//DEBUG!!

mySerial.printf("Hit adjusted wall offset limit (%2.2f cm) at %lu with frontdist = %d, Setpoint = %2.2f\n",

adjustedFrontObstacleDetectionRangeCm, millis(), frontdistval, WallTrackSetPoint);

//DEBUG!!

//06/06/21 rev to use code from 'if (bObstacleAhead)' block

//StepTurn(TRACKING_RIGHT);

//PrevOpMode = MODE_NONE; //this will force wall offset recapture

//WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

//11/13/20 added code to back up to WALL_OFFSET_TGTDIST_CM before beginning the spin turn

mySerial.printf("backing up to %d using front distance sensor\n", WALL_OFFSET_TGTDIST_CM);

MoveToDesiredFrontDistCm(WALL_OFFSET_TGTDIST_CM);

mySerial.printf("backed up successfully to %d using front distance sensor\n", frontdistval);

SpinTurn(true, 90);//turn 90 deg left

PrevOpMode = MODE_NONE; //this will force wall offset recapture

WallTrackPID.SetMode(MANUAL); //11/21/20 this will force PID re-init

break; //don't execute the rest - just go back to GetOpMode()

}

//if we get to here, not stuck and no nearby obstacle issues

else if (PrevOpMode != MODE_WALLFOLLOW)//02/26/20 first time through capture offset

{

//04/18/21 experiment with IRHomingNavigateToIAP() here

RotateToParallelOrientation(TRACKING_RIGHT);

if (bIRBeamAvail)

{

IRHomingNavigateToIAP(TRACKING_RIGHT);

if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry

{

Serial.println("Not Hungry - Avoiding Charger");

curMinObstacleDistance = CHG_STN_AVOIDANCE_DIST_CM; //hopefully this will cause avoidance

IRHomeToChgStn(CHG_STN_AVOIDANCE_DIST_CM, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

}

else //hungry - try to connect

{

Serial.println("Low battery - homing to Charger");

IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

StopBothMotors();

digitalWrite(RED_LASER_DIODE_PIN, LOW); //turn off the laser

mySerial.printf("Reset VL53L0X Teensy to reduce current consumption\n");

digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW); //hold Teensy reset to reduce current consumption

MonitorChargeUntilDone();

mySerial.printf("Restart VL53L0X Teensy...\n");

digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH); //restart Teensy

WaitForVL53L0XTeensy(); //wait for it to come back up.

ExecDisconManeuver();

PrevOpMode = MODE_NONE; //04/27/21: this will force wall offset recapture

mySerial.printf("%lu: just after ExecDisconManeuver() in TRACKING_RIGHT case block\n", millis());

break;

}

mySerial.printf("After else if (PrevOpMode != MODE_WALLFOLLOW) block\n");

//mySerial.printf("%lu: just after else if (PrevOpMode != MODE_WALLFOLLOW)\n");

//if we get here, we are continuing tracking operation

PrevOpMode = MODE_WALLFOLLOW; //added 04/27/21

WallTrackSteerVal = RightSteeringVal; //computed by Teensy 3.5

//at 30mm from tgt offset, setpoint will be +/-0.3

WallTrackSetPoint = (double)(10 * WALL_OFFSET_TGTDIST_CM - Lidar_RightCenter) / 100.f; //10/04/20 positive value drives robot toward wall

//limit approach angle to reasonable values

if (WallTrackSetPoint > WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = WALL_OFFSET_TRACK_SETPOINT_LIMIT;

if (WallTrackSetPoint < -WALL_OFFSET_TRACK_SETPOINT_LIMIT) WallTrackSetPoint = -WALL_OFFSET_TRACK_SETPOINT_LIMIT;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

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

WallTrackPID.Compute();

leftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

rightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

rightspeednum = (rightspeednum <= MOTOR_SPEED_FULL) ? rightspeednum : MOTOR_SPEED_FULL;

rightspeednum = (rightspeednum > 0) ? rightspeednum : 0;

leftspeednum = (leftspeednum <= MOTOR_SPEED_FULL) ? leftspeednum : MOTOR_SPEED_FULL;

leftspeednum = (leftspeednum > 0) ? leftspeednum : 0;

MoveAhead(leftspeednum, rightspeednum);

mySerial.printf("%lu\t%d\t%d\t%d\t%d\t%6.2f\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\t%d\t%lu\t%2.3f\n", millis(),

Lidar_RightFront, Lidar_RightCenter, Lidar_RightRear,

frontdistval, frontvar, WallTrackSteerVal, WallTrackOutput,

WallTrackSetPoint, adjustedFrontObstacleDetectionRangeCm, leftspeednum, rightspeednum, IRHomingValTotalAvg, IRHomingLRSteeringVal);

}

#pragma endregion Tracking Right Wall Case

//mySerial.printf("End of Tracking Right Wall Case\n");

break;

case TRACKING_NEITHER:

#pragma region TRACKNEITHER

trackstr = "Neither"; //used for telemetry printouts

mySerial.printf("time(ms) in TRACKNEITHER = %lu\n", millis());

//added 01/20/16: turn direction arbitrary

if (bObstacleAhead || bIsStuck) //08/10/20 now bObstacleAhead & bIsStuck updated in timer1 ISR

{

if (bIsStuck)

{

InitFrontDistArray(); //added 08/12/20 to prevent multiple 'stuck' detections

}

//DEBUG!!

Serial.println(F("---- Backup & Turn to the Left"));

//DEBUG!!

BackupAndTurn90Deg(true, true, MOTOR_SPEED_HALF);

//go back to normal tracking mode after B&T completes

NavCase = NAV_WALLTRK;

}

//Open Corner case

else if (frontdistval > STEPTURN_DIST_CM)

{

//OK, this is the Open Corner case - turn toward the last-tracked wall

#ifndef NO_MOTORS

delay(1000); //01/20/16 - delay to give Wall-E2 time to clear the corner a bit

#endif

if (PrevTrackingCase == TRACKING_LEFT)

{

//RotateCWDeg(false, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);

}

else if (PrevTrackingCase == TRACKING_RIGHT)

{

//RotateCWDeg(true, NUM_DEG_FOR_90_DEG_TURN, MOTOR_SPEED_HALF);

}

else //oops! not tracking either wall - out in middle of room somewhere?

{

//not open corner, but not tracking a wall, either - go straight

leftspeednum = rightspeednum = MOTOR_SPEED_HALF;

MoveAhead(leftspeednum, rightspeednum);

}

#pragma endregion Not Tracking Either Wall

break;

default:

break;

//mySerial.printf("Bottom of opMode switch\n");

} //switch (TrackingCase)

#pragma endregion MODE_WALLFOLLOW

//mySerial.printf("%lu: at end of MODE_WALLFOLLOW\n", millis());

//07/16/21 added to manage offset capture operation

#pragma region MODE_WALLCAPTURE

case MODE_WALLCAPTURE:

break;

#pragma endregion MODE_WALLCAPTURE

}

digitalWrite(RED_LASER_DIODE_PIN, LOW);

//mySerial.printf("%lu: Bottom of loop()\n", millis());

}//if (bTimeForNavUpdate)

}//loop()

The MODE_CHARGING and MODE_HOMING cases are self-contained, so no changes would be needed for them. The MODE_WALLFOLLOW case is sub-divided into TRACKING_LEFT and TRACKING_RIGHT cases. If all the inline code in TRACKING_LEFT was replaced with TrackLeftWallOffset() and that of TRACKING_RIGHT with TrackRightWallOffset(), with these two functions augmented by the current if (bIsStuck) , if(bObstacleAhead) and if(bObstacleBehind) guard code (pretty much as it now stands), then that should work. I think I’ll give that whirl and see what happens.

To start the process, I created yet another project – WallE3_WallTrack_V3 (to preserve the currently ‘working OK on left side’ status of WallE3_WallTrack_V2) and try porting the GetOpMode() and loop() code from FourWD_WallE2_V12.

02 March 2022 Update:

I now have a ‘loop() only’ version of WallE3 running that properly tracks the left side. Everything is basically the same as before, except the loop() function, shown below:

void loop()
{
  CheckForUserInput();

#pragma region IR HOMING 
  UpdateIRHomingValues(); //update all IR Homing related values

  bool bIRBeamAvail = (IRHomingValTotalAvg > IR_BEAM_DETECTION_THRESHOLD
    && abs(IRHomingLRSteeringVal) < IR_HOMING_STEERING_VAL_DETECTION_THRESHOLD);

  if (bIRBeamAvail)
  {
    digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer
    float batVoltage = GetBattVoltage();
    myTeePrint.printf("IR Beam Detected with Battery Voltage = %1.2f\n", batVoltage);

    if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry
    {
      Serial.println("Not Hungry - Avoiding Charger");

      //04/27/21 too complicated - just have robot reverse course
      int leftdist = GetAvgLeftDistCm();
      int rightdist = GetAvgRightDistCm();
      SpinTurn(leftdist > rightdist, 180);
    }
    else //hungry - try to connect
    {
      Serial.println("Low battery - homing to Charger");
      IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);
    }
  }
#pragma endregion IR HOMING

#pragma region CHARGING
  if (IsChargerConnected()) 
  {
    myTeePrint.printf("Robot has successfully connected to charger!\n");
    digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer
    chgStartMsec = millis(); //used for charge duration telemetry
    Serial.print(F("Starting Charge Mode at ")); Serial.println(chgStartMsec);
    StopBothMotors(); //added 03/13/17

    //04/27/21
    myTeePrint.printf("Reset VL53L0X Teensy to reduce current consumption\n");
    digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW); //hold Teensy reset to reduce current consumption
    MonitorChargeUntilDone();
    myTeePrint.printf("Restart VL53L0X Teensy...\n");
    digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH); //restart Teensy
    WaitForVL53L0XTeensy(); //wait for it to come back up.
    ExecDisconManeuver();
  }
#pragma endregion CHARGING

#pragma region WALL TRACKING
  UpdateAllDistances();

  if (glLeftCenterCm <= glRightCenterCm)
  {
    TrackLeftWallOffset(); //parameterless overload 
  }
  else if (glLeftCenterCm > glRightCenterCm)
  {
    TrackRightWallOffset(); //parameterless  overload  
  }
#pragma endregion WALL TRACKING
}

void loop()

{

CheckForUserInput();

#pragma region IR HOMING

UpdateIRHomingValues(); //update all IR Homing related values

bool bIRBeamAvail = (IRHomingValTotalAvg > IR_BEAM_DETECTION_THRESHOLD

&& abs(IRHomingLRSteeringVal) < IR_HOMING_STEERING_VAL_DETECTION_THRESHOLD);

if (bIRBeamAvail)

{

digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer

float batVoltage = GetBattVoltage();

myTeePrint.printf("IR Beam Detected with Battery Voltage = %1.2f\n", batVoltage);

if (batVoltage > LOW_BATT_THRESH_VOLTS) //not hungry

{

Serial.println("Not Hungry - Avoiding Charger");

//04/27/21 too complicated - just have robot reverse course

int leftdist = GetAvgLeftDistCm();

int rightdist = GetAvgRightDistCm();

SpinTurn(leftdist > rightdist, 180);

}

else //hungry - try to connect

{

Serial.println("Low battery - homing to Charger");

IRHomeToChgStn(0, MOTOR_SPEED_HALF, MOTOR_SPEED_HALF);

}

#pragma endregion IR HOMING

#pragma region CHARGING

if (IsChargerConnected())

{

myTeePrint.printf("Robot has successfully connected to charger!\n");

digitalWrite(RED_LASER_DIODE_PIN, LOW); //disable the laser pointer

chgStartMsec = millis(); //used for charge duration telemetry

Serial.print(F("Starting Charge Mode at ")); Serial.println(chgStartMsec);

StopBothMotors(); //added 03/13/17

//04/27/21

myTeePrint.printf("Reset VL53L0X Teensy to reduce current consumption\n");

digitalWrite(VL53L0X_TEENSY_RESET_PIN, LOW); //hold Teensy reset to reduce current consumption

MonitorChargeUntilDone();

myTeePrint.printf("Restart VL53L0X Teensy...\n");

digitalWrite(VL53L0X_TEENSY_RESET_PIN, HIGH); //restart Teensy

WaitForVL53L0XTeensy(); //wait for it to come back up.

ExecDisconManeuver();

}

#pragma endregion CHARGING

#pragma region WALL TRACKING

UpdateAllDistances();

if (glLeftCenterCm <= glRightCenterCm)

{

TrackLeftWallOffset(); //parameterless overload

}

else if (glLeftCenterCm > glRightCenterCm)

{

TrackRightWallOffset(); //parameterless overload

}

#pragma endregion WALL TRACKING

}

The ‘IR HOMING’ and ‘CHARGING’ blocks are essentially unchanged, and the ‘WALL TRACKING’ block is much simpler. All ‘anomaly’ (robot stuck either forward or backward, robot approaching an obstacle ahead or behind, dead battery, etc are all handled internally to the two ‘TrackLeft/RightWallOffset()’ functions. Here’s the (potentially infinite) ‘while()’ loop:

  while (errcode == NO_ANOMALIES)
  {
    CheckForUserInput(); //this is a bit recursive, but should still work (I hope)

    //09/04/21 back to elapsedMillis timing
    if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)
    {
      mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

      UpdateAllDistances(); //need to update ALL distances to detect stuck conditions
      CheckForErrorCondx();

      //have to weight value by both angle and wall offset
      //WallTrackSteerVal = glLeftSteeringVal + (glLeftCenterCm - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;
      WallTrackSteerVal = glLeftSteeringVal + (glLeftCenterCm - WALL_OFFSET_TGTDIST_CM) / 1000.f;

      //update motor speeds, skipping bad values
      if (!isnan(WallTrackSteerVal))
      {

        //02/05/22 sampleTime removed from signature
        WallTrackOutput = PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror,
          kp, ki, kd);

        int glLeftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;
        int glRightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

        glRightspeednum = (glRightspeednum <= MOTOR_SPEED_FULL) ? glRightspeednum : MOTOR_SPEED_FULL;
        glRightspeednum = (glRightspeednum > 0) ? glRightspeednum : 0;
        glLeftspeednum = (glLeftspeednum <= MOTOR_SPEED_FULL) ? glLeftspeednum : MOTOR_SPEED_FULL;
        glLeftspeednum = (glLeftspeednum > 0) ? glLeftspeednum : 0;

        MoveAhead(glLeftspeednum, glRightspeednum);

        myTeePrint.printf("%lu \t%d\t%d\t%d\t%d\t%2.2f\t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%d\t%d\n",
          millis(),
          glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glRearCm,
          WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,
          kp * lastError, ki * lastIval, kd * lastDerror,
          WallTrackOutput, glLeftspeednum, glRightspeednum);
      }
    }
  }

while (errcode == NO_ANOMALIES)

{

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

//09/04/21 back to elapsedMillis timing

if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC)

{

mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC;

UpdateAllDistances(); //need to update ALL distances to detect stuck conditions

CheckForErrorCondx();

//have to weight value by both angle and wall offset

//WallTrackSteerVal = glLeftSteeringVal + (glLeftCenterCm - 10 * WALL_OFFSET_TGTDIST_CM) / 1000.f;

WallTrackSteerVal = glLeftSteeringVal + (glLeftCenterCm - WALL_OFFSET_TGTDIST_CM) / 1000.f;

//update motor speeds, skipping bad values

if (!isnan(WallTrackSteerVal))

{

//02/05/22 sampleTime removed from signature

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

kp, ki, kd);

int glLeftspeednum = MOTOR_SPEED_QTR + WallTrackOutput;

int glRightspeednum = MOTOR_SPEED_QTR - WallTrackOutput;

glRightspeednum = (glRightspeednum <= MOTOR_SPEED_FULL) ? glRightspeednum : MOTOR_SPEED_FULL;

glRightspeednum = (glRightspeednum > 0) ? glRightspeednum : 0;

glLeftspeednum = (glLeftspeednum <= MOTOR_SPEED_FULL) ? glLeftspeednum : MOTOR_SPEED_FULL;

glLeftspeednum = (glLeftspeednum > 0) ? glLeftspeednum : 0;

MoveAhead(glLeftspeednum, glRightspeednum);

myTeePrint.printf("%lu \t%d\t%d\t%d\t%d\t%2.2f\t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%2.2f\t%2.2f \t%2.2f\t%d\t%d\n",

millis(),

glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glRearCm,

WallTrackSteerVal, WallTrackSetPoint, lastError, lastIval,

kp * lastError, ki * lastIval, kd * lastDerror,

WallTrackOutput, glLeftspeednum, glRightspeednum);

}

As the code above shows, the while loop will continue to execute as long as the ‘errcode’ value is ‘NO_ANOMALIES’. Internally the ‘CheckForErrorCondx()’ function surveys the inputs from all sensors and attempts to detect any anomalous behavior. Any return value except NO_ANOMALIES causes the while() loop to exit. Each potential anomaly condition has its own handling function, which exits back to ‘loop()’ and the process starts all over again. I believe this is a much cleaner approach than I had before with the ‘GetOpMode()’ function.

I also took the opportunity at this point to fix a long-standing problem with the code. The front-facing LIDAR unit returns distances in Cm, while all seven VL53L0X time-of-flight distance sensors report in mm. Not only did this torture me mentally (let’s see – is it Cm or mm here?), but it caused the new ‘CalcRearVariance()’ function to crater, because the 10x larger numbers, when squared, caused the ‘uint16_t’ type to overrun and produce crazy variance numbers. So, I changed the GetRequestedVL53L0XValues() function to convert mm to Cm, changed all the variable names from xxxxMM to xxxxCm and carefully combed through the entire codebase, correcting the inevitable wash of ’10x’ errors. In the end though, I made the codebase much more consistent and understandable (I hope).

Stay Tuned,

Frank