Tag Archives: robots

WallE3 Doesn’t Like Reflective Surfaces

Posted 08 December 2023

WallE3 went with us last month when we travelled to St. Louis for Thanksgiving with family, and I showed off his autonomous wall following skills. WallE3 actually did great for quite a while – that is until he found himself staring at the side of a floor-mounted wine cooler (wine ‘safe’?). As can be seen in the following short video, WallE3 fell in love with the cooler, and showed his love by repeatedly head-butting it – oops!

After looking at the telemetry data for the run, I saw that WallE3 was measuring much larger front distances – like several hundred centimeters – when it was only a few centimeters from the object. It appears he was backing up to a defined front distance in response to a ‘WALL_OFFSET_DISTANCE_AHEAD’ anomaly, but somehow convinced himself that instead of 20cm from the wall, he was actually more like 100cm away. Of course, since he wanted to be at 30cm (the desired wall offset distance), he drove forward to lessen the distance, thereby bonking into the wall. Then, when he hit the wall, the front LIDAR line of sight geometry changed enough to produce a true measurement of just a few centimeters, which then sent WallE3 running backwards to open up the distance. Lather, rinse, repeat. Here’s an Excel plot of a representative (but not exact – I somehow lost the actual telemetry data for this run).

Representative reflective surface ‘headbutt’ telemetry

As can be seen from the above the measured distance oscillates between the maximum measurable distance of 1000cm to nearly zero. Here’s a photo of the experimental setup that produced the above data.

The ‘reflective surface’ is a piece of glossy black translucent plastic, oriented at an angle to reflect WallE3’s LIDAR beam upward to the ceiling and then the reflected signal from the ceiling back to WallE3.

I’ve been thinking about this issue ever since first seeing it in St. Louis, but hadn’t come up with any firm ideas about how to solve it. I tinkered with the idea of generating two running averages of the front distance when in the ‘MoveToDesiredFrontDistCm()’ function, with the two averages separated in time by some amount. When approaching a normal non-reflective surface, the two averages would closely track each other, but when approaching a reflective surface that produced the above dramatic distance shifts, then the two averages would be dramatically different around the transitions. This could then be detected, and something done to recover. Then last night while falling asleep, I wondered whether or not the STMicro VL53LXX infra-red LIDAR sensors would have the same problem – hmm, maybe not! If that were the case, then I could probably run one in parallel with the Garmin LIDAR unit, and use it instead of the Garmin for all ‘MoveToDesiredFront/RearDistCm()’ calls.

I tried this experiment using the currently installed rear distance sensors by calling ‘MoveToDesiredRearDistCm()’ with the same reflective surface setup as before, as shown in the following photo:

‘MoveToDesiredRearDistCm()’ setup with reflective surface

Here’s an excel plot of the rear distance run:

MoveToRearDistCm() with 30cm target and reflective surface

As can be seen in the above plot, the rear distance run was completely normal, so the VL53LXX infra-red LIDAR sensors don’t have the same problem – at least not with this translucent glossy plastic material.

10 December 2023 Update:

I got to thinking that maybe the reason the rear distance sensor worked so well with the shiny black material is that it could be IR transparent, meaning that while the front sensor (an LED LIDAR system) would see a ‘mirror’, the rear sensor would just see the toolbox. So, I jumped up on Amazon and got a cheap mirror square so I could answer that question. Here is a short video and Excel plot showing a ‘MoveToDesiredFrontDistCm()’ run with the new mirror square.

As can be seen from the video, WallE was perfectly happy to drive right through the mirror, but I had visions of mirror pieces all over the bench, the floor, and me, so I manually prevented that from happening. Here’s an Excel plot showing the same run:

MoveToFrontDist(cm) approaching tilted mirror

As the Excel plot shows, the robot did OK for the first 20 measurements (about 1sec) but immediately thereafter started a steady 200cm, and it stayed that way until I stopped it at about 2sec

Then I tried the same experiment, but this time utilizing the STMicro VL53LXX IR LIDAR sensor on the rear of the robot, as shown in the following short video:

As the video shows, the IR LIDAR behaved pretty much the same as the LED LIDAR (no real surprise, as they are both LIDAR technology, but still a bummer!

Here’s the Excel plot for this run:

As the Excel plot shows, the distance decreased monotonically for the first 30 points (about 1.5sec) but then shot up to 200 due to the mirror. I believe the lower distances after about point 40 (2sec) were due to me interfering with the IR beam.

So, unfortunately my theory about the rear IR sensor doing better with the shiny black plastic ‘mirror’ because it appeared transparent at that wavelength seems to be bolstered, so I now think that using the VL53LXX sensor instead of the Garmin LIDAR LED sensor for ‘MoveToFrontDistCm()’ operations is NOT going to work. Back to the drawing board :(.

23 December 2023 Update:

I’ve been thinking about this problem for a while now, and have not come up with a good answer; WallE3’s perception of the world around it depends entirely on LIDAR-type distance sensors, so if those sensors produce ‘false’ distance reports due to mirror or mirror-ish surfaces, WallE3 has no way to know the ‘true’ distance – bummer!

So, what I decided to do is to simply detect the symptoms of the ‘mirrored surface’ situation, halt the robot and yell for help. The detection algorithm uses the knowledge that when the robot is moving forward toward a ‘normal’ object, the front distance should decrease monotonically with time. Similarly when the robot is moving backwards toward a ‘normal’ object, the rear distance should decrease monotonically with time. Conversely, when approaching a ‘mirror-like’ object, the measured distance tends to be unstable, with distance increasing instead of decreasing.

The detection algorithm calculates a three-point average each time through the action loop, and compares the result to the last time through the loop. If the new average is greater than the old average, a ‘mirrored surface detection’ is declared and the robot calls ‘YellForHelp()’ which stops the motors and emits an audible Morse code ‘SOS’.

Here’s the full code for ‘DoOneMoveToFrontDistCm()’ function that actually moves the robot and checks for ‘mirrored surface’ error conditions

bool DoOneMoveToFrontDistCm(uint16_t offsetCm, float Kp, float Ki, float Kd)
{
  float lastError = 0;
  float lastInput = 0;
  float lastIval = 0;
  float lastDerror = 0;

  uint16_t oldAvgDist = GetAvgFrontDistCm(); //added 12/19/23

  MsecSinceLastDistUpdate = 0; //01/08/22 not using ISR anymore
  MsecSinceLastFrontDistUpdate = 0; //added 10/02/22 to slow front LIDAR update rate

  gl_LastAnomalyCode = ANOMALY_NONE;

  //12/23/23 display ten distances from front dist array before start
  gl_pSerPort->printf("In DoOneMoveToFrontDistCm() at top, displaying 10 front dist entries\n\n");

  gl_pSerPort->printf("idx\tFrontD[idx]\n");
  for (int i = 9; i >= 0; i--)
  {
    gl_pSerPort->printf("%d\t%d\n", FRONT_DIST_ARRAY_SIZE - 1 - i, aFrontDistCM[FRONT_DIST_ARRAY_SIZE-1-i]);
  }
  gl_pSerPort->printf("\n");

  gl_pSerPort->printf("Msec\tFrontD\tTgtD\toldAvgD\tnewAvgD\tErr\tIval\tOutput\tspeed\tFrontVar\tRearVar\n");

  while (abs(gl_FrontCm - offsetCm) > 1 && gl_LastAnomalyCode != ANOMALY_STUCK_AHEAD
    && gl_LastAnomalyCode != ANOMALY_STUCK_BEHIND)
  {
    CheckForUserInput();

    if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)
    {
      MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE;

      UpdateAllEnvironmentParameters(); //10/01/23 this can change anomaly code

      uint16_t newAvgDist = GetAvgFrontDistCm(); //avg of last three front dist readings
      
      OffsetDistOutput = PIDCalcs((double)gl_FrontCm, (float)offsetCm, lastError,
        lastInput, lastIval, lastDerror, Kp, Ki, Kd);

      int16_t speed = (int16_t)OffsetDistOutput;

      //PIDCalcs out can be negative, must handle both signs
      if (speed >= 0)
      {
        speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed;
      }
      else //speed < 0
      {
        speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed;
      }

      gl_pSerPort->printf("%lu\t%d\t%d\t%d\t%d\t%2.1f\t%2.1f\t%2.1f\t%d\t%2.0f\t%2.0f\n",
        millis(), gl_FrontCm, offsetCm, oldAvgDist, newAvgDist, lastError, lastIval, OffsetDistOutput, speed, gl_Frontvar, gl_Rearvar);

      RunBothMotorsBidirectional(-speed, -speed);
      delay(100);

      //11/17/23 added to zero out lastIval anytime Kp*Err > max speed (MOTOR_SPEED_QTR here)
      //if (abs(gl_FrontCm - offsetCm)*OffsetDistKp > MOTOR_SPEED_QTR)
      if (abs(gl_FrontCm - offsetCm) * Kp > MOTOR_SPEED_QTR)
      {
        lastIval = 0;
      }


      //12/19/23 rev to return false if the average distance starts going the wrong way
      //         (i.e. going up if the robot is moving forward, or down if the robot
      //         is moving backwards)
	//12/24/23 Just '>' wasn't enough to prevent false positives, so Added '*1.1' factor
      if ((AreBothMotorsForward() && newAvgDist > oldAvgDist * 1.1) || (AreBothMotorsReversed() && newAvgDist * 1.1 < oldAvgDist)
       && abs(speed) > MOTOR_SPEED_MIN)
      {
        gl_pSerPort->printf("\nIn DoOneMoveToFrontDistCm() in 'mirrored sfc' detection block, displaying 10 front dist entries\n\n");

        //12/23/23 display last 10 distances from front dist array
        gl_pSerPort->printf("idx\tFrontD[idx]\n");
        for (int i = 9; i >= 0; i--)
        {
          gl_pSerPort->printf("%d\t%d\n", FRONT_DIST_ARRAY_SIZE - 1 - i, aFrontDistCM[FRONT_DIST_ARRAY_SIZE - 1 - i]);
        }
        gl_pSerPort->printf("\n");

        return false;
      }

      oldAvgDist = newAvgDist;
    }
  }

  return true;
}

bool DoOneMoveToFrontDistCm(uint16_t offsetCm, float Kp, float Ki, float Kd)

{

float lastError = 0;

float lastInput = 0;

float lastIval = 0;

float lastDerror = 0;

uint16_t oldAvgDist = GetAvgFrontDistCm(); //added 12/19/23

MsecSinceLastDistUpdate = 0; //01/08/22 not using ISR anymore

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

gl_LastAnomalyCode = ANOMALY_NONE;

//12/23/23 display ten distances from front dist array before start

gl_pSerPort->printf("In DoOneMoveToFrontDistCm() at top, displaying 10 front dist entries\n\n");

gl_pSerPort->printf("idx\tFrontD[idx]\n");

for (int i = 9; i >= 0; i--)

{

gl_pSerPort->printf("%d\t%d\n", FRONT_DIST_ARRAY_SIZE - 1 - i, aFrontDistCM[FRONT_DIST_ARRAY_SIZE-1-i]);

}

gl_pSerPort->printf("\n");

gl_pSerPort->printf("Msec\tFrontD\tTgtD\toldAvgD\tnewAvgD\tErr\tIval\tOutput\tspeed\tFrontVar\tRearVar\n");

while (abs(gl_FrontCm - offsetCm) > 1 && gl_LastAnomalyCode != ANOMALY_STUCK_AHEAD

&& gl_LastAnomalyCode != ANOMALY_STUCK_BEHIND)

{

CheckForUserInput();

if (MsecSinceLastDistUpdate >= MSEC_PER_DIST_UPDATE)

{

MsecSinceLastDistUpdate -= MSEC_PER_DIST_UPDATE;

UpdateAllEnvironmentParameters(); //10/01/23 this can change anomaly code

uint16_t newAvgDist = GetAvgFrontDistCm(); //avg of last three front dist readings

OffsetDistOutput = PIDCalcs((double)gl_FrontCm, (float)offsetCm, lastError,

lastInput, lastIval, lastDerror, Kp, Ki, Kd);

int16_t speed = (int16_t)OffsetDistOutput;

//PIDCalcs out can be negative, must handle both signs

if (speed >= 0)

{

speed = (speed > MOTOR_SPEED_QTR) ? MOTOR_SPEED_QTR : speed;

}

else //speed < 0

{

speed = (speed < -MOTOR_SPEED_QTR) ? -MOTOR_SPEED_QTR : speed;

}

gl_pSerPort->printf("%lu\t%d\t%d\t%d\t%d\t%2.1f\t%2.1f\t%2.1f\t%d\t%2.0f\t%2.0f\n",

millis(), gl_FrontCm, offsetCm, oldAvgDist, newAvgDist, lastError, lastIval, OffsetDistOutput, speed, gl_Frontvar, gl_Rearvar);

RunBothMotorsBidirectional(-speed, -speed);

delay(100);

//11/17/23 added to zero out lastIval anytime Kp*Err > max speed (MOTOR_SPEED_QTR here)

//if (abs(gl_FrontCm - offsetCm)*OffsetDistKp > MOTOR_SPEED_QTR)

if (abs(gl_FrontCm - offsetCm) * Kp > MOTOR_SPEED_QTR)

{

lastIval = 0;

}

//12/19/23 rev to return false if the average distance starts going the wrong way

// (i.e. going up if the robot is moving forward, or down if the robot

// is moving backwards)

//12/24/23 Just '>' wasn't enough to prevent false positives, so Added '*1.1' factor

if ((AreBothMotorsForward() && newAvgDist > oldAvgDist * 1.1) || (AreBothMotorsReversed() && newAvgDist * 1.1 < oldAvgDist)

&& abs(speed) > MOTOR_SPEED_MIN)

{

gl_pSerPort->printf("\nIn DoOneMoveToFrontDistCm() in 'mirrored sfc' detection block, displaying 10 front dist entries\n\n");

//12/23/23 display last 10 distances from front dist array

gl_pSerPort->printf("idx\tFrontD[idx]\n");

for (int i = 9; i >= 0; i--)

{

gl_pSerPort->printf("%d\t%d\n", FRONT_DIST_ARRAY_SIZE - 1 - i, aFrontDistCM[FRONT_DIST_ARRAY_SIZE - 1 - i]);

}

gl_pSerPort->printf("\n");

return false;

}

oldAvgDist = newAvgDist;

}

return true;

}

02 January 2024 Update:

I wasn’t really happy with my previous attempt at ‘mirrored surface’ detection as it seemed pretty to produce false positives. After thinking about the problem some more, I thought I might be able to use a distance variance calculation as a more robust detection method. The idea is that a normal monotonic increase or decrease in distance measurements would have a pretty low variance, while a distance reversal would generate a much larger value.

So, I ran some simulations in Excel using a 5-point running variance calculation, and the results were encouraging. Then, with the help of my lovely lab assistant, I set up an experiment in my office sandbox to see if I could capture a representative mirrored surface ‘screwup’, as shown below:

And here is an Excel plot showing the results (both the distance and 5-pt variance vertical scales have been truncated to show the smaller scale variations)

Front Distance and 5-pt Variance (truncated vertical scales for better visibility)

As can be see in the video and the Excel plot, the robot undergoes a number of distinct front/back oscillations, but then eventually settles down. It is clear from the plot that the 5-pt variance calculation is a good indicator of a ‘mirrored surface’ condition. Just looking at the plot, it appears that a variance threshold of 40-60 should provide for robust detection without much of a risk of false positives.

One other note about this experiment. To get multiple oscillations as shown in the video and plot, the mirrored surface had to be slanted slightly up toward the ceiling. If the surface was oriented vertically like a normal wall, the robot would often miss the first distance and hit the wall, but then would typically back off to the correct distance. I think this indicates that in the vertical configuration, there is enough backscatter from the floor and/or the robot itself to get a reasonable (if not entirely accurate) LIDAR distance measurement.

07 January 2024 Update:

After playing around some more with this issue, I think the above 5pt variance calculation for ‘mirrored surface’ detection will work, so I revised my current well-tested ‘CalcBruteFrontDistArrayVariance()’ function to take a integer argument denoting the number of elements to use, starting at the end (most recent data) and working backwards.

Then I ran another test in my lab, but this time on a non-mirrored surface, to verify that the 5pt variance calc would still work properly and to settle on a good threshold for ‘mirror’ detection. The Excel plot below shows the results of a run where the robot moved backwards (still using the front LIDAR sensor) to 90cm from the wall.

Running 5-point variance of front distances

As can be seen, the variance starts out in the 10 to 20 range during the initial ‘coarse’ distance movement, but then drops into the 0 to 5 range during the second ‘fine tuning’ movement. Comparing this to the previous ‘mirrored surface’ plot leads me to believe that a threshold of 40 would almost certainly (eventually) detect a ‘mirrored surface’ condition.

24 January 2024 Update:

I added the ‘CalcFrontNPointVar(uint16_t N)’ function to the robot code so I could obtain the front variance using just the last N front distances instead of the entire 100 point array. This turned out to work very well for detecting the ‘Mirrored Surface’ anomaly condition. Then I added ‘ANOMALY_MIRRORED_SFC’ to the list of anomaly codes, and added a ‘case’ block in ‘HandleAnomalousConditions()’ to deal with this condition. The handler function is:

  case ANOMALY_MIRRORED_SFC: //01/21/24 added
    gl_LastAnomalyCode = ANOMALY_NONE; //rev 09/30/23 to clear anomaly flag
    gl_pSerPort->printf("ANOMALY_MIRRORED_SFC case detected with tracking case %s\n", WallTrackStrArray[trackcase]);
    gl_pSerPort->printf("In ANOMALY_MIRRORED_SFC case just before call to RunToDaylightV2() with last anomaly code = %s\n", AnomalyStrArray[gl_LastAnomalyCode]);
    gl_CurTrackingCase = RunToDaylightV2(); //added 10/2/23
    gl_pSerPort->printf("In ANOMALY_MIRRORED_SFC case: RunToDaylightV2 returned = %s\n", WallTrackStrArray[gl_CurTrackingCase]);
    break;

case ANOMALY_MIRRORED_SFC: //01/21/24 added

gl_LastAnomalyCode = ANOMALY_NONE; //rev 09/30/23 to clear anomaly flag

gl_pSerPort->printf("ANOMALY_MIRRORED_SFC case detected with tracking case %s\n", WallTrackStrArray[trackcase]);

gl_pSerPort->printf("In ANOMALY_MIRRORED_SFC case just before call to RunToDaylightV2() with last anomaly code = %s\n", AnomalyStrArray[gl_LastAnomalyCode]);

gl_CurTrackingCase = RunToDaylightV2(); //added 10/2/23

gl_pSerPort->printf("In ANOMALY_MIRRORED_SFC case: RunToDaylightV2 returned = %s\n", WallTrackStrArray[gl_CurTrackingCase]);

break;

At the moment, all it does is call the ‘RunToDaylightV2()’ function, which does a 360º search for the best direction in which to move next. Here’s the telemetry and a short video showing the action.

DoOneMoveToFrontDistCm() returned false with gl_LastAnomalyCode == ANOMALY_MIRRORED_SFC
MoveToDesiredFrontDistCm returning false
	FrontBackMotionTest(): MoveToDesiredFrontDistCm() failed with anomaly code = MIRRORED_SURFACE - Quitting!
In HandleAnomalousConditions(NEITHER) with last anomaly code = MIRRORED_SURFACE
ANOMALY_MIRRORED_SFC case detected with tracking case NEITHER
In ANOMALY_MIRRORED_SFC case just before call to RunToDaylightV2() with last anomaly code = NONE
In RunToDaylightV2()

Idx	Hdg	FrontD	LeftD	RightD	LSteer	RSteer	Avail
0	62.6	380	82.0	246.7	0.7	-1.0	0
1	75.9	526	95.4	318.2	0.2	0.5	1
2	86.4	142	101.3	294.9	0.3	-0.6	0
3	97.9	181	107.4	241.4	0.3	0.1	0
4	109.2	168	89.7	242.8	0.7	-0.7	0
5	119.3	143	102.2	209.9	1.0	1.0	0
6	129.3	123	179.5	162.2	0.1	0.3	0
7	139.8	379	170.0	149.3	0.2	-0.2	0
8	150.4	443	149.5	171.7	0.0	-0.1	0
9	160.8	390	126.1	103.9	0.2	0.7	0
10	170.9	315	143.8	88.6	0.0	-0.1	1
11	-178.9	340	148.7	168.4	-0.1	1.0	0
12	-169.2	227	170.1	86.5	0.2	0.5	1
13	-158.0	249	160.3	94.0	1.0	-1.0	0
14	-148.8	192	130.1	112.6	1.0	0.7	0
15	-138.8	147	127.7	83.6	1.0	0.7	0
16	-128.4	147	189.4	85.8	0.5	-0.4	1
17	-118.2	108	312.0	97.3	1.0	0.2	1
18	-108.6	84	204.5	97.5	1.0	-0.1	1
19	-98.4	137	293.3	101.8	-0.1	-0.1	0
20	-88.5	182	307.3	109.3	-0.6	-0.2	0
21	-78.9	75	242.9	102.3	-0.3	0.6	0
22	-67.9	116	238.6	90.6	1.0	-1.0	0
23	-57.1	79	178.8	167.9	1.0	-0.3	0
24	-47.3	61	151.2	177.9	0.3	0.2	0
25	-37.7	95	152.4	166.7	-0.4	0.2	0
26	-28.2	95	164.2	144.0	-0.3	0.9	0
27	-18.5	98	100.3	147.4	-0.1	-1.0	0
28	-8.6	101	89.1	148.9	0.5	-0.0	0
29	1.1	101	174.3	149.4	-1.0	-0.3	0
30	11.5	69	85.2	172.5	-0.3	-0.2	1
31	21.3	69	87.2	183.7	0.7	1.0	0
32	31.1	182	112.1	138.9	-0.3	0.1	0
33	40.5	170	76.6	137.6	0.7	-1.0	0
34	50.2	162	80.0	248.1	0.4	-1.0	1
35	61.6	362	94.8	352.2	0.1	-0.6	1
In RunToDaylightV2()
RunToDaylightV2(): After REV sort, arrays are

Idx	Hdg	FrontD	LeftD	RightD	LSteer	RSteer	Avail
0	75.9	526	95.4	318.2	0.2	0.5	1
1	150.4	443	149.5	171.7	0.0	-0.1	0
2	160.8	390	126.1	103.9	0.2	0.7	0
3	62.6	380	82.0	246.7	0.7	-1.0	0
4	139.8	379	170.0	149.3	0.2	-0.2	0
5	61.6	362	94.8	352.2	0.1	-0.6	1
6	-178.9	340	148.7	168.4	-0.1	1.0	0
7	170.9	315	143.8	88.6	0.0	-0.1	1
8	-158.0	249	160.3	94.0	1.0	-1.0	0
9	-169.2	227	170.1	86.5	0.2	0.5	1
10	-148.8	192	130.1	112.6	1.0	0.7	0
11	31.1	182	112.1	138.9	-0.3	0.1	0
12	-88.5	182	307.3	109.3	-0.6	-0.2	0
13	97.9	181	107.4	241.4	0.3	0.1	0
14	40.5	170	76.6	137.6	0.7	-1.0	0
15	109.2	168	89.7	242.8	0.7	-0.7	0
16	50.2	162	80.0	248.1	0.4	-1.0	1
17	-128.4	147	189.4	85.8	0.5	-0.4	1
18	-138.8	147	127.7	83.6	1.0	0.7	0
19	119.3	143	102.2	209.9	1.0	1.0	0
20	86.4	142	101.3	294.9	0.3	-0.6	0
21	-98.4	137	293.3	101.8	-0.1	-0.1	0
22	129.3	123	179.5	162.2	0.1	0.3	0
23	-67.9	116	238.6	90.6	1.0	-1.0	0
24	-118.2	108	312.0	97.3	1.0	0.2	1
25	-8.6	101	89.1	148.9	0.5	-0.0	0
26	1.1	101	174.3	149.4	-1.0	-0.3	0
27	-18.5	98	100.3	147.4	-0.1	-1.0	0
28	-37.7	95	152.4	166.7	-0.4	0.2	0
29	-28.2	95	164.2	144.0	-0.3	0.9	0
30	-108.6	84	204.5	97.5	1.0	-0.1	1
31	-57.1	79	178.8	167.9	1.0	-0.3	0
32	-78.9	75	242.9	102.3	-0.3	0.6	0
33	21.3	69	87.2	183.7	0.7	1.0	0
34	11.5	69	85.2	172.5	-0.3	-0.2	1
35	-47.3	61	151.2	177.9	0.3	0.2	0
FrontD	Hdg	LeftD	LSteer	RightD	RSteer
at index = 0, FrontD = 526, bWallTrackAvailable = 1
Trackable Wall found at index = 0
FrontD	Hdg	LeftD	LSteer	RightD	RSteer
526	75.9	95.4	0.2	318.2	0.5
Turning to heading = 75.9
Just after turning to 75.9
FrontD	Hdg	LeftD	LSteer	RightD	RSteer
526	75.9	95.4	0.2	318.2	0.5
In RunToDaylightV2() with trackable wall on left side: trkdir = LEFT
RunToDaylightV2() returning LEFT
In ANOMALY_MIRRORED_SFC case: RunToDaylightV2 returned = LEFT

DoOneMoveToFrontDistCm() returned false with gl_LastAnomalyCode == ANOMALY_MIRRORED_SFC

MoveToDesiredFrontDistCm returning false

FrontBackMotionTest(): MoveToDesiredFrontDistCm() failed with anomaly code = MIRRORED_SURFACE - Quitting!

In HandleAnomalousConditions(NEITHER) with last anomaly code = MIRRORED_SURFACE

ANOMALY_MIRRORED_SFC case detected with tracking case NEITHER

In ANOMALY_MIRRORED_SFC case just before call to RunToDaylightV2() with last anomaly code = NONE

In RunToDaylightV2()

Idx Hdg FrontD LeftD RightD LSteer RSteer Avail

0 62.6 380 82.0 246.7 0.7 -1.0 0

1 75.9 526 95.4 318.2 0.2 0.5 1

2 86.4 142 101.3 294.9 0.3 -0.6 0

3 97.9 181 107.4 241.4 0.3 0.1 0

4 109.2 168 89.7 242.8 0.7 -0.7 0

5 119.3 143 102.2 209.9 1.0 1.0 0

6 129.3 123 179.5 162.2 0.1 0.3 0

7 139.8 379 170.0 149.3 0.2 -0.2 0

8 150.4 443 149.5 171.7 0.0 -0.1 0

9 160.8 390 126.1 103.9 0.2 0.7 0

10 170.9 315 143.8 88.6 0.0 -0.1 1

11 -178.9 340 148.7 168.4 -0.1 1.0 0

12 -169.2 227 170.1 86.5 0.2 0.5 1

13 -158.0 249 160.3 94.0 1.0 -1.0 0

14 -148.8 192 130.1 112.6 1.0 0.7 0

15 -138.8 147 127.7 83.6 1.0 0.7 0

16 -128.4 147 189.4 85.8 0.5 -0.4 1

17 -118.2 108 312.0 97.3 1.0 0.2 1

18 -108.6 84 204.5 97.5 1.0 -0.1 1

19 -98.4 137 293.3 101.8 -0.1 -0.1 0

20 -88.5 182 307.3 109.3 -0.6 -0.2 0

21 -78.9 75 242.9 102.3 -0.3 0.6 0

22 -67.9 116 238.6 90.6 1.0 -1.0 0

23 -57.1 79 178.8 167.9 1.0 -0.3 0

24 -47.3 61 151.2 177.9 0.3 0.2 0

25 -37.7 95 152.4 166.7 -0.4 0.2 0

26 -28.2 95 164.2 144.0 -0.3 0.9 0

27 -18.5 98 100.3 147.4 -0.1 -1.0 0

28 -8.6 101 89.1 148.9 0.5 -0.0 0

29 1.1 101 174.3 149.4 -1.0 -0.3 0

30 11.5 69 85.2 172.5 -0.3 -0.2 1

31 21.3 69 87.2 183.7 0.7 1.0 0

32 31.1 182 112.1 138.9 -0.3 0.1 0

33 40.5 170 76.6 137.6 0.7 -1.0 0

34 50.2 162 80.0 248.1 0.4 -1.0 1

35 61.6 362 94.8 352.2 0.1 -0.6 1

In RunToDaylightV2()

RunToDaylightV2(): After REV sort, arrays are

Idx Hdg FrontD LeftD RightD LSteer RSteer Avail

0 75.9 526 95.4 318.2 0.2 0.5 1

1 150.4 443 149.5 171.7 0.0 -0.1 0

2 160.8 390 126.1 103.9 0.2 0.7 0

3 62.6 380 82.0 246.7 0.7 -1.0 0

4 139.8 379 170.0 149.3 0.2 -0.2 0

5 61.6 362 94.8 352.2 0.1 -0.6 1

6 -178.9 340 148.7 168.4 -0.1 1.0 0

7 170.9 315 143.8 88.6 0.0 -0.1 1

8 -158.0 249 160.3 94.0 1.0 -1.0 0

9 -169.2 227 170.1 86.5 0.2 0.5 1

10 -148.8 192 130.1 112.6 1.0 0.7 0

11 31.1 182 112.1 138.9 -0.3 0.1 0

12 -88.5 182 307.3 109.3 -0.6 -0.2 0

13 97.9 181 107.4 241.4 0.3 0.1 0

14 40.5 170 76.6 137.6 0.7 -1.0 0

15 109.2 168 89.7 242.8 0.7 -0.7 0

16 50.2 162 80.0 248.1 0.4 -1.0 1

17 -128.4 147 189.4 85.8 0.5 -0.4 1

18 -138.8 147 127.7 83.6 1.0 0.7 0

19 119.3 143 102.2 209.9 1.0 1.0 0

20 86.4 142 101.3 294.9 0.3 -0.6 0

21 -98.4 137 293.3 101.8 -0.1 -0.1 0

22 129.3 123 179.5 162.2 0.1 0.3 0

23 -67.9 116 238.6 90.6 1.0 -1.0 0

24 -118.2 108 312.0 97.3 1.0 0.2 1

25 -8.6 101 89.1 148.9 0.5 -0.0 0

26 1.1 101 174.3 149.4 -1.0 -0.3 0

27 -18.5 98 100.3 147.4 -0.1 -1.0 0

28 -37.7 95 152.4 166.7 -0.4 0.2 0

29 -28.2 95 164.2 144.0 -0.3 0.9 0

30 -108.6 84 204.5 97.5 1.0 -0.1 1

31 -57.1 79 178.8 167.9 1.0 -0.3 0

32 -78.9 75 242.9 102.3 -0.3 0.6 0

33 21.3 69 87.2 183.7 0.7 1.0 0

34 11.5 69 85.2 172.5 -0.3 -0.2 1

35 -47.3 61 151.2 177.9 0.3 0.2 0

FrontD Hdg LeftD LSteer RightD RSteer

at index = 0, FrontD = 526, bWallTrackAvailable = 1

Trackable Wall found at index = 0

FrontD Hdg LeftD LSteer RightD RSteer

526 75.9 95.4 0.2 318.2 0.5

Turning to heading = 75.9

Just after turning to 75.9

FrontD Hdg LeftD LSteer RightD RSteer

526 75.9 95.4 0.2 318.2 0.5

In RunToDaylightV2() with trackable wall on left side: trkdir = LEFT

RunToDaylightV2() returning LEFT

In ANOMALY_MIRRORED_SFC case: RunToDaylightV2 returned = LEFT

This experiment led to one of those “Well, DUH!! moments, as the robot’s preferred ‘RunToDaylight()’ heading was right back at the mirrored surface!

After recovering from my ‘face-palm’ moment, I realized I needed to modify the ‘RunToDaylight() function to take heading-start & heading-end parameters to exclude the mirrored surface sector from the ‘RunToDaylight()’ search for an appropriate recovery heading.

28 January 2024 Update:

I modified ‘RunToDaylightV2()’ to take two float parameters denoting the start and ending headings for the search. In the normal non-mirrored surface case, ‘RunToDaylightV2()’ is called with no arguments. The no-argument overload simply calls ‘RunToDaylightV2(startHdg, endHdg)’ with startHdg = endHdg. In the mirrored surface case, the two-parameter version of ‘RunToDaylightV2()’ is called from the ‘ANOMALY_MIRRORED_SFC’ case block of ‘HandleAnomalousConditions()’

Here’s a short video and the telemetry from a test run toward a mirrored surface.

MTFD: at start, tgt = 30cm, curr_dist = 518, PID = 1.00, 0.10, 0.10
In DoOneMoveToFrontDistCm() at top, displaying 10 front dist entries

Msec	FrontD	5ptVar
abs(gl_FrontCm - offsetCm) * Kp = 497.0 -- Zeroing lastIval!
227898	527	186.8
Warning!  Mirrored surface detected in DoOneMoveToFrontDistCm()! Setting anomaly code to ANOMALY_MIRRORED_SFC
idx	FrontD
0	99	527
1	98	527
2	97	504
3	96	504
4	95	538
DoOneMoveToFrontDistCm() returned false with gl_LastAnomalyCode == ANOMALY_MIRRORED_SFC
MoveToDesiredFrontDistCm returning false
	FrontBackMotionTest(): MoveToDesiredFrontDistCm() failed with anomaly code = MIRRORED_SURFACE - Quitting!
In HandleAnomalousConditions(NEITHER) with last anomaly code = MIRRORED_SURFACE
ANOMALY_MIRRORED_SFC case detected with tracking case NEITHER
In ANOMALY_MIRRORED_SFC with IMUHdgValDeg = -0.3
In ANOMALY_MIRRORED_SFC with startdeg = 19.7,  enddeg = -20.3
In RunToDaylightV2(19.7, -20.3)

GetHdgDiff(1, 19.7, -20.3)
GetHdgDiff - Else block: starthdg = 19.7, endhdg = -20.3
GetHdgDiff - Else block: CCW_hdg_diff = 40.0, CW_hdg_diff = 320.0
GetHdgDiff: hdg_diff = 320.0
RunToDaylightV2: spindeg 8.0
RunToDaylightV2: turning 320.0deg CW from 19.7 to -20.3
turning to startHdg 19.7
Idx	Hdg	FrontD	LeftD	RightD	LSteer	RSteer	Avail
0	16.1	162	109.2	283.6	0.1	1.0	0
1	24.6	171	110.7	292.3	0.1	0.3	0
2	34.9	208	115.9	309.9	0.2	0.7	0
3	44.7	195	127.8	264.0	0.3	0.5	0
4	54.3	226	123.2	200.4	0.6	1.0	0
5	65.1	192	198.4	150.5	0.6	0.4	0
6	73.3	159	205.1	136.7	0.1	0.2	0
7	82.9	410	196.8	122.4	-0.1	-0.3	0
8	93.8	607	165.8	54.0	0.1	0.9	0
9	101.8	313	149.6	50.3	0.2	-0.1	1
10	112.6	279	171.9	64.2	-0.1	-1.0	0
11	122.1	331	182.5	87.0	-0.0	-1.0	0
12	132.9	202	202.6	150.5	0.6	1.0	0
13	141.1	199	199.4	72.9	1.0	0.2	1
14	149.1	151	205.0	72.9	1.0	-0.8	0
15	158.3	119	178.5	104.8	1.0	-0.9	0
16	167.7	104	175.2	104.9	1.0	0.8	0
17	176.6	127	205.8	84.2	1.0	-0.3	1
18	-175.5	46	270.5	93.6	0.1	-1.0	0
19	-166.1	69	289.7	108.2	-1.0	-0.1	0
20	-158.2	72	302.1	110.8	-1.0	-0.1	0
21	-150.3	118	297.2	115.7	-1.0	-0.3	0
22	-141.0	165	285.9	127.1	1.0	-0.5	0
23	-131.8	67	193.4	134.6	1.0	-0.4	0
24	-122.6	66	160.3	139.1	1.0	-1.0	0
25	-113.3	108	137.1	204.3	0.7	0.1	0
26	-104.4	102	120.0	205.7	0.3	-0.3	0
27	-95.2	68	118.4	195.8	-0.9	0.6	0
28	-86.1	96	50.9	163.2	-0.6	1.0	0
29	-77.2	104	47.9	172.6	0.1	-0.1	1
30	-68.0	110	57.8	178.1	1.0	0.1	0
31	-58.8	120	80.4	194.0	1.0	0.2	0
32	-49.1	123	147.1	208.7	-0.2	-0.1	0
33	-39.2	103	72.1	214.0	-0.1	0.0	1
34	-29.5	184	69.7	205.2	0.2	0.8	1
35	-21.1	205	101.0	176.7	0.0	0.9	0
RunToDaylightV2(): After REV sort, arrays are

Idx	Hdg	FrontD	LeftD	RightD	LSteer	RSteer	Avail
0	93.8	607	165.8	54.0	0.1	0.9	0
1	82.9	410	196.8	122.4	-0.1	-0.3	0
2	122.1	331	182.5	87.0	-0.0	-1.0	0
3	101.8	313	149.6	50.3	0.2	-0.1	1
4	112.6	279	171.9	64.2	-0.1	-1.0	0
5	54.3	226	123.2	200.4	0.6	1.0	0
6	34.9	208	115.9	309.9	0.2	0.7	0
7	-21.1	205	101.0	176.7	0.0	0.9	0
8	132.9	202	202.6	150.5	0.6	1.0	0
9	141.1	199	199.4	72.9	1.0	0.2	1
10	44.7	195	127.8	264.0	0.3	0.5	0
11	65.1	192	198.4	150.5	0.6	0.4	0
12	-29.5	184	69.7	205.2	0.2	0.8	1
13	24.6	171	110.7	292.3	0.1	0.3	0
14	-141.0	165	285.9	127.1	1.0	-0.5	0
15	16.1	162	109.2	283.6	0.1	1.0	0
16	73.3	159	205.1	136.7	0.1	0.2	0
17	149.1	151	205.0	72.9	1.0	-0.8	0
18	176.6	127	205.8	84.2	1.0	-0.3	1
19	-49.1	123	147.1	208.7	-0.2	-0.1	0
20	-58.8	120	80.4	194.0	1.0	0.2	0
21	158.3	119	178.5	104.8	1.0	-0.9	0
22	-150.3	118	297.2	115.7	-1.0	-0.3	0
23	-68.0	110	57.8	178.1	1.0	0.1	0
24	-113.3	108	137.1	204.3	0.7	0.1	0
25	167.7	104	175.2	104.9	1.0	0.8	0
26	-77.2	104	47.9	172.6	0.1	-0.1	1
27	-39.2	103	72.1	214.0	-0.1	0.0	1
28	-104.4	102	120.0	205.7	0.3	-0.3	0
29	-86.1	96	50.9	163.2	-0.6	1.0	0
30	-158.2	72	302.1	110.8	-1.0	-0.1	0
31	-166.1	69	289.7	108.2	-1.0	-0.1	0
32	-95.2	68	118.4	195.8	-0.9	0.6	0
33	-131.8	67	193.4	134.6	1.0	-0.4	0
34	-122.6	66	160.3	139.1	1.0	-1.0	0
35	-175.5	46	270.5	93.6	0.1	-1.0	0
FrontD	Hdg	LeftD	LSteer	RightD	RSteer
at index = 0, FrontD = 607, bWallTrackAvailable = 0
at index = 1, FrontD = 410, bWallTrackAvailable = 0
at index = 2, FrontD = 331, bWallTrackAvailable = 0
at index = 3, FrontD = 313, bWallTrackAvailable = 1
Trackable Wall found at index = 3
FrontD	Hdg	LeftD	LSteer	RightD	RSteer
313	101.8	149.6	0.2	50.3	-0.1
Turning to heading = 101.8
Just after turning to 101.8
FrontD	Hdg	LeftD	LSteer	RightD	RSteer
313	101.8	149.6	0.2	50.3	-0.1
In RunToDaylightV2() with trackable wall on right side: trkdir = RIGHT
RunToDaylightV2() returning RIGHT
In ANOMALY_MIRRORED_SFC case: RunToDaylightV2 returned = RIGHT

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

MTFD: at start, tgt = 30cm, curr_dist = 518, PID = 1.00, 0.10, 0.10

In DoOneMoveToFrontDistCm() at top, displaying 10 front dist entries

Msec FrontD 5ptVar

abs(gl_FrontCm - offsetCm) * Kp = 497.0 -- Zeroing lastIval!

227898 527 186.8

Warning! Mirrored surface detected in DoOneMoveToFrontDistCm()! Setting anomaly code to ANOMALY_MIRRORED_SFC

idx FrontD

0 99 527

1 98 527

2 97 504

3 96 504

4 95 538

DoOneMoveToFrontDistCm() returned false with gl_LastAnomalyCode == ANOMALY_MIRRORED_SFC

MoveToDesiredFrontDistCm returning false

FrontBackMotionTest(): MoveToDesiredFrontDistCm() failed with anomaly code = MIRRORED_SURFACE - Quitting!

In HandleAnomalousConditions(NEITHER) with last anomaly code = MIRRORED_SURFACE

ANOMALY_MIRRORED_SFC case detected with tracking case NEITHER

In ANOMALY_MIRRORED_SFC with IMUHdgValDeg = -0.3

In ANOMALY_MIRRORED_SFC with startdeg = 19.7, enddeg = -20.3

In RunToDaylightV2(19.7, -20.3)

GetHdgDiff(1, 19.7, -20.3)

GetHdgDiff - Else block: starthdg = 19.7, endhdg = -20.3

GetHdgDiff - Else block: CCW_hdg_diff = 40.0, CW_hdg_diff = 320.0

GetHdgDiff: hdg_diff = 320.0

RunToDaylightV2: spindeg 8.0

RunToDaylightV2: turning 320.0deg CW from 19.7 to -20.3

turning to startHdg 19.7

Idx Hdg FrontD LeftD RightD LSteer RSteer Avail

0 16.1 162 109.2 283.6 0.1 1.0 0

1 24.6 171 110.7 292.3 0.1 0.3 0

2 34.9 208 115.9 309.9 0.2 0.7 0

3 44.7 195 127.8 264.0 0.3 0.5 0

4 54.3 226 123.2 200.4 0.6 1.0 0

5 65.1 192 198.4 150.5 0.6 0.4 0

6 73.3 159 205.1 136.7 0.1 0.2 0

7 82.9 410 196.8 122.4 -0.1 -0.3 0

8 93.8 607 165.8 54.0 0.1 0.9 0

9 101.8 313 149.6 50.3 0.2 -0.1 1

10 112.6 279 171.9 64.2 -0.1 -1.0 0

11 122.1 331 182.5 87.0 -0.0 -1.0 0

12 132.9 202 202.6 150.5 0.6 1.0 0

13 141.1 199 199.4 72.9 1.0 0.2 1

14 149.1 151 205.0 72.9 1.0 -0.8 0

15 158.3 119 178.5 104.8 1.0 -0.9 0

16 167.7 104 175.2 104.9 1.0 0.8 0

17 176.6 127 205.8 84.2 1.0 -0.3 1

18 -175.5 46 270.5 93.6 0.1 -1.0 0

19 -166.1 69 289.7 108.2 -1.0 -0.1 0

20 -158.2 72 302.1 110.8 -1.0 -0.1 0

21 -150.3 118 297.2 115.7 -1.0 -0.3 0

22 -141.0 165 285.9 127.1 1.0 -0.5 0

23 -131.8 67 193.4 134.6 1.0 -0.4 0

24 -122.6 66 160.3 139.1 1.0 -1.0 0

25 -113.3 108 137.1 204.3 0.7 0.1 0

26 -104.4 102 120.0 205.7 0.3 -0.3 0

27 -95.2 68 118.4 195.8 -0.9 0.6 0

28 -86.1 96 50.9 163.2 -0.6 1.0 0

29 -77.2 104 47.9 172.6 0.1 -0.1 1

30 -68.0 110 57.8 178.1 1.0 0.1 0

31 -58.8 120 80.4 194.0 1.0 0.2 0

32 -49.1 123 147.1 208.7 -0.2 -0.1 0

33 -39.2 103 72.1 214.0 -0.1 0.0 1

34 -29.5 184 69.7 205.2 0.2 0.8 1

35 -21.1 205 101.0 176.7 0.0 0.9 0

RunToDaylightV2(): After REV sort, arrays are

Idx Hdg FrontD LeftD RightD LSteer RSteer Avail

0 93.8 607 165.8 54.0 0.1 0.9 0

1 82.9 410 196.8 122.4 -0.1 -0.3 0

2 122.1 331 182.5 87.0 -0.0 -1.0 0

3 101.8 313 149.6 50.3 0.2 -0.1 1

4 112.6 279 171.9 64.2 -0.1 -1.0 0

5 54.3 226 123.2 200.4 0.6 1.0 0

6 34.9 208 115.9 309.9 0.2 0.7 0

7 -21.1 205 101.0 176.7 0.0 0.9 0

8 132.9 202 202.6 150.5 0.6 1.0 0

9 141.1 199 199.4 72.9 1.0 0.2 1

10 44.7 195 127.8 264.0 0.3 0.5 0

11 65.1 192 198.4 150.5 0.6 0.4 0

12 -29.5 184 69.7 205.2 0.2 0.8 1

13 24.6 171 110.7 292.3 0.1 0.3 0

14 -141.0 165 285.9 127.1 1.0 -0.5 0

15 16.1 162 109.2 283.6 0.1 1.0 0

16 73.3 159 205.1 136.7 0.1 0.2 0

17 149.1 151 205.0 72.9 1.0 -0.8 0

18 176.6 127 205.8 84.2 1.0 -0.3 1

19 -49.1 123 147.1 208.7 -0.2 -0.1 0

20 -58.8 120 80.4 194.0 1.0 0.2 0

21 158.3 119 178.5 104.8 1.0 -0.9 0

22 -150.3 118 297.2 115.7 -1.0 -0.3 0

23 -68.0 110 57.8 178.1 1.0 0.1 0

24 -113.3 108 137.1 204.3 0.7 0.1 0

25 167.7 104 175.2 104.9 1.0 0.8 0

26 -77.2 104 47.9 172.6 0.1 -0.1 1

27 -39.2 103 72.1 214.0 -0.1 0.0 1

28 -104.4 102 120.0 205.7 0.3 -0.3 0

29 -86.1 96 50.9 163.2 -0.6 1.0 0

30 -158.2 72 302.1 110.8 -1.0 -0.1 0

31 -166.1 69 289.7 108.2 -1.0 -0.1 0

32 -95.2 68 118.4 195.8 -0.9 0.6 0

33 -131.8 67 193.4 134.6 1.0 -0.4 0

34 -122.6 66 160.3 139.1 1.0 -1.0 0

35 -175.5 46 270.5 93.6 0.1 -1.0 0

FrontD Hdg LeftD LSteer RightD RSteer

at index = 0, FrontD = 607, bWallTrackAvailable = 0

at index = 1, FrontD = 410, bWallTrackAvailable = 0

at index = 2, FrontD = 331, bWallTrackAvailable = 0

at index = 3, FrontD = 313, bWallTrackAvailable = 1

Trackable Wall found at index = 3

FrontD Hdg LeftD LSteer RightD RSteer

313 101.8 149.6 0.2 50.3 -0.1

Turning to heading = 101.8

Just after turning to 101.8

FrontD Hdg LeftD LSteer RightD RSteer

313 101.8 149.6 0.2 50.3 -0.1

In RunToDaylightV2() with trackable wall on right side: trkdir = RIGHT

RunToDaylightV2() returning RIGHT

In ANOMALY_MIRRORED_SFC case: RunToDaylightV2 returned = RIGHT

Stay tuned!

Frank

Wide Body Robot Charging Station Homing PID Tuning

Leave a reply

Posted 30 October 2023

Almost exactly two years ago I redid the robot form factor for better turning performance, as described in this post. Now, two years later when integrating the new ‘wide body’ robot with the charging station, I belatedly discovered that the PID values optimized for the old ‘narrow body’ robot don’t do so well anymore. Here’s a short video and telemetry showing the wide body robot homing with the old narrow body PID values.

Time	BattV	Fin1	Fin2	Steer	PID_Out		LSpd	RSpd	FrontD	RearD
12151	7.15	16718	26518	-0.23	54.40		104	0	181	106.1
12299	7.15	18460	25216	-0.15	62.06		112	0	182	109.3
12451	7.15	28224	15760	0.28	-28.33		21	78	144	160.4
12600	7.15	32296	8420	0.59	-133.77		0	127	108	151.9
12752	7.13	33624	10052	0.54	-184.24		0	127	122	154.4
12901	7.14	30552	20436	0.20	-155.83		0	127	171	176.5
13050	7.10	15170	36138	-0.41	-26.56		23	76	165	59.7
13201	7.15	5004	40958	-0.78	110.04		127	0	168	46.8
13351	7.09	2866	40098	-0.87	212.24		127	0	175	150.6
13502	7.12	4056	43520	-0.83	290.84		127	0	154	72.2
13652	7.11	8744	47612	-0.69	339.87		127	0	144	59.2
13801	7.14	27748	39440	-0.17	283.68		127	0	137	166.5
13940	7.14	48690	18454	0.45	146.08		127	0	96	54.7
14090	7.14	56246	6844	0.78	14.95		64	35	67	178.2
14240	7.15	56976	3392	0.89	-91.78		0	127	55	120.8
14389	7.07	60498	1158	0.96	-199.54		0	127	51	143.0
14539	7.14	61822	3338	0.90	-280.96		0	127	50	131.9
14692	7.17	59922	5744	0.83	-352.67		0	127	54	108.6
14840	7.07	62750	13846	0.64	-389.67		0	127	82	93.0
14990	7.08	53360	43756	0.10	-322.15		0	127	113	130.7
15144	7.15	19554	81548	-0.61	-155.74		0	127	110	145.0
15292	7.08	8156	85468	-0.83	-36.28		13	86	120	242.8
15441	7.13	3416	87866	-0.93	72.27		122	0	201	77.9
15591	7.10	104	74786	-1.00	183.08		127	0	201	65.7
15741	7.16	356	79724	-0.99	282.05		127	0	181	78.6
15894	7.11	4142	86572	-0.91	361.32		127	0	104	269.3
16044	7.06	9450	80994	-0.79	423.13		127	0	90	203.0
16194	7.16	24020	87242	-0.57	447.58		127	0	86	157.9
16344	7.16	79906	71806	0.05	352.99		127	0	85	114.7
16494	7.11	132774	34106	0.59	212.35		127	0	62	198.9
16644	7.07	151746	16970	0.80	98.02		127	0	46	172.3
16795	7.16	159188	5600	0.93	-15.91		34	65	36	157.3
16945	7.07	107022	154	1.00	-126.06		0	127	32	154.7
17093	7.09	702	96	0.76	-169.37		0	127	27	159.5
17243	7.15	97170	140	1.00	-299.99		0	127	23	167.7
17393	7.14	81534	760	0.98	-398.34		0	127	22	182.6
17543	7.09	55520	6772	0.78	-448.58		0	127	24	195.7
17695	7.10	26502	14270	0.30	-409.04		0	127	39	152.8
17847	7.15	22402	48462	-0.37	-273.95		0	127	55	167.9
17997	7.14	23774	142296	-0.71	-147.47		0	127	58	252.3
18147	7.14	19992	229632	-0.84	-42.37		7	92	61	89.1
18297	7.09	14950	334642	-0.91	60.79		110	0	73	24.5
18447	7.14	926	319120	-0.99	172.12		127	0	78	21.7
18597	7.14	686	329866	-1.00	272.73		127	0	64	30.3
18747	7.10	5674	297274	-0.96	364.34		127	0	47	47.2
18897	7.11	23964	217550	-0.80	421.62		127	0	38	98.2
19047	7.13	43030	130940	-0.51	429.07		127	0	32	258.8
19196	7.12	42354	50240	-0.09	375.80		127	0	30	233.4
19346	7.05	38416	33180	0.07	342.13		127	0	29	232.1
19497	7.03	37318	30834	0.10	327.95		127	0	31	232.1
19647	7.13	36808	29562	0.11	314.85		127	0	31	225.2
19797	7.15	36246	28320	0.12	300.53		127	0	31	221.5
19947	7.12	36002	27646	0.13	286.07		127	0	31	231.1
20097	7.04	35608	26952	0.14	271.16		127	0	31	231.0
20250	7.15	34844	25892	0.15	255.09		127	0	32	227.0
20400	7.12	34082	24688	0.16	237.27		127	0	31	226.0
20550	7.06	34030	24474	0.16	220.32		127	0	31	225.7
20700	7.14	33804	24126	0.17	203.06		127	0	32	227.2
20850	7.15	32978	23444	0.17	185.85		127	0	33	232.2
21000	7.15	32616	22774	0.18	166.85		127	0	32	227.7
21150	7.03	32336	22540	0.18	148.79		127	0	32	234.7
21300	7.04	32276	22310	0.18	129.96		127	0	32	247.8
21448	7.15	31906	22096	0.18	111.88		127	0	30	248.8
21600	7.11	31190	21294	0.19	92.07		127	0	31	250.4
21748	7.02	30936	20890	0.19	71.88		121	0	30	251.4
21900	7.04	30622	20432	0.20	51.06		101	0	31	249.8
22050	7.15	29956	19812	0.20	30.03		80	19	32	253.4
22200	7.14	27300	18558	0.19	12.77		62	37	31	252.7
22352	7.15	19732	15254	0.13	8.87		58	41	32	254.0
22500	7.13	5248	9828	-0.30	100.33		127	0	31	264.7
22652	7.14	1274	6706	-0.68	225.48		127	0	32	292.1
22802	7.10	980	6492	-0.74	311.00		127	0	31	294.7
22952	7.01	1018	6488	-0.73	383.19		127	0	31	293.5
23101	7.14	980	6594	-0.74	458.97		127	0	30	297.1
23250	7.12	954	6512	-0.74	533.99		127	0	30	293.4
23401	7.06	994	6568	-0.74	606.70		127	0	29	295.6
23540	7.06	1022	6566	-0.73	678.79		127	0	31	293.4
23688	7.14	924	6568	-0.75	757.23		127	0	30	293.2
23840	7.14	930	6498	-0.75	831.90		127	0	31	295.3
23988	7.07	920	6492	-0.75	907.34		127	0	29	293.0
24138	7.03	898	6540	-0.76	984.16		127	0	29	293.4
24290	7.15	894	6492	-0.76	1059.94		127	0	31	295.8
24438	7.12	834	6436	-0.77	1138.76		127	0	30	294.4
24588	7.08	830	6540	-0.77	1216.95		127	0	30	293.6
24741	7.13	840	6474	-0.77	1293.39		127	0	30	294.3
24892	7.13	734	6454	-0.80	1376.49		127	0	31	294.2
25041	7.08	746	6446	-0.79	1455.55		127	0	30	295.3
25193	7.14	760	6338	-0.79	1533.17		127	0	29	292.2
25343	7.14	716	6394	-0.80	1614.74		127	0	30	293.8
25493	7.11	816	6392	-0.77	1688.73		127	0	29	297.0
25643	7.06	772	6310	-0.78	1767.85		127	0	30	293.0
25795	7.05	798	6250	-0.77	1844.11		127	0	31	292.4
25943	7.14	820	6268	-0.77	1920.20		127	0	30	290.9
26093	7.11	842	6174	-0.76	1994.94		127	0	30	294.8
26243	7.05	902	6196	-0.75	2067.46		127	0	30	293.2
26393	7.03	914	6128	-0.74	2140.60		127	0	30	293.6
26543	7.14	924	6058	-0.74	2213.36		127	0	30	291.7
26695	7.13	884	6034	-0.74	2289.03		127	0	31	290.6
Stuck in IRHomeToChargingStation() - Attempting to recover

100

Time BattV Fin1 Fin2 Steer PID_Out LSpd RSpd FrontD RearD

12151 7.15 16718 26518 -0.23 54.40 104 0 181 106.1

12299 7.15 18460 25216 -0.15 62.06 112 0 182 109.3

12451 7.15 28224 15760 0.28 -28.33 21 78 144 160.4

12600 7.15 32296 8420 0.59 -133.77 0 127 108 151.9

12752 7.13 33624 10052 0.54 -184.24 0 127 122 154.4

12901 7.14 30552 20436 0.20 -155.83 0 127 171 176.5

13050 7.10 15170 36138 -0.41 -26.56 23 76 165 59.7

13201 7.15 5004 40958 -0.78 110.04 127 0 168 46.8

13351 7.09 2866 40098 -0.87 212.24 127 0 175 150.6

13502 7.12 4056 43520 -0.83 290.84 127 0 154 72.2

13652 7.11 8744 47612 -0.69 339.87 127 0 144 59.2

13801 7.14 27748 39440 -0.17 283.68 127 0 137 166.5

13940 7.14 48690 18454 0.45 146.08 127 0 96 54.7

14090 7.14 56246 6844 0.78 14.95 64 35 67 178.2

14240 7.15 56976 3392 0.89 -91.78 0 127 55 120.8

14389 7.07 60498 1158 0.96 -199.54 0 127 51 143.0

14539 7.14 61822 3338 0.90 -280.96 0 127 50 131.9

14692 7.17 59922 5744 0.83 -352.67 0 127 54 108.6

14840 7.07 62750 13846 0.64 -389.67 0 127 82 93.0

14990 7.08 53360 43756 0.10 -322.15 0 127 113 130.7

15144 7.15 19554 81548 -0.61 -155.74 0 127 110 145.0

15292 7.08 8156 85468 -0.83 -36.28 13 86 120 242.8

15441 7.13 3416 87866 -0.93 72.27 122 0 201 77.9

15591 7.10 104 74786 -1.00 183.08 127 0 201 65.7

15741 7.16 356 79724 -0.99 282.05 127 0 181 78.6

15894 7.11 4142 86572 -0.91 361.32 127 0 104 269.3

16044 7.06 9450 80994 -0.79 423.13 127 0 90 203.0

16194 7.16 24020 87242 -0.57 447.58 127 0 86 157.9

16344 7.16 79906 71806 0.05 352.99 127 0 85 114.7

16494 7.11 132774 34106 0.59 212.35 127 0 62 198.9

16644 7.07 151746 16970 0.80 98.02 127 0 46 172.3

16795 7.16 159188 5600 0.93 -15.91 34 65 36 157.3

16945 7.07 107022 154 1.00 -126.06 0 127 32 154.7

17093 7.09 702 96 0.76 -169.37 0 127 27 159.5

17243 7.15 97170 140 1.00 -299.99 0 127 23 167.7

17393 7.14 81534 760 0.98 -398.34 0 127 22 182.6

17543 7.09 55520 6772 0.78 -448.58 0 127 24 195.7

17695 7.10 26502 14270 0.30 -409.04 0 127 39 152.8

17847 7.15 22402 48462 -0.37 -273.95 0 127 55 167.9

17997 7.14 23774 142296 -0.71 -147.47 0 127 58 252.3

18147 7.14 19992 229632 -0.84 -42.37 7 92 61 89.1

18297 7.09 14950 334642 -0.91 60.79 110 0 73 24.5

18447 7.14 926 319120 -0.99 172.12 127 0 78 21.7

18597 7.14 686 329866 -1.00 272.73 127 0 64 30.3

18747 7.10 5674 297274 -0.96 364.34 127 0 47 47.2

18897 7.11 23964 217550 -0.80 421.62 127 0 38 98.2

19047 7.13 43030 130940 -0.51 429.07 127 0 32 258.8

19196 7.12 42354 50240 -0.09 375.80 127 0 30 233.4

19346 7.05 38416 33180 0.07 342.13 127 0 29 232.1

19497 7.03 37318 30834 0.10 327.95 127 0 31 232.1

19647 7.13 36808 29562 0.11 314.85 127 0 31 225.2

19797 7.15 36246 28320 0.12 300.53 127 0 31 221.5

19947 7.12 36002 27646 0.13 286.07 127 0 31 231.1

20097 7.04 35608 26952 0.14 271.16 127 0 31 231.0

20250 7.15 34844 25892 0.15 255.09 127 0 32 227.0

20400 7.12 34082 24688 0.16 237.27 127 0 31 226.0

20550 7.06 34030 24474 0.16 220.32 127 0 31 225.7

20700 7.14 33804 24126 0.17 203.06 127 0 32 227.2

20850 7.15 32978 23444 0.17 185.85 127 0 33 232.2

21000 7.15 32616 22774 0.18 166.85 127 0 32 227.7

21150 7.03 32336 22540 0.18 148.79 127 0 32 234.7

21300 7.04 32276 22310 0.18 129.96 127 0 32 247.8

21448 7.15 31906 22096 0.18 111.88 127 0 30 248.8

21600 7.11 31190 21294 0.19 92.07 127 0 31 250.4

21748 7.02 30936 20890 0.19 71.88 121 0 30 251.4

21900 7.04 30622 20432 0.20 51.06 101 0 31 249.8

22050 7.15 29956 19812 0.20 30.03 80 19 32 253.4

22200 7.14 27300 18558 0.19 12.77 62 37 31 252.7

22352 7.15 19732 15254 0.13 8.87 58 41 32 254.0

22500 7.13 5248 9828 -0.30 100.33 127 0 31 264.7

22652 7.14 1274 6706 -0.68 225.48 127 0 32 292.1

22802 7.10 980 6492 -0.74 311.00 127 0 31 294.7

22952 7.01 1018 6488 -0.73 383.19 127 0 31 293.5

23101 7.14 980 6594 -0.74 458.97 127 0 30 297.1

23250 7.12 954 6512 -0.74 533.99 127 0 30 293.4

23401 7.06 994 6568 -0.74 606.70 127 0 29 295.6

23540 7.06 1022 6566 -0.73 678.79 127 0 31 293.4

23688 7.14 924 6568 -0.75 757.23 127 0 30 293.2

23840 7.14 930 6498 -0.75 831.90 127 0 31 295.3

23988 7.07 920 6492 -0.75 907.34 127 0 29 293.0

24138 7.03 898 6540 -0.76 984.16 127 0 29 293.4

24290 7.15 894 6492 -0.76 1059.94 127 0 31 295.8

24438 7.12 834 6436 -0.77 1138.76 127 0 30 294.4

24588 7.08 830 6540 -0.77 1216.95 127 0 30 293.6

24741 7.13 840 6474 -0.77 1293.39 127 0 30 294.3

24892 7.13 734 6454 -0.80 1376.49 127 0 31 294.2

25041 7.08 746 6446 -0.79 1455.55 127 0 30 295.3

25193 7.14 760 6338 -0.79 1533.17 127 0 29 292.2

25343 7.14 716 6394 -0.80 1614.74 127 0 30 293.8

25493 7.11 816 6392 -0.77 1688.73 127 0 29 297.0

25643 7.06 772 6310 -0.78 1767.85 127 0 30 293.0

25795 7.05 798 6250 -0.77 1844.11 127 0 31 292.4

25943 7.14 820 6268 -0.77 1920.20 127 0 30 290.9

26093 7.11 842 6174 -0.76 1994.94 127 0 30 294.8

26243 7.05 902 6196 -0.75 2067.46 127 0 30 293.2

26393 7.03 914 6128 -0.74 2140.60 127 0 30 293.6

26543 7.14 924 6058 -0.74 2213.36 127 0 30 291.7

26695 7.13 884 6034 -0.74 2289.03 127 0 31 290.6

Stuck in IRHomeToChargingStation() - Attempting to recover

I strongly suspect that the new form factor will require a significantly different set of PID values. Here

04 November 2023 Update:

Well, I went down a bit of a rabbit-hole on this one, but I think I have successfully found my way back to the real world. It’s been a while since I’ve had to do any PID tuning, so my skills (and code) have atrophied significantly. Fortunately, my former self left me some rudimentary clues, so I didn’t have to recreate everything from scratch.

In previous tuning sessions, I had constructed a test harness that accepted PID values on the command line of the OTA serial connection to the robot, allowing rapid parameter optimization. Unfortunately, I didn’t document this in my previous posts (what was I thinking!), so it took me a while to find some previous programs that contained the desired test harness code. After porting the test harness code to my current WallE3_Quicksort_V4 program and modifying it a bit for more testing convenience, I am documenting it here so a future me will have a better chance of avoiding unnecessary work.

PID Tuning Test Harness:

The idea behind the PID tuning test harness is to allow the user to input PID (and other) parameters on the command line and have the robot execute the desired action – in this case homing to the charging station – without having to go through the entire program structure. The test harness code is placed at the end of the setup() routine and doesn’t exit but returns to the point where a new set of parameters can be provided. This allows for rapid iteration over the PID parameter space. Here is the code as it appears in WallE3_Quicksort_V4.ino

#pragma region CHG STATION PID TUNING TEST
  while (true)
  {
    //gl_pSerPort->printf("Press any key to run chg station PID tuning test\n");

    //while (!gl_pSerPort->available())
    //{
    //  delay(200);
    //  //gl_pSerPort->printf(".\n");
    //}
    //while (gl_pSerPort->available())
    //{
    //  gl_pSerPort->read();
    //}

#pragma region PARAMETER CAPTURE
    //gl_pSerPort->printf("\n\nWallE3_WallTrackTuning_V5: enter Wall (1 = right/0 = left), offset(cm), PID triplet Kp,Ki,Kd\n");
    gl_pSerPort->printf("\n\n%s: enter init spd, PID triplet Kp,Ki,Kd\n", __FILENAME__);

    //09/25/22 wait here for user input
    while (!gl_pSerPort->available())
    {
      delay(100);
    }

    const int bufflen = 6;
    char buff[bufflen];
    memset(buff, 0, bufflen);

    gl_pSerPort->printf("At top of parameter retrieval section\n");

    String initspdstr = gl_pSerPort->readStringUntil(',');
    initspdstr.toCharArray(buff, bufflen);
    gl_pSerPort->printf("initspdstr = %s\n", buff);
    int initspds = initspdstr.toInt();

    String Pstr = gl_pSerPort->readStringUntil(',');
    Pstr.toCharArray(buff, bufflen);
    gl_pSerPort->printf("Pstr = %s\n", buff);
    float ChgStn_Kp = Pstr.toFloat();

    String Istr = gl_pSerPort->readStringUntil(',');
    Istr.toCharArray(buff, bufflen);
    gl_pSerPort->printf("Istr = %s\n", buff);
    float ChgStn_Ki = Istr.toFloat();

    String Dstr = gl_pSerPort->readStringUntil(',');
    Dstr.toCharArray(buff, bufflen);
    gl_pSerPort->printf("Dstr = %s\n", buff);
    float ChgStn_Kd = Dstr.toFloat();

    gl_pSerPort->printf("%s: Charge Station PID Tuning with initspds %d, PID = (%2.1f,%2.1f,%2.1f)\n", __FILENAME__,
      initspds, ChgStn_Kp, ChgStn_Ki, ChgStn_Kd);
#pragma endregion PARAMETER CAPTURE


    gl_pSerPort->printf("Charge Station PID Tuning: Input Kp, Ki, Kd\n");

    gl_pSerPort->printf("Before IRHomeToChgStnNoPings() with gl_bChgConnect = %d\n", gl_bChgConnect);
    gl_bChgConnect = IRHomeToChgStnNoPingsPID(initspds, ChgStn_Kp, ChgStn_Ki, ChgStn_Kd);
    gl_pSerPort->printf("After IRHomeToChgStnNoPings() with gl_bChgConnect = %d\n", gl_bChgConnect);

    if (gl_bChgConnect)
    {
      gl_pSerPort->printf("Connected to Charger - Stopping Motors!\n");
      StopBothMotors();
    }
    while (CheckForUserInput())//11/04/23 now CheckForUserInput() returns a bool value
    {
      delay(200);
    }
  }
#pragma endregion CHG STATION PID TUNING TEST

#pragma region CHG STATION PID TUNING TEST

while (true)

{

//gl_pSerPort->printf("Press any key to run chg station PID tuning test\n");

//while (!gl_pSerPort->available())

//{

// delay(200);

// //gl_pSerPort->printf(".\n");

//}

//while (gl_pSerPort->available())

//{

// gl_pSerPort->read();

//}

#pragma region PARAMETER CAPTURE

//gl_pSerPort->printf("\n\nWallE3_WallTrackTuning_V5: enter Wall (1 = right/0 = left), offset(cm), PID triplet Kp,Ki,Kd\n");

gl_pSerPort->printf("\n\n%s: enter init spd, PID triplet Kp,Ki,Kd\n", __FILENAME__);

//09/25/22 wait here for user input

while (!gl_pSerPort->available())

{

delay(100);

}

const int bufflen = 6;

char buff[bufflen];

memset(buff, 0, bufflen);

gl_pSerPort->printf("At top of parameter retrieval section\n");

String initspdstr = gl_pSerPort->readStringUntil(',');

initspdstr.toCharArray(buff, bufflen);

gl_pSerPort->printf("initspdstr = %s\n", buff);

int initspds = initspdstr.toInt();

String Pstr = gl_pSerPort->readStringUntil(',');

Pstr.toCharArray(buff, bufflen);

gl_pSerPort->printf("Pstr = %s\n", buff);

float ChgStn_Kp = Pstr.toFloat();

String Istr = gl_pSerPort->readStringUntil(',');

Istr.toCharArray(buff, bufflen);

gl_pSerPort->printf("Istr = %s\n", buff);

float ChgStn_Ki = Istr.toFloat();

String Dstr = gl_pSerPort->readStringUntil(',');

Dstr.toCharArray(buff, bufflen);

gl_pSerPort->printf("Dstr = %s\n", buff);

float ChgStn_Kd = Dstr.toFloat();

gl_pSerPort->printf("%s: Charge Station PID Tuning with initspds %d, PID = (%2.1f,%2.1f,%2.1f)\n", __FILENAME__,

initspds, ChgStn_Kp, ChgStn_Ki, ChgStn_Kd);

#pragma endregion PARAMETER CAPTURE

gl_pSerPort->printf("Charge Station PID Tuning: Input Kp, Ki, Kd\n");

gl_pSerPort->printf("Before IRHomeToChgStnNoPings() with gl_bChgConnect = %d\n", gl_bChgConnect);

gl_bChgConnect = IRHomeToChgStnNoPingsPID(initspds, ChgStn_Kp, ChgStn_Ki, ChgStn_Kd);

gl_pSerPort->printf("After IRHomeToChgStnNoPings() with gl_bChgConnect = %d\n", gl_bChgConnect);

if (gl_bChgConnect)

{

gl_pSerPort->printf("Connected to Charger - Stopping Motors!\n");

StopBothMotors();

}

while (CheckForUserInput())//11/04/23 now CheckForUserInput() returns a bool value

{

delay(200);

}

#pragma endregion CHG STATION PID TUNING TEST

Note that the ‘CheckForUserInput()’ function calls in the above assume a boolean return value; this change was necessary to allow the test harness to be run multiple times without having to reboot the robot code. This also required adding a new function key (*) to the list of keys handled by CheckForUserInput(). Here are the new versions of the two flavors of CheckForUserInput():

bool CheckForUserInput()
{
  //Notes:
  //  11/21/21 rev to accomodate cmd input from either serial, and output to both
  //  12/25/21 another try at using pointer for Serial object
  //  09/19/22 Serial object is now global - no need to figure it out here
  //  09/25/22 remove if (incomingByte != 0), extend if (gl_pSerPort->available() > 0) to entire function
  //  09/25/22 chg return type from void to char so 'auto' (Aa) exit can be detected
  //  09/27/22 back to void type - 'Aa' now soft-reboots the processor
  //  09/27/22 now this function just grabs character & calls CheckForUserInput(char in_char)
  //  11/04/23 chg to bool ret value so can use to exit calling while() loop if necessary

  //DEBUG!!
  //gl_pSerPort->printf("%lu: In CheckForUserInput() with gl_pSerPort->available() = %s\n",(uint32_t)gl_ElapsedRunMillisec, gl_pSerPort->available() > 0?"TRUE":"FALSE");

  const int bufflen = 3;
  char buff[bufflen];
  memset(buff, 0, bufflen);
  byte incomingByte = 0; //moved here 11/21/21
  //int numchars = 0;
  bool retval = true; //11/04/23 chg to bool ret value so can use to exit calling while() loop if necessary

  if (gl_pSerPort->available() > 0)
  {
    // read the incoming byte://05/08/23 this relies on having 'Both CR & LF' 
    // enabled at the bottom of the serial port window
    gl_pSerPort->readBytesUntil('\n', buff, sizeof(buff));
    incomingByte = buff[0];
    //gl_pSerPort->printf("%lu: In CheckForUserInput() with byte = %c\n",(uint32_t)gl_ElapsedRunMillisec, incomingByte);

    // say what you got:
    //gl_pSerPort->printf("I received %d chars, first char 0X%X\n", numchars, incomingByte)

    //09/27/22 now just call CheckForUserInput(incomingByte)
    retval = CheckForUserInput(incomingByte);
  }
  return retval;//11/04/23 chg to bool ret value so can use to exit calling while() loop if necessary 
}

//09/17/22 added for use in situations where calling program manages serial input
//void CheckForUserInput(char in_char)
bool CheckForUserInput(char in_char)//11/04/23 chg to bool ret value so can use to exit calling while() loop if necessary
{
  //gl_pSerPort->printf("%lu: At top of CheckForUserInput(0X%X)\n", (uint32_t)gl_ElapsedRunMillisec, in_char);
//Purpose: Check for user override
//Inputs: in_char = char object representing user override input
//Outputs: override actions taken.  Returns if 'auto' mode selected
//Notes:
//  09/19/22 copied from CheckUserInput() and modified to accept char argument 
//  09/28/22 'Aa' now causes a processor reboot
//  11/04/23 '*' now causes fcn to return FALSE

  const int bufflen = 3;
  char buff[bufflen];
  memset(buff, 0, bufflen);
  byte incomingByte = 0; //moved here 11/21/21
  bool retval = true; //11/04/23 chg to bool ret value so can use to exit calling while() loop if necessary

  buff[0] = in_char; //need to to this as %s only works with char[]
  //gl_pSerPort->printf("In CheckForUserInput(%s)\n",buff);
  if (in_char != 0)
  {
    //gl_pSerPort->printf("%lu: In CheckForUserInput() just before switch() with in_char = %c\n", (uint32_t)gl_ElapsedRunMillisec, in_char);
    switch (in_char)
    {
    case 0x55: //ASCII 'U'
    case 0x75: //ASCII 'u'
#pragma region FIRMWARE_UPDATE_MAIN
      StopBothMotors();
      gl_pSerPort->printf(F("Start Program Update - Send new HEX file!"));

      //09/20/21 copied from FlasherX - loop()
      if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)
      {
        gl_pSerPort->printf("unable to create buffer\n"); Serial.flush();

        for (;;) {}
      }

      gl_pSerPort->printf("buffer = %1luK %s (%08lX - %08lX)\n",
        buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",
        buffer_addr, buffer_addr + buffer_size);

      //09/20/21 clear the serial buffer
      while (gl_pSerPort->available())
      {
        gl_pSerPort->read();
      }

      // receive hex file via serial, write new firmware to flash, clean up, reboot
      update_firmware(&Serial1, buffer_addr, buffer_size); // no return if success

      // return from update_firmware() means error or user abort, so clean up and
      // reboot to ensure that static vars get boot-up initialized before retry
      gl_pSerPort->printf("erase FLASH buffer / free RAM buffer...\n");
      firmware_buffer_free(buffer_addr, buffer_size);
      Serial1.flush();
      REBOOT;
#pragma endregion FIRMWARE_UPDATE_MAIN //doesn't return
      break;
    case 0x43: //ASCII 'C'
    case 0x63: //ASCII 'c'
#pragma region COMMAND_MODE
      gl_pSerPort->printf("%lu: At top of COMMAND_MODE Case\n", (uint32_t)gl_ElapsedRunMillisec);
      gl_pSerPort->printf(F("ENTERING COMMAND MODE:\n"));
      gl_pSerPort->printf(F("0 = 180 deg CCW Turn\n"));
      gl_pSerPort->printf(F("1 = 180 deg CW Turn\n"));
      //gl_pSerPort->println(F("A = Back to Auto Mode"));
      gl_pSerPort->println(F("A = Abort - Reboots Processor"));//rev 09/27/22
      //gl_pSerPort->printf(F("S = Stop\n"));//c/o 05/08/23
      gl_pSerPort->printf(F("/ = Forward\n"));
      gl_pSerPort->printf(F(".(dot) = Reverse\n"));
      gl_pSerPort->printf(F("* = Exit ChkForUserInput()\n"));
      gl_pSerPort->printf(F("\n"));
      gl_pSerPort->printf(F("       Faster\n"));
      gl_pSerPort->printf(F("\t8\n"));
      gl_pSerPort->printf(F("Left 4\t5  6 Right\n"));
      gl_pSerPort->printf(F("\t2\n"));
      gl_pSerPort->printf(F("       Slower\n"));

      //gl_pSerPort->printf("%lu: Just before StopBothMotors()\n", (uint32_t)gl_ElapsedRunMillisec);
      StopBothMotors();
      int speed = 0;
      //bool bAutoMode = false;
      //gl_pSerPort->printf("Just before while (gl_pSerPort->available())\n");

      //int res = gl_pSerPort->available();
      //int res = Serial.available();
      //gl_pSerPort->printf("gl_pSerPort->available() returned %d\n", res);


      while (gl_pSerPort->available())
      {
        incomingByte = gl_pSerPort->read();
        //gl_pSerPort->printf("%lu: I removed 0X%X from Serial1\n",millis(), incomingByte);
        //delay(200);
      }

      //gl_pSerPort->printf("Just after while (gl_pSerPort->available())\n");
      incomingByte = 0;
      bool bDone = false;//added 11/04/23 to implement user-controlled exit

      //while (1) //09/27/22 removed bAutoMode check - now 'Aa' reboots processor
      while (!bDone) //09/27/22 removed bAutoMode check - now 'Aa' reboots processor
      {
        //12/25/21 now using Stream* for serial
        if (gl_pSerPort->available() > 0)
        {
          // read the incoming bytes:
          gl_pSerPort->readBytesUntil('\n', buff, sizeof(buff));
          incomingByte = buff[0];

          // say what you got:
          //gl_pSerPort->printf("I received %s (0X%x)\n", buff, buff);

          while (gl_pSerPort->available())
          {
            gl_pSerPort->read();
            //gl_pSerPort->printf("I removed 0X%X from Serial1\n", incomingByte);
            //gl_pSerPort->printf("%lu: I removed 0X%X from Serial1\n", (uint32_t)gl_ElapsedRunMillisec, incomingByte);
          }
        }

        //11/21/21 incomingByte can come from either serial input
        if (incomingByte != 0)
        {
          //gl_pSerPort->printf("%lu: Top of Switch Statement\n", (uint32_t)gl_ElapsedRunMillisec);
          switch (incomingByte)
          {
          case 0x30: //Dec '0'
            gl_pSerPort->printf(F("CCW 180 deg Turn\n"));
            SpinTurn(true, 180, 90);
            MoveAhead(speed, speed);
            break;
          case 0x31: //Dec '1'
            gl_pSerPort->printf(F("CW 180 deg Turn\n"));
            SpinTurn(false, 180, 45);
            break;
          case 0x34: //Turn left 10 deg and keep moving
            //gl_pSerPort->printf("%lu: In CCW 10 deg turn block\n", (uint32_t)gl_ElapsedRunMillisec);
            gl_pSerPort->printf(F("CCW 10 deg Turn\n"));
            SpinTurn(true, 10, 30);

            if (bIsForwardDir)
            {
              MoveAhead(speed, speed);
            }
            else
            {
              MoveReverse(speed, speed);
            }
            break;
          case 0x36: //Turn right 10 deg and keep moving
            //gl_pSerPort->print("CW 10 deg Turn\n");
            gl_pSerPort->printf(F("CW 10 deg Turn\n"));
            SpinTurn(false, 10, 30);

            //added 05/03/20
            if (bIsForwardDir)
            {
              MoveAhead(speed, speed);
            }
            else
            {
              MoveReverse(speed, speed);
            }
            break;
          case 0x38: //Speed up 
            speed += 50;
            speed = (speed >= MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : speed;
            //gl_pSerPort->printf("Speeding up: speed now %d\n", speed);
            if (bIsForwardDir)
            {
              MoveAhead(speed, speed);
            }
            else
            {
              MoveReverse(speed, speed);
            }
            break;
          case 0x32: //Slow down 
            speed -= 50;
            speed = (speed < 0) ? 0 : speed;
            //gl_pSerPort->printf("Slowing down: speed now %d\n", speed);
            if (bIsForwardDir)
            {
              MoveAhead(speed, speed);
            }
            else
            {
              MoveReverse(speed, speed);
            }
            break;
          case 0x35: //05/07/20 changed to use '5' vs 'S'
            //gl_pSerPort->println(F("Stopping Motors!"));
            StopBothMotors();
            speed = 0;
            break;
          case 0x41: //ASCII 'A'
          case 0x61: //ASCII 'a'
            StopBothMotors();

            //09/27/22 rev to execute soft reboot
            gl_pSerPort->printf(F("Received 'A' or 'a' - Rebooting in 1 second...\n"));
            delay(1000);
            REBOOT;
            break;
          case 0x2E: //ASCII '.' (dot)
            gl_pSerPort->printf(F("Setting both motors to reverse\n"));
            bIsForwardDir = false;
            MoveReverse(speed, speed);
            break;
            //case 0x46: //ASCII 'F'
            //case 0x66: //ASCII 'f'
          case 0x2F: //ASCII '/'
            gl_pSerPort->printf(F("Setting both motors to forward\n"));
            bIsForwardDir = true;
            MoveAhead(speed, speed);
#pragma endregion COMMAND_MODE //only returns for 'a' (auto) input
            break;

            //01/11/22 copied here from main switch statement to allow firmware updates
            //even when in 'command' mode
          case 0x55: //ASCII 'U'
          case 0x75: //ASCII 'u'
#pragma region FIRMWARE UPDATE
            StopBothMotors();
            gl_pSerPort->printf(F("Start Program Update - Send new HEX file!\n"));

            //09/20/21 copied from FlasherX - loop()
            if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)
            {
              gl_pSerPort->printf("unable to create buffer\n"); Serial.flush();

              for (;;) {}
            }

            gl_pSerPort->printf("buffer = %1luK %s (%08lX - %08lX)\n",
              buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",
              buffer_addr, buffer_addr + buffer_size);

            //09/20/21 clear the serial buffer
            while (gl_pSerPort->available())
            {
              gl_pSerPort->read();
            }

            // receive hex file via serial, write new firmware to flash, clean up, reboot
            update_firmware(&Serial1, buffer_addr, buffer_size); // no return if success

            // return from update_firmware() means error or user abort, so clean up and
            // reboot to ensure that static vars get boot-up initialized before retry
            gl_pSerPort->printf("erase FLASH buffer / free RAM buffer...\n");
            firmware_buffer_free(buffer_addr, buffer_size);
            Serial1.flush();
            REBOOT;
#pragma endregion FIRMWARE UPDATE  //doesn't return
            break;
          case 0x2A: //ASCII '*' //11/04/23 added to force FALSE return
            StopBothMotors();
            gl_pSerPort->printf(F("Exiting ChkForUserInput()\n"));
            bDone = true;
            retval = false;
            break;
          default:
            gl_pSerPort->printf(F("In Default Case: Stopping Motors!\n"));
            StopBothMotors();
          }
          incomingByte = 0;
        }
      }
      gl_pSerPort->printf(F("Exited  'while (!bDone)'\n"));
    }
  }
  gl_pSerPort->printf("Returning from 'CheckForUserInput()' with retval = %s\n", retval? "TRUE" : "FALSE");
  return retval;
}

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

bool CheckForUserInput()

{

//Notes:

// 11/21/21 rev to accomodate cmd input from either serial, and output to both

// 12/25/21 another try at using pointer for Serial object

// 09/19/22 Serial object is now global - no need to figure it out here

// 09/25/22 remove if (incomingByte != 0), extend if (gl_pSerPort->available() > 0) to entire function

// 09/25/22 chg return type from void to char so 'auto' (Aa) exit can be detected

// 09/27/22 back to void type - 'Aa' now soft-reboots the processor

// 09/27/22 now this function just grabs character & calls CheckForUserInput(char in_char)

// 11/04/23 chg to bool ret value so can use to exit calling while() loop if necessary

//DEBUG!!

//gl_pSerPort->printf("%lu: In CheckForUserInput() with gl_pSerPort->available() = %s\n",(uint32_t)gl_ElapsedRunMillisec, gl_pSerPort->available() > 0?"TRUE":"FALSE");

const int bufflen = 3;

char buff[bufflen];

memset(buff, 0, bufflen);

byte incomingByte = 0; //moved here 11/21/21

//int numchars = 0;

bool retval = true; //11/04/23 chg to bool ret value so can use to exit calling while() loop if necessary

if (gl_pSerPort->available() > 0)

{

// read the incoming byte://05/08/23 this relies on having 'Both CR & LF'

// enabled at the bottom of the serial port window

gl_pSerPort->readBytesUntil('\n', buff, sizeof(buff));

incomingByte = buff[0];

//gl_pSerPort->printf("%lu: In CheckForUserInput() with byte = %c\n",(uint32_t)gl_ElapsedRunMillisec, incomingByte);

// say what you got:

//gl_pSerPort->printf("I received %d chars, first char 0X%X\n", numchars, incomingByte)

//09/27/22 now just call CheckForUserInput(incomingByte)

retval = CheckForUserInput(incomingByte);

}

return retval;//11/04/23 chg to bool ret value so can use to exit calling while() loop if necessary

}

//09/17/22 added for use in situations where calling program manages serial input

//void CheckForUserInput(char in_char)

bool CheckForUserInput(char in_char)//11/04/23 chg to bool ret value so can use to exit calling while() loop if necessary

{

//gl_pSerPort->printf("%lu: At top of CheckForUserInput(0X%X)\n", (uint32_t)gl_ElapsedRunMillisec, in_char);

//Purpose: Check for user override

//Inputs: in_char = char object representing user override input

//Outputs: override actions taken. Returns if 'auto' mode selected

//Notes:

// 09/19/22 copied from CheckUserInput() and modified to accept char argument

// 09/28/22 'Aa' now causes a processor reboot

// 11/04/23 '*' now causes fcn to return FALSE

const int bufflen = 3;

char buff[bufflen];

memset(buff, 0, bufflen);

byte incomingByte = 0; //moved here 11/21/21

bool retval = true; //11/04/23 chg to bool ret value so can use to exit calling while() loop if necessary

buff[0] = in_char; //need to to this as %s only works with char[]

//gl_pSerPort->printf("In CheckForUserInput(%s)\n",buff);

if (in_char != 0)

{

//gl_pSerPort->printf("%lu: In CheckForUserInput() just before switch() with in_char = %c\n", (uint32_t)gl_ElapsedRunMillisec, in_char);

switch (in_char)

{

case 0x55: //ASCII 'U'

case 0x75: //ASCII 'u'

#pragma region FIRMWARE_UPDATE_MAIN

StopBothMotors();

gl_pSerPort->printf(F("Start Program Update - Send new HEX file!"));

//09/20/21 copied from FlasherX - loop()

if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)

{

gl_pSerPort->printf("unable to create buffer\n"); Serial.flush();

for (;;) {}

}

gl_pSerPort->printf("buffer = %1luK %s (%08lX - %08lX)\n",

buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",

buffer_addr, buffer_addr + buffer_size);

//09/20/21 clear the serial buffer

while (gl_pSerPort->available())

{

gl_pSerPort->read();

}

// receive hex file via serial, write new firmware to flash, clean up, reboot

update_firmware(&Serial1, buffer_addr, buffer_size); // no return if success

// return from update_firmware() means error or user abort, so clean up and

// reboot to ensure that static vars get boot-up initialized before retry

gl_pSerPort->printf("erase FLASH buffer / free RAM buffer...\n");

firmware_buffer_free(buffer_addr, buffer_size);

Serial1.flush();

REBOOT;

#pragma endregion FIRMWARE_UPDATE_MAIN //doesn't return

break;

case 0x43: //ASCII 'C'

case 0x63: //ASCII 'c'

#pragma region COMMAND_MODE

gl_pSerPort->printf("%lu: At top of COMMAND_MODE Case\n", (uint32_t)gl_ElapsedRunMillisec);

gl_pSerPort->printf(F("ENTERING COMMAND MODE:\n"));

gl_pSerPort->printf(F("0 = 180 deg CCW Turn\n"));

gl_pSerPort->printf(F("1 = 180 deg CW Turn\n"));

//gl_pSerPort->println(F("A = Back to Auto Mode"));

gl_pSerPort->println(F("A = Abort - Reboots Processor"));//rev 09/27/22

//gl_pSerPort->printf(F("S = Stop\n"));//c/o 05/08/23

gl_pSerPort->printf(F("/ = Forward\n"));

gl_pSerPort->printf(F(".(dot) = Reverse\n"));

gl_pSerPort->printf(F("* = Exit ChkForUserInput()\n"));

gl_pSerPort->printf(F("\n"));

gl_pSerPort->printf(F(" Faster\n"));

gl_pSerPort->printf(F("\t8\n"));

gl_pSerPort->printf(F("Left 4\t5 6 Right\n"));

gl_pSerPort->printf(F("\t2\n"));

gl_pSerPort->printf(F(" Slower\n"));

//gl_pSerPort->printf("%lu: Just before StopBothMotors()\n", (uint32_t)gl_ElapsedRunMillisec);

StopBothMotors();

int speed = 0;

//bool bAutoMode = false;

//gl_pSerPort->printf("Just before while (gl_pSerPort->available())\n");

//int res = gl_pSerPort->available();

//int res = Serial.available();

//gl_pSerPort->printf("gl_pSerPort->available() returned %d\n", res);

while (gl_pSerPort->available())

{

incomingByte = gl_pSerPort->read();

//gl_pSerPort->printf("%lu: I removed 0X%X from Serial1\n",millis(), incomingByte);

//delay(200);

}

//gl_pSerPort->printf("Just after while (gl_pSerPort->available())\n");

incomingByte = 0;

bool bDone = false;//added 11/04/23 to implement user-controlled exit

//while (1) //09/27/22 removed bAutoMode check - now 'Aa' reboots processor

while (!bDone) //09/27/22 removed bAutoMode check - now 'Aa' reboots processor

{

//12/25/21 now using Stream* for serial

if (gl_pSerPort->available() > 0)

{

// read the incoming bytes:

gl_pSerPort->readBytesUntil('\n', buff, sizeof(buff));

incomingByte = buff[0];

// say what you got:

//gl_pSerPort->printf("I received %s (0X%x)\n", buff, buff);

while (gl_pSerPort->available())

{

gl_pSerPort->read();

//gl_pSerPort->printf("I removed 0X%X from Serial1\n", incomingByte);

//gl_pSerPort->printf("%lu: I removed 0X%X from Serial1\n", (uint32_t)gl_ElapsedRunMillisec, incomingByte);

}

//11/21/21 incomingByte can come from either serial input

if (incomingByte != 0)

{

//gl_pSerPort->printf("%lu: Top of Switch Statement\n", (uint32_t)gl_ElapsedRunMillisec);

switch (incomingByte)

{

case 0x30: //Dec '0'

gl_pSerPort->printf(F("CCW 180 deg Turn\n"));

SpinTurn(true, 180, 90);

MoveAhead(speed, speed);

break;

case 0x31: //Dec '1'

gl_pSerPort->printf(F("CW 180 deg Turn\n"));

SpinTurn(false, 180, 45);

break;

case 0x34: //Turn left 10 deg and keep moving

//gl_pSerPort->printf("%lu: In CCW 10 deg turn block\n", (uint32_t)gl_ElapsedRunMillisec);

gl_pSerPort->printf(F("CCW 10 deg Turn\n"));

SpinTurn(true, 10, 30);

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x36: //Turn right 10 deg and keep moving

//gl_pSerPort->print("CW 10 deg Turn\n");

gl_pSerPort->printf(F("CW 10 deg Turn\n"));

SpinTurn(false, 10, 30);

//added 05/03/20

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x38: //Speed up

speed += 50;

speed = (speed >= MOTOR_SPEED_MAX) ? MOTOR_SPEED_MAX : speed;

//gl_pSerPort->printf("Speeding up: speed now %d\n", speed);

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x32: //Slow down

speed -= 50;

speed = (speed < 0) ? 0 : speed;

//gl_pSerPort->printf("Slowing down: speed now %d\n", speed);

if (bIsForwardDir)

{

MoveAhead(speed, speed);

}

else

{

MoveReverse(speed, speed);

}

break;

case 0x35: //05/07/20 changed to use '5' vs 'S'

//gl_pSerPort->println(F("Stopping Motors!"));

StopBothMotors();

speed = 0;

break;

case 0x41: //ASCII 'A'

case 0x61: //ASCII 'a'

StopBothMotors();

//09/27/22 rev to execute soft reboot

gl_pSerPort->printf(F("Received 'A' or 'a' - Rebooting in 1 second...\n"));

delay(1000);

REBOOT;

break;

case 0x2E: //ASCII '.' (dot)

gl_pSerPort->printf(F("Setting both motors to reverse\n"));

bIsForwardDir = false;

MoveReverse(speed, speed);

break;

//case 0x46: //ASCII 'F'

//case 0x66: //ASCII 'f'

case 0x2F: //ASCII '/'

gl_pSerPort->printf(F("Setting both motors to forward\n"));

bIsForwardDir = true;

MoveAhead(speed, speed);

#pragma endregion COMMAND_MODE //only returns for 'a' (auto) input

break;

//01/11/22 copied here from main switch statement to allow firmware updates

//even when in 'command' mode

case 0x55: //ASCII 'U'

case 0x75: //ASCII 'u'

#pragma region FIRMWARE UPDATE

StopBothMotors();

gl_pSerPort->printf(F("Start Program Update - Send new HEX file!\n"));

//09/20/21 copied from FlasherX - loop()

if (firmware_buffer_init(&buffer_addr, &buffer_size) == 0)

{

gl_pSerPort->printf("unable to create buffer\n"); Serial.flush();

for (;;) {}

}

gl_pSerPort->printf("buffer = %1luK %s (%08lX - %08lX)\n",

buffer_size / 1024, IN_FLASH(buffer_addr) ? "FLASH" : "RAM",

buffer_addr, buffer_addr + buffer_size);

//09/20/21 clear the serial buffer

while (gl_pSerPort->available())

{

gl_pSerPort->read();

}

// receive hex file via serial, write new firmware to flash, clean up, reboot

update_firmware(&Serial1, buffer_addr, buffer_size); // no return if success

// return from update_firmware() means error or user abort, so clean up and

// reboot to ensure that static vars get boot-up initialized before retry

gl_pSerPort->printf("erase FLASH buffer / free RAM buffer...\n");

firmware_buffer_free(buffer_addr, buffer_size);

Serial1.flush();

REBOOT;

#pragma endregion FIRMWARE UPDATE //doesn't return

break;

case 0x2A: //ASCII '*' //11/04/23 added to force FALSE return

StopBothMotors();

gl_pSerPort->printf(F("Exiting ChkForUserInput()\n"));

bDone = true;

retval = false;

break;

default:

gl_pSerPort->printf(F("In Default Case: Stopping Motors!\n"));

StopBothMotors();

}

incomingByte = 0;

}

gl_pSerPort->printf(F("Exited 'while (!bDone)'\n"));

}

gl_pSerPort->printf("Returning from 'CheckForUserInput()' with retval = %s\n", retval? "TRUE" : "FALSE");

return retval;

}

Now that I’ve gotten the test harness set up, I can actually start re-tuning the charge station homing PID values for the new wide-body robot – yay! Here’s a short video and telemetry from the first run with PID(50,0,0).

WallE3_Quicksort_V4.ino: enter init spd, PID triplet Kp,Ki,Kd
At top of parameter retrieval section
initspdstr = 50
Pstr = 50
Istr = 0
Dstr = 0

WallE3_Quicksort_V4.ino: Charge Station PID Tuning with initspds 50, PID = (50.0,0.0,0.0)
Charge Station PID Tuning: Input Kp, Ki, Kd
Before IRHomeToChgStnNoPings() with gl_bChgConnect = 1
IRHomeToChgStnNoPings with detection value = 3244372, Steering = -0.36 & IRHOMING Kp,Ki,Kd = (50, 0, 0)
Time	BattV	Fin1	Fin2	Steer	PID_Out		LSpd	RSpd

3054878	7.89	13162	28260	-0.36	18.22		68	31
3055028	7.74	13910	27986	-0.34	16.80		66	33
3055178	7.87	18048	26074	-0.18	9.10		59	40
3055328	7.88	21612	25018	-0.07	3.65		53	46
3055478	7.88	23880	25120	-0.03	1.27		51	48
3055628	7.68	25674	25968	-0.01	0.28		50	49
3055778	7.87	27258	27646	-0.01	0.35		50	49
3055928	7.88	29044	29502	-0.01	0.39		50	49
3056078	7.88	30858	31926	-0.02	0.85		50	49
3056228	7.70	33248	34360	-0.02	0.82		50	49
3056378	7.86	36096	36732	-0.01	0.44		50	49
3056528	7.88	39180	39564	-0.00	0.24		50	49
3056678	7.87	43566	41626	0.02	-1.14		48	51
3056828	7.72	48448	44238	0.05	-2.27		47	52
3056978	7.88	54154	47032	0.07	-3.52		46	53
3057128	7.89	60196	51060	0.08	-4.11		45	54
3057278	7.87	67280	55664	0.09	-4.72		45	54
3057428	7.74	76266	60412	0.12	-5.80		44	55
3057578	7.86	84516	68544	0.10	-5.22		44	55
3057728	7.88	92080	81186	0.06	-3.14		46	53
3057878	7.86	101710	96840	0.02	-1.23		48	51
3058028	7.78	114304	113078	0.01	-0.27		49	50
3058178	7.86	132040	127262	0.02	-0.92		49	50
3058328	7.88	150966	151258	-0.00	0.05		50	49
3058478	7.87	172256	185534	-0.04	1.86		51	48
3058628	7.81	200434	232196	-0.07	3.67		53	46
3058778	7.84	242280	290892	-0.09	4.56		54	45
3058928	7.87	304514	357620	-0.08	4.01		54	45
3059078	7.86	387878	457728	-0.08	4.13		54	45
3059228	7.88	505394	626948	-0.11	5.37		55	44
3059378	7.84	706004	852688	-0.09	4.71		54	45
3059528	7.86	1018210	1280002	-0.11	5.70		55	44
3059678	7.87	1643146	2120322	-0.13	6.34		56	43
3059828	7.87	2409428	2447544	-0.01	0.39		50	49
3059978	8.05	2415732	2394766	0.00	-0.22		49	50
After while loop in IRHomeToChgStnNoPings
After IRHomeToChgStnNoPings() with gl_bChgConnect = 1
Connected to Charger - Stopping Motors!
3056645: At top of COMMAND_MODE Case
ENTERING COMMAND MODE:
0 = 180 deg CCW Turn
1 = 180 deg CW Turn
A = Abort - Reboots Processor
/ = Forward
.(dot) = Reverse
* = Exit ChkForUserInput()

       Faster
	8
Left 4	5  6 Right
	2
       Slower
Setting both motors to reverse
Exiting ChkForUserInput()
Exited  'while (!bDone)'
Returning from 'CheckForUserInput()' with retval = FALSE


WallE3_Quicksort_V4.ino: enter init spd, PID triplet Kp,Ki,Kd

WallE3_Quicksort_V4.ino: enter init spd, PID triplet Kp,Ki,Kd

At top of parameter retrieval section

initspdstr = 50

Pstr = 50

Istr = 0

Dstr = 0

WallE3_Quicksort_V4.ino: Charge Station PID Tuning with initspds 50, PID = (50.0,0.0,0.0)

Charge Station PID Tuning: Input Kp, Ki, Kd

Before IRHomeToChgStnNoPings() with gl_bChgConnect = 1

IRHomeToChgStnNoPings with detection value = 3244372, Steering = -0.36 & IRHOMING Kp,Ki,Kd = (50, 0, 0)

Time BattV Fin1 Fin2 Steer PID_Out LSpd RSpd

3054878 7.89 13162 28260 -0.36 18.22 68 31

3055028 7.74 13910 27986 -0.34 16.80 66 33

3055178 7.87 18048 26074 -0.18 9.10 59 40

3055328 7.88 21612 25018 -0.07 3.65 53 46

3055478 7.88 23880 25120 -0.03 1.27 51 48

3055628 7.68 25674 25968 -0.01 0.28 50 49

3055778 7.87 27258 27646 -0.01 0.35 50 49

3055928 7.88 29044 29502 -0.01 0.39 50 49

3056078 7.88 30858 31926 -0.02 0.85 50 49

3056228 7.70 33248 34360 -0.02 0.82 50 49

3056378 7.86 36096 36732 -0.01 0.44 50 49

3056528 7.88 39180 39564 -0.00 0.24 50 49

3056678 7.87 43566 41626 0.02 -1.14 48 51

3056828 7.72 48448 44238 0.05 -2.27 47 52

3056978 7.88 54154 47032 0.07 -3.52 46 53

3057128 7.89 60196 51060 0.08 -4.11 45 54

3057278 7.87 67280 55664 0.09 -4.72 45 54

3057428 7.74 76266 60412 0.12 -5.80 44 55

3057578 7.86 84516 68544 0.10 -5.22 44 55

3057728 7.88 92080 81186 0.06 -3.14 46 53

3057878 7.86 101710 96840 0.02 -1.23 48 51

3058028 7.78 114304 113078 0.01 -0.27 49 50

3058178 7.86 132040 127262 0.02 -0.92 49 50

3058328 7.88 150966 151258 -0.00 0.05 50 49

3058478 7.87 172256 185534 -0.04 1.86 51 48

3058628 7.81 200434 232196 -0.07 3.67 53 46

3058778 7.84 242280 290892 -0.09 4.56 54 45

3058928 7.87 304514 357620 -0.08 4.01 54 45

3059078 7.86 387878 457728 -0.08 4.13 54 45

3059228 7.88 505394 626948 -0.11 5.37 55 44

3059378 7.84 706004 852688 -0.09 4.71 54 45

3059528 7.86 1018210 1280002 -0.11 5.70 55 44

3059678 7.87 1643146 2120322 -0.13 6.34 56 43

3059828 7.87 2409428 2447544 -0.01 0.39 50 49

3059978 8.05 2415732 2394766 0.00 -0.22 49 50

After while loop in IRHomeToChgStnNoPings

After IRHomeToChgStnNoPings() with gl_bChgConnect = 1

Connected to Charger - Stopping Motors!

3056645: At top of COMMAND_MODE Case

ENTERING COMMAND MODE:

0 = 180 deg CCW Turn

1 = 180 deg CW Turn

A = Abort - Reboots Processor

/ = Forward

.(dot) = Reverse

* = Exit ChkForUserInput()

Faster

Left 4 5 6 Right

Slower

Setting both motors to reverse

Exiting ChkForUserInput()

Exited 'while (!bDone)'

Returning from 'CheckForUserInput()' with retval = FALSE

WallE3_Quicksort_V4.ino: enter init spd, PID triplet Kp,Ki,Kd

Here’s another run, this time with PID = (100,0,0)

IRHomeToChgStnNoPings with detection value = 3260080, Steering = -0.01 & IRHOMING Kp,Ki,Kd = (100, 0, 0)
Time	BattV	Fin1	Fin2	Steer	PID_Out		LSpd	RSpd

1902987	7.77	20816	21358	-0.01	0.64		50	49
1903137	7.75	20984	21424	-0.01	0.52		50	49
1903287	7.76	21710	21868	-0.00	0.18		50	49
1903437	7.76	22744	22518	0.00	-0.25		49	50
1903587	7.56	24182	23150	0.02	-1.09		48	51
1903737	7.75	25962	23998	0.04	-1.97		48	51
1903887	7.76	27004	26226	0.01	-0.73		49	50
1904037	7.77	28106	28930	-0.01	0.72		50	49
1904187	7.61	29630	31324	-0.03	1.39		51	48
1904337	7.75	32032	33162	-0.02	0.87		50	49
1904487	7.76	35480	34692	0.01	-0.56		49	50
1904637	7.76	38058	37884	0.00	-0.11		49	50
1904787	7.62	40608	41876	-0.02	0.77		50	49
1904937	7.75	45082	44566	0.01	-0.29		49	50
1905087	7.76	50210	47630	0.03	-1.32		48	51
1905237	7.76	56326	51058	0.05	-2.45		47	52
1905387	7.68	60550	57784	0.02	-1.17		48	51
1905537	7.74	67318	65824	0.01	-0.56		49	50
1905687	7.75	76190	76218	-0.00	0.01		50	49
1905837	7.76	87652	84008	0.02	-1.06		48	51
1905987	7.70	100634	93802	0.04	-1.76		48	51
1906137	7.74	113020	108338	0.02	-1.06		48	51
1906287	7.75	126658	128248	-0.01	0.31		50	49
1906437	7.76	142896	154210	-0.04	1.90		51	48
1906587	7.67	168204	181034	-0.04	1.84		51	48
1906737	7.74	206900	209850	-0.01	0.35		50	49
1906887	7.75	247432	260496	-0.03	1.29		51	48
1907037	7.76	296986	328646	-0.05	2.53		52	47
1907187	7.76	365482	420516	-0.07	3.50		53	46
1907337	7.76	460564	551134	-0.09	4.48		54	45
1907487	7.75	634682	725004	-0.07	3.32		53	46
1907637	7.76	932852	970918	-0.02	1.00		50	49
1907787	7.75	1542968	1429500	0.04	-1.91		48	51
1907937	7.51	2401132	2281188	0.03	-1.28		48	51
1908087	7.81	2421558	2414844	0.00	-0.07		49	50

IRHomeToChgStnNoPings with detection value = 3260080, Steering = -0.01 & IRHOMING Kp,Ki,Kd = (100, 0, 0)

Time BattV Fin1 Fin2 Steer PID_Out LSpd RSpd

1902987 7.77 20816 21358 -0.01 0.64 50 49

1903137 7.75 20984 21424 -0.01 0.52 50 49

1903287 7.76 21710 21868 -0.00 0.18 50 49

1903437 7.76 22744 22518 0.00 -0.25 49 50

1903587 7.56 24182 23150 0.02 -1.09 48 51

1903737 7.75 25962 23998 0.04 -1.97 48 51

1903887 7.76 27004 26226 0.01 -0.73 49 50

1904037 7.77 28106 28930 -0.01 0.72 50 49

1904187 7.61 29630 31324 -0.03 1.39 51 48

1904337 7.75 32032 33162 -0.02 0.87 50 49

1904487 7.76 35480 34692 0.01 -0.56 49 50

1904637 7.76 38058 37884 0.00 -0.11 49 50

1904787 7.62 40608 41876 -0.02 0.77 50 49

1904937 7.75 45082 44566 0.01 -0.29 49 50

1905087 7.76 50210 47630 0.03 -1.32 48 51

1905237 7.76 56326 51058 0.05 -2.45 47 52

1905387 7.68 60550 57784 0.02 -1.17 48 51

1905537 7.74 67318 65824 0.01 -0.56 49 50

1905687 7.75 76190 76218 -0.00 0.01 50 49

1905837 7.76 87652 84008 0.02 -1.06 48 51

1905987 7.70 100634 93802 0.04 -1.76 48 51

1906137 7.74 113020 108338 0.02 -1.06 48 51

1906287 7.75 126658 128248 -0.01 0.31 50 49

1906437 7.76 142896 154210 -0.04 1.90 51 48

1906587 7.67 168204 181034 -0.04 1.84 51 48

1906737 7.74 206900 209850 -0.01 0.35 50 49

1906887 7.75 247432 260496 -0.03 1.29 51 48

1907037 7.76 296986 328646 -0.05 2.53 52 47

1907187 7.76 365482 420516 -0.07 3.50 53 46

1907337 7.76 460564 551134 -0.09 4.48 54 45

1907487 7.75 634682 725004 -0.07 3.32 53 46

1907637 7.76 932852 970918 -0.02 1.00 50 49

1907787 7.75 1542968 1429500 0.04 -1.91 48 51

1907937 7.51 2401132 2281188 0.03 -1.28 48 51

1908087 7.81 2421558 2414844 0.00 -0.07 49 50

Here’s the PID = (100,0,0) run again, but starting with an initial orientation offset:

IRHomeToChgStnNoPings with detection value = 3304164, Steering = -0.90 & IRHOMING Kp,Ki,Kd = (100, 0, 0)
Time	BattV	Fin1	Fin2	Steer	PID_Out		LSpd	RSpd

394421	7.75	1604	29698	-0.90	44.88		94	5
394571	7.74	1586	29578	-0.90	44.91		94	5
394721	7.68	2478	31630	-0.85	42.73		92	7
394871	7.72	6212	34006	-0.69	34.55		84	15
395021	7.74	19308	27240	-0.17	8.52		58	41
395171	7.74	32728	13662	0.41	-20.55		29	70
395321	7.64	35238	10558	0.54	-26.95		23	76
395471	7.72	37020	12596	0.49	-24.61		25	74
395621	7.74	36110	19276	0.30	-15.20		34	65
395771	7.75	31032	29448	0.03	-1.31		48	51
395921	7.56	26986	37328	-0.16	8.04		58	41
396071	7.72	27628	40976	-0.19	9.73		59	40
396221	7.73	31602	42572	-0.15	7.39		57	42
396371	7.74	35608	44638	-0.11	5.63		55	44
396521	7.62	39018	48116	-0.10	5.22		55	44
396671	7.72	43402	51364	-0.08	4.20		54	45
396821	7.74	49738	54668	-0.05	2.36		52	47
396971	7.74	57844	59934	-0.02	0.89		50	49
397121	7.67	64858	65272	-0.00	0.16		50	49
397271	7.72	74134	71536	0.02	-0.88		49	50
397421	7.73	85448	77606	0.05	-2.40		47	52
397571	7.74	93886	90724	0.02	-0.86		49	50
397721	7.69	103196	107446	-0.02	1.01		51	48
397871	7.72	116794	124518	-0.03	1.60		51	48
398021	7.73	136816	141188	-0.02	0.79		50	49
398171	7.74	158782	165522	-0.02	1.04		51	48
398321	7.74	182426	202808	-0.05	2.65		52	47
398471	7.71	213228	255026	-0.09	4.45		54	45
398621	7.73	259632	316232	-0.10	4.91		54	45
398771	7.74	323414	397102	-0.10	5.11		55	44
398921	7.73	410332	515852	-0.11	5.70		55	44
399071	7.60	538884	701624	-0.13	6.56		56	43
399221	7.73	788468	969488	-0.10	5.15		55	44
399371	7.74	1218504	1427530	-0.08	3.95		53	46
399521	7.74	2072574	2180768	-0.03	1.27		51	48
399671	7.54	2462500	2459286	0.00	-0.03		49	50
399821	7.82	2484638	2474326	0.00	-0.10		49	50

IRHomeToChgStnNoPings with detection value = 3304164, Steering = -0.90 & IRHOMING Kp,Ki,Kd = (100, 0, 0)

Time BattV Fin1 Fin2 Steer PID_Out LSpd RSpd

394421 7.75 1604 29698 -0.90 44.88 94 5

394571 7.74 1586 29578 -0.90 44.91 94 5

394721 7.68 2478 31630 -0.85 42.73 92 7

394871 7.72 6212 34006 -0.69 34.55 84 15

395021 7.74 19308 27240 -0.17 8.52 58 41

395171 7.74 32728 13662 0.41 -20.55 29 70

395321 7.64 35238 10558 0.54 -26.95 23 76

395471 7.72 37020 12596 0.49 -24.61 25 74

395621 7.74 36110 19276 0.30 -15.20 34 65

395771 7.75 31032 29448 0.03 -1.31 48 51

395921 7.56 26986 37328 -0.16 8.04 58 41

396071 7.72 27628 40976 -0.19 9.73 59 40

396221 7.73 31602 42572 -0.15 7.39 57 42

396371 7.74 35608 44638 -0.11 5.63 55 44

396521 7.62 39018 48116 -0.10 5.22 55 44

396671 7.72 43402 51364 -0.08 4.20 54 45

396821 7.74 49738 54668 -0.05 2.36 52 47

396971 7.74 57844 59934 -0.02 0.89 50 49

397121 7.67 64858 65272 -0.00 0.16 50 49

397271 7.72 74134 71536 0.02 -0.88 49 50

397421 7.73 85448 77606 0.05 -2.40 47 52

397571 7.74 93886 90724 0.02 -0.86 49 50

397721 7.69 103196 107446 -0.02 1.01 51 48

397871 7.72 116794 124518 -0.03 1.60 51 48

398021 7.73 136816 141188 -0.02 0.79 50 49

398171 7.74 158782 165522 -0.02 1.04 51 48

398321 7.74 182426 202808 -0.05 2.65 52 47

398471 7.71 213228 255026 -0.09 4.45 54 45

398621 7.73 259632 316232 -0.10 4.91 54 45

398771 7.74 323414 397102 -0.10 5.11 55 44

398921 7.73 410332 515852 -0.11 5.70 55 44

399071 7.60 538884 701624 -0.13 6.56 56 43

399221 7.73 788468 969488 -0.10 5.15 55 44

399371 7.74 1218504 1427530 -0.08 3.95 53 46

399521 7.74 2072574 2180768 -0.03 1.27 51 48

399671 7.54 2462500 2459286 0.00 -0.03 49 50

399821 7.82 2484638 2474326 0.00 -0.10 49 50

And here is an Excel plot showing the steering value vs time:

This is a pretty nice result, and I’m tempted to leave it just like this.

VL53L1X Distance Measurement Compensation

Leave a reply

Posted 28 April 2023

After replacing all the VL53L0X Time-of-Flight distance sensors on WallE3 with VL53L1X versions with twice the effective range (400cm vs 200cm), I followed the procedure described in this post to derive distance correction expressions for each sensor.

First I set up a distance test range as shown in the following photo:

I captured the distance readings from all seven sensors at 10cm intervals from 10 to 60cm, using ‘WallE3_Complete_V2.ino’ in ‘Distances Only’ mode. For each reading I recorded a 20-point average as shown in the following table and plot:

Distance calibration Excel table and plot

This data looks exceptionally good except for the recorded values for the rear sensor. It looks like I may have made a mistake on the 40cm measurement. To investigate this, I re-ran just the rear sensor measurements, resulting in the following table and plot:

Revised plot showing corrected rear distance readings

Looking at the data I suspect I simply forgot to move the robot to the new distance for the 40cm ‘rear’ measurement. I dropped the faulty dataset and then used Excel’s ‘trendline’ feature to show a linear expression fit to the data for each sensor, as shown below:

Distance plots with linear trendline expressions

It turned out to be pretty difficult to select individual sensor plot lines to add trendlines, because (with the exception of the left front sensor) they were all almost identical. In order to add trendlines, I had to use Excel’s ‘Select Data’ feature to show only one line at time. Then when I added the associated trend line, I was able to edit the displayed expression to tag it to the associated sensor, as shown above.

As I did in my previous work with the VL53L0X sensors, I used the trendline expressions to come up with compensation expressions for each sensor, as follows:

LF: LFCorr = (LF-3.44)/0.8746
LC: LCCorr = (LC-0.7533)/0.9751
LR: LRCorr = (LR+0.34)/0.9961

RF: RFCorr = (RF+0.4133)/1.0001
RC: RCCorr = (RC+0.1867)/0.9877
RR: RRCorr = (RR+0.8)/0.9971

Rear: RearCorr = (Rear-0.7267)/0.9969

When I plotted the corrected values, I got the following plot:

As can be seen in the above plot, the compensation is almost perfect (for that matter, most of the sensors, with the exception of the LF (Left-Front) sensor, were pretty much spot on without compensation.

With the above information, I modified all seven ‘lidar_XX_Correction()’ functions in ‘Teensy_7VL53L1X_I2C_Slave_V1.ino’ to use the new correction factors, and then made another distance measurement run, with the results shown below:

After adding compensation to WallE3 programs

At this point I believe it is time to declare victory and move on with WallE3 ‘field testing’.

Stay tuned,

Frank

Replacing VL53L0X Time-of-Flight Distance Sensors on WallE3 with VL53L1X

Leave a reply

Posted 21 April 2023

This material was an addition to my earlier ‘More ‘WallE3_Complete_V2’ Testing‘ post, but I decided it deserved its own post.

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

I had the thought that ST Micro’s latest brainchild, the VL53L5CX sensor (investigated in this post) might be the answer, and would radically simplify the ‘parallel find’ problem as well. Unfortunately, the reality turned out to be somewhat less than spectacular. See the ’20 March 2023′ update to the above post for details.

Noodling around the STMicro site, I ran across the VL53L1X, which appears to offer about twice the maximum range than the VL53L0X; could this be the answer to my range issue? A quick check in my ‘sensors’ drawer didn’t turn up any, so I’m now on the prowl for a source for VL53L1X units. I Found a couple of VL53L1X units on eBay that I can get in a couple of days – yay!

Then I searched for and found a source for VL53L1X units that have roughly the same form factor and pinout as my current VL53L0X units, which should allow me to use a modified form of the 3-element array (one for each side) PCB I created earlier (see this post). Then I opened up the PCB design in DipTrace, modified it as required to get the pinouts correct, and then sent the design off to JLPCB for manufacture. With any luck, the PCB and the VL53L1X sensors should get here at about the same time!

After the usual number of errors, I was able to get a 3-element array of VL53L1X units working with a Teensy 3.5 on a plugboard, and then I moved the sensors to my newly-arrived V2 PCB’s, as shown in the following photo:

3-element array of VL53L1X sensors on new PCB

As shown in the photo, the array was pointed diagonally up toward the ceiling about 2.4-2.5m away, while the test was running, I waved my hand rapidly back and forth through the ‘beam’, to see how quickly the sensors could react. As shown in the following Excel plot, the answer is “pretty darned quickly!”.

My wife and I spent last week in Gatlinburg, Tenn. For those of you who have never heard of Gatlinburg, it is a small town nestled in the Great Smoky Mountains National Park. For most people, it is known for it’s beautiful scenery, great shopping, and its colorful history. However, for us bridge buffs, it is famous as the host of a regional bridge tournament, one of the largest in the nation.

There is a fair amount of down time between games, so I brought my VL53L1X test setup along to play with. This morning I was able to test my two new 3-element VL53L1X arrays connected to a Teensy 3.5 on a plugboard as shown in the following photo:

two 3-element VL53L1X arrays, 5 of which worked fine

I used my ‘VL53L1X_Pololu_V1.ino’ (shown in it’s entirety below) to test the arrays.

/*
    Name:       VL53L1X_Pololu_V1.ino
    Created:	4/14/2023 4:55:00 PM
    Author:     FRANKNEWXPS15\Frank
*/

/*
This example shows how to set up and read multiple VL53L1X sensors connected to
the same I2C bus. Each sensor needs to have its XSHUT pin connected to a
different Arduino pin, and you should change sensorCount and the xshutPins array
below to match your setup.

For more information, see ST's application note AN4846 ("Using multiple VL53L0X
in a single design"). The principles described there apply to the VL53L1X as
well.
*/

#include <Wire.h>
#include <VL53L1X.h>

// The number of sensors in your system.
const uint8_t sensorCount = 5;
//const uint8_t sensorCount = 6;

// The Arduino pin connected to the XSHUT pin of each sensor.
//const uint8_t xshutPins[sensorCount] = { 4, 5, 6 };
//const uint8_t xshutPins[sensorCount] = {32,31,30};//04/14/23
//const uint8_t xshutPins[sensorCount] = {23,22,21};//04/14/23
//const uint8_t xshutPins[sensorCount] = {5,6,7};//04/14/23
//const uint8_t xshutPins[sensorCount] = {23,22,21,5,6,7};//04/14/23
const uint8_t xshutPins[sensorCount] = {23,22,21,7,6};//04/14/23

VL53L1X sensors[sensorCount];

void setup()
{


  while (!Serial) {}
  Serial.begin(115200);
  delay(2000); //04/14/23 needed for Teensy
  Wire.begin();
  Wire1.begin();
  Wire2.begin();
  //Wire.setClock(400000); // use 400 kHz I2C
  //move sensors to Wire1
  for (uint8_t i = 0; i < sensorCount; i++)
  {
    //sensors[i].setBus(&Wire2);
    sensors[i].setBus(&Wire1);
  }

  // Disable/reset all sensors by driving their XSHUT pins low.
  for (uint8_t i = 0; i < sensorCount; i++)
  {
    pinMode(xshutPins[i], OUTPUT);
    digitalWrite(xshutPins[i], LOW);
    Serial.printf("Set pin %d LOW\n", xshutPins[i]);
  }

  // Enable, initialize, and start each sensor, one by one.
  for (uint8_t i = 0; i < sensorCount; i++)
  {
    // Stop driving this sensor's XSHUT low. This should allow the carrier
    // board to pull it high. (We do NOT want to drive XSHUT high since it is
    // not level shifted.) Then wait a bit for the sensor to start up.
    //pinMode(xshutPins[i], INPUT);
    digitalWrite(xshutPins[i], HIGH);
    Serial.printf("Set pin %d to HIGH\n", xshutPins[i]);
    delay(10);
    //delay(100);

    sensors[i].setTimeout(500);
    if (!sensors[i].init())
    {
      Serial.print("Failed to detect and initialize sensor ");
      Serial.println(i);
      while (1);
    }
    else
    {
      Serial.printf("init() succeeded for sensor %d\n", i);
    }

    // Each sensor must have its address changed to a unique value other than
    // the default of 0x29 (except for the last one, which could be left at
    // the default). To make it simple, we'll just count up from 0x2A.
    sensors[i].setAddress(0x2A + i);
    //sensors[i].setAddress(0x3A + i);

    Serial.printf("sensor %d I2C address set to 0X%X\n",i, sensors[i].getAddress());

    sensors[i].startContinuous(50);
  }
}

void loop()
{
  for (uint8_t i = 0; i < sensorCount; i++)
  {
    Serial.print(sensors[i].read());
    if (sensors[i].timeoutOccurred()) { Serial.print(" TIMEOUT"); }
    Serial.print('\t');
  }
  Serial.println();
}

100

101

102

103

104

105

106

Name: VL53L1X_Pololu_V1.ino

Created: 4/14/2023 4:55:00 PM

Author: FRANKNEWXPS15\Frank

This example shows how to set up and read multiple VL53L1X sensors connected to

the same I2C bus. Each sensor needs to have its XSHUT pin connected to a

different Arduino pin, and you should change sensorCount and the xshutPins array

below to match your setup.

For more information, see ST's application note AN4846 ("Using multiple VL53L0X

in a single design"). The principles described there apply to the VL53L1X as

well.

#include <Wire.h>

#include <VL53L1X.h>

// The number of sensors in your system.

const uint8_t sensorCount = 5;

//const uint8_t sensorCount = 6;

// The Arduino pin connected to the XSHUT pin of each sensor.

//const uint8_t xshutPins[sensorCount] = { 4, 5, 6 };

//const uint8_t xshutPins[sensorCount] = {32,31,30};//04/14/23

//const uint8_t xshutPins[sensorCount] = {23,22,21};//04/14/23

//const uint8_t xshutPins[sensorCount] = {5,6,7};//04/14/23

//const uint8_t xshutPins[sensorCount] = {23,22,21,5,6,7};//04/14/23

const uint8_t xshutPins[sensorCount] = {23,22,21,7,6};//04/14/23

VL53L1X sensors[sensorCount];

void setup()

{

while (!Serial) {}

Serial.begin(115200);

delay(2000); //04/14/23 needed for Teensy

Wire.begin();

Wire1.begin();

Wire2.begin();

//Wire.setClock(400000); // use 400 kHz I2C

//move sensors to Wire1

for (uint8_t i = 0; i < sensorCount; i++)

{

//sensors[i].setBus(&Wire2);

sensors[i].setBus(&Wire1);

}

// Disable/reset all sensors by driving their XSHUT pins low.

for (uint8_t i = 0; i < sensorCount; i++)

{

pinMode(xshutPins[i], OUTPUT);

digitalWrite(xshutPins[i], LOW);

Serial.printf("Set pin %d LOW\n", xshutPins[i]);

}

// Enable, initialize, and start each sensor, one by one.

for (uint8_t i = 0; i < sensorCount; i++)

{

// Stop driving this sensor's XSHUT low. This should allow the carrier

// board to pull it high. (We do NOT want to drive XSHUT high since it is

// not level shifted.) Then wait a bit for the sensor to start up.

//pinMode(xshutPins[i], INPUT);

digitalWrite(xshutPins[i], HIGH);

Serial.printf("Set pin %d to HIGH\n", xshutPins[i]);

delay(10);

//delay(100);

sensors[i].setTimeout(500);

if (!sensors[i].init())

{

Serial.print("Failed to detect and initialize sensor ");

Serial.println(i);

while (1);

}

else

{

Serial.printf("init() succeeded for sensor %d\n", i);

}

// Each sensor must have its address changed to a unique value other than

// the default of 0x29 (except for the last one, which could be left at

// the default). To make it simple, we'll just count up from 0x2A.

sensors[i].setAddress(0x2A + i);

//sensors[i].setAddress(0x3A + i);

Serial.printf("sensor %d I2C address set to 0X%X\n",i, sensors[i].getAddress());

sensors[i].startContinuous(50);

}

void loop()

{

for (uint8_t i = 0; i < sensorCount; i++)

{

Serial.print(sensors[i].read());

if (sensors[i].timeoutOccurred()) { Serial.print(" TIMEOUT"); }

Serial.print('\t');

}

Serial.println();

}

This program instantiates an array of VL53L1X objects named ‘sensors’, and the user initializes this array with the pin numbers attached to the XSHUT input of each device. In its original ‘out of the box’ configuration, the program expects three sensors to be attached to the default I2C port (Wire0), with XSHUT lines connected to controller pins 4, 5, & 6. As I described earlier in my 15 April update, I first modified the program to use Teensy 3.5 pins 32, 31, & 30 and verified that all three sensors were recognized and produced good data. Then I added a second 3-element sensor array on the same I2C bus, with XSHUT pins tied to Teensy 3.5 pins 4,5, & 6. Unfortunately, the program refused to recognize any of the sensors on the second array. So, I used the ‘sensorCount’ value and the contents of the xshutPins[sensorCount] array to selectively disable individual sensors on the second array, and I was able to determine that one of the VL53L1X sensors on the second array wasn’t responding for some reason, but the other two worked find. So, now I know that the Wire1 I2C bus on the Teensy 3.5 can handle at least 5 sensors, with no external pullup resistors – yay!

The next step was to move one of the arrays to Wire2 to more closely emulate the current situation on the robot. Here is the completed code, with only two of the three elements in the second array being utilized:

/*
    Name:       VL53L1X_Pololu_V1.ino
    Created:	4/14/2023 4:55:00 PM
    Author:     FRANKNEWXPS15\Frank
*/

/*
This example shows how to set up and read multiple VL53L1X sensors connected to
the same I2C bus. Each sensor needs to have its XSHUT pin connected to a
different Arduino pin, and you should change sensorCount and the xshutPins array
below to match your setup.

For more information, see ST's application note AN4846 ("Using multiple VL53L0X
in a single design"). The principles described there apply to the VL53L1X as
well.
*/

#include <Wire.h>
#include <VL53L1X.h>

// The number of sensors in your system.
//const uint8_t sensorCount = 5;
//const uint8_t sensorCount = 6;
const uint8_t Wire1sensorCount = 3;
const uint8_t Wire2sensorCount = 2;

// The Arduino pin connected to the XSHUT pin of each sensor.
//const uint8_t xshutPins[sensorCount] = { 4, 5, 6 };
//const uint8_t xshutPins[sensorCount] = {32,31,30};//04/14/23
//const uint8_t xshutPins[sensorCount] = {23,22,21};//04/14/23
//const uint8_t xshutPins[sensorCount] = {5,6,7};//04/14/23
//const uint8_t xshutPins[sensorCount] = {23,22,21,5,6,7};//04/14/23
const uint8_t Wire1xshutPins[Wire1sensorCount] = {23,22,21};//04/14/23
const uint8_t Wire2xshutPins[Wire2sensorCount] = {6,7};//04/14/23

VL53L1X Wire1sensors[Wire1sensorCount];
VL53L1X Wire2sensors[Wire2sensorCount];

void setup()
{
  while (!Serial) {}
  Serial.begin(115200);
  delay(2000); //04/14/23 needed for Teensy
  Wire.begin();
  Wire1.begin();
  Wire2.begin();

  //move some sensors to Wire1
  for (uint8_t i = 0; i < Wire1sensorCount; i++)
  {
    //sensors[i].setBus(&Wire2);
    Wire1sensors[i].setBus(&Wire1);
  }
  for (uint8_t i = 0; i < Wire2sensorCount; i++)
  {
    //sensors[i].setBus(&Wire2);
    Wire2sensors[i].setBus(&Wire2);
  }

  // Disable/reset all sensors by driving their XSHUT pins low.
  for (uint8_t i = 0; i < Wire1sensorCount; i++)
  {
    pinMode(Wire1xshutPins[i], OUTPUT);
    digitalWrite(Wire1xshutPins[i], LOW);
    Serial.printf("Set pin %d LOW\n", Wire1xshutPins[i]);
  }
  for (uint8_t i = 0; i < Wire2sensorCount; i++)
  {
    pinMode(Wire2xshutPins[i], OUTPUT);
    digitalWrite(Wire2xshutPins[i], LOW);
    Serial.printf("Set pin %d LOW\n", Wire2xshutPins[i]);
  }

  // Enable, initialize, and start each sensor, one by one.
  for (uint8_t i = 0; i < Wire1sensorCount; i++)
  {
    // Stop driving this sensor's XSHUT low. This should allow the carrier
    // board to pull it high. (We do NOT want to drive XSHUT high since it is
    // not level shifted.) Then wait a bit for the sensor to start up.
    //pinMode(xshutPins[i], INPUT);
    digitalWrite(Wire1xshutPins[i], HIGH);
    Serial.printf("Set pin %d to HIGH\n", Wire1xshutPins[i]);
    delay(10);
    //delay(100);

    Wire1sensors[i].setTimeout(500);
    if (!Wire1sensors[i].init())
    {
      Serial.print("Failed to detect and initialize sensor ");
      Serial.println(i);
      while (1);
    }
    else
    {
      Serial.printf("init() succeeded for sensor %d\n", i);
    }

    // Each sensor must have its address changed to a unique value other than
    // the default of 0x29 (except for the last one, which could be left at
    // the default). To make it simple, we'll just count up from 0x2A.
    Wire1sensors[i].setAddress(0x2A + i);
    //sensors[i].setAddress(0x3A + i);

    Serial.printf("sensor %d I2C address set to 0X%X\n",i, Wire1sensors[i].getAddress());

    Wire1sensors[i].startContinuous(50);
  }

  for (uint8_t i = 0; i < Wire2sensorCount; i++)
  {
    // Stop driving this sensor's XSHUT low. This should allow the carrier
    // board to pull it high. (We do NOT want to drive XSHUT high since it is
    // not level shifted.) Then wait a bit for the sensor to start up.
    //pinMode(xshutPins[i], INPUT);
    digitalWrite(Wire2xshutPins[i], HIGH);
    Serial.printf("Set pin %d to HIGH\n", Wire2xshutPins[i]);
    delay(10);
    //delay(100);

    Wire2sensors[i].setTimeout(500);
    if (!Wire2sensors[i].init())
    {
      Serial.print("Failed to detect and initialize sensor ");
      Serial.println(i);
      while (1);
    }
    else
    {
      Serial.printf("init() succeeded for sensor %d\n", i);
    }

    // Each sensor must have its address changed to a unique value other than
    // the default of 0x29 (except for the last one, which could be left at
    // the default). To make it simple, we'll just count up from 0x2A.
    Wire2sensors[i].setAddress(0x2A + i);
    //sensors[i].setAddress(0x3A + i);

    Serial.printf("sensor %d I2C address set to 0X%X\n", i, Wire2sensors[i].getAddress());

    Wire2sensors[i].startContinuous(50);
  }
}

void loop()
{
  for (uint8_t i = 0; i < Wire1sensorCount; i++)
  {
    Serial.print(Wire1sensors[i].read());
    if (Wire1sensors[i].timeoutOccurred()) { Serial.print(" TIMEOUT"); }
    Serial.print('\t');
  }
  for (uint8_t i = 0; i < Wire2sensorCount; i++)
  {
    Serial.print(Wire2sensors[i].read());
    if (Wire2sensors[i].timeoutOccurred()) { Serial.print(" TIMEOUT"); }
    Serial.print('\t');
  }
  Serial.println();
}

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

Name: VL53L1X_Pololu_V1.ino

Created: 4/14/2023 4:55:00 PM

Author: FRANKNEWXPS15\Frank

This example shows how to set up and read multiple VL53L1X sensors connected to

the same I2C bus. Each sensor needs to have its XSHUT pin connected to a

different Arduino pin, and you should change sensorCount and the xshutPins array

below to match your setup.

For more information, see ST's application note AN4846 ("Using multiple VL53L0X

in a single design"). The principles described there apply to the VL53L1X as

well.

#include <Wire.h>

#include <VL53L1X.h>

// The number of sensors in your system.

//const uint8_t sensorCount = 5;

//const uint8_t sensorCount = 6;

const uint8_t Wire1sensorCount = 3;

const uint8_t Wire2sensorCount = 2;

// The Arduino pin connected to the XSHUT pin of each sensor.

//const uint8_t xshutPins[sensorCount] = { 4, 5, 6 };

//const uint8_t xshutPins[sensorCount] = {32,31,30};//04/14/23

//const uint8_t xshutPins[sensorCount] = {23,22,21};//04/14/23

//const uint8_t xshutPins[sensorCount] = {5,6,7};//04/14/23

//const uint8_t xshutPins[sensorCount] = {23,22,21,5,6,7};//04/14/23

const uint8_t Wire1xshutPins[Wire1sensorCount] = {23,22,21};//04/14/23

const uint8_t Wire2xshutPins[Wire2sensorCount] = {6,7};//04/14/23

VL53L1X Wire1sensors[Wire1sensorCount];

VL53L1X Wire2sensors[Wire2sensorCount];

void setup()

{

while (!Serial) {}

Serial.begin(115200);

delay(2000); //04/14/23 needed for Teensy

Wire.begin();

Wire1.begin();

Wire2.begin();

//move some sensors to Wire1

for (uint8_t i = 0; i < Wire1sensorCount; i++)

{

//sensors[i].setBus(&Wire2);

Wire1sensors[i].setBus(&Wire1);

}

for (uint8_t i = 0; i < Wire2sensorCount; i++)

{

//sensors[i].setBus(&Wire2);

Wire2sensors[i].setBus(&Wire2);

}

// Disable/reset all sensors by driving their XSHUT pins low.

for (uint8_t i = 0; i < Wire1sensorCount; i++)

{

pinMode(Wire1xshutPins[i], OUTPUT);

digitalWrite(Wire1xshutPins[i], LOW);

Serial.printf("Set pin %d LOW\n", Wire1xshutPins[i]);

}

for (uint8_t i = 0; i < Wire2sensorCount; i++)

{

pinMode(Wire2xshutPins[i], OUTPUT);

digitalWrite(Wire2xshutPins[i], LOW);

Serial.printf("Set pin %d LOW\n", Wire2xshutPins[i]);

}

// Enable, initialize, and start each sensor, one by one.

for (uint8_t i = 0; i < Wire1sensorCount; i++)

{

// Stop driving this sensor's XSHUT low. This should allow the carrier

// board to pull it high. (We do NOT want to drive XSHUT high since it is

// not level shifted.) Then wait a bit for the sensor to start up.

//pinMode(xshutPins[i], INPUT);

digitalWrite(Wire1xshutPins[i], HIGH);

Serial.printf("Set pin %d to HIGH\n", Wire1xshutPins[i]);

delay(10);

//delay(100);

Wire1sensors[i].setTimeout(500);

if (!Wire1sensors[i].init())

{

Serial.print("Failed to detect and initialize sensor ");

Serial.println(i);

while (1);

}

else

{

Serial.printf("init() succeeded for sensor %d\n", i);

}

// Each sensor must have its address changed to a unique value other than

// the default of 0x29 (except for the last one, which could be left at

// the default). To make it simple, we'll just count up from 0x2A.

Wire1sensors[i].setAddress(0x2A + i);

//sensors[i].setAddress(0x3A + i);

Serial.printf("sensor %d I2C address set to 0X%X\n",i, Wire1sensors[i].getAddress());

Wire1sensors[i].startContinuous(50);

}

for (uint8_t i = 0; i < Wire2sensorCount; i++)

{

// Stop driving this sensor's XSHUT low. This should allow the carrier

// board to pull it high. (We do NOT want to drive XSHUT high since it is

// not level shifted.) Then wait a bit for the sensor to start up.

//pinMode(xshutPins[i], INPUT);

digitalWrite(Wire2xshutPins[i], HIGH);

Serial.printf("Set pin %d to HIGH\n", Wire2xshutPins[i]);

delay(10);

//delay(100);

Wire2sensors[i].setTimeout(500);

if (!Wire2sensors[i].init())

{

Serial.print("Failed to detect and initialize sensor ");

Serial.println(i);

while (1);

}

else

{

Serial.printf("init() succeeded for sensor %d\n", i);

}

// Each sensor must have its address changed to a unique value other than

// the default of 0x29 (except for the last one, which could be left at

// the default). To make it simple, we'll just count up from 0x2A.

Wire2sensors[i].setAddress(0x2A + i);

//sensors[i].setAddress(0x3A + i);

Serial.printf("sensor %d I2C address set to 0X%X\n", i, Wire2sensors[i].getAddress());

Wire2sensors[i].startContinuous(50);

}

void loop()

{

for (uint8_t i = 0; i < Wire1sensorCount; i++)

{

Serial.print(Wire1sensors[i].read());

if (Wire1sensors[i].timeoutOccurred()) { Serial.print(" TIMEOUT"); }

Serial.print('\t');

}

for (uint8_t i = 0; i < Wire2sensorCount; i++)

{

Serial.print(Wire2sensors[i].read());

if (Wire2sensors[i].timeoutOccurred()) { Serial.print(" TIMEOUT"); }

Serial.print('\t');

}

Serial.println();

}

Here is a short section of the output from the above program:

Opening port
Port open
Set pin 23 LOW
Set pin 22 LOW
Set pin 21 LOW
Set pin 6 LOW
Set pin 7 LOW
Set pin 23 to HIGH
got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)
Inside setDistanceMode(DistanceMode 2)
init() succeeded for sensor 0
sensor 0 I2C address set to 0X2A
Set pin 22 to HIGH
got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)
Inside setDistanceMode(DistanceMode 2)
init() succeeded for sensor 1
sensor 1 I2C address set to 0X2B
Set pin 21 to HIGH
got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)
Inside setDistanceMode(DistanceMode 2)
init() succeeded for sensor 2
sensor 2 I2C address set to 0X2C
Set pin 6 to HIGH
got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)
Inside setDistanceMode(DistanceMode 2)
init() succeeded for sensor 0
sensor 0 I2C address set to 0X2A
Set pin 7 to HIGH
got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)
Inside setDistanceMode(DistanceMode 2)
init() succeeded for sensor 1
sensor 1 I2C address set to 0X2B
3471	2932	180	462	389	
3479	2950	170	473	397	
3423	2942	196	471	396	
3434	2926	178	471	397	
3529	2931	183	473	398	
3457	2932	176	472	396	
3466	2951	188	472	400	
3483	2938	191	470	397	
3431	2920	175	470	400	
3473	2946	169	474	395	
3496	2932	198	471	394	
3496	2920	173	474	399	
3422	2962	180	473	400	
3450	2962	183	472	393	
3468	2929	174	476	401	
3429	2936	181	472	395	
3483	2956	171	473	397	
3496	2914	195	471	392	
3447	2948	193	473	401	
3474	2958	170	475	392	
3506	2945	164	473	397	
3530	2959	164	471	393	
3461	2944	175	471	399	
3521	2957	163	474	395	
3516	2939	160	473	394	
3490	2940	176	472	396	
3472	2940	169	471	396	
3445	2948	168	476	397	
3490	2968	174	473	395	
3489	2929	166	474	393	
3411	2933	186	473	396	
3501	2948	179	471	398	
3526	2932	182	473	396	
3521	2945	185	471	392	
3476	2946	191	473	396	
3502	2958	172	473	396	
3484	2937	174	472	396	
3521	2942	182	474	398	
3438	2944	175	473	397	
3515	2941	202	472	399	
3488	2960	190	472	397	
3471	2951	186	474	398	
3524	2941	191	473	395	
3504	2979	174	472	392	
3501	2918	177	472	396	
3463	2941	184	472	397	
3440	2943	185	471	392	
3495	2940	175	474	397	
3492	2910	183	475	395	
3460	2946	174	470	398	
3454	2939	203	476	394	
3451	2955	
Port closed

Opening port

Port open

Set pin 23 LOW

Set pin 22 LOW

Set pin 21 LOW

Set pin 6 LOW

Set pin 7 LOW

Set pin 23 to HIGH

got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)

Inside setDistanceMode(DistanceMode 2)

init() succeeded for sensor 0

sensor 0 I2C address set to 0X2A

Set pin 22 to HIGH

got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)

Inside setDistanceMode(DistanceMode 2)

init() succeeded for sensor 1

sensor 1 I2C address set to 0X2B

Set pin 21 to HIGH

got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)

Inside setDistanceMode(DistanceMode 2)

init() succeeded for sensor 2

sensor 2 I2C address set to 0X2C

Set pin 6 to HIGH

got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)

Inside setDistanceMode(DistanceMode 2)

init() succeeded for sensor 0

sensor 0 I2C address set to 0X2A

Set pin 7 to HIGH

got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)

Inside setDistanceMode(DistanceMode 2)

init() succeeded for sensor 1

sensor 1 I2C address set to 0X2B

3471 2932 180 462 389

3479 2950 170 473 397

3423 2942 196 471 396

3434 2926 178 471 397

3529 2931 183 473 398

3457 2932 176 472 396

3466 2951 188 472 400

3483 2938 191 470 397

3431 2920 175 470 400

3473 2946 169 474 395

3496 2932 198 471 394

3496 2920 173 474 399

3422 2962 180 473 400

3450 2962 183 472 393

3468 2929 174 476 401

3429 2936 181 472 395

3483 2956 171 473 397

3496 2914 195 471 392

3447 2948 193 473 401

3474 2958 170 475 392

3506 2945 164 473 397

3530 2959 164 471 393

3461 2944 175 471 399

3521 2957 163 474 395

3516 2939 160 473 394

3490 2940 176 472 396

3472 2940 169 471 396

3445 2948 168 476 397

3490 2968 174 473 395

3489 2929 166 474 393

3411 2933 186 473 396

3501 2948 179 471 398

3526 2932 182 473 396

3521 2945 185 471 392

3476 2946 191 473 396

3502 2958 172 473 396

3484 2937 174 472 396

3521 2942 182 474 398

3438 2944 175 473 397

3515 2941 202 472 399

3488 2960 190 472 397

3471 2951 186 474 398

3524 2941 191 473 395

3504 2979 174 472 392

3501 2918 177 472 396

3463 2941 184 472 397

3440 2943 185 471 392

3495 2940 175 474 397

3492 2910 183 475 395

3460 2946 174 470 398

3454 2939 203 476 394

3451 2955

Port closed

From the above telemetry I have picked out the following lines:

sensor 0 I2C address set to 0X2A
sensor 1 I2C address set to 0X2B
sensor 2 I2C address set to 0X2C

sensor 0 I2C address set to 0X2A
sensor 1 I2C address set to 0X2B

sensor 0 I2C address set to 0X2A

sensor 1 I2C address set to 0X2B

sensor 2 I2C address set to 0X2C

sensor 0 I2C address set to 0X2A

sensor 1 I2C address set to 0X2B

Note that in the above there are two sensors set for 0X2A and two for 0X2B. This works, because the first three sensors (0, 1, & 2) are on the Wire1 I2C bus, and the remaining two (also sensors 0 & 1) are on the Wire2 bus. This setup essentially duplicates the dual 3-element sensor arrays on WallE3.

After getting the above program working with Wire1 & Wire2, I used it to modify my previous Teensy_VL53L0X_I2C_Slave_V4.ino program to use the new VL53L1X sensors. The new program , ‘Teensy_VL53L1X_I2C_Slave_V4.ino’ is shown in it’s entirety below:

/*
    Name:       Teensy_7VL53L1X_I2C_Slave_V1.ino
    Created:	4/12/2023 3:40:21 PM
    Author:     FRANKNEWXPS15\Frank
    Notes:
    04/12/23 Copied from Teensy_7VL53L10X_I2C_Slave_V4 and revised to use 
      VL53L1X sensors instead of VL53L0X, and the Pololu VL53L1X library 
      instead of the VL53L0X one
    04/21/23 code ported from VL53L1X_Pololu_V1.ino
*/

#pragma region EARLIER_VERSIONS

/*
    Name:       Teensy_7VL53L1X_I2C_Slave_V4.ino
    Created:	4/3/2022 1:04:28 PM
    Author:     FRANKNEWXPS15\Frank
    Notes:
      04/03/22 This version adds a linear distance correction algorithm to each VL53L1X
      11/17/22 changed measurement budget from 20 to 50mSec for right-side sensors only
      11/20/22 changed measurement budget from 20 to 50mSec for left-side and rear sensors
      12/14/22 changed measurement budget back to 30mSec from 50mSec for all sensors
*/

/*
    Name:       Teensy_7VL53L1X_I2C_Slave_V3.ino
    Created:	1/24/2021 4:03:35 PM
    Author:     FRANKNEWXPS15\Frank

    This is the 'final' version of the Teensy 3.5 project to manage all
    seven VL53L1X lidar distance sensors, three on left, three on right,
    and now including a rear distance sensor.  As of this date it was an
    identical copy of the TeensyHexVL53L1XHighSpeedDemo project, just
    renamed for clarity

    01/23/22 - revised to work properly with Teensy 3.5 main processor
*/

/*
    Name:       TeensyHexVL53L1XHighSpeedDemo.ino
    Created:	10/1/2020 3:49:01 PM
    Author:     FRANKNEWXPS15\Frank

    This demo program combines the code from my Teensy_VL53L1X_I2C_Slave_V2
    project with the VL53L1X 'high speed' code from the Pololu support
    forum post at https://forum.pololu.com/t/high-speed-with-vl53l0x/16585/3

*/


/*
    Name:       Teensy_VL53L1X_I2C_Slave_V2.ino
    Created:	8/4/2020 7:42:22 PM
    Author:     FRANKNEWXPS15\Frank

    This project merges KurtE's multi-wire capable version of Pololu's VL53L1X library
    with my previous Teensy_VL53L1X_I2C_Slave project, which used the Adafruit VL53L1X
    library.  This (hopefully) will be the version uploaded to the Teensy on the 2nd
    deck of Wall-E2 and which controls the left & right 3-VL53L1X sensor arrays

    01/17/22:  Copied VL53L1X.h/cpp from C:\Users\paynt\Documents\Arduino\Libraries\vl53l0x-arduino-multi_wire
    so I won't have to guess which library folder these files came from

    01/17/22: if you have a local file.h same as a library\xx\file.h, then if VMicro's 'deep search'option
     is enabled, must use #include "file.h" instead of #include <file.h>.  This is the reason for using
     #include "VL53L1X.h" and #include "I2C_Anything.h"  below

    01/17/22 the local version of "I2C_Anything.h" uses '#include <i2c_t3.h>' vice <Wire.h> for multiple
    I2C buss availability.

    01/18/22 removed I2C_Anything.h & VL53L1X.h from project folder so can use library versions
    01/22/22 added #ifdef NO_VL53L1X and assoc #ifndef blocks for debug

*/
#pragma endregion EARLIER_VERSIONS

#include <Wire.h> //01/18/22 Wire.h has multiple I2C bus access capability
#include <VL53L1X.h>
#include <I2C_Anything.h> 
#include "elapsedMillis.h"

#pragma region DEFINES
//#define NO_VL53L1X
#define __FILENAME__ (strrchr(__FILE__, '\\') ? strrchr(__FILE__, '\\') + 1 : __FILE__) //added 01/26/23 to extract pgm filename from full path
#pragma endregion DEFINES

enum VL53L1X_REQUEST
{
  VL53L1X_READYCHECK, //added 11/10/20 to prevent bad reads during Teensy setup()
  VL53L1X_CENTERS_ONLY,
  VL53L1X_RIGHT,
  VL53L1X_LEFT,
  VL53L1X_ALL, //added 08/05/20, chg 'BOTH' to 'ALL' 10/31/20
  VL53L1X_REAR_ONLY //added 10/24/20
};

#pragma region GLOBALS
volatile uint8_t request_type;
const int SETUP_DURATION_OUTPUT_PIN = 33;
const int REQUEST_EVENT_DURATION_OUTPUT_PIN = 34;
bool bTeensyReady = false;
//const int MSEC_BETWEEN_LED_TOGGLES = 100;
const int MSEC_BETWEEN_LED_TOGGLES = 300;//02/21/22 chg so can visually detect live I2C xfrs
elapsedMillis mSecBetweenLEDToggle;

//right side array
uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;

//left side array & rear
uint16_t LF_Dist_mm;
uint16_t LC_Dist_mm;
uint16_t LR_Dist_mm;
uint16_t Rear_Dist_mm;

//02/20/22 back to using float for steer values
float RightSteeringVal;
float LeftSteeringVal;

const int SLAVE_ADDRESS = 0x20; //(32Dec) just a guess for now
const int DEFAULT_VL53L1X_ADDR = 0x29;

//XSHUT required for I2C address init 
//right side array
const int RF_XSHUT_PIN = 23;
const int RC_XSHUT_PIN = 22;
const int RR_XSHUT_PIN = 21;

//left side array
const int LF_XSHUT_PIN = 5;
const int LC_XSHUT_PIN = 6;
const int LR_XSHUT_PIN = 7;
const int Rear_XSHUT_PIN = 8;

//const int MAX_LIDAR_RANGE_MM = 1000; //make it obvious when nearest object is out of range
const int MAX_LIDAR_RANGE_MM = 3000; //VL53L1X can go out past 3m

//04/15/23 The Pololu VL53L1X library uses a parameterless constructor
//  and a subsequent 'setBus()' function to change the I2C bus

//right side array
VL53L1X lidar_RF;
VL53L1X lidar_RC;
VL53L1X lidar_RR;

//left side array
VL53L1X lidar_LF;
VL53L1X lidar_LC;
VL53L1X lidar_LR;

VL53L1X lidar_Rear;
#pragma endregion GLOBALS

void setup()
{
  bTeensyReady = false; //11/10/20 prevent bad data reads by main controller

  Serial.begin(115200);

  // wait until serial port opens 
  delay(3000);

  Wire.begin(SLAVE_ADDRESS); //set Teensy Wire0 port up as slave
  Wire.onRequest(requestEvent);  //ISR for I2C requests from master (Mega 2560)
  Wire.onReceive(receiveEvent); //ISR for I2C data from master (Mega 2560)

  //initialize Wire1 & Wire2 to talk to the two arrays, plus the rear sensor
  Wire1.begin();
  Wire2.begin();

  Serial.printf("Teensy_7VL53L1X_I2C_Slave_V1\n\n");

#pragma region PIN_INITIALIZATION
  pinMode(SETUP_DURATION_OUTPUT_PIN, OUTPUT);
  pinMode(REQUEST_EVENT_DURATION_OUTPUT_PIN, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(SETUP_DURATION_OUTPUT_PIN, HIGH);

  //right-side XSHUT control pins
  pinMode(RF_XSHUT_PIN, OUTPUT);
  pinMode(RC_XSHUT_PIN, OUTPUT);
  pinMode(RR_XSHUT_PIN, OUTPUT);

  //left-side XSHUT control pins
  pinMode(LF_XSHUT_PIN, OUTPUT);
  pinMode(LC_XSHUT_PIN, OUTPUT);
  pinMode(LR_XSHUT_PIN, OUTPUT);

  pinMode(Rear_XSHUT_PIN, OUTPUT);
#pragma endregion PIN_INITIALIZATION

#pragma region VL53L1X_INIT
  //04/15/23 The Pololu VL53L1X library uses a parameterless constructor
//  and a subsequent 'setBus()' function to change the I2C bus

//left side array I2C SLC2/SDA2 pins 3/4
  lidar_LF.setBus(&Wire2);
  lidar_LC.setBus(&Wire2);
  lidar_LR.setBus(&Wire2);

  //right side array I2C SLC0/SDA0 pins 27/28
  lidar_RF.setBus(&Wire1);
  lidar_RC.setBus(&Wire1);
  lidar_RR.setBus(&Wire1);

  //Put all sensors in reset mode by pulling XSHUT low
  digitalWrite(RF_XSHUT_PIN, LOW);
  digitalWrite(RC_XSHUT_PIN, LOW);
  digitalWrite(RR_XSHUT_PIN, LOW);

  digitalWrite(LF_XSHUT_PIN, LOW);
  digitalWrite(LC_XSHUT_PIN, LOW);
  digitalWrite(LR_XSHUT_PIN, LOW);

  digitalWrite(Rear_XSHUT_PIN, LOW);


  //now bring lidar_RF only out of reset and set it's address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RF_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_RF.init())
  {
    Serial.println("Failed to detect and initialize lidar_RF!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_RF.setMeasurementTimingBudget(30000); //11/20/22
  //lidar_RF.setMeasurementTimingBudget(50000); //11/17/22
  lidar_RF.startContinuous(50);

  lidar_RF.setAddress(DEFAULT_VL53L1X_ADDR + 1);
  Serial.printf("lidar_RF address is 0x%x\n", lidar_RF.getAddress());

  //now bring lidar_RC only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RC_XSHUT_PIN, INPUT);

  delay(10);
  if (!lidar_RC.init())
  {
    Serial.println("Failed to detect and initialize lidar_RC!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_RC.setMeasurementTimingBudget(30000); //11/20/22
  //lidar_RC.setMeasurementTimingBudget(50000); //11/17/22
  lidar_RC.startContinuous(50);

  lidar_RC.setAddress(DEFAULT_VL53L1X_ADDR + 2);
  Serial.printf("lidar_RC address is 0x%x\n", lidar_RC.getAddress());

  //now bring lidar_RR only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RR_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_RR.init())
  {
    Serial.println("Failed to detect and initialize lidar_RR!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_RR.setMeasurementTimingBudget(30000); //11/20/22
  //lidar_RR.setMeasurementTimingBudget(50000); //11/17/22
  lidar_RR.startContinuous(50);

  lidar_RR.setAddress(DEFAULT_VL53L1X_ADDR + 3);
  Serial.printf("lidar_RR address is 0x%x\n", lidar_RR.getAddress());

  //now bring lidar_LF only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LF_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_LF.init())
  {
    Serial.println("Failed to detect and initialize lidar_LF!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_LF.setMeasurementTimingBudget(30000);
  //lidar_LF.setMeasurementTimingBudget(50000); //11/20/22
  lidar_LF.startContinuous(50);

  lidar_LF.setAddress(DEFAULT_VL53L1X_ADDR + 4);
  Serial.printf("lidar_LF address is 0x%x\n", lidar_LF.getAddress());

  //now bring lidar_LC only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LC_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_LC.init())
  {
    Serial.println("Failed to detect and initialize lidar_LC!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_LC.setMeasurementTimingBudget(30000);
  //lidar_LC.setMeasurementTimingBudget(50000); //11/20/22
  lidar_LC.startContinuous(50);

  lidar_LC.setAddress(DEFAULT_VL53L1X_ADDR + 5);
  Serial.printf("lidar_LC address is 0x%x\n", lidar_LC.getAddress());

  //now bring lidar_LR only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LR_XSHUT_PIN, INPUT);

  if (!lidar_LR.init())
  {
    Serial.println("Failed to detect and initialize lidar_LR!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_LR.setMeasurementTimingBudget(30000);
  //lidar_LR.setMeasurementTimingBudget(50000); //11/20/22
  lidar_LR.startContinuous(50);

  lidar_LR.setAddress(DEFAULT_VL53L1X_ADDR + 6);
  Serial.printf("lidar_LR address is 0x%x\n", lidar_LR.getAddress());

  //added 10/23/20 for rear sensor
  //now bring lidar_Rear only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(Rear_XSHUT_PIN, INPUT);

  if (!lidar_Rear.init())
  {
    Serial.println("Failed to detect and initialize lidar_Rear!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_Rear.setMeasurementTimingBudget(30000);
  //lidar_Rear.setMeasurementTimingBudget(50000); //11/20/22
  lidar_Rear.startContinuous(50);

  lidar_Rear.setAddress(DEFAULT_VL53L1X_ADDR + 7);
  Serial.printf("lidar_Rear address is 0x%x\n", lidar_Rear.getAddress());
#pragma endregion VL53L1X_INIT

  digitalWrite(SETUP_DURATION_OUTPUT_PIN, LOW);
  bTeensyReady = true; //11/10/20 added to prevent bad data reads by main controller
  mSecBetweenLEDToggle = 0;

  Serial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tRear\tLSteer\tRSteer\n");
}

void loop()
{
}

void requestEvent()
{
  //Purpose: Send requested sensor data to the Mega controller via main I2C bus
  //Inputs: 
  //  request_type = uint8_t value denoting type of data requested (from receiveEvent())
  //      0 = left & right center distances only
  //      1 = right side front, center and rear distances, plus steering value
  //      2 = left side front, center and rear distances, plus steering value
  //      3 = both side front, center and rear distances, plus both steering values
//Outputs:
  //  Requested data sent to master
  //Notes:
  //  08/05/20 added VL53L1X_ALL request type to get both sides at once
  //  10/24/20 added VL53L1X_REAR_ONLY request type
  //  11/09/20 added write to pin for O'scope monitoring

  //DEBUG!!
  //int data_size = 0;
  //DEBUG!!

  //DEBUG!!
      //Serial.printf("RequestEvent() with request_type = %d: VL53L0X Front/Center/Rear distances = %d, %d, %d\n", 
      //    request_type, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);
  //DEBUG!!

    //digitalWrite(LED_BUILTIN, HIGH);//added 01/22/22
  digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, HIGH);

  digitalToggle(LED_BUILTIN);//2/21/22 added for visual indication of I2C activity
  switch (request_type)
  {
  case 	VL53L1X_READYCHECK: //added 11/10/20 to prevent bad reads during Teensy setup()
    Serial.printf("in VL53L1X_READYCHECK case at %lu with bTeensyReady = %d\n", millis(), bTeensyReady);
    I2C_writeAnything(bTeensyReady);
    break;

  case VL53L1X_CENTERS_ONLY:

    //DEBUG!!
      //data_size = 2*sizeof(uint16_t);
      //Serial.printf("Sending %d bytes LC_Dist_mm = %d, RC_Dist_mm = %d to master\n", data_size, LC_Dist_mm, RC_Dist_mm);
    //DEBUG!!

    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    break;

  case VL53L1X_RIGHT:

    //DEBUG!!
            //data_size = 3 * sizeof(uint16_t) + sizeof(float);
            //Serial.printf("Sending %d bytes RF/RC/RR/RS vals = %d, %d, %d, %3.2f to master\n",
            //    data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal);
    //DEBUG!!

    I2C_writeAnything(RF_Dist_mm);
    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(RR_Dist_mm);
    I2C_writeAnything(RightSteeringVal);
    break;

  case VL53L1X_LEFT:

    //DEBUG!!
            //data_size = 3 * sizeof(uint16_t) + sizeof(float);
            //Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",
            //    data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);
    //DEBUG!!

    I2C_writeAnything(LF_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    I2C_writeAnything(LR_Dist_mm);
    I2C_writeAnything(LeftSteeringVal);
    break;

  case VL53L1X_ALL:
    //Serial.printf("In VL53L1X_ALL case\n");
    //added 08/05/20 to get data from both sides at once
    //10/31/20 chg to VL53L1X_ALL & report all 7 sensor values

//DEBUG!!
    //data_size = 3 * sizeof(uint16_t) + sizeof(float);
    //data_size = 7 * sizeof(uint16_t) + 2 * sizeof(float);
    //Serial.printf("Sending %d bytes to master\n", data_size);
    //Serial.printf("%d bytes: %d\t%d\t%d\t%3.2f\t%d\t%d\t%d\t%d\t%3.2f\n",
    //    data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal,
    //  RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, Rear_Dist_mm, RightSteeringVal);
//DEBUG!!



    //DEBUG!!
      //data_size = 3 * sizeof(uint16_t) + sizeof(float);
      //Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",
      //    data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);
    //DEBUG!!

    //left side  
    I2C_writeAnything(LF_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    I2C_writeAnything(LR_Dist_mm);
    I2C_writeAnything(LeftSteeringVal);


    //DEBUG!!
      //data_size = 3 * sizeof(uint16_t) + sizeof(float);
      //data_size = 4 * sizeof(uint16_t) + sizeof(float);
      //Serial.printf("Sending %d bytes RF/RC/RR/R/RS vals = %d, %d, %d, %d, %3.2f to master\n",
      //    data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, Rear_Dist_mm, RightSteeringVal);
    //DEBUG!!

       //right side
    I2C_writeAnything(RF_Dist_mm);
    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(RR_Dist_mm);
    I2C_writeAnything(Rear_Dist_mm);
    I2C_writeAnything(RightSteeringVal);

    break;

  case VL53L1X_REAR_ONLY:

    //DEBUG!!
      //data_size = sizeof(uint16_t);
      //Serial.printf("Sending %d bytes Rear_Dist_mm = %d to master\n", data_size, Rear_Dist_mm);
    //DEBUG!!

    I2C_writeAnything(Rear_Dist_mm);
    break;

  default:
    break;
  }
  digitalToggle(LED_BUILTIN);//2/21/22 added for visual indication of I2C activity

  digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, LOW);
  //digitalWrite(LED_BUILTIN, LOW);//added 01/22/22
}

void receiveEvent(int numBytes)
{
  //Purpose: capture data request type from Mega I2C master
  //Inputs:
  //  numBytes = int value denoting number of bytes received from master
  //Outputs:
  //  uint8_t request_type filled with request type value from master


//DEBUG!!
    //Serial.printf("receiveEvent(%d)\n", numBytes);
//DEBUG!!

  I2C_readAnything(request_type);

  //DEBUG!!
      //Serial.printf("receiveEvent: Got %d from master\n", request_type);
  //DEBUG!!
}

uint16_t lidar_LF_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in left front VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist (mm) corrected for linear distance measurement error
  //Notes:
  //  correction = =(raw + 0.4)/1.125
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) + 4)/1.125

  //uint16_t corrVal = (uint16_t)((float)raw_dist + 0.4f) / 1.125f;
  //uint16_t corrVal = (uint16_t)((float)raw_dist + 4.f) / 1.125f;
  //uint16_t corrVal = raw_dist;
  uint16_t corrVal = (uint16_t)((float)raw_dist - 5.478f) / 1.0918f;//11/20/22

  //DEBUG!!
  //Serial.printf("LF: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_LC_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in left center VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error
  //Notes:
  //  correction = ==(raw - 0.6333)/1.03
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) - 6.333)/1.03

  //uint16_t corrVal = (uint16_t)((float)raw_dist - 0.6333f) / 1.03f;
  //uint16_t corrVal = (uint16_t)((float)raw_dist - 6.333f) / 1.03f;
  //uint16_t corrVal = raw_dist;
  uint16_t corrVal = (uint16_t)((float)raw_dist - 19.89f) / 0.9913f;//11/20/22

  //DEBUG!!
  //Serial.printf("LC: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_LR_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in left rear VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error
  //Notes:
  //  correction = =raw - 2.0667
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = = raw(mm) - 20.667

  //uint16_t corrVal = (uint16_t)((float)raw_dist - 2.0667f);
  //uint16_t corrVal = (uint16_t)((float)raw_dist - 20.667f);
  //uint16_t corrVal = raw_dist;
  uint16_t corrVal = (uint16_t)((float)raw_dist + 9.676f) / 1.1616;

  //DEBUG!!
  //Serial.printf("LR: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_RF_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in right front VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error in mm
  //Notes:
  //  correction = =(raw + 0.96)/1.045
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) + 9.6f)/1.03f

  //uint16_t corrVal = (uint16_t)((float)raw_dist + 0.96f) / 1.045f;
  //uint16_t corrVal = (uint16_t)((float)raw_dist + 9.6f) / 1.045f;
  //uint16_t corrVal = raw_dist; //11/18/22
  uint16_t corrVal = (uint16_t)((float)raw_dist - 4.297f) / 1.0808f;

  //DEBUG!!
  //Serial.printf("RF: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_RC_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in right center VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error
  //Notes:
  //  correction = =(raw - 3.2)/1.08
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) - 3.2)/1.08

  //uint16_t corrVal = (uint16_t)((float)raw_dist - 3.2f) / 1.08f;
  //uint16_t corrVal = (uint16_t)((float)raw_dist - 32.f) / 1.08f;
  //uint16_t corrVal = raw_dist;
  uint16_t corrVal = (uint16_t)((float)raw_dist - 50.603f) / 1.0438f;


  //DEBUG!!
  //Serial.printf("RC: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_RR_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in right rear VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error
  //Notes:
  //  correction = =(raw - 3.1)/1.12
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) - 31)/1.12

  //uint16_t corrVal = (uint16_t)((float)raw_dist - 3.1f) / 1.12f;
  //uint16_t corrVal = (uint16_t)((float)raw_dist - 31.f) / 1.12f;
  //uint16_t corrVal = raw_dist;
  uint16_t corrVal = (uint16_t)((float)raw_dist - 56.746f) / 1.0353;

  //DEBUG!!
  //Serial.printf("RR: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_Rear_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in rear VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error
  //Notes:
  //  correction = ==(raw - 1.1)/1.09
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) - 11)/1.09

  //uint16_t corrVal = (uint16_t)((float)raw_dist - 1.1f) / 1.09f;
  uint16_t corrVal = (uint16_t)((float)raw_dist - 11.f) / 1.09f;

  //DEBUG!!
  //Serial.printf("Rear: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

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

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

Name: Teensy_7VL53L1X_I2C_Slave_V1.ino

Created: 4/12/2023 3:40:21 PM

Author: FRANKNEWXPS15\Frank

Notes:

04/12/23 Copied from Teensy_7VL53L10X_I2C_Slave_V4 and revised to use

VL53L1X sensors instead of VL53L0X, and the Pololu VL53L1X library

instead of the VL53L0X one

04/21/23 code ported from VL53L1X_Pololu_V1.ino

#pragma region EARLIER_VERSIONS

Name: Teensy_7VL53L1X_I2C_Slave_V4.ino

Created: 4/3/2022 1:04:28 PM

Author: FRANKNEWXPS15\Frank

Notes:

04/03/22 This version adds a linear distance correction algorithm to each VL53L1X

11/17/22 changed measurement budget from 20 to 50mSec for right-side sensors only

11/20/22 changed measurement budget from 20 to 50mSec for left-side and rear sensors

12/14/22 changed measurement budget back to 30mSec from 50mSec for all sensors

Name: Teensy_7VL53L1X_I2C_Slave_V3.ino

Created: 1/24/2021 4:03:35 PM

Author: FRANKNEWXPS15\Frank

This is the 'final' version of the Teensy 3.5 project to manage all

seven VL53L1X lidar distance sensors, three on left, three on right,

and now including a rear distance sensor. As of this date it was an

identical copy of the TeensyHexVL53L1XHighSpeedDemo project, just

renamed for clarity

01/23/22 - revised to work properly with Teensy 3.5 main processor

Name: TeensyHexVL53L1XHighSpeedDemo.ino

Created: 10/1/2020 3:49:01 PM

Author: FRANKNEWXPS15\Frank

This demo program combines the code from my Teensy_VL53L1X_I2C_Slave_V2

project with the VL53L1X 'high speed' code from the Pololu support

forum post at https://forum.pololu.com/t/high-speed-with-vl53l0x/16585/3

Name: Teensy_VL53L1X_I2C_Slave_V2.ino

Created: 8/4/2020 7:42:22 PM

Author: FRANKNEWXPS15\Frank

This project merges KurtE's multi-wire capable version of Pololu's VL53L1X library

with my previous Teensy_VL53L1X_I2C_Slave project, which used the Adafruit VL53L1X

library. This (hopefully) will be the version uploaded to the Teensy on the 2nd

deck of Wall-E2 and which controls the left & right 3-VL53L1X sensor arrays

01/17/22: Copied VL53L1X.h/cpp from C:\Users\paynt\Documents\Arduino\Libraries\vl53l0x-arduino-multi_wire

so I won't have to guess which library folder these files came from

01/17/22: if you have a local file.h same as a library\xx\file.h, then if VMicro's 'deep search'option

is enabled, must use #include "file.h" instead of #include <file.h>. This is the reason for using

#include "VL53L1X.h" and #include "I2C_Anything.h" below

01/17/22 the local version of "I2C_Anything.h" uses '#include <i2c_t3.h>' vice <Wire.h> for multiple

I2C buss availability.

01/18/22 removed I2C_Anything.h & VL53L1X.h from project folder so can use library versions

01/22/22 added #ifdef NO_VL53L1X and assoc #ifndef blocks for debug

#pragma endregion EARLIER_VERSIONS

#include <Wire.h> //01/18/22 Wire.h has multiple I2C bus access capability

#include <VL53L1X.h>

#include <I2C_Anything.h>

#include "elapsedMillis.h"

#pragma region DEFINES

//#define NO_VL53L1X

#define __FILENAME__ (strrchr(__FILE__, '\\') ? strrchr(__FILE__, '\\') + 1 : __FILE__) //added 01/26/23 to extract pgm filename from full path

#pragma endregion DEFINES

enum VL53L1X_REQUEST

{

VL53L1X_READYCHECK, //added 11/10/20 to prevent bad reads during Teensy setup()

VL53L1X_CENTERS_ONLY,

VL53L1X_RIGHT,

VL53L1X_LEFT,

VL53L1X_ALL, //added 08/05/20, chg 'BOTH' to 'ALL' 10/31/20

VL53L1X_REAR_ONLY //added 10/24/20

};

#pragma region GLOBALS

volatile uint8_t request_type;

const int SETUP_DURATION_OUTPUT_PIN = 33;

const int REQUEST_EVENT_DURATION_OUTPUT_PIN = 34;

bool bTeensyReady = false;

//const int MSEC_BETWEEN_LED_TOGGLES = 100;

const int MSEC_BETWEEN_LED_TOGGLES = 300;//02/21/22 chg so can visually detect live I2C xfrs

elapsedMillis mSecBetweenLEDToggle;

//right side array

uint16_t RF_Dist_mm;

uint16_t RC_Dist_mm;

uint16_t RR_Dist_mm;

//left side array & rear

uint16_t LF_Dist_mm;

uint16_t LC_Dist_mm;

uint16_t LR_Dist_mm;

uint16_t Rear_Dist_mm;

//02/20/22 back to using float for steer values

float RightSteeringVal;

float LeftSteeringVal;

const int SLAVE_ADDRESS = 0x20; //(32Dec) just a guess for now

const int DEFAULT_VL53L1X_ADDR = 0x29;

//XSHUT required for I2C address init

//right side array

const int RF_XSHUT_PIN = 23;

const int RC_XSHUT_PIN = 22;

const int RR_XSHUT_PIN = 21;

//left side array

const int LF_XSHUT_PIN = 5;

const int LC_XSHUT_PIN = 6;

const int LR_XSHUT_PIN = 7;

const int Rear_XSHUT_PIN = 8;

//const int MAX_LIDAR_RANGE_MM = 1000; //make it obvious when nearest object is out of range

const int MAX_LIDAR_RANGE_MM = 3000; //VL53L1X can go out past 3m

//04/15/23 The Pololu VL53L1X library uses a parameterless constructor

// and a subsequent 'setBus()' function to change the I2C bus

//right side array

VL53L1X lidar_RF;

VL53L1X lidar_RC;

VL53L1X lidar_RR;

//left side array

VL53L1X lidar_LF;

VL53L1X lidar_LC;

VL53L1X lidar_LR;

VL53L1X lidar_Rear;

#pragma endregion GLOBALS

void setup()

{

bTeensyReady = false; //11/10/20 prevent bad data reads by main controller

Serial.begin(115200);

// wait until serial port opens

delay(3000);

Wire.begin(SLAVE_ADDRESS); //set Teensy Wire0 port up as slave

Wire.onRequest(requestEvent); //ISR for I2C requests from master (Mega 2560)

Wire.onReceive(receiveEvent); //ISR for I2C data from master (Mega 2560)

//initialize Wire1 & Wire2 to talk to the two arrays, plus the rear sensor

Wire1.begin();

Wire2.begin();

Serial.printf("Teensy_7VL53L1X_I2C_Slave_V1\n\n");

#pragma region PIN_INITIALIZATION

pinMode(SETUP_DURATION_OUTPUT_PIN, OUTPUT);

pinMode(REQUEST_EVENT_DURATION_OUTPUT_PIN, OUTPUT);

pinMode(LED_BUILTIN, OUTPUT);

digitalWrite(SETUP_DURATION_OUTPUT_PIN, HIGH);

//right-side XSHUT control pins

pinMode(RF_XSHUT_PIN, OUTPUT);

pinMode(RC_XSHUT_PIN, OUTPUT);

pinMode(RR_XSHUT_PIN, OUTPUT);

//left-side XSHUT control pins

pinMode(LF_XSHUT_PIN, OUTPUT);

pinMode(LC_XSHUT_PIN, OUTPUT);

pinMode(LR_XSHUT_PIN, OUTPUT);

pinMode(Rear_XSHUT_PIN, OUTPUT);

#pragma endregion PIN_INITIALIZATION

#pragma region VL53L1X_INIT

//04/15/23 The Pololu VL53L1X library uses a parameterless constructor

// and a subsequent 'setBus()' function to change the I2C bus

//left side array I2C SLC2/SDA2 pins 3/4

lidar_LF.setBus(&Wire2);

lidar_LC.setBus(&Wire2);

lidar_LR.setBus(&Wire2);

//right side array I2C SLC0/SDA0 pins 27/28

lidar_RF.setBus(&Wire1);

lidar_RC.setBus(&Wire1);

lidar_RR.setBus(&Wire1);

//Put all sensors in reset mode by pulling XSHUT low

digitalWrite(RF_XSHUT_PIN, LOW);

digitalWrite(RC_XSHUT_PIN, LOW);

digitalWrite(RR_XSHUT_PIN, LOW);

digitalWrite(LF_XSHUT_PIN, LOW);

digitalWrite(LC_XSHUT_PIN, LOW);

digitalWrite(LR_XSHUT_PIN, LOW);

digitalWrite(Rear_XSHUT_PIN, LOW);

//now bring lidar_RF only out of reset and set it's address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RF_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RF.init())

{

Serial.println("Failed to detect and initialize lidar_RF!");

while (1) {}

}

//from Pololu forum post

lidar_RF.setMeasurementTimingBudget(30000); //11/20/22

//lidar_RF.setMeasurementTimingBudget(50000); //11/17/22

lidar_RF.startContinuous(50);

lidar_RF.setAddress(DEFAULT_VL53L1X_ADDR + 1);

Serial.printf("lidar_RF address is 0x%x\n", lidar_RF.getAddress());

//now bring lidar_RC only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RC.init())

{

Serial.println("Failed to detect and initialize lidar_RC!");

while (1) {}

}

//from Pololu forum post

lidar_RC.setMeasurementTimingBudget(30000); //11/20/22

//lidar_RC.setMeasurementTimingBudget(50000); //11/17/22

lidar_RC.startContinuous(50);

lidar_RC.setAddress(DEFAULT_VL53L1X_ADDR + 2);

Serial.printf("lidar_RC address is 0x%x\n", lidar_RC.getAddress());

//now bring lidar_RR only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(RR_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_RR.init())

{

Serial.println("Failed to detect and initialize lidar_RR!");

while (1) {}

}

//from Pololu forum post

lidar_RR.setMeasurementTimingBudget(30000); //11/20/22

//lidar_RR.setMeasurementTimingBudget(50000); //11/17/22

lidar_RR.startContinuous(50);

lidar_RR.setAddress(DEFAULT_VL53L1X_ADDR + 3);

Serial.printf("lidar_RR address is 0x%x\n", lidar_RR.getAddress());

//now bring lidar_LF only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LF_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LF.init())

{

Serial.println("Failed to detect and initialize lidar_LF!");

while (1) {}

}

//from Pololu forum post

lidar_LF.setMeasurementTimingBudget(30000);

//lidar_LF.setMeasurementTimingBudget(50000); //11/20/22

lidar_LF.startContinuous(50);

lidar_LF.setAddress(DEFAULT_VL53L1X_ADDR + 4);

Serial.printf("lidar_LF address is 0x%x\n", lidar_LF.getAddress());

//now bring lidar_LC only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LC_XSHUT_PIN, INPUT);

delay(10);

if (!lidar_LC.init())

{

Serial.println("Failed to detect and initialize lidar_LC!");

while (1) {}

}

//from Pololu forum post

lidar_LC.setMeasurementTimingBudget(30000);

//lidar_LC.setMeasurementTimingBudget(50000); //11/20/22

lidar_LC.startContinuous(50);

lidar_LC.setAddress(DEFAULT_VL53L1X_ADDR + 5);

Serial.printf("lidar_LC address is 0x%x\n", lidar_LC.getAddress());

//now bring lidar_LR only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(LR_XSHUT_PIN, INPUT);

if (!lidar_LR.init())

{

Serial.println("Failed to detect and initialize lidar_LR!");

while (1) {}

}

//from Pololu forum post

lidar_LR.setMeasurementTimingBudget(30000);

//lidar_LR.setMeasurementTimingBudget(50000); //11/20/22

lidar_LR.startContinuous(50);

lidar_LR.setAddress(DEFAULT_VL53L1X_ADDR + 6);

Serial.printf("lidar_LR address is 0x%x\n", lidar_LR.getAddress());

//added 10/23/20 for rear sensor

//now bring lidar_Rear only out of reset, initialize it, and set its address

//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over

pinMode(Rear_XSHUT_PIN, INPUT);

if (!lidar_Rear.init())

{

Serial.println("Failed to detect and initialize lidar_Rear!");

while (1) {}

}

//from Pololu forum post

lidar_Rear.setMeasurementTimingBudget(30000);

//lidar_Rear.setMeasurementTimingBudget(50000); //11/20/22

lidar_Rear.startContinuous(50);

lidar_Rear.setAddress(DEFAULT_VL53L1X_ADDR + 7);

Serial.printf("lidar_Rear address is 0x%x\n", lidar_Rear.getAddress());

#pragma endregion VL53L1X_INIT

digitalWrite(SETUP_DURATION_OUTPUT_PIN, LOW);

bTeensyReady = true; //11/10/20 added to prevent bad data reads by main controller

mSecBetweenLEDToggle = 0;

Serial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tRear\tLSteer\tRSteer\n");

}

void loop()

{

}

void requestEvent()

{

//Purpose: Send requested sensor data to the Mega controller via main I2C bus

//Inputs:

// request_type = uint8_t value denoting type of data requested (from receiveEvent())

// 0 = left & right center distances only

// 1 = right side front, center and rear distances, plus steering value

// 2 = left side front, center and rear distances, plus steering value

// 3 = both side front, center and rear distances, plus both steering values

//Outputs:

// Requested data sent to master

//Notes:

// 08/05/20 added VL53L1X_ALL request type to get both sides at once

// 10/24/20 added VL53L1X_REAR_ONLY request type

// 11/09/20 added write to pin for O'scope monitoring

//DEBUG!!

//int data_size = 0;

//DEBUG!!

//Serial.printf("RequestEvent() with request_type = %d: VL53L0X Front/Center/Rear distances = %d, %d, %d\n",

// request_type, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);

//DEBUG!!

//digitalWrite(LED_BUILTIN, HIGH);//added 01/22/22

digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, HIGH);

digitalToggle(LED_BUILTIN);//2/21/22 added for visual indication of I2C activity

switch (request_type)

{

case VL53L1X_READYCHECK: //added 11/10/20 to prevent bad reads during Teensy setup()

Serial.printf("in VL53L1X_READYCHECK case at %lu with bTeensyReady = %d\n", millis(), bTeensyReady);

I2C_writeAnything(bTeensyReady);

break;

case VL53L1X_CENTERS_ONLY:

//DEBUG!!

//data_size = 2*sizeof(uint16_t);

//Serial.printf("Sending %d bytes LC_Dist_mm = %d, RC_Dist_mm = %d to master\n", data_size, LC_Dist_mm, RC_Dist_mm);

//DEBUG!!

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

break;

case VL53L1X_RIGHT:

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//Serial.printf("Sending %d bytes RF/RC/RR/RS vals = %d, %d, %d, %3.2f to master\n",

// data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal);

//DEBUG!!

I2C_writeAnything(RF_Dist_mm);

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(RR_Dist_mm);

I2C_writeAnything(RightSteeringVal);

break;

case VL53L1X_LEFT:

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",

// data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);

//DEBUG!!

I2C_writeAnything(LF_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

I2C_writeAnything(LR_Dist_mm);

I2C_writeAnything(LeftSteeringVal);

break;

case VL53L1X_ALL:

//Serial.printf("In VL53L1X_ALL case\n");

//added 08/05/20 to get data from both sides at once

//10/31/20 chg to VL53L1X_ALL & report all 7 sensor values

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//data_size = 7 * sizeof(uint16_t) + 2 * sizeof(float);

//Serial.printf("Sending %d bytes to master\n", data_size);

//Serial.printf("%d bytes: %d\t%d\t%d\t%3.2f\t%d\t%d\t%d\t%d\t%3.2f\n",

// data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal,

// RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, Rear_Dist_mm, RightSteeringVal);

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",

// data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);

//DEBUG!!

//left side

I2C_writeAnything(LF_Dist_mm);

I2C_writeAnything(LC_Dist_mm);

I2C_writeAnything(LR_Dist_mm);

I2C_writeAnything(LeftSteeringVal);

//DEBUG!!

//data_size = 3 * sizeof(uint16_t) + sizeof(float);

//data_size = 4 * sizeof(uint16_t) + sizeof(float);

//Serial.printf("Sending %d bytes RF/RC/RR/R/RS vals = %d, %d, %d, %d, %3.2f to master\n",

// data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, Rear_Dist_mm, RightSteeringVal);

//DEBUG!!

//right side

I2C_writeAnything(RF_Dist_mm);

I2C_writeAnything(RC_Dist_mm);

I2C_writeAnything(RR_Dist_mm);

I2C_writeAnything(Rear_Dist_mm);

I2C_writeAnything(RightSteeringVal);

break;

case VL53L1X_REAR_ONLY:

//DEBUG!!

//data_size = sizeof(uint16_t);

//Serial.printf("Sending %d bytes Rear_Dist_mm = %d to master\n", data_size, Rear_Dist_mm);

//DEBUG!!

I2C_writeAnything(Rear_Dist_mm);

break;

default:

break;

}

digitalToggle(LED_BUILTIN);//2/21/22 added for visual indication of I2C activity

digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, LOW);

//digitalWrite(LED_BUILTIN, LOW);//added 01/22/22

}

void receiveEvent(int numBytes)

{

//Purpose: capture data request type from Mega I2C master

//Inputs:

// numBytes = int value denoting number of bytes received from master

//Outputs:

// uint8_t request_type filled with request type value from master

//DEBUG!!

//Serial.printf("receiveEvent(%d)\n", numBytes);

//DEBUG!!

I2C_readAnything(request_type);

//DEBUG!!

//Serial.printf("receiveEvent: Got %d from master\n", request_type);

//DEBUG!!

}

uint16_t lidar_LF_Correction(uint16_t raw_dist)

{

//Purpose: Correct for linear error in left front VL53L0X

//Provenance: Created 04/03/22 gfp

//Inputs:

// raw_dist = uint16_t value denoting raw measurement in mm

//Outputs:

// returns raw_dist (mm) corrected for linear distance measurement error

//Notes:

// correction = =(raw + 0.4)/1.125

// 04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below

// correction = =(raw(mm) + 4)/1.125

//uint16_t corrVal = (uint16_t)((float)raw_dist + 0.4f) / 1.125f;

//uint16_t corrVal = (uint16_t)((float)raw_dist + 4.f) / 1.125f;

//uint16_t corrVal = raw_dist;

uint16_t corrVal = (uint16_t)((float)raw_dist - 5.478f) / 1.0918f;//11/20/22

//DEBUG!!

//Serial.printf("LF: raw = %d, corr = %d\n", raw_dist, corrVal);

//DEBUG!!

return corrVal;

}

uint16_t lidar_LC_Correction(uint16_t raw_dist)

{

//Purpose: Correct for linear error in left center VL53L0X

//Provenance: Created 04/03/22 gfp

//Inputs:

// raw_dist = uint16_t value denoting raw measurement in mm

//Outputs:

// returns raw_dist corrected for linear distance measurement error

//Notes:

// correction = ==(raw - 0.6333)/1.03

// 04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below

// correction = =(raw(mm) - 6.333)/1.03

//uint16_t corrVal = (uint16_t)((float)raw_dist - 0.6333f) / 1.03f;

//uint16_t corrVal = (uint16_t)((float)raw_dist - 6.333f) / 1.03f;

//uint16_t corrVal = raw_dist;

uint16_t corrVal = (uint16_t)((float)raw_dist - 19.89f) / 0.9913f;//11/20/22

//DEBUG!!

//Serial.printf("LC: raw = %d, corr = %d\n", raw_dist, corrVal);

//DEBUG!!

return corrVal;

}

uint16_t lidar_LR_Correction(uint16_t raw_dist)

{

//Purpose: Correct for linear error in left rear VL53L0X

//Provenance: Created 04/03/22 gfp

//Inputs:

// raw_dist = uint16_t value denoting raw measurement in mm

//Outputs:

// returns raw_dist corrected for linear distance measurement error

//Notes:

// correction = =raw - 2.0667

// 04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below

// correction = = raw(mm) - 20.667

//uint16_t corrVal = (uint16_t)((float)raw_dist - 2.0667f);

//uint16_t corrVal = (uint16_t)((float)raw_dist - 20.667f);

//uint16_t corrVal = raw_dist;

uint16_t corrVal = (uint16_t)((float)raw_dist + 9.676f) / 1.1616;

//DEBUG!!

//Serial.printf("LR: raw = %d, corr = %d\n", raw_dist, corrVal);

//DEBUG!!

return corrVal;

}

uint16_t lidar_RF_Correction(uint16_t raw_dist)

{

//Purpose: Correct for linear error in right front VL53L0X

//Provenance: Created 04/03/22 gfp

//Inputs:

// raw_dist = uint16_t value denoting raw measurement in mm

//Outputs:

// returns raw_dist corrected for linear distance measurement error in mm

//Notes:

// correction = =(raw + 0.96)/1.045

// 04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below

// correction = =(raw(mm) + 9.6f)/1.03f

//uint16_t corrVal = (uint16_t)((float)raw_dist + 0.96f) / 1.045f;

//uint16_t corrVal = (uint16_t)((float)raw_dist + 9.6f) / 1.045f;

//uint16_t corrVal = raw_dist; //11/18/22

uint16_t corrVal = (uint16_t)((float)raw_dist - 4.297f) / 1.0808f;

//DEBUG!!

//Serial.printf("RF: raw = %d, corr = %d\n", raw_dist, corrVal);

//DEBUG!!

return corrVal;

}

uint16_t lidar_RC_Correction(uint16_t raw_dist)

{

//Purpose: Correct for linear error in right center VL53L0X

//Provenance: Created 04/03/22 gfp

//Inputs:

// raw_dist = uint16_t value denoting raw measurement in mm

//Outputs:

// returns raw_dist corrected for linear distance measurement error

//Notes:

// correction = =(raw - 3.2)/1.08

// 04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below

// correction = =(raw(mm) - 3.2)/1.08

//uint16_t corrVal = (uint16_t)((float)raw_dist - 3.2f) / 1.08f;

//uint16_t corrVal = (uint16_t)((float)raw_dist - 32.f) / 1.08f;

//uint16_t corrVal = raw_dist;

uint16_t corrVal = (uint16_t)((float)raw_dist - 50.603f) / 1.0438f;

//DEBUG!!

//Serial.printf("RC: raw = %d, corr = %d\n", raw_dist, corrVal);

//DEBUG!!

return corrVal;

}

uint16_t lidar_RR_Correction(uint16_t raw_dist)

{

//Purpose: Correct for linear error in right rear VL53L0X

//Provenance: Created 04/03/22 gfp

//Inputs:

// raw_dist = uint16_t value denoting raw measurement in mm

//Outputs:

// returns raw_dist corrected for linear distance measurement error

//Notes:

// correction = =(raw - 3.1)/1.12

// 04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below

// correction = =(raw(mm) - 31)/1.12

//uint16_t corrVal = (uint16_t)((float)raw_dist - 3.1f) / 1.12f;

//uint16_t corrVal = (uint16_t)((float)raw_dist - 31.f) / 1.12f;

//uint16_t corrVal = raw_dist;

uint16_t corrVal = (uint16_t)((float)raw_dist - 56.746f) / 1.0353;

//DEBUG!!

//Serial.printf("RR: raw = %d, corr = %d\n", raw_dist, corrVal);

//DEBUG!!

return corrVal;

}

uint16_t lidar_Rear_Correction(uint16_t raw_dist)

{

//Purpose: Correct for linear error in rear VL53L0X

//Provenance: Created 04/03/22 gfp

//Inputs:

// raw_dist = uint16_t value denoting raw measurement in mm

//Outputs:

// returns raw_dist corrected for linear distance measurement error

//Notes:

// correction = ==(raw - 1.1)/1.09

// 04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below

// correction = =(raw(mm) - 11)/1.09

//uint16_t corrVal = (uint16_t)((float)raw_dist - 1.1f) / 1.09f;

uint16_t corrVal = (uint16_t)((float)raw_dist - 11.f) / 1.09f;

//DEBUG!!

//Serial.printf("Rear: raw = %d, corr = %d\n", raw_dist, corrVal);

//DEBUG!!

return corrVal;

}

22 April 2023 Update:

We got back home from Gatlinburg last night, and so this morning I decided to verify my theory that one of my six VL53L1X distance sensors was indeed defective. I have a pretty healthy skepticism about blaming hardware failures in a hardware/software system; in fact my motto is “Hardware never fails” (it does occasionally fail, but much much less often than a software issue causing the hardware to LOOK like it fails).

So, I loaded my one-sensor VL53L1X_Demo.ino example onto the Teensy 3.5 and used Wire0 (pins 18/19) to drive just this one sensor. Naturally it worked fine, as shwon below:

Port open
got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)
Inside setDistanceMode(DistanceMode 2)
Inside setDistanceMode(DistanceMode 2)
1956
1952
1953
1957
1964
1952
1951
1963
1951
9
94
83
2030
1961
333
208
150
1313
1248
1957
1379
736
220
1607
992
1943
1957
42
121
1985
1956


Port closed

Port open

got 0XEACC from readReg16Bit(IDENTIFICATION__MODEL_ID)

Inside setDistanceMode(DistanceMode 2)

1956

1952

1953

1957

1964

1952

1951

1963

1951

2030

1961

333

208

150

1313

1248

1957

1379

736

220

1607

992

1943

1957

121

1985

1956

Port closed

Well, as I suspected, the hardware seems OK so now I have to figure out why it didn’t respond properly in my six-element setup.

I replaced the ‘XSHUT’ wire from Teensy pin 5 to the sensor, but this did not solve the problem. I also tried driving the XSHUT line HIGH instead of letting it float, but no joy. Next I tried switching the suspect sensor with the one right next to it, to see if the problem follows the sensor. After several iterations, it now appears that the problem stays with the sensor associated with whatever sensor’s XSHUT pin is connected to Teensy 3.5 pin 5, or possibly with the 3-element array PCB itself.

I moved the XSHUT wire on T3.5 pin 5 to T3.5 pin 9, and re-ran the program. Same problem. Replaced the jumper wire from T3.5 pin 9 to the sensor; no change.

So now it is looking more likely that there is a problem on the PCB associated with the sensor socket closest to the T3.5-to-PCB cable. A glance at the back of the sensor socket revealed the problem – some idiot (whose name is being withheld to protect the author) had failed to solder three of the four socket pins to the PCB – oops!

How to waste a week of work – forget to solder three out of four socket pins to the PCB!

After fixing my solder (or lack thereof) screwup, everything started working – yay! Just as an aside, I claim credit for starting this troubleshooting effort with the statement “Hardware Never Fails”, which turned out to indeed be the case. Small comfort, but I’ll take it!

After confirming that both 3-element arrays were working properly, I added the rear sensor VL53L1X as a fourth sensor on Wire2, with XSHUT connected to pin 8. This mimics the hardware arrangement implemented on WallE3. Here’s a photo showing the plugboard setup:

In the above image, the ‘rear’ sensor is show standing upright on the right side of the plugboard.

/*
    Name:       VL53L1X_Pololu_V1.ino
    Created:	4/14/2023 4:55:00 PM
    Author:     FRANKNEWXPS15\Frank
*/

/*
This example shows how to set up and read multiple VL53L1X sensors connected to
the same I2C bus. Each sensor needs to have its XSHUT pin connected to a
different Arduino pin, and you should change sensorCount and the xshutPins array
below to match your setup.

For more information, see ST's application note AN4846 ("Using multiple VL53L0X
in a single design"). The principles described there apply to the VL53L1X as
well.

04/22/23 - this example was modified to connect three sensors (RF, RC, RR) to I2C Wire1, and
            four sensors (LF, LC, LR & Rear) to I2C Wire2.  This mimics the hardware arrangment
            used on the actual robot.
*/

#include <Wire.h>
#include <VL53L1X.h>

// The number of sensors in your system.
const uint8_t Wire1sensorCount = 3;
//const uint8_t Wire2sensorCount = 3;
const uint8_t Wire2sensorCount = 4;

// The Arduino pin connected to the XSHUT pin of each sensor.
const uint8_t Wire1xshutPins[Wire1sensorCount] = {23,22,21};//04/14/23
//const uint8_t Wire2xshutPins[Wire2sensorCount] = {5,6,7};//04/22/23, post soldering fix
const uint8_t Wire2xshutPins[Wire2sensorCount] = {5,6,7,8};//04/22/23, post soldering fix

VL53L1X Wire1sensors[Wire1sensorCount];
VL53L1X Wire2sensors[Wire2sensorCount];

void setup()
{
  while (!Serial) {}
  Serial.begin(115200);
  delay(2000); //04/14/23 needed for Teensy
  Wire.begin();
  Wire1.begin();
  Wire2.begin();

  //move some sensors to Wire1
  for (uint8_t i = 0; i < Wire1sensorCount; i++)
  {
    //sensors[i].setBus(&Wire2);
    Wire1sensors[i].setBus(&Wire1);
  }
  for (uint8_t i = 0; i < Wire2sensorCount; i++)
  {
    //sensors[i].setBus(&Wire2);
    Wire2sensors[i].setBus(&Wire2);
  }

  // Disable/reset all sensors by driving their XSHUT pins low.
  for (uint8_t i = 0; i < Wire1sensorCount; i++)
  {
    pinMode(Wire1xshutPins[i], OUTPUT);
    digitalWrite(Wire1xshutPins[i], LOW);
    Serial.printf("Set pin %d LOW\n", Wire1xshutPins[i]);
  }
  for (uint8_t i = 0; i < Wire2sensorCount; i++)
  {
    pinMode(Wire2xshutPins[i], OUTPUT);
    digitalWrite(Wire2xshutPins[i], LOW);
    Serial.printf("Set pin %d LOW\n", Wire2xshutPins[i]);
  }

  // Enable, initialize, and start each sensor, one by one.
  for (uint8_t i = 0; i < Wire1sensorCount; i++)
  {
    // Stop driving this sensor's XSHUT low. This should allow the carrier
    // board to pull it high. (We do NOT want to drive XSHUT high since it is
    // not level shifted.) Then wait a bit for the sensor to start up.
    //pinMode(xshutPins[i], INPUT);
    pinMode(Wire1xshutPins[i], INPUT);
    Serial.printf("Set pin %d to INPUT\n", Wire1xshutPins[i]);
    delay(10);

    Wire1sensors[i].setTimeout(500);
    if (!Wire1sensors[i].init())
    {
      Serial.print("Failed to detect and initialize sensor ");
      Serial.println(i);
      while (1);
    }
    else
    {
      Serial.printf("init() succeeded for sensor %d\n", i);
    }

    // Each sensor must have its address changed to a unique value other than
    // the default of 0x29 (except for the last one, which could be left at
    // the default). To make it simple, we'll just count up from 0x2A.
    Wire1sensors[i].setAddress(0x2A + i);

    Serial.printf("sensor %d I2C address set to 0X%X\n",i, Wire1sensors[i].getAddress());

    Wire1sensors[i].startContinuous(50);
  }

  for (uint8_t i = 0; i < Wire2sensorCount; i++)
  {
    // Stop driving this sensor's XSHUT low. This should allow the carrier
    // board to pull it high. (We do NOT want to drive XSHUT high since it is
    // not level shifted.) Then wait a bit for the sensor to start up.
    //pinMode(xshutPins[i], INPUT);
    pinMode(Wire2xshutPins[i], INPUT);
    Serial.printf("Set pin %d to INPUT\n", Wire2xshutPins[i]);
    delay(10);

    Wire2sensors[i].setTimeout(500);
    if (!Wire2sensors[i].init())
    {
      Serial.print("Failed to detect and initialize sensor ");
      Serial.println(i);
      while (1);
    }
    else
    {
      Serial.printf("init() succeeded for sensor %d\n", i);
    }

    // Each sensor must have its address changed to a unique value other than
    // the default of 0x29 (except for the last one, which could be left at
    // the default). To make it simple, we'll just count up from 0x2A.
    Wire2sensors[i].setAddress(0x2A + i);

    Serial.printf("sensor %d I2C address set to 0X%X\n", i, Wire2sensors[i].getAddress());

    Wire2sensors[i].startContinuous(50);
  }
}

void loop()
{
  for (uint8_t i = 0; i < Wire1sensorCount; i++)
  {
    Serial.print(Wire1sensors[i].read());
    if (Wire1sensors[i].timeoutOccurred()) { Serial.print(" TIMEOUT"); }
    Serial.print('\t');
  }
  for (uint8_t i = 0; i < Wire2sensorCount; i++)
  {
    Serial.print(Wire2sensors[i].read());
    if (Wire2sensors[i].timeoutOccurred()) { Serial.print(" TIMEOUT"); }
    Serial.print('\t');
  }
  Serial.println();
}

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

Name: VL53L1X_Pololu_V1.ino

Created: 4/14/2023 4:55:00 PM

Author: FRANKNEWXPS15\Frank

This example shows how to set up and read multiple VL53L1X sensors connected to

the same I2C bus. Each sensor needs to have its XSHUT pin connected to a

different Arduino pin, and you should change sensorCount and the xshutPins array

below to match your setup.

For more information, see ST's application note AN4846 ("Using multiple VL53L0X

in a single design"). The principles described there apply to the VL53L1X as

well.

04/22/23 - this example was modified to connect three sensors (RF, RC, RR) to I2C Wire1, and

four sensors (LF, LC, LR & Rear) to I2C Wire2. This mimics the hardware arrangment

used on the actual robot.

#include <Wire.h>

#include <VL53L1X.h>

// The number of sensors in your system.

const uint8_t Wire1sensorCount = 3;

//const uint8_t Wire2sensorCount = 3;

const uint8_t Wire2sensorCount = 4;

// The Arduino pin connected to the XSHUT pin of each sensor.

const uint8_t Wire1xshutPins[Wire1sensorCount] = {23,22,21};//04/14/23

//const uint8_t Wire2xshutPins[Wire2sensorCount] = {5,6,7};//04/22/23, post soldering fix

const uint8_t Wire2xshutPins[Wire2sensorCount] = {5,6,7,8};//04/22/23, post soldering fix

VL53L1X Wire1sensors[Wire1sensorCount];

VL53L1X Wire2sensors[Wire2sensorCount];

void setup()

{

while (!Serial) {}

Serial.begin(115200);

delay(2000); //04/14/23 needed for Teensy

Wire.begin();

Wire1.begin();

Wire2.begin();

//move some sensors to Wire1

for (uint8_t i = 0; i < Wire1sensorCount; i++)

{

//sensors[i].setBus(&Wire2);

Wire1sensors[i].setBus(&Wire1);

}

for (uint8_t i = 0; i < Wire2sensorCount; i++)

{

//sensors[i].setBus(&Wire2);

Wire2sensors[i].setBus(&Wire2);

}

// Disable/reset all sensors by driving their XSHUT pins low.

for (uint8_t i = 0; i < Wire1sensorCount; i++)

{

pinMode(Wire1xshutPins[i], OUTPUT);

digitalWrite(Wire1xshutPins[i], LOW);

Serial.printf("Set pin %d LOW\n", Wire1xshutPins[i]);

}

for (uint8_t i = 0; i < Wire2sensorCount; i++)

{

pinMode(Wire2xshutPins[i], OUTPUT);

digitalWrite(Wire2xshutPins[i], LOW);

Serial.printf("Set pin %d LOW\n", Wire2xshutPins[i]);

}

// Enable, initialize, and start each sensor, one by one.

for (uint8_t i = 0; i < Wire1sensorCount; i++)

{

// Stop driving this sensor's XSHUT low. This should allow the carrier

// board to pull it high. (We do NOT want to drive XSHUT high since it is

// not level shifted.) Then wait a bit for the sensor to start up.

//pinMode(xshutPins[i], INPUT);

pinMode(Wire1xshutPins[i], INPUT);

Serial.printf("Set pin %d to INPUT\n", Wire1xshutPins[i]);

delay(10);

Wire1sensors[i].setTimeout(500);

if (!Wire1sensors[i].init())

{

Serial.print("Failed to detect and initialize sensor ");

Serial.println(i);

while (1);

}

else

{

Serial.printf("init() succeeded for sensor %d\n", i);

}

// Each sensor must have its address changed to a unique value other than

// the default of 0x29 (except for the last one, which could be left at

// the default). To make it simple, we'll just count up from 0x2A.

Wire1sensors[i].setAddress(0x2A + i);

Serial.printf("sensor %d I2C address set to 0X%X\n",i, Wire1sensors[i].getAddress());

Wire1sensors[i].startContinuous(50);

}

for (uint8_t i = 0; i < Wire2sensorCount; i++)

{

// Stop driving this sensor's XSHUT low. This should allow the carrier

// board to pull it high. (We do NOT want to drive XSHUT high since it is

// not level shifted.) Then wait a bit for the sensor to start up.

//pinMode(xshutPins[i], INPUT);

pinMode(Wire2xshutPins[i], INPUT);

Serial.printf("Set pin %d to INPUT\n", Wire2xshutPins[i]);

delay(10);

Wire2sensors[i].setTimeout(500);

if (!Wire2sensors[i].init())

{

Serial.print("Failed to detect and initialize sensor ");

Serial.println(i);

while (1);

}

else

{

Serial.printf("init() succeeded for sensor %d\n", i);

}

// Each sensor must have its address changed to a unique value other than

// the default of 0x29 (except for the last one, which could be left at

// the default). To make it simple, we'll just count up from 0x2A.

Wire2sensors[i].setAddress(0x2A + i);

Serial.printf("sensor %d I2C address set to 0X%X\n", i, Wire2sensors[i].getAddress());

Wire2sensors[i].startContinuous(50);

}

void loop()

{

for (uint8_t i = 0; i < Wire1sensorCount; i++)

{

Serial.print(Wire1sensors[i].read());

if (Wire1sensors[i].timeoutOccurred()) { Serial.print(" TIMEOUT"); }

Serial.print('\t');

}

for (uint8_t i = 0; i < Wire2sensorCount; i++)

{

Serial.print(Wire2sensors[i].read());

if (Wire2sensors[i].timeoutOccurred()) { Serial.print(" TIMEOUT"); }

Serial.print('\t');

}

Serial.println();

}

And some typical output:

Opening port
Port open
Set pin 23 LOW
Set pin 22 LOW
Set pin 21 LOW
Set pin 5 LOW
Set pin 6 LOW
Set pin 7 LOW
Set pin 8 LOW
Set pin 23 to INPUT
init() succeeded for sensor 0
sensor 0 I2C address set to 0X2A
Set pin 22 to INPUT
init() succeeded for sensor 1
sensor 1 I2C address set to 0X2B
Set pin 21 to INPUT
init() succeeded for sensor 2
sensor 2 I2C address set to 0X2C
Set pin 5 to INPUT
init() succeeded for sensor 0
sensor 0 I2C address set to 0X2A
Set pin 6 to INPUT
init() succeeded for sensor 1
sensor 1 I2C address set to 0X2B
Set pin 7 to INPUT
init() succeeded for sensor 2
sensor 2 I2C address set to 0X2C
Set pin 8 to INPUT
init() succeeded for sensor 3
sensor 3 I2C address set to 0X2D
350	361	321	252	0	0	133	
348	361	332	250	0	95	138	
361	381	332	272	86	99	139	
362	376	329	274	108	102	141	
361	378	332	272	103	104	138	
360	377	332	271	104	100	137	
359	379	332	265	106	100	140	
360	380	332	267	106	97	137	
358	379	331	259	101	95	138	
358	378	332	254	102	93	138	
361	379	332	249	98	94	141	
361	381	331	245	95	89	139	
356	378	335	241	96	89	139	
358	376	331	236	94	91	138	
357	380	329	234	94	89	138	
356	376	332	232	94	91	139	
359	378	327	232	95	89	138	
360	379	331	237	96	91	140	
358	377	331	236	95	90	139	
360	376	334	237	98	91	136	
357	379	328	236	93	93	137	
358	379	333	238	97	91	139	
359	378	330	240	96	95	139	
359	378	329	243	99	93	141	
357	377	331	245	99	96	139	
358	379	332	246	98	96	139	
359	378	333	245	96	94	140	
357	377	330	245	99	93	140	
361	374	331	244	98	91	140	
358	380	332	246	96	95	140	
358	380	332	245	98	91	138	
360	375	332	245	99	93	141	
359	380	331	245	98	92	138	
359	378	330	246	98	94	140

Opening port

Port open

Set pin 23 LOW

Set pin 22 LOW

Set pin 21 LOW

Set pin 5 LOW

Set pin 6 LOW

Set pin 7 LOW

Set pin 8 LOW

Set pin 23 to INPUT

init() succeeded for sensor 0

sensor 0 I2C address set to 0X2A

Set pin 22 to INPUT

init() succeeded for sensor 1

sensor 1 I2C address set to 0X2B

Set pin 21 to INPUT

init() succeeded for sensor 2

sensor 2 I2C address set to 0X2C

Set pin 5 to INPUT

init() succeeded for sensor 0

sensor 0 I2C address set to 0X2A

Set pin 6 to INPUT

init() succeeded for sensor 1

sensor 1 I2C address set to 0X2B

Set pin 7 to INPUT

init() succeeded for sensor 2

sensor 2 I2C address set to 0X2C

Set pin 8 to INPUT

init() succeeded for sensor 3

sensor 3 I2C address set to 0X2D

350 361 321 252 0 0 133

348 361 332 250 0 95 138

361 381 332 272 86 99 139

362 376 329 274 108 102 141

361 378 332 272 103 104 138

360 377 332 271 104 100 137

359 379 332 265 106 100 140

360 380 332 267 106 97 137

358 379 331 259 101 95 138

358 378 332 254 102 93 138

361 379 332 249 98 94 141

361 381 331 245 95 89 139

356 378 335 241 96 89 139

358 376 331 236 94 91 138

357 380 329 234 94 89 138

356 376 332 232 94 91 139

359 378 327 232 95 89 138

360 379 331 237 96 91 140

358 377 331 236 95 90 139

360 376 334 237 98 91 136

357 379 328 236 93 93 137

358 379 333 238 97 91 139

359 378 330 240 96 95 139

359 378 329 243 99 93 141

357 377 331 245 99 96 139

358 379 332 246 98 96 139

359 378 333 245 96 94 140

357 377 330 245 99 93 140

361 374 331 244 98 91 140

358 380 332 246 96 95 140

358 380 332 245 98 91 138

360 375 332 245 99 93 141

359 380 331 245 98 92 138

359 378 330 246 98 94 140

At this point I have the above ‘VL53L1X_Pololu_V1.ino’ program doing exactly what I want – handling seven different VL53L1X sensors on two different I2C busses. Now I need to port the necessary changes into my ‘Teensy_7VL53L1X_I2C_Slave_V1.ino’ program, which itself is a clone of ‘Teensy_7VL53L0X_I2C_Slave_V4.ino’, the program currently running on WallE3.

After a few minor missteps, I believe I now have ‘Teensy_7VL53L1X_I2C_Slave_V1.ino’ working with all seven VL53L1X sensors. Here’s the complete program:

/*
    Name:       Teensy_7VL53L1X_I2C_Slave_V1.ino
    Created:	4/12/2023 3:40:21 PM
    Author:     FRANKNEWXPS15\Frank
    Notes:
    04/12/23 Copied from Teensy_7VL53L10X_I2C_Slave_V4 and revised to use 
      VL53L1X sensors instead of VL53L0X, and the Pololu VL53L1X library 
      instead of the VL53L0X one
    04/21/23 code ported from VL53L1X_Pololu_V1.ino
*/

#pragma region EARLIER_VERSIONS

/*
    Name:       Teensy_7VL53L1X_I2C_Slave_V4.ino
    Created:	4/3/2022 1:04:28 PM
    Author:     FRANKNEWXPS15\Frank
    Notes:
      04/03/22 This version adds a linear distance correction algorithm to each VL53L1X
      11/17/22 changed measurement budget from 20 to 50mSec for right-side sensors only
      11/20/22 changed measurement budget from 20 to 50mSec for left-side and rear sensors
      12/14/22 changed measurement budget back to 30mSec from 50mSec for all sensors
*/

/*
    Name:       Teensy_7VL53L1X_I2C_Slave_V3.ino
    Created:	1/24/2021 4:03:35 PM
    Author:     FRANKNEWXPS15\Frank

    This is the 'final' version of the Teensy 3.5 project to manage all
    seven VL53L1X lidar distance sensors, three on left, three on right,
    and now including a rear distance sensor.  As of this date it was an
    identical copy of the TeensyHexVL53L1XHighSpeedDemo project, just
    renamed for clarity

    01/23/22 - revised to work properly with Teensy 3.5 main processor
*/

/*
    Name:       TeensyHexVL53L1XHighSpeedDemo.ino
    Created:	10/1/2020 3:49:01 PM
    Author:     FRANKNEWXPS15\Frank

    This demo program combines the code from my Teensy_VL53L1X_I2C_Slave_V2
    project with the VL53L1X 'high speed' code from the Pololu support
    forum post at https://forum.pololu.com/t/high-speed-with-vl53l0x/16585/3

*/


/*
    Name:       Teensy_VL53L1X_I2C_Slave_V2.ino
    Created:	8/4/2020 7:42:22 PM
    Author:     FRANKNEWXPS15\Frank

    This project merges KurtE's multi-wire capable version of Pololu's VL53L1X library
    with my previous Teensy_VL53L1X_I2C_Slave project, which used the Adafruit VL53L1X
    library.  This (hopefully) will be the version uploaded to the Teensy on the 2nd
    deck of Wall-E2 and which controls the left & right 3-VL53L1X sensor arrays

    01/17/22:  Copied VL53L1X.h/cpp from C:\Users\paynt\Documents\Arduino\Libraries\vl53l0x-arduino-multi_wire
    so I won't have to guess which library folder these files came from

    01/17/22: if you have a local file.h same as a library\xx\file.h, then if VMicro's 'deep search'option
     is enabled, must use #include "file.h" instead of #include <file.h>.  This is the reason for using
     #include "VL53L1X.h" and #include "I2C_Anything.h"  below

    01/17/22 the local version of "I2C_Anything.h" uses '#include <i2c_t3.h>' vice <Wire.h> for multiple
    I2C buss availability.

    01/18/22 removed I2C_Anything.h & VL53L1X.h from project folder so can use library versions
    01/22/22 added #ifdef NO_VL53L1X and assoc #ifndef blocks for debug

*/
#pragma endregion EARLIER_VERSIONS

#include <Wire.h> //01/18/22 Wire.h has multiple I2C bus access capability
#include <VL53L1X.h>
#include <I2C_Anything.h> 
#include "elapsedMillis.h"

#pragma region DEFINES
//#define NO_VL53L1X
#define __FILENAME__ (strrchr(__FILE__, '\\') ? strrchr(__FILE__, '\\') + 1 : __FILE__) //added 01/26/23 to extract pgm filename from full path
#pragma endregion DEFINES

enum VL53L1X_REQUEST
{
  VL53L1X_READYCHECK, //added 11/10/20 to prevent bad reads during Teensy setup()
  VL53L1X_CENTERS_ONLY,
  VL53L1X_RIGHT,
  VL53L1X_LEFT,
  VL53L1X_ALL, //added 08/05/20, chg 'BOTH' to 'ALL' 10/31/20
  VL53L1X_REAR_ONLY //added 10/24/20
};

#pragma region GLOBALS
volatile uint8_t request_type;
const int SETUP_DURATION_OUTPUT_PIN = 33;
const int REQUEST_EVENT_DURATION_OUTPUT_PIN = 34;
bool bTeensyReady = false;
//const int MSEC_BETWEEN_LED_TOGGLES = 100;
const int MSEC_BETWEEN_LED_TOGGLES = 300;//02/21/22 chg so can visually detect live I2C xfrs
elapsedMillis mSecBetweenLEDToggle;

//right side array
uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;

//left side array & rear
uint16_t LF_Dist_mm;
uint16_t LC_Dist_mm;
uint16_t LR_Dist_mm;
uint16_t Rear_Dist_mm;

//02/20/22 back to using float for steer values
float RightSteeringVal;
float LeftSteeringVal;

const int SLAVE_ADDRESS = 0x20; //(32Dec) just a guess for now
const int DEFAULT_VL53L1X_ADDR = 0x29;

//XSHUT required for I2C address init 
//right side array
const int RF_XSHUT_PIN = 23;
const int RC_XSHUT_PIN = 22;
const int RR_XSHUT_PIN = 21;

//left side array
const int LF_XSHUT_PIN = 5;
const int LC_XSHUT_PIN = 6;
const int LR_XSHUT_PIN = 7;
const int Rear_XSHUT_PIN = 8;

//const int MAX_LIDAR_RANGE_MM = 1000; //make it obvious when nearest object is out of range
const int MAX_LIDAR_RANGE_MM = 3000; //VL53L1X can go out past 3m

//04/15/23 The Pololu VL53L1X library uses a parameterless constructor
//  and a subsequent 'setBus()' function to change the I2C bus

//right side array
VL53L1X lidar_RF;
VL53L1X lidar_RC;
VL53L1X lidar_RR;

//left side array
VL53L1X lidar_LF;
VL53L1X lidar_LC;
VL53L1X lidar_LR;

//Rear sensor
VL53L1X lidar_Rear;
#pragma endregion GLOBALS

void setup()
{
  bTeensyReady = false; //11/10/20 prevent bad data reads by main controller

  Serial.begin(115200);

  // wait until serial port opens 
  delay(3000);

  Wire.begin(SLAVE_ADDRESS); //set Teensy Wire0 port up as slave
  Wire.onRequest(requestEvent);  //ISR for I2C requests from master (Mega 2560)
  Wire.onReceive(receiveEvent); //ISR for I2C data from master (Mega 2560)

  //initialize Wire1 & Wire2 to talk to the two arrays, plus the rear sensor
  Wire1.begin();
  Wire2.begin();


  //__FILENAME__ dynamically filled with program name
  Serial.printf("%lu: Starting setup() for %s\n", millis(), __FILENAME__);


#pragma region PIN_INITIALIZATION
  pinMode(SETUP_DURATION_OUTPUT_PIN, OUTPUT);
  pinMode(REQUEST_EVENT_DURATION_OUTPUT_PIN, OUTPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(SETUP_DURATION_OUTPUT_PIN, HIGH);

  //right-side XSHUT control pins
  pinMode(RF_XSHUT_PIN, OUTPUT);
  pinMode(RC_XSHUT_PIN, OUTPUT);
  pinMode(RR_XSHUT_PIN, OUTPUT);

  //left-side XSHUT control pins
  pinMode(LF_XSHUT_PIN, OUTPUT);
  pinMode(LC_XSHUT_PIN, OUTPUT);
  pinMode(LR_XSHUT_PIN, OUTPUT);

  pinMode(Rear_XSHUT_PIN, OUTPUT);
#pragma endregion PIN_INITIALIZATION

#pragma region VL53L1X_INIT
  //04/15/23 The Pololu VL53L1X library uses a parameterless constructor
//  and a subsequent 'setBus()' function to change the I2C bus

//left side array I2C SLC2/SDA2 pins 3/4
  lidar_LF.setBus(&Wire2);
  lidar_LC.setBus(&Wire2);
  lidar_LR.setBus(&Wire2);
  lidar_Rear.setBus(&Wire2);

  //right side array I2C SLC1/SDA1 pins 37/38
  lidar_RF.setBus(&Wire1);
  lidar_RC.setBus(&Wire1);
  lidar_RR.setBus(&Wire1);

  //Put all sensors in reset mode by pulling XSHUT low
  digitalWrite(RF_XSHUT_PIN, LOW);
  digitalWrite(RC_XSHUT_PIN, LOW);
  digitalWrite(RR_XSHUT_PIN, LOW);

  digitalWrite(LF_XSHUT_PIN, LOW);
  digitalWrite(LC_XSHUT_PIN, LOW);
  digitalWrite(LR_XSHUT_PIN, LOW);

  digitalWrite(Rear_XSHUT_PIN, LOW);


  //now bring lidar_RF only out of reset and set it's address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RF_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_RF.init())
  {
    Serial.println("Failed to detect and initialize lidar_RF!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_RF.setMeasurementTimingBudget(30000); //11/20/22
  //lidar_RF.setMeasurementTimingBudget(50000); //11/17/22
  lidar_RF.startContinuous(50);

  lidar_RF.setAddress(DEFAULT_VL53L1X_ADDR + 1);
  Serial.printf("lidar_RF address is 0x%x\n", lidar_RF.getAddress());

  //now bring lidar_RC only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RC_XSHUT_PIN, INPUT);

  delay(10);
  if (!lidar_RC.init())
  {
    Serial.println("Failed to detect and initialize lidar_RC!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_RC.setMeasurementTimingBudget(30000); //11/20/22
  //lidar_RC.setMeasurementTimingBudget(50000); //11/17/22
  lidar_RC.startContinuous(50);

  lidar_RC.setAddress(DEFAULT_VL53L1X_ADDR + 2);
  Serial.printf("lidar_RC address is 0x%x\n", lidar_RC.getAddress());

  //now bring lidar_RR only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(RR_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_RR.init())
  {
    Serial.println("Failed to detect and initialize lidar_RR!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_RR.setMeasurementTimingBudget(30000); //11/20/22
  //lidar_RR.setMeasurementTimingBudget(50000); //11/17/22
  lidar_RR.startContinuous(50);

  lidar_RR.setAddress(DEFAULT_VL53L1X_ADDR + 3);
  Serial.printf("lidar_RR address is 0x%x\n", lidar_RR.getAddress());

  //now bring lidar_LF only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  //pinMode(LF_XSHUT_PIN, INPUT);
  digitalWrite(LF_XSHUT_PIN, HIGH);
  //Serial.printf("lidar_LF LF_XSHUT_PIN now reads %d\n", digitalRead(LF_XSHUT_PIN));
  delay(10);

  if (!lidar_LF.init())
  {
    Serial.println("Failed to detect and initialize lidar_LF!");
    //while (1) {}
  }

  //from Pololu forum post
  lidar_LF.setMeasurementTimingBudget(30000);
  //lidar_LF.setMeasurementTimingBudget(50000); //11/20/22
  lidar_LF.startContinuous(50);

  lidar_LF.setAddress(DEFAULT_VL53L1X_ADDR + 4);
  Serial.printf("lidar_LF address is 0x%x\n", lidar_LF.getAddress());

  //now bring lidar_LC only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LC_XSHUT_PIN, INPUT);
  delay(10);

  if (!lidar_LC.init())
  {
    Serial.println("Failed to detect and initialize lidar_LC!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_LC.setMeasurementTimingBudget(30000);
  //lidar_LC.setMeasurementTimingBudget(50000); //11/20/22
  lidar_LC.startContinuous(50);

  lidar_LC.setAddress(DEFAULT_VL53L1X_ADDR + 5);
  Serial.printf("lidar_LC address is 0x%x\n", lidar_LC.getAddress());

  //now bring lidar_LR only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(LR_XSHUT_PIN, INPUT);

  if (!lidar_LR.init())
  {
    Serial.println("Failed to detect and initialize lidar_LR!");
    while (1) {}
  }

  //from Pololu forum post
  lidar_LR.setMeasurementTimingBudget(30000);
  //lidar_LR.setMeasurementTimingBudget(50000); //11/20/22
  lidar_LR.startContinuous(50);

  lidar_LR.setAddress(DEFAULT_VL53L1X_ADDR + 6);
  Serial.printf("lidar_LR address is 0x%x\n", lidar_LR.getAddress());

  //added 10/23/20 for rear sensor
  //now bring lidar_Rear only out of reset, initialize it, and set its address
  //input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
  pinMode(Rear_XSHUT_PIN, INPUT);

  if (!lidar_Rear.init())
  {
    Serial.println("Failed to detect and initialize lidar_Rear!");
    //while (1) {}
  }

  //from Pololu forum post
  lidar_Rear.setMeasurementTimingBudget(30000);
  //lidar_Rear.setMeasurementTimingBudget(50000); //11/20/22
  lidar_Rear.startContinuous(50);

  lidar_Rear.setAddress(DEFAULT_VL53L1X_ADDR + 7);
  Serial.printf("lidar_Rear address is 0x%x\n", lidar_Rear.getAddress());
#pragma endregion VL53L1X_INIT

  digitalWrite(SETUP_DURATION_OUTPUT_PIN, LOW);
  bTeensyReady = true; //11/10/20 added to prevent bad data reads by main controller
  mSecBetweenLEDToggle = 0;

  Serial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tRear\tLSteer\tRSteer\n");
}

void loop()
{

  //04/03/22 now using a per-unit correction algorithm
  RF_Dist_mm = lidar_RF_Correction(lidar_RF.readRangeContinuousMillimeters());
  RC_Dist_mm = lidar_RC_Correction(lidar_RC.readRangeContinuousMillimeters());
  RR_Dist_mm = lidar_RR_Correction(lidar_RR.readRangeContinuousMillimeters());

  RightSteeringVal = (RF_Dist_mm - RR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post

  //from Pololu forum post
  //LF_Dist_mm = lidar_LF.readRangeContinuousMillimeters();
  //LC_Dist_mm = lidar_LC.readRangeContinuousMillimeters();
  //LR_Dist_mm = lidar_LR.readRangeContinuousMillimeters();

  //04/03/22 now using a per-unit correction algorithm
  LF_Dist_mm = lidar_LF_Correction(lidar_LF.readRangeContinuousMillimeters());
  LC_Dist_mm = lidar_LC_Correction(lidar_LC.readRangeContinuousMillimeters());
  LR_Dist_mm = lidar_LR_Correction(lidar_LR.readRangeContinuousMillimeters());

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

  //added 10/23/20 for rear sensor
  //Rear_Dist_mm = lidar_Rear.readRangeContinuousMillimeters();
  Rear_Dist_mm = lidar_Rear_Correction(lidar_Rear.readRangeContinuousMillimeters());

  if (mSecBetweenLEDToggle <= MSEC_BETWEEN_LED_TOGGLES)
  {
    mSecBetweenLEDToggle -= MSEC_BETWEEN_LED_TOGGLES;
    //digitalToggle(LED_BUILTIN);
    Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%2.2f\t%2.2f\n",
      millis(), LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, Rear_Dist_mm,
      LeftSteeringVal, RightSteeringVal);
  }
}

void requestEvent()
{
  //Purpose: Send requested sensor data to the Mega controller via main I2C bus
  //Inputs: 
  //  request_type = uint8_t value denoting type of data requested (from receiveEvent())
  //      0 = left & right center distances only
  //      1 = right side front, center and rear distances, plus steering value
  //      2 = left side front, center and rear distances, plus steering value
  //      3 = both side front, center and rear distances, plus both steering values
//Outputs:
  //  Requested data sent to master
  //Notes:
  //  08/05/20 added VL53L1X_ALL request type to get both sides at once
  //  10/24/20 added VL53L1X_REAR_ONLY request type
  //  11/09/20 added write to pin for O'scope monitoring

  //DEBUG!!
  //int data_size = 0;
  //DEBUG!!

  //DEBUG!!
      //Serial.printf("RequestEvent() with request_type = %d: VL53L0X Front/Center/Rear distances = %d, %d, %d\n", 
      //    request_type, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm);
  //DEBUG!!

    //digitalWrite(LED_BUILTIN, HIGH);//added 01/22/22
  digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, HIGH);

  digitalToggle(LED_BUILTIN);//2/21/22 added for visual indication of I2C activity
  switch (request_type)
  {
  case 	VL53L1X_READYCHECK: //added 11/10/20 to prevent bad reads during Teensy setup()
    Serial.printf("in VL53L1X_READYCHECK case at %lu with bTeensyReady = %d\n", millis(), bTeensyReady);
    I2C_writeAnything(bTeensyReady);
    break;

  case VL53L1X_CENTERS_ONLY:

    //DEBUG!!
      //data_size = 2*sizeof(uint16_t);
      //Serial.printf("Sending %d bytes LC_Dist_mm = %d, RC_Dist_mm = %d to master\n", data_size, LC_Dist_mm, RC_Dist_mm);
    //DEBUG!!

    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    break;

  case VL53L1X_RIGHT:

    //DEBUG!!
            //data_size = 3 * sizeof(uint16_t) + sizeof(float);
            //Serial.printf("Sending %d bytes RF/RC/RR/RS vals = %d, %d, %d, %3.2f to master\n",
            //    data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, RightSteeringVal);
    //DEBUG!!

    I2C_writeAnything(RF_Dist_mm);
    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(RR_Dist_mm);
    I2C_writeAnything(RightSteeringVal);
    break;

  case VL53L1X_LEFT:

    //DEBUG!!
            //data_size = 3 * sizeof(uint16_t) + sizeof(float);
            //Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",
            //    data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);
    //DEBUG!!

    I2C_writeAnything(LF_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    I2C_writeAnything(LR_Dist_mm);
    I2C_writeAnything(LeftSteeringVal);
    break;

  case VL53L1X_ALL:
    //Serial.printf("In VL53L1X_ALL case\n");
    //added 08/05/20 to get data from both sides at once
    //10/31/20 chg to VL53L1X_ALL & report all 7 sensor values

//DEBUG!!
    //data_size = 3 * sizeof(uint16_t) + sizeof(float);
    //data_size = 7 * sizeof(uint16_t) + 2 * sizeof(float);
    //Serial.printf("Sending %d bytes to master\n", data_size);
    //Serial.printf("%d bytes: %d\t%d\t%d\t%3.2f\t%d\t%d\t%d\t%d\t%3.2f\n",
    //    data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal,
    //  RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, Rear_Dist_mm, RightSteeringVal);
//DEBUG!!



    //DEBUG!!
      //data_size = 3 * sizeof(uint16_t) + sizeof(float);
      //Serial.printf("Sending %d bytes LF/LC/LR/LS vals = %d, %d, %d, %3.2f to master\n",
      //    data_size, LF_Dist_mm, LC_Dist_mm, LR_Dist_mm, LeftSteeringVal);
    //DEBUG!!

    //left side  
    I2C_writeAnything(LF_Dist_mm);
    I2C_writeAnything(LC_Dist_mm);
    I2C_writeAnything(LR_Dist_mm);
    I2C_writeAnything(LeftSteeringVal);


    //DEBUG!!
      //data_size = 3 * sizeof(uint16_t) + sizeof(float);
      //data_size = 4 * sizeof(uint16_t) + sizeof(float);
      //Serial.printf("Sending %d bytes RF/RC/RR/R/RS vals = %d, %d, %d, %d, %3.2f to master\n",
      //    data_size, RF_Dist_mm, RC_Dist_mm, RR_Dist_mm, Rear_Dist_mm, RightSteeringVal);
    //DEBUG!!

       //right side
    I2C_writeAnything(RF_Dist_mm);
    I2C_writeAnything(RC_Dist_mm);
    I2C_writeAnything(RR_Dist_mm);
    I2C_writeAnything(Rear_Dist_mm);
    I2C_writeAnything(RightSteeringVal);

    break;

  case VL53L1X_REAR_ONLY:

    //DEBUG!!
      //data_size = sizeof(uint16_t);
      //Serial.printf("Sending %d bytes Rear_Dist_mm = %d to master\n", data_size, Rear_Dist_mm);
    //DEBUG!!

    I2C_writeAnything(Rear_Dist_mm);
    break;

  default:
    break;
  }
  digitalToggle(LED_BUILTIN);//2/21/22 added for visual indication of I2C activity

  digitalWrite(REQUEST_EVENT_DURATION_OUTPUT_PIN, LOW);
  //digitalWrite(LED_BUILTIN, LOW);//added 01/22/22
}

void receiveEvent(int numBytes)
{
  //Purpose: capture data request type from Mega I2C master
  //Inputs:
  //  numBytes = int value denoting number of bytes received from master
  //Outputs:
  //  uint8_t request_type filled with request type value from master


//DEBUG!!
    //Serial.printf("receiveEvent(%d)\n", numBytes);
//DEBUG!!

  I2C_readAnything(request_type);

  //DEBUG!!
      //Serial.printf("receiveEvent: Got %d from master\n", request_type);
  //DEBUG!!
}

uint16_t lidar_LF_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in left front VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist (mm) corrected for linear distance measurement error
  //Notes:
  //  correction = =(raw + 0.4)/1.125
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) + 4)/1.125

  //uint16_t corrVal = (uint16_t)((float)raw_dist + 0.4f) / 1.125f;
  //uint16_t corrVal = (uint16_t)((float)raw_dist + 4.f) / 1.125f;
  //uint16_t corrVal = raw_dist;
  uint16_t corrVal = (uint16_t)((float)raw_dist - 5.478f) / 1.0918f;//11/20/22

  //DEBUG!!
  //Serial.printf("LF: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_LC_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in left center VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error
  //Notes:
  //  correction = ==(raw - 0.6333)/1.03
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) - 6.333)/1.03

  //uint16_t corrVal = (uint16_t)((float)raw_dist - 0.6333f) / 1.03f;
  //uint16_t corrVal = (uint16_t)((float)raw_dist - 6.333f) / 1.03f;
  //uint16_t corrVal = raw_dist;
  uint16_t corrVal = (uint16_t)((float)raw_dist - 19.89f) / 0.9913f;//11/20/22

  //DEBUG!!
  //Serial.printf("LC: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_LR_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in left rear VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error
  //Notes:
  //  correction = =raw - 2.0667
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = = raw(mm) - 20.667

  //uint16_t corrVal = (uint16_t)((float)raw_dist - 2.0667f);
  //uint16_t corrVal = (uint16_t)((float)raw_dist - 20.667f);
  //uint16_t corrVal = raw_dist;
  uint16_t corrVal = (uint16_t)((float)raw_dist + 9.676f) / 1.1616;

  //DEBUG!!
  //Serial.printf("LR: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_RF_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in right front VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error in mm
  //Notes:
  //  correction = =(raw + 0.96)/1.045
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) + 9.6f)/1.03f

  //uint16_t corrVal = (uint16_t)((float)raw_dist + 0.96f) / 1.045f;
  //uint16_t corrVal = (uint16_t)((float)raw_dist + 9.6f) / 1.045f;
  //uint16_t corrVal = raw_dist; //11/18/22
  uint16_t corrVal = (uint16_t)((float)raw_dist - 4.297f) / 1.0808f;

  //DEBUG!!
  //Serial.printf("RF: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_RC_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in right center VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error
  //Notes:
  //  correction = =(raw - 3.2)/1.08
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) - 3.2)/1.08

  //uint16_t corrVal = (uint16_t)((float)raw_dist - 3.2f) / 1.08f;
  //uint16_t corrVal = (uint16_t)((float)raw_dist - 32.f) / 1.08f;
  //uint16_t corrVal = raw_dist;
  uint16_t corrVal = (uint16_t)((float)raw_dist - 50.603f) / 1.0438f;


  //DEBUG!!
  //Serial.printf("RC: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_RR_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in right rear VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error
  //Notes:
  //  correction = =(raw - 3.1)/1.12
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) - 31)/1.12

  //uint16_t corrVal = (uint16_t)((float)raw_dist - 3.1f) / 1.12f;
  //uint16_t corrVal = (uint16_t)((float)raw_dist - 31.f) / 1.12f;
  //uint16_t corrVal = raw_dist;
  uint16_t corrVal = (uint16_t)((float)raw_dist - 56.746f) / 1.0353;

  //DEBUG!!
  //Serial.printf("RR: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

uint16_t lidar_Rear_Correction(uint16_t raw_dist)
{
  //Purpose:  Correct for linear error in rear VL53L0X
  //Provenance: Created 04/03/22 gfp
  //Inputs: 
  //  raw_dist = uint16_t value denoting raw measurement in mm
  //Outputs:
  //  returns raw_dist corrected for linear distance measurement error
  //Notes:
  //  correction = ==(raw - 1.1)/1.09
  //  04/05/22 bugfix. Raw dist here are MM, but above formula assumed CM - fixed below
  //  correction = =(raw(mm) - 11)/1.09

  //uint16_t corrVal = (uint16_t)((float)raw_dist - 1.1f) / 1.09f;
  uint16_t corrVal = (uint16_t)((float)raw_dist - 11.f) / 1.09f;

  //DEBUG!!
  //Serial.printf("Rear: raw = %d, corr = %d\n", raw_dist, corrVal);
  //DEBUG!!

  return corrVal;
}

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

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

602

603

604

605

606

607

608

609

610

611

612

613

614

615

616

617

618

619

620

621

622

623

624

625

626

627

628

629

630

631

632

633

634

635

636

637

638

639

640

641

642

643

644

645

646

647

648

649

650

651

652

653

654

655

656

657

658

659

660

661

662

663

664

665

666

667

668

669

670

671

672

673

674

675

676

677

678

679

680

681

682

683

684

685

686

687

688

689

690

691

692

693

694

695

696

697

698

699

700

701

702

703

704

705

706

707

708

709

710

711

712

713

714

715

716

717

718

719

720

721

722

723

724

725

726

727

728

729

730

731

732

733