Yearly Archives: 2026

Vision-enhanced Robot ‘Find Best Direction’ Issue

WallE, my autonomous wall-following robot had a nominal capability called “run to daylight”. The idea was to use this as a last resort when all the other ‘find a wall and follow it’ tricks failed. Unfortunately, I never could get this to work and never figured out why. Fast forward to the present, and Grok and I were trying to implement the same kind of capability into the new vision-enhanced version, to address the issue of what to do when the robot gets stuck.

Basically, the algorithm is the same as it was for the wall-following version; back up, look around and turn to the heading that provides the most freedom (‘largest depth’ in vision processing language). We (Grok and I) implemented the “back up and look around” operations using the new ‘L/Rxxx’ (where ‘xxx’ is in degrees), and then the ‘turn to the best heading’ utilizes a new ‘Txx’ command which is implemented via the already-existing ‘TurnToHdgDeg()’ function.

Unfortunately, although the ‘back up and look around’ part worked almost immediately, the ‘turn to the best heading’ part failed miserably, with the end result consistently going off in a completely different direction. I was somewhat mystified by this, as I was suffering from the (soon-to-be-debunked) belief that my MPU6050 IMU/DMP was actually delivering accurate heading measurements, and the problem must be in the ‘TurnToHdgDeg()’ function. However, data beats belief, so I decided to go back to basic ‘show me’ experiments.

To validate the MPU6050 IMU function operation, I set up an experiment where I manually rotated the robot on a printed compass rose, and measured the output of the IMU at each point, as shown in the following Excel table and resulting plot:

The above plot looks ugly, but a lot of the ugliness is due to the way IMU heading output jumps from +180 to -179; this is normal. What isn’t normal is the way the IMU reported heading diverges from the actual heading – the +180 to -179 transition occurs between 150 and 160 degrees rather than 180, and the -179 to +180 transition occurs between -140 and -150 degrees on the other side.

At first I thought this must be due to a bad IMU module, but changing out the module for a brand-new one produced the same results – so not that (further validation of my “hardware never fails – its ALWAYS the software” mantra).

So, I made another plot, this time manually changing the ‘opposite sign’ measurements back into the proper range by adding/subtracting 360 degrees. This produced the following plot:

Just looking at the ‘positive’ half from 0 to 180 deg, it is clear that the measured values are consistently higher than the actual physical headings. Looking at the first five values (before the first breakpoint in the plot) we see the differential goes from zero to about 156.8-120 = 36.8 deg over a 150 deg range, or about 36.8/150 = 0.25 deg/deg. Assuming the difference is linear, then the required multiplication factor should be 156.8X = 120 –> X = 120/156.8 = 0.769. For example, a multiplicative factor of 0.787 yielded the following plot:

Good enough for government work ;).

This factor can’t be applied directly to the IMU yaw output, as that value has already been corrected for the +/- 180 arrangement, so Grok suggested an algorithm to hold a ‘running total’ heading that uses IMU heading changes to produce a separate heading value that is corrected for the slope error and only afterwards applies the +/- 360 adjustment for the +/-180 scheme, as follows:

float UpdateIMUHdgValDeg()
{
  //Purpose: Get latest yaw (heading) value from IMU
  //Inputs: None.  This function should only be called after mpu.dmpPacketAvailable() returns TRUE
  //Outputs: 
  //	returns true if successful, otherwise false
  //	IMUHdgValDeg updated on success
  //Plan:
  //Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet
  //Step2: read all available packets to get to latest data
  //Step3: 10/05/26: calculate delta from last raw heading value
  //Step4: 10/05/26: scale delta by correction factor (1/1.27)
  //Step5: 10/05/26: add scaled delta to last adjusted value to produce current corrected heading
  //Step6: update IMUHdgValDeg with latest corrected heading after adjusting for 180/-179 split
  //Notes:
  //	10/08/19 changed return type to boolean
  //	10/08/19 no longer need mpuIntStatus
  //	10/21/19 completely rewritten to use Homer's algorithm
  //	05/05/20 changed return type to float vs bool.
  //  06/13/23 added code to update gl_HdgHistoryArray for 'spinning' condx detection
  //  10/04/26 added Grok code to properly correct raw IMU headings

  //10/04/26 static makes them survive across calls for as long as the Teensy is powered
  static float prevRawDeg = 0;
  static float continuousDeg = 0;
  static bool haveYaw = false;

  int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

  if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow
  {
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    //compute the yaw value

    //10/03/26 added fudge factor to synch IMU output with physical reality
    float rawDeg = ypr[0] * 180.0f / M_PI;   // still -180 to +180

    if (!haveYaw) {
      prevRawDeg = rawDeg;
      continuousDeg = 0;    // heading at power-up is 0
      haveYaw = true;
    }
    else {
      float step = rawDeg - prevRawDeg;
      if (step > 180.0f)  step -= 360.0f;   // crossed the fold backward
      if (step < -180.0f) step += 360.0f;   // crossed the fold forward
      continuousDeg += step * IMU_HDG_CORRECTION_FACTOR; //1.27 is the long scale from the protractor
      prevRawDeg = rawDeg;
    }

    IMUHdgValDeg = continuousDeg;
    while (IMUHdgValDeg > 180.0f)  IMUHdgValDeg -= 360.0f;
    while (IMUHdgValDeg < -180.0f) IMUHdgValDeg += 360.0f;
  }

  //06/13/23 added to update gl_HdgHistoryArray for 'spinning' condx detection
  //all array entries bumped down one, with most recent value at i = HEADING_HISTORY_ARRAY_SIZE-1
  for (uint16_t i = 1; i < HEADING_HISTORY_ARRAY_SIZE; i++)
  {
    gl_HdgHistoryArray[i - 1] = gl_HdgHistoryArray[i];
    //gl_pSerPort->printf("gl_HdgHistoryArray[%d] = %2.2f\n", i, gl_HdgHistoryArray[i]);
  }
  gl_HdgHistoryArray[HEADING_HISTORY_ARRAY_SIZE - 1] = IMUHdgValDeg;


  return IMUHdgValDeg;//05/05/20 now returns updated value for use convenience
}

float UpdateIMUHdgValDeg()

{

//Purpose: Get latest yaw (heading) value from IMU

//Inputs: None. This function should only be called after mpu.dmpPacketAvailable() returns TRUE

//Outputs:

// returns true if successful, otherwise false

// IMUHdgValDeg updated on success

//Plan:

//Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet

//Step2: read all available packets to get to latest data

//Step3: 10/05/26: calculate delta from last raw heading value

//Step4: 10/05/26: scale delta by correction factor (1/1.27)

//Step5: 10/05/26: add scaled delta to last adjusted value to produce current corrected heading

//Step6: update IMUHdgValDeg with latest corrected heading after adjusting for 180/-179 split

//Notes:

// 10/08/19 changed return type to boolean

// 10/08/19 no longer need mpuIntStatus

// 10/21/19 completely rewritten to use Homer's algorithm

// 05/05/20 changed return type to float vs bool.

// 06/13/23 added code to update gl_HdgHistoryArray for 'spinning' condx detection

// 10/04/26 added Grok code to properly correct raw IMU headings

//10/04/26 static makes them survive across calls for as long as the Teensy is powered

static float prevRawDeg = 0;

static float continuousDeg = 0;

static bool haveYaw = false;

int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow

{

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

//compute the yaw value

//10/03/26 added fudge factor to synch IMU output with physical reality

float rawDeg = ypr[0] * 180.0f / M_PI; // still -180 to +180

if (!haveYaw) {

prevRawDeg = rawDeg;

continuousDeg = 0; // heading at power-up is 0

haveYaw = true;

}

else {

float step = rawDeg - prevRawDeg;

if (step > 180.0f) step -= 360.0f; // crossed the fold backward

if (step < -180.0f) step += 360.0f; // crossed the fold forward

continuousDeg += step * IMU_HDG_CORRECTION_FACTOR; //1.27 is the long scale from the protractor

prevRawDeg = rawDeg;

}

IMUHdgValDeg = continuousDeg;

while (IMUHdgValDeg > 180.0f) IMUHdgValDeg -= 360.0f;

while (IMUHdgValDeg < -180.0f) IMUHdgValDeg += 360.0f;

}

//06/13/23 added to update gl_HdgHistoryArray for 'spinning' condx detection

//all array entries bumped down one, with most recent value at i = HEADING_HISTORY_ARRAY_SIZE-1

for (uint16_t i = 1; i < HEADING_HISTORY_ARRAY_SIZE; i++)

{

gl_HdgHistoryArray[i - 1] = gl_HdgHistoryArray[i];

//gl_pSerPort->printf("gl_HdgHistoryArray[%d] = %2.2f\n", i, gl_HdgHistoryArray[i]);

}

gl_HdgHistoryArray[HEADING_HISTORY_ARRAY_SIZE - 1] = IMUHdgValDeg;

return IMUHdgValDeg;//05/05/20 now returns updated value for use convenience

}

Moving Vision Processing to My 4-Wheel Robot, Part 3

Leave a reply

Posted 08 September 2026,

This post is a follow up to Moving Vision Processing to My 4-Wheel Robot, Part 2 | Paynter’s Palace. I WallE now has a very crude ability to navigate using only vision processing from the onboard OAKD-Lite camera. Here’s a not-so-short video showing the first real trial.

Wall-E Gets New Wheels

Leave a reply

Posted 09 September 2026

While upgrading my 4-wheel autonomous wall-following robot to add vision enhancement, Wall-E managed to dive off my bench onto the floor – twice! No major damage except to my ego, but one wheel did get dinged up to the point where it needed to be replaced.

The original wheels were designed in OSCad many years ago, so the print files were no longer available. I designed a replacement wheel using my new best friend OnShape, as shown in the screenshot below:

After getting the design dialed in (version 6), I printed out the remaining three wheels and transferred the tires from the old wheels to the new ones. Since the tires were already red, I chose to print the wheels in gray PETG to produce an Ohio State University themed robot ;). Here’s a short video showing off the new wheels:

AC Power Generator Issues During Recent House Power Loss

1 Reply

Posted 31 August

On 11 August 2026 we had a multi-hour night-time AC power loss at our home, and although I was able to get our backup power generator running and AC power partially restored, there were some ‘issues’.

When our house was built in 2000, I had the electrician install a 240-volt twist-lock receptacle in the garage and an auxiliary breaker panel at the main AC breaker location to support the use of a backup AC power generator. Then I fabricated a 50′ jumper cable so the generator could be connected to the 240-volt connector while still outside the garage.

I didn’t actually buy the generator shown below until 2022, as AC power in our area has been very reliable, and other than testing the unit and transfer switch, it was never used ‘in anger’ until a couple of weeks ago when we had a night-time multi-hour power interruption.

I have a hobbyist engineering lab in our home, with several PC’s protected from short AC interruptions with typical single-PC UPS units. So, when the power loss occurred all the lights went out, but our PCs (and their associated displays) stayed up so we weren’t totally in the dark, but I knew that wasn’t going to last long – I needed to get that generator going before we were totally in the dark.

Using my phone as a flashlight (VERY helpful!) I went out into the garage, where I was immediately faced with the problem of getting the garage door open with no AC power. After finally getting the door open, I got the generator moved outside and tried to start it up – no dice; the electric starter solenoid just chattered without actually engaging the motor. A quick visual inspection showed that the onboard 12V SLA batter was grossly swollen – most likely due to my use of an old-style battery charger to keep the battery topped off. Fortunately, I was able to get the generator started using the manual pull cord – yay!

After getting the generator started, I got the 50′ extension cable connected to the 240-volt power receptacle in the garage and toggled the transfer switch on the emergency power sub-panel in the utility room, again using my phone as a flashlight. After toggling the transfer switch, I got lights and PC power back in my engineering lab, but there were still some issues.

Both the in-house refrigerator and the freezer in the garage were still unpowered (this was later found to be due to me finger-fumbling the transfer switch)
The house internet router and LAN ethernet switches were still unpowered. This was unexpected, but not a big deal as I was able to run an extension cord from my engineering lab space to the router/switch power strip.
The garage door was still unpowered.

Utility power was restored after about 3 hours, so I was able to shut down the generator, toggle the transfer switch back to the default position, and generally put everything back together before going to bed. Overall an exciting evening.

31 August Update:

The next day I was able to review the previous night’s activities and make some adjustments for the next time. I ordered a new battery for the generator, and a new modern SLA-compatible float charger, as shown below:

Interestingly, I couldn’t find a replacement battery with the same terminal arrangement as the one that came with the generator. The original had the positive terminal on the left, and all compatible offerings from Amazon had the positive terminal on the right. Fortunately I was able to confirm that the attachment cables had enough slack to reach.

After getting the battery replaced and charged, I went through the entire process again – getting the garage door open, getting the generator outside the garage and started up, getting the 240-volt cable connected to the garage 240-volt receptacle, and toggling the transfer switch on the emergency power panel. This time both the house refrigerator and the in-garage freezer had power, and I realized that I had fumble-fingered the transfer switch during the actual power failure.

Moving Vision Processing to My 4-Wheel Robot, Part 2

Leave a reply

Posted 03 August 2026,

Part 1 in this series laid out the strategy for adding vision processing capabilities to my 4-wheel robot. This post continues that effort.

The first step was to replace the Teensy 3.6 main controller with a Teensy 4.1. As part of this process, I also replaced the glued-on ‘Gnd’ and ‘+5V’ bussbars with a bit more finished product, as you can see in the following photos:

The next step is moving the old 4-wheel firmware to the ‘new-improved’ 4-wheel hardware. I started this by porting “C:\Users\Frank\Documents\Arduino\WallE3_Git\WallE3_Git\WallE3_Git.ino” to “C:\Users\Frank\Documents\Robot_Projects\VisionEnhaned4WheelRobot_V1\VisionEnhaned4WheelRobot_V1.ino”. This took a while, and I’m not sure I wouldn’t have been better off just cloning the entire WallE3_Git.ino file, but I “got it done’.

05 August 2026 Update:

It took a while, but I now have a working 4-wheel drive robot program running again on the 4-wheel chassis with a Teensy 4.1 main controller The only small ‘gotcha’ I encountered during the Teensy 3.5 -> Teensy 4.1 controller switch was the control lines for the right motor driver. It turns out the ‘speed’ (PWM) pin used on the Teensy 3.5 isn’t PWM-capable on the Teensy 4.1, so this required some minor pin swapping, as shown below:

//08/05/26 pin 35 isn't PWMable on Teensy 4.1, but pin 33 is
//const uint16_t InA_Right = 34;
//const uint16_t InB_Right = 33;
//const uint16_t Spd_Right = 35;

//Teensy 4.1
const uint16_t InA_Right = 34;
const uint16_t InB_Right = 35;
const uint16_t Spd_Right = 33;

//08/05/26 pin 35 isn't PWMable on Teensy 4.1, but pin 33 is

//const uint16_t InA_Right = 34;

//const uint16_t InB_Right = 33;

//const uint16_t Spd_Right = 35;

//Teensy 4.1

const uint16_t InA_Right = 34;

const uint16_t InB_Right = 35;

const uint16_t Spd_Right = 33;

Using the manual control features built accessible via the ‘CheckforUserInput() function, I was able to perform left/right 10 degree turns and speed up/slow down, all from my keyboard. I don’t have the second deck connected up yet, but that is next on the agenda.

Upgraded 4-wheel robot demonstrating basic motor

06 August 2026 Update:

4-wheel robot with Raspberry Pi5 and OAKD-Lite camera mounted on 2nd deck

At this point I have a complete 4-wheel drive robot assembled with a Raspberry Pi5 and OAKD-Lite camera, and I can send it basic (left/right/faster/slower/forward/reverse) movement commands via a serial port. In previous work on the 2-wheel robot I demonstrated the ability to send vision-processing-derived movement commands to the main processor from the Pi5 via a serial port.

It occurs to me that with the above capability, The robot doesn’t really need anything else for navigation and obstacle avoidance. The OAKD-Lite can provide visual depth information to the Pi5, and the Pi5 can send basic movement commands to the robot. In addition, it seems to me that I might be able to eliminate the entire ‘homing-to-charger’ software and hardware blocks, as the camera should be able to tell the Pi5 where the charger dock is located in the field of view, and the Pi5 should be able to tell the robot which way to turn and by how much, and when to speed up and slow down. This could make the robot a LOT simpler (as Elon Musk said about Tesla FSD “Once you have vision solved, you no longer need anything else’).

08 August 2026 Update:

The robot is alive! With Grok’s help I now have a python script running on the Pi5 that can direct the robot to turn left or right depending on image processing by the OAKD-Lite camera.

First, the Python shell script ‘restart_camera.sh’ is executed to get the OAKD-Lite camera ready:

pi@RobotPi5:~$ cd my_vision_robot/
pi@RobotPi5:~/my_vision_robot$ ./software/scripts/restart_camera.sh 
=== Killing old camera processes ===
=== Sourcing environments ===
=== Starting camera ===
[INFO] [launch]: All log files can be found below /home/pi/.ros/log/2026-08-08-08-25-25-554555-RobotPi5-3196
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [camera_node-1]: process started with pid [3200]
[camera_node-1] [INFO] [1786191927.851165044] [camera]: Starting camera.
[camera_node-1] [INFO] [1786191927.875363720] [camera]: No ip/mxid specified, connecting to the next available device.
[camera_node-1] [INFO] [1786191930.182731755] [camera]: Camera with MXID: 19443010B166F01200 and Name: 4.1 connected!
[camera_node-1] [INFO] [1786191930.184436975] [camera]: USB SPEED: SUPER
[camera_node-1] [INFO] [1786191930.211320937] [camera]: Device type: OAK-D-LITE
[camera_node-1] [INFO] [1786191930.213305230] [camera]: Pipeline type: Depth
[camera_node-1] [ERROR] [1786191930.379447850] [camera]: rgb parameters not set, defaulting to 1280x720 unless specified otherwise.
[camera_node-1] [INFO] [1786191930.380454015] [camera]: Finished setting up pipeline.
[camera_node-1] [INFO] [1786191930.680038467] [camera]: Camera ready!

pi@RobotPi5:~$ cd my_vision_robot/

pi@RobotPi5:~/my_vision_robot$ ./software/scripts/restart_camera.sh

=== Killing old camera processes ===

=== Sourcing environments ===

=== Starting camera ===

[INFO] [launch]: All log files can be found below /home/pi/.ros/log/2026-08-08-08-25-25-554555-RobotPi5-3196

[INFO] [launch]: Default logging verbosity is set to INFO

[INFO] [camera_node-1]: process started with pid [3200]

[camera_node-1] [INFO] [1786191927.851165044] [camera]: Starting camera.

[camera_node-1] [INFO] [1786191927.875363720] [camera]: No ip/mxid specified, connecting to the next available device.

[camera_node-1] [INFO] [1786191930.182731755] [camera]: Camera with MXID: 19443010B166F01200 and Name: 4.1 connected!

[camera_node-1] [INFO] [1786191930.184436975] [camera]: USB SPEED: SUPER

[camera_node-1] [INFO] [1786191930.211320937] [camera]: Device type: OAK-D-LITE

[camera_node-1] [INFO] [1786191930.213305230] [camera]: Pipeline type: Depth

[camera_node-1] [ERROR] [1786191930.379447850] [camera]: rgb parameters not set, defaulting to 1280x720 unless specified otherwise.

[camera_node-1] [INFO] [1786191930.380454015] [camera]: Finished setting up pipeline.

[camera_node-1] [INFO] [1786191930.680038467] [camera]: Camera ready!

Once the camera is ready, then the demo program ‘clearest_direction_node.py’ is executed on the Pi5 (with ‘VisionEnhanced4WheelRobot.ino already running on the Teensy 4.1)

Based on a rudimentary analysis of the depth (distance) distribution in the camera field of view, the ‘clearest_direction_node.py’ Python script sends left/right/stop commands to the Teensy 4.1 from the Pi5’s serial port to the Teensy 4.1’s ‘Serial1’ port. The commands use the already-existing robot manual motion control command structure shown below:

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

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

pi@RobotPi5:~/my_vision_robot$ python3 software/scripts/clearest_direction_node.py --ros-args -p enable_motion:=True
[INFO] [1786192457.618821619] [clearest_direction_node]: Motion ENABLED – opened /dev/ttyAMA0 @ 115200 and sent "C" (command mode)
[INFO] [1786192457.672358648] [clearest_direction_node]: Clearest Direction Node started (7 sectors, bottom half only)
[INFO] [1786192457.689451373] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)
[INFO] [1786192467.311608370] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786192468.114612060] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)
[INFO] [1786192478.041447563] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)
[INFO] [1786192478.846585878] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)
[INFO] [1786192481.741594815] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786192482.545474512] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)
[INFO] [1786192490.073479144] [clearest_direction_node]: Sent "4" → left 10° (recommended -31°)
[INFO] [1786192490.877634051] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)
[INFO] [1786192493.239450490] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786192494.044599603] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

pi@RobotPi5:~/my_vision_robot$ python3 software/scripts/clearest_direction_node.py --ros-args -p enable_motion:=True

[INFO] [1786192457.618821619] [clearest_direction_node]: Motion ENABLED – opened /dev/ttyAMA0 @ 115200 and sent "C" (command mode)

[INFO] [1786192457.672358648] [clearest_direction_node]: Clearest Direction Node started (7 sectors, bottom half only)

[INFO] [1786192457.689451373] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

[INFO] [1786192467.311608370] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786192468.114612060] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

[INFO] [1786192478.041447563] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)

[INFO] [1786192478.846585878] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

[INFO] [1786192481.741594815] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786192482.545474512] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

[INFO] [1786192490.073479144] [clearest_direction_node]: Sent "4" → left 10° (recommended -31°)

[INFO] [1786192490.877634051] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

[INFO] [1786192493.239450490] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786192494.044599603] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

Here’s a short video showing the action

Here’s another short video showing the robot actually making 10-deg turns left & right in response to a simulated obstacle in the field of view:

And here is some of the telemetry output from the above run:

m)
[INFO] [1786294925.766194888] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)
[INFO] [1786294926.567160145] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)
[INFO] [1786294928.101189495] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786294928.901243868] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786294929.701313191] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)
[INFO] [1786294930.601312174] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786294931.434187553] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)
[INFO] [1786294932.266154731] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786294933.067315737] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)
[INFO] [1786294933.900181102] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)
[INFO] [1786294934.700201135] [clearest_direction_node]: Sent "6" → right 10° (recommended 10°)
[INFO] [1786294936.067153241] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786294936.900076933] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)
[INFO] [1786294939.201264474] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)
[INFO] [1786294940.067229716] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)
[INFO] [1786294941.068341556] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786294941.901316245] [clearest_direction_node]: Sent "6" → right 10° (recommended 10°)
[INFO] [1786294943.168337641] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786294944.001189364] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)
[INFO] [1786294944.866160026] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)
[INFO] [1786294945.798186488] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)
[INFO] [1786294946.600316490] [clearest_direction_node]: Sent "4" → left 10° (recommended -21°)
[INFO] [1786294947.600156058] [clearest_direction_node]: Sent "6" → right 10° (recommended 10°)
[INFO] [1786294949.299158322] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786294950.100182709] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)
[INFO] [1786294951.433310857] [clearest_direction_node]: Sent "4" → left 10° (recommended -21°)
[INFO] [1786294952.265305195] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)
[INFO] [1786294953.832184082] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)
[INFO] [1786294954.664313739] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)
[INFO] [1786294956.765188420] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786294957.597195768] [clearest_direction_node]: Sent "4" → left 10° (recommended -31°)
[INFO] [1786294958.429191481] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)
[INFO] [1786294959.230315045] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)
[INFO] [1786294960.031185898] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)
[INFO] [1786294960.998157709] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)
[INFO] [1786294961.831224208] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)
[INFO] [1786294964.598186768] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)
[INFO] [1786294965.431191999] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)
[INFO] [1786294971.431270013] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)
[INFO] [1786294972.264315801] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)
[INFO] [1786294974.930187367] [clearest_direction_node]: Sent "4" → left 10° (recommended -21°)
[INFO] [1786294975.731189452] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)
[INFO] [1786294978.798182012] [clearest_direction_node]: Sent "4" → left 10° (recommended -31°)

[INFO] [1786294925.766194888] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)

[INFO] [1786294926.567160145] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)

[INFO] [1786294928.101189495] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786294928.901243868] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786294929.701313191] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)

[INFO] [1786294930.601312174] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786294931.434187553] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)

[INFO] [1786294932.266154731] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786294933.067315737] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)

[INFO] [1786294933.900181102] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)

[INFO] [1786294934.700201135] [clearest_direction_node]: Sent "6" → right 10° (recommended 10°)

[INFO] [1786294936.067153241] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786294936.900076933] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)

[INFO] [1786294939.201264474] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)

[INFO] [1786294940.067229716] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)

[INFO] [1786294941.068341556] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786294941.901316245] [clearest_direction_node]: Sent "6" → right 10° (recommended 10°)

[INFO] [1786294943.168337641] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786294944.001189364] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

[INFO] [1786294944.866160026] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)

[INFO] [1786294945.798186488] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

[INFO] [1786294946.600316490] [clearest_direction_node]: Sent "4" → left 10° (recommended -21°)

[INFO] [1786294947.600156058] [clearest_direction_node]: Sent "6" → right 10° (recommended 10°)

[INFO] [1786294949.299158322] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786294950.100182709] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

[INFO] [1786294951.433310857] [clearest_direction_node]: Sent "4" → left 10° (recommended -21°)

[INFO] [1786294952.265305195] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

[INFO] [1786294953.832184082] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)

[INFO] [1786294954.664313739] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

[INFO] [1786294956.765188420] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786294957.597195768] [clearest_direction_node]: Sent "4" → left 10° (recommended -31°)

[INFO] [1786294958.429191481] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)

[INFO] [1786294959.230315045] [clearest_direction_node]: Sent "5" → stop (best depth 0 mm < 400 mm)

[INFO] [1786294960.031185898] [clearest_direction_node]: Sent "6" → right 10° (recommended 31°)

[INFO] [1786294960.998157709] [clearest_direction_node]: Sent "5" → stop (turn 0° inside deadband ±6.0°)

[INFO] [1786294961.831224208] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)

[INFO] [1786294964.598186768] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)

[INFO] [1786294965.431191999] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)

[INFO] [1786294971.431270013] [clearest_direction_node]: Sent "4" → left 10° (recommended -10°)

[INFO] [1786294972.264315801] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)

[INFO] [1786294974.930187367] [clearest_direction_node]: Sent "4" → left 10° (recommended -21°)

[INFO] [1786294975.731189452] [clearest_direction_node]: Sent "6" → right 10° (recommended 21°)

[INFO] [1786294978.798182012] [clearest_direction_node]: Sent "4" → left 10° (recommended -31°)

At this point in the project, we have demonstrated that visual ‘depth’ information can be used to guide the robot, and we have separately demonstrated that the Teensy firmware can be updated ‘Over The Air’ (OTA) via the wifi connection to the Pi5. I believe the next step should be to add wifi OTA to the ‘clearest_direction_node.py’ script in order to remove the requirement for physically attaching a USB cable to the Teensy 4.1 to update the firmware.

08 August 2026 Update:

Adding Wifi OTA to the current ‘clearest_direction_node.py’ configuration turned out to be relatively easy, as this ability had already been developed as a stand-alone demo back in June 2026. All we had to do was update the ‘board.txt’ file to point to the new location of ‘PostBuild_OTA.py’ on my PC, and make sure ‘Wifi_OTA.py’ was running on the Pi5 so it would look for changes in “/home/pi/my_vision_robot/firmware/latest.hex” and trigger the update.

09 August 2026 Update:

Thinking while drifting off to sleep last night, I realized that the addition of the Pi5 to the robot means that all the navigation and obstacle avoidance tasks (including possibly navigating to and connecting with the charger module) will be handled by the Pi5, which significantly simplifies the Teensy 4.1 code. All the Teensy code has to do now is respond to motion commands from the Pi5, monitor the various hardware sensors and update the rear-facing LED array.

Teensy Responsibilities:

Run the motors in response to motion commands from Pi5 on Serial1
Update the ‘state-of-charge’ (and left/right steering) LEDs
Monitor the output from three different 1NA169 current sensors (I_chg, I_tot, I_upper-deck)
Monitor the output from two different Vbatt voltage sensors (V_batt, V_upper-deck)
Monitor the ‘charger connected’ output from the TPS5100 battery pack
Monitor the rear-facing VL53L1X LIDAR for rear collision avoidance
Run the speaker
Home in on and connect to the charger module (may be done by the Pi5 instead)

Pi5 Responsibilities:

Manage all navigation and obstacle avoidance tasks
Possibly manage charger module homing (April tag?)
Create map (SLAM)?
Respond to rear-facing obstacle alerts from Teensy
Pass robot telemetry data on Serial1 to PC
Pass Pi5-software-related telemetry data to VS Code terminal

10 August 2026 Update:

Good progress on the vision-enhanced robot project today. Yesterday we installed two additional cases in the switch block that accepts manual control commands for the robot, to allow the user to command left/right turns of an arbitrary number of degrees at an arbitrary turn rate. The command format consists of the letter ‘L’ (left/CCW) or ‘R’ (Right/CW) followed by the number of degrees to turn (and optionally a specified turn rate in deg/sec), but the commands failed. Today (while waiting in the cellphone lot for my wife) I figured out what was wrong with the teensy command parsing code. Now I can type “L30.5” at a VS Code prompt that gets transmitted via SSH over Wifi to the Pi5, then via the dedicated serial to the Teensy. Then the Teensy calls ‘SpinTurn(30.5, rate = 30) to actually effect the turn. Here’s a short video showing a left (CCW) turn of approximately 90 deg at a 90 deg/sec rate, followed by another left turn of 90 deg at the default 30 deg/sec rate, followed by a right turn of 180 deg at the default 30 deg/sec rate:

We also implemented a replacement for my old telemetry display using the Visual Studio serial port monitor. In theory I could maintain that feature as I still have the HC-05 Bluetooth serial module installed on the robot, but I would rather put everything over wifi for better range & reliability. What we did instead was to have WallE_5.py log everything that comes to it on its serial port (Teeny ‘Serial1’) to a ‘telemetry.log’ file, which allows me to start up a new Pi5 terminal session in its own window and run

tail -f ~/my_vision_robot/logs/telemetry.log

1	tail -f ~/my_vision_robot/logs/telemetry.log

to display whatever telemetry information the teensy is sending to ‘Serial1’. Here’s a short sampling of the telemetry data setup:

pi@RobotPi5:~/my_vision_robot$ tail -f logs/telemetry.log 
In Default Case: Stopping Motors!
I received L30
In R/L Case with incomingByte = L
In R/L Case with b_ccw = CW numBuf = 30
CCW 30.00 deg @ 45.0 deg/s
In SpinTurn(CCW, 30.00, 45.00) with PID = (0.7,0.3,0.0)
I received C
In Default Case: Stopping Motors!
I received L30.5,10
In R/L Case with incomingByte = L
In R/L Case with b_ccw = CW numBuf = 30.5,
CCW 30.50 deg @ 45.0 deg/s
In SpinTurn(CCW, 30.50, 45.00) with PID = (0.7,0.3,0.0)
I received L30
In R/L Case with incomingByte = L
In R/L Case with b_ccw = CW numBuf = 30
CCW 30.00 deg @ 45.0 deg/s
In SpinTurn(CCW, 30.00, 45.00) with PID = (0.7,0.3,0.0)
In R/L Case with incomingByte = L
In R/L Case with b_ccw = CW numBuf = 30,45
CCW 30.00 deg @ 45.0 deg/s
In SpinTurn(CCW, 30.00, 45.00) with PID = (0.7,0.3,0.0)
In R/L Case with incomingByte = R
In R/L Case with b_ccw = CCW numBuf = 30,60
CW 30.00 deg @ 60.0 deg/s
In SpinTurn(CW, 30.00, 60.00) with PID = (0.7,0.3,0.0)
I received R60,60
In R/L Case with incomingByte = R
In R/L Case with b_ccw = CCW numBuf = 60,60
CW 60.00 deg @ 60.0 deg/s
In SpinTurn(CW, 60.00, 60.00) with PID = (0.7,0.3,0.0)
In R/L Case with incomingByte = R
In R/L Case with b_ccw = CCW numBuf = 60
CW 60.00 deg @ 45.0 deg/s
In SpinTurn(CW, 60.00, 45.00) with PID = (0.7,0.3,0.0)
In R/L Case with incomingByte = R
In R/L Case with b_ccw = CCW numBuf = 30
CW 30.00 deg @ 45.0 deg/s
In SpinTurn(CW, 30.00, 45.00) with PID = (0.7,0.3,0.0)
I received L90
In R/L Case with incomingByte = L
In R/L Case with b_ccw = CW numBuf = 90
CCW 90.00 deg @ 45.0 deg/s
In SpinTurn(CCW, 90.00, 45.00) with PID = (0.7,0.3,0.0)
In R/L Case with incomingByte = R
In R/L Case with b_ccw = CCW numBuf = 10
CW 10.00 deg @ 45.0 deg/s
In SpinTurn(CW, 10.00, 45.00) with PID = (0.7,0.3,0.0)
In R/L Case with incomingByte = R
In R/L Case with b_ccw = CCW numBuf = 90,90
CW 90.00 deg @ 90.0 deg/s
In SpinTurn(CW, 90.00, 90.00) with PID = (0.7,0.3,0.0)
In R/L Case with incomingByte = L
In R/L Case with b_ccw = CW numBuf = 90,90
CCW 90.00 deg @ 90.0 deg/s
In SpinTurn(CCW, 90.00, 90.00) with PID = (0.7,0.3,0.0)
In R/L Case with incomingByte = R
In R/L Case with b_ccw = CCW numBuf = 90
CW 90.00 deg @ 45.0 deg/s
In SpinTurn(CW, 90.00, 45.00) with PID = (0.7,0.3,0.0)
I received L90,90
In R/L Case with incomingByte = L
In R/L Case with b_ccw = CW numBuf = 90,90
CCW 90.00 deg @ 90.0 deg/s
In SpinTurn(CCW, 90.00, 90.00) with PID = (0.7,0.3,0.0)
In R/L Case with incomingByte = L
In R/L Case with b_ccw = CW numBuf = 90
CCW 90.00 deg @ 45.0 deg/s
In SpinTurn(CCW, 90.00, 45.00) with PID = (0.7,0.3,0.0)
In R/L Case with incomingByte = R
In R/L Case with b_ccw = CCW numBuf = 180
CW 180.00 deg @ 45.0 deg/s
In SpinTurn(CW, 180.00, 45.00) with PID = (0.7,0.3,0.0)

pi@RobotPi5:~/my_vision_robot$ tail -f logs/telemetry.log

In Default Case: Stopping Motors!

I received L30

In R/L Case with incomingByte = L

In R/L Case with b_ccw = CW numBuf = 30

CCW 30.00 deg @ 45.0 deg/s

In SpinTurn(CCW, 30.00, 45.00) with PID = (0.7,0.3,0.0)

I received C

In Default Case: Stopping Motors!

I received L30.5,10

In R/L Case with incomingByte = L

In R/L Case with b_ccw = CW numBuf = 30.5,

CCW 30.50 deg @ 45.0 deg/s

In SpinTurn(CCW, 30.50, 45.00) with PID = (0.7,0.3,0.0)

I received L30

In R/L Case with incomingByte = L

In R/L Case with b_ccw = CW numBuf = 30

CCW 30.00 deg @ 45.0 deg/s

In SpinTurn(CCW, 30.00, 45.00) with PID = (0.7,0.3,0.0)

In R/L Case with incomingByte = L

In R/L Case with b_ccw = CW numBuf = 30,45

CCW 30.00 deg @ 45.0 deg/s

In SpinTurn(CCW, 30.00, 45.00) with PID = (0.7,0.3,0.0)

In R/L Case with incomingByte = R

In R/L Case with b_ccw = CCW numBuf = 30,60

CW 30.00 deg @ 60.0 deg/s

In SpinTurn(CW, 30.00, 60.00) with PID = (0.7,0.3,0.0)

I received R60,60

In R/L Case with incomingByte = R

In R/L Case with b_ccw = CCW numBuf = 60,60

CW 60.00 deg @ 60.0 deg/s

In SpinTurn(CW, 60.00, 60.00) with PID = (0.7,0.3,0.0)

In R/L Case with incomingByte = R

In R/L Case with b_ccw = CCW numBuf = 60

CW 60.00 deg @ 45.0 deg/s

In SpinTurn(CW, 60.00, 45.00) with PID = (0.7,0.3,0.0)

In R/L Case with incomingByte = R

In R/L Case with b_ccw = CCW numBuf = 30

CW 30.00 deg @ 45.0 deg/s

In SpinTurn(CW, 30.00, 45.00) with PID = (0.7,0.3,0.0)

I received L90

In R/L Case with incomingByte = L

In R/L Case with b_ccw = CW numBuf = 90

CCW 90.00 deg @ 45.0 deg/s

In SpinTurn(CCW, 90.00, 45.00) with PID = (0.7,0.3,0.0)

In R/L Case with incomingByte = R

In R/L Case with b_ccw = CCW numBuf = 10

CW 10.00 deg @ 45.0 deg/s

In SpinTurn(CW, 10.00, 45.00) with PID = (0.7,0.3,0.0)

In R/L Case with incomingByte = R

In R/L Case with b_ccw = CCW numBuf = 90,90

CW 90.00 deg @ 90.0 deg/s

In SpinTurn(CW, 90.00, 90.00) with PID = (0.7,0.3,0.0)

In R/L Case with incomingByte = L

In R/L Case with b_ccw = CW numBuf = 90,90

CCW 90.00 deg @ 90.0 deg/s

In SpinTurn(CCW, 90.00, 90.00) with PID = (0.7,0.3,0.0)

In R/L Case with incomingByte = R

In R/L Case with b_ccw = CCW numBuf = 90

CW 90.00 deg @ 45.0 deg/s

In SpinTurn(CW, 90.00, 45.00) with PID = (0.7,0.3,0.0)

I received L90,90

In R/L Case with incomingByte = L

In R/L Case with b_ccw = CW numBuf = 90,90

CCW 90.00 deg @ 90.0 deg/s

In SpinTurn(CCW, 90.00, 90.00) with PID = (0.7,0.3,0.0)

In R/L Case with incomingByte = L

In R/L Case with b_ccw = CW numBuf = 90

CCW 90.00 deg @ 45.0 deg/s

In SpinTurn(CCW, 90.00, 45.00) with PID = (0.7,0.3,0.0)

In R/L Case with incomingByte = R

In R/L Case with b_ccw = CCW numBuf = 180

CW 180.00 deg @ 45.0 deg/s

In SpinTurn(CW, 180.00, 45.00) with PID = (0.7,0.3,0.0)

15 August 2026 Update:

I discovered that the Teensy 4.1 was unresponsive – yuk! After some research and troubleshooting, I realized that I had fallen into a classsic Teensy 3.5->4.1 transition trap; Teensy 3.5 is 5V tolerant on (almost) all pins, while the 4.5 is not. I didn’t really look hard at the circuit before making the 3.5->4.1 change, but almost all the peripheral hardware produces 5V signals on the MCU pins – oops!

So, I have spent the last few days going backwards – from Teensy 4.1 to Teensy 3.5. I took the opportunity to improve wire/pin labelling while doing the reverse transition. Here is the updated schematic:

And here are some photos showing the wire labelling:

I created a small test program, originally to just test the rear-facing VL53L1X LIDAR, but later expanded to test all sensors. Here’s the code:

/*
    Name:       VL53L1X_Demo.ino
    Created:	8/13/2026 8:09:07 PM
    Author:     FRANK_XPS_9530\Frank
*/
#include <Wire.h>
#include <VL53L1X.h>
#include "MPU6050_6Axis_MotionApps612.h"  //01/18/22 changed to use the \I2CDevLib\Arduino\MPU6050\ version

VL53L1X lidar;

#pragma region PIN ASSIGNMENTS
const uint8_t XSHUT_PIN = 26;
const uint8_t SW_BATT_VOLT_PIN = 15;
const uint8_t I_TOP_PIN = 39;
const uint8_t I_BOT_PIN = 17;
const uint8_t I_CHG_PIN = 16;
const uint8_t LASER_PIN = 5;
const uint8_t SPEAKER_PIN = 2;


#pragma endregion PIN ASSIGNMENTS

#pragma region ADC CONSTANTS
const float ADC_REF_VOLTS = 3.3; //teensy default for analog inputs
const int MAX_AD_COUNT = 1023;
const float AmpsPerVolt = 1.00; //default 10K Rs
const float VoltsPerCount = ADC_REF_VOLTS / MAX_AD_COUNT;
const float VOLTAGE_TO_CURRENT_RATIO = 1.f; //Used for both 'Total' and 'Run' sensors
#pragma endregion ADC CONSTANTS

const float ZENER_VOLTAGE_OFFSET = 5.96; //03/14/22 measured zener voltage
const char* WallFollowTelemStr = "%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.2f\t%2.2f\t%2.0f\t%2.1f\t%d\t%d\t%s\t%s\n";//05/12/23 added L/R motor speeds
const char* ColumnHeaderStr = "Msec\tBattV\tTop_I\tBot_I\tChg_I\tRear_mm\tHdgDeg\n";


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

#pragma region MPU6050_SUPPORT
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
int GetPacketLoopCount = 0;
int OuterGetPacketLoopCount = 0;

//MPU6050 status flags
bool bMPU6050Ready = true;
bool dmpReady = false;  // set true if DMP init was successful
volatile float IMUHdgValDeg = 0; //updated by UpdateIMUHdgValDeg()//11/02/20 now updated in ISR
const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()
uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param
bool bFirstTime = true;
//#define MPU6050_CCW_INCREASES_YAWVAL //added 12/05/19 commented out 11/22/21 as now the MPU6050 module is mounted 'Z-up'
#pragma endregion MPU6050 Support

#pragma region HEADING_AND_RATE_BASED_TURN_PARAMETERS
float Prev_HdgDeg = 0; //02/01/23 - this should be a local variable in SpinTurn()
float TurnRatePIDOutput; //02/01/23 - this should be a local variable in SpinTurn()

//06/04/22 from WallE3_SpinTurnTuning.ino
float TurnRate_Kp = 0.7;//02/03/23 updated Kp from 1.0 to 0.7 per https://www.fpaynter.com/2022/12/walle3-spin-turn-revisited/
float TurnRate_Ki = 0.3;
float TurnRate_Kd = 0.0;//02/03/23 updated Kd from 0.1 to 0.0 per https://www.fpaynter.com/2022/12/walle3-spin-turn-revisited/

//ported from FourWD_PulseTurnRateTest.ino

const float HDG_NEAR_MATCH_VAL = 0.8; //slow the turn down here
const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here //rev 06/01/21
const float HDG_MIN_MATCH_VAL = 0.6; //added 09/08/18: don't start checking slope until turn is well started
const float DEFAULT_TURN_RATE_DEGPERSEC = 45.f; //06/06/21 updated
const uint16_t HEADING_HISTORY_ARRAY_SIZE = 50; //06/12/23 added for 'spinning' condx detection
float gl_HdgHistoryArray[HEADING_HISTORY_ARRAY_SIZE];

//02/16/22 fwd decl reqd for fcns using default param
//09/16/23 added RunBothMotorsMsec with default params
//bool SpinTurn(bool b_ccw, float numDeg, float degPersec = DEFAULT_TURN_RATE_DEGPERSEC);
//
////06/01/24 chg int gl_Leftspeednum to uint16_t leftspeednum, int gl_Rightspeednum to uint16_t rightspeednum
////void RunBothMotorsMsec(bool bisFwd, int timeMsec = 500, int gl_Leftspeednum = MOTOR_SPEED_HALF, int gl_Rightspeednum = MOTOR_SPEED_HALF);
//void RunBothMotorsMsec(bool bisFwd, int timeMsec = 500, uint16_t leftspeednum = MOTOR_SPEED_HALF, uint16_t rightspeednum = MOTOR_SPEED_HALF);
//bool RollingTurn(bool b_ccw, bool b_fwd, float numDeg, float Kp, float Ki, float Kd, float degPersec = DEFAULT_TURN_RATE_DEGPERSEC);
//bool IsChargerConnected(bool curState = false);

#pragma endregion HEADING_AND_RATE_BASED_TURN_PARAMETERS

MPU6050 mpu(MPU6050_I2C_ADDR);

void setup()
{
  Serial.begin(115200);
  while (!Serial && millis() < 3000);
  delay(500);

  // Reset the sensor
  pinMode(XSHUT_PIN, OUTPUT);
  digitalWrite(XSHUT_PIN, LOW);
  delay(10);
  digitalWrite(XSHUT_PIN, HIGH);
  delay(20);

  Wire.begin();
  Wire.setClock(400000);

  // Use Wire1 instead of Wire for rear LIDAR
  Wire1.begin();
  Wire1.setClock(400000);

  lidar.setBus(&Wire1);          // <-- this is the key line
  lidar.setTimeout(500);

  if (!lidar.init())
  {
    Serial.println("Failed to detect and initialize VL53L1X on Wire1!");
    while (1);
  }

  Serial.println("VL53L1X detected on Wire1 and initialized");

  lidar.setDistanceMode(VL53L1X::Long);
  lidar.setMeasurementTimingBudget(50000);
  lidar.startContinuous(50);

  //init current sensor pins
  pinMode(I_BOT_PIN, INPUT);
  pinMode(I_TOP_PIN, INPUT);
  pinMode(I_CHG_PIN, INPUT);
  pinMode(LASER_PIN, OUTPUT);

  //exercise the laser diode
  digitalWrite(LASER_PIN, HIGH);
  delay(200);
  digitalWrite(LASER_PIN, LOW);
  delay(200);
  digitalWrite(LASER_PIN, HIGH);
  delay(200);
  digitalWrite(LASER_PIN, LOW);

  //Exercise the speaker
  tone(SPEAKER_PIN, 1000, 2000); //returns immediately


#pragma region MPU6050
#ifndef NO_MPU6050
  Serial.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR);
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  mpu.initialize();

  // verify connection

  float StartSec = 0; //used to time MPU6050 init
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // make sure it worked (returns 0 if successful)
  if (devStatus == 0)
  {
    // turn on the DMP, now that it's ready
    Serial.printf(F("Enabling DMP...\n"));
    mpu.setDMPEnabled(true);

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();

    Serial.printf(F("Calibrating...Retrieving Calibration Values\n\n"));
    mpu.CalibrateGyro(); //using default value of 15
    mpu.PrintActiveOffsets();

    //loop heading display until stabilized
    Serial.printf(F("\nMsec\tHdg\n"));

    UpdateIMUHdgValDeg();
    Prev_HdgDeg = IMUHdgValDeg;
    delay(100);
    UpdateIMUHdgValDeg();

    Serial.printf("%lu\t%2.3f\t%2.3f\n", millis(), IMUHdgValDeg, Prev_HdgDeg);
    while (abs(IMUHdgValDeg - Prev_HdgDeg) > 0.1f)
    {
      Serial.printf("%lu\t%2.3f\n", millis(), IMUHdgValDeg);
      Prev_HdgDeg = IMUHdgValDeg;
      delay(100);
      UpdateIMUHdgValDeg();
    }

    StartSec = millis() / 1000.f;
    Serial.printf("MPU6050 Ready at %2.2f Sec with delta = %2.3f\n", StartSec, IMUHdgValDeg - Prev_HdgDeg);
    bMPU6050Ready = true;

    delay(1000);
  }
  else //MPU6050 Init failed for some reason
  {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.printf("DMP Initialization failed with code %d", devStatus);

    //08/29/21 print out battery voltage on failure
    //float batV = GetBattVoltage();
    //gl_batteryVoltage = GetBattVoltage();
    //Serial.printf("Battery Voltage = %2.2f\n", gl_batteryVoltage);

    bMPU6050Ready = false;
  }
#endif // !NO_MPU6050
#pragma endregion MPU6050

  //Serial.printf("Msec\tBattV\tTop_I\tBot_I\tChg_I\tRear_mm\n");
  Serial.printf(ColumnHeaderStr);
}

void loop()
{
  //rear-facing VL53L1X
  uint16_t rear_mm = lidar.read();

  //Serial.print(rear_mm);
  if (lidar.timeoutOccurred())
  {
    Serial.print("  TIMEOUT");
  }

  //04/02/21 moved to 'fast' part of loop
  float BattV = GetVoltage(SW_BATT_VOLT_PIN);
  float TopI = GetAmps(I_TOP_PIN);
  float BotI = GetAmps(I_BOT_PIN);
  float ChgI = GetAmps(I_CHG_PIN);
  UpdateIMUHdgValDeg(); //updatees IMUHdgValDeg

  //Serial.printf("\Msec\tBattV\tTop_I\tBot_I\tChg_I\tRear_mm\n");
  Serial.printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\t%2.2f\n", millis(), BattV, TopI, BotI, ChgI, rear_mm, IMUHdgValDeg );

  delay(100);
}

float GetAmps(int pin_number)
{
  //Purpose:  Get current in amps
  //Inputs: 
  // pin_number = integer denoting analog pin to be used for measurement
  //	VOLTAGE_TO_CURRENT_RATIO = measured voltage to current ratio
  // MAX_AD_COUNT = int denoting max A/D reading value
  // VOLTAGE_TO_CURRENT_RATIO = int denoting conversion ratio
  //Outputs:
  //	returns total robot current (chg current plus running current)
  //Notes:
  //	02/28/18 chg name from GetBattChgAmps() to GetTotalAmps()
  //  11/24/21 chg name, add pin_number param so can use for both Itot & Irun

  int reading = analogRead(pin_number); //range is 0-1023
  float volts = ((float)reading / (float)MAX_AD_COUNT) * ADC_REF_VOLTS;
  float amps = volts * VOLTAGE_TO_CURRENT_RATIO;

  //DEBUG!!
    //Serial.printf("GetAmps(%d): reading, volts, amps = %d, %3.2f, %3.2f\n", 
    //  pin_number, reading, volts, amps);
  //DEBUG!!

  return amps;
}

float GetVoltage(uint8_t volt_pin)
{
  //02/18/17 get corrected battery voltage.  Voltage reading is 1/3 actual Vbatt value
  int analog_batt_reading = analogRead(volt_pin);//analogReadAveraging(8) in setup() does internal averaging
  float calc_volts = ZENER_VOLTAGE_OFFSET + ADC_REF_VOLTS * (float)analog_batt_reading / (float)MAX_AD_COUNT;

  //DEBUG!!
  //for(int i = 0; i < 8;i++)
  //{
  //  int analog_batt_reading = analogRead(BATT_MON_PIN);//analogReadAveraging(8) in setup() does internal averaging
  //  float calc_volts = ZENER_VOLTAGE_OFFSET + ADC_REF_VOLTS * (float)analog_batt_reading / (float)MAX_AD_COUNT;
  //  gl_pSerPort->printf("a/d = %d, calc = %2.2f\n", analog_batt_reading,calc_volts);
  //  delay(100);
  //}
  //DEBUG!!
  return calc_volts;
}

float UpdateIMUHdgValDeg()
{
  //Purpose: Get latest yaw (heading) value from IMU
  //Inputs: None.  This function should only be called after mpu.dmpPacketAvailable() returns TRUE
  //Outputs: 
  //	returns true if successful, otherwise false
  //	IMUHdgValDeg updated on success
  //Plan:
  //Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet
  //Step2: read all available packets to get to latest data
  //Step3: update IMUHdgValDeg with latest value
  //Notes:
  //	10/08/19 changed return type to boolean
  //	10/08/19 no longer need mpuIntStatus
  //	10/21/19 completely rewritten to use Homer's algorithm
  //	05/05/20 changed return type to float vs bool.
  //  06/13/23 added code to update gl_HdgHistoryArray for 'spinning' condx detection

  int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

  if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow
  {
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    //compute the yaw value
    IMUHdgValDeg = ypr[0] * 180 / M_PI;
  }

  //06/13/23 added to update gl_HdgHistoryArray for 'spinning' condx detection
  //all array entries bumped down one, with most recent value at i = HEADING_HISTORY_ARRAY_SIZE-1
  for (uint16_t i = 1; i < HEADING_HISTORY_ARRAY_SIZE; i++)
  {
    gl_HdgHistoryArray[i - 1] = gl_HdgHistoryArray[i];
    //gl_pSerPort->printf("gl_HdgHistoryArray[%d] = %2.2f\n", i, gl_HdgHistoryArray[i]);
  }
  gl_HdgHistoryArray[HEADING_HISTORY_ARRAY_SIZE - 1] = IMUHdgValDeg;


  return IMUHdgValDeg;//05/05/20 now returns updated value for use convenience
}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)
{
  mpu.resetFIFO();
  delay(1);
  //int countloop = 0;

  fifoCount = mpu.getFIFOCount();
  GetPacketLoopCount = 0;

  //gl_pSerPort->printf("In GetCurrentFIFOPacket: before loop fifoC = %d\t", fifoCount);
  while (fifoCount < packetSize && GetPacketLoopCount < max_loops)
  {
    GetPacketLoopCount++;
    fifoCount = mpu.getFIFOCount();
    delay(2);
  }

  //gl_pSerPort->printf("In GetCurrentFIFOPacket: after loop fifoC = %d, loop count = %d\n", fifoCount, GetPacketLoopCount);

  if (GetPacketLoopCount >= max_loops)
  {
    return 0;
  }

  //if we get to here, there should be exactly one packet in the FIFO
  mpu.getFIFOBytes(data, packetSize);
  return 1;
}

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

Name: VL53L1X_Demo.ino

Created: 8/13/2026 8:09:07 PM

Author: FRANK_XPS_9530\Frank

#include <Wire.h>

#include <VL53L1X.h>

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

VL53L1X lidar;

#pragma region PIN ASSIGNMENTS

const uint8_t XSHUT_PIN = 26;

const uint8_t SW_BATT_VOLT_PIN = 15;

const uint8_t I_TOP_PIN = 39;

const uint8_t I_BOT_PIN = 17;

const uint8_t I_CHG_PIN = 16;

const uint8_t LASER_PIN = 5;

const uint8_t SPEAKER_PIN = 2;

#pragma endregion PIN ASSIGNMENTS

#pragma region ADC CONSTANTS

const float ADC_REF_VOLTS = 3.3; //teensy default for analog inputs

const int MAX_AD_COUNT = 1023;

const float AmpsPerVolt = 1.00; //default 10K Rs

const float VoltsPerCount = ADC_REF_VOLTS / MAX_AD_COUNT;

const float VOLTAGE_TO_CURRENT_RATIO = 1.f; //Used for both 'Total' and 'Run' sensors

#pragma endregion ADC CONSTANTS

const float ZENER_VOLTAGE_OFFSET = 5.96; //03/14/22 measured zener voltage

const char* WallFollowTelemStr = "%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.2f\t%2.2f\t%2.0f\t%2.1f\t%d\t%d\t%s\t%s\n";//05/12/23 added L/R motor speeds

const char* ColumnHeaderStr = "Msec\tBattV\tTop_I\tBot_I\tChg_I\tRear_mm\tHdgDeg\n";

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

#pragma region I2C_ADDRESSES

#define IRDET_I2C_ADDR 0x08

#define MPU6050_I2C_ADDR 0x68

#pragma endregion I2C_ADDRESSES

#pragma region MPU6050_SUPPORT

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU. Used in Homer's Overflow routine

uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize; // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount; // count of all bytes currently in FIFO

uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars

Quaternion q; // [w, x, y, z] quaternion container

VectorInt16 aa; // [x, y, z] accel sensor measurements

VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements

VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements

VectorFloat gravity; // [x, y, z] gravity vector

float euler[3]; // [psi, theta, phi] Euler angle container

float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

int GetPacketLoopCount = 0;

int OuterGetPacketLoopCount = 0;

//MPU6050 status flags

bool bMPU6050Ready = true;

bool dmpReady = false; // set true if DMP init was successful

volatile float IMUHdgValDeg = 0; //updated by UpdateIMUHdgValDeg()//11/02/20 now updated in ISR

const uint16_t MAX_GETPACKET_LOOPS = 100; //10/30/19 added for backup loop exit condition in GetCurrentFIFOPacket()

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops = MAX_GETPACKET_LOOPS); //prototype here so can define a default param

bool bFirstTime = true;

//#define MPU6050_CCW_INCREASES_YAWVAL //added 12/05/19 commented out 11/22/21 as now the MPU6050 module is mounted 'Z-up'

#pragma endregion MPU6050 Support

#pragma region HEADING_AND_RATE_BASED_TURN_PARAMETERS

float Prev_HdgDeg = 0; //02/01/23 - this should be a local variable in SpinTurn()

float TurnRatePIDOutput; //02/01/23 - this should be a local variable in SpinTurn()

//06/04/22 from WallE3_SpinTurnTuning.ino

float TurnRate_Kp = 0.7;//02/03/23 updated Kp from 1.0 to 0.7 per https://www.fpaynter.com/2022/12/walle3-spin-turn-revisited/

float TurnRate_Ki = 0.3;

float TurnRate_Kd = 0.0;//02/03/23 updated Kd from 0.1 to 0.0 per https://www.fpaynter.com/2022/12/walle3-spin-turn-revisited/

//ported from FourWD_PulseTurnRateTest.ino

const float HDG_NEAR_MATCH_VAL = 0.8; //slow the turn down here

const float HDG_FULL_MATCH_VAL = 0.99; //stop the turn here //rev 06/01/21

const float HDG_MIN_MATCH_VAL = 0.6; //added 09/08/18: don't start checking slope until turn is well started

const float DEFAULT_TURN_RATE_DEGPERSEC = 45.f; //06/06/21 updated

const uint16_t HEADING_HISTORY_ARRAY_SIZE = 50; //06/12/23 added for 'spinning' condx detection

float gl_HdgHistoryArray[HEADING_HISTORY_ARRAY_SIZE];

//02/16/22 fwd decl reqd for fcns using default param

//09/16/23 added RunBothMotorsMsec with default params

//bool SpinTurn(bool b_ccw, float numDeg, float degPersec = DEFAULT_TURN_RATE_DEGPERSEC);

////06/01/24 chg int gl_Leftspeednum to uint16_t leftspeednum, int gl_Rightspeednum to uint16_t rightspeednum

////void RunBothMotorsMsec(bool bisFwd, int timeMsec = 500, int gl_Leftspeednum = MOTOR_SPEED_HALF, int gl_Rightspeednum = MOTOR_SPEED_HALF);

//void RunBothMotorsMsec(bool bisFwd, int timeMsec = 500, uint16_t leftspeednum = MOTOR_SPEED_HALF, uint16_t rightspeednum = MOTOR_SPEED_HALF);

//bool RollingTurn(bool b_ccw, bool b_fwd, float numDeg, float Kp, float Ki, float Kd, float degPersec = DEFAULT_TURN_RATE_DEGPERSEC);

//bool IsChargerConnected(bool curState = false);

#pragma endregion HEADING_AND_RATE_BASED_TURN_PARAMETERS

MPU6050 mpu(MPU6050_I2C_ADDR);

void setup()

{

Serial.begin(115200);

while (!Serial && millis() < 3000);

delay(500);

// Reset the sensor

pinMode(XSHUT_PIN, OUTPUT);

digitalWrite(XSHUT_PIN, LOW);

delay(10);

digitalWrite(XSHUT_PIN, HIGH);

delay(20);

Wire.begin();

Wire.setClock(400000);

// Use Wire1 instead of Wire for rear LIDAR

Wire1.begin();

Wire1.setClock(400000);

lidar.setBus(&Wire1); // <-- this is the key line

lidar.setTimeout(500);

if (!lidar.init())

{

Serial.println("Failed to detect and initialize VL53L1X on Wire1!");

while (1);

}

Serial.println("VL53L1X detected on Wire1 and initialized");

lidar.setDistanceMode(VL53L1X::Long);

lidar.setMeasurementTimingBudget(50000);

lidar.startContinuous(50);

//init current sensor pins

pinMode(I_BOT_PIN, INPUT);

pinMode(I_TOP_PIN, INPUT);

pinMode(I_CHG_PIN, INPUT);

pinMode(LASER_PIN, OUTPUT);

//exercise the laser diode

digitalWrite(LASER_PIN, HIGH);

delay(200);

digitalWrite(LASER_PIN, LOW);

delay(200);

digitalWrite(LASER_PIN, HIGH);

delay(200);

digitalWrite(LASER_PIN, LOW);

//Exercise the speaker

tone(SPEAKER_PIN, 1000, 2000); //returns immediately

#pragma region MPU6050

#ifndef NO_MPU6050

Serial.printf("\nChecking for MPU6050 IMU at I2C Addr 0x%x\n", MPU6050_I2C_ADDR);

Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

mpu.initialize();

// verify connection

float StartSec = 0; //used to time MPU6050 init

Serial.println(F("Initializing DMP..."));

devStatus = mpu.dmpInitialize();

// make sure it worked (returns 0 if successful)

if (devStatus == 0)

{

// turn on the DMP, now that it's ready

Serial.printf(F("Enabling DMP...\n"));

mpu.setDMPEnabled(true);

// set our DMP Ready flag so the main loop() function knows it's okay to use it

Serial.println(F("DMP ready! Waiting for MPU6050 drift rate to settle..."));

dmpReady = true;

// get expected DMP packet size for later comparison

packetSize = mpu.dmpGetFIFOPacketSize();

Serial.printf(F("Calibrating...Retrieving Calibration Values\n\n"));

mpu.CalibrateGyro(); //using default value of 15

mpu.PrintActiveOffsets();

//loop heading display until stabilized

Serial.printf(F("\nMsec\tHdg\n"));

UpdateIMUHdgValDeg();

Prev_HdgDeg = IMUHdgValDeg;

delay(100);

UpdateIMUHdgValDeg();

Serial.printf("%lu\t%2.3f\t%2.3f\n", millis(), IMUHdgValDeg, Prev_HdgDeg);

while (abs(IMUHdgValDeg - Prev_HdgDeg) > 0.1f)

{

Serial.printf("%lu\t%2.3f\n", millis(), IMUHdgValDeg);

Prev_HdgDeg = IMUHdgValDeg;

delay(100);

UpdateIMUHdgValDeg();

}

StartSec = millis() / 1000.f;

Serial.printf("MPU6050 Ready at %2.2f Sec with delta = %2.3f\n", StartSec, IMUHdgValDeg - Prev_HdgDeg);

bMPU6050Ready = true;

delay(1000);

}

else //MPU6050 Init failed for some reason

{

// ERROR!

// 1 = initial memory load failed

// 2 = DMP configuration updates failed

// (if it's going to break, usually the code will be 1)

Serial.printf("DMP Initialization failed with code %d", devStatus);

//08/29/21 print out battery voltage on failure

//float batV = GetBattVoltage();

//gl_batteryVoltage = GetBattVoltage();

//Serial.printf("Battery Voltage = %2.2f\n", gl_batteryVoltage);

bMPU6050Ready = false;

}

#endif // !NO_MPU6050

#pragma endregion MPU6050

//Serial.printf("Msec\tBattV\tTop_I\tBot_I\tChg_I\tRear_mm\n");

Serial.printf(ColumnHeaderStr);

}

void loop()

{

//rear-facing VL53L1X

uint16_t rear_mm = lidar.read();

//Serial.print(rear_mm);

if (lidar.timeoutOccurred())

{

Serial.print(" TIMEOUT");

}

//04/02/21 moved to 'fast' part of loop

float BattV = GetVoltage(SW_BATT_VOLT_PIN);

float TopI = GetAmps(I_TOP_PIN);

float BotI = GetAmps(I_BOT_PIN);

float ChgI = GetAmps(I_CHG_PIN);

UpdateIMUHdgValDeg(); //updatees IMUHdgValDeg

//Serial.printf("\Msec\tBattV\tTop_I\tBot_I\tChg_I\tRear_mm\n");

Serial.printf("%lu\t%2.2f\t%2.2f\t%2.2f\t%2.2f\t%d\t%2.2f\n", millis(), BattV, TopI, BotI, ChgI, rear_mm, IMUHdgValDeg );

delay(100);

}

float GetAmps(int pin_number)

{

//Purpose: Get current in amps

//Inputs:

// pin_number = integer denoting analog pin to be used for measurement

// VOLTAGE_TO_CURRENT_RATIO = measured voltage to current ratio

// MAX_AD_COUNT = int denoting max A/D reading value

// VOLTAGE_TO_CURRENT_RATIO = int denoting conversion ratio

//Outputs:

// returns total robot current (chg current plus running current)

//Notes:

// 02/28/18 chg name from GetBattChgAmps() to GetTotalAmps()

// 11/24/21 chg name, add pin_number param so can use for both Itot & Irun

int reading = analogRead(pin_number); //range is 0-1023

float volts = ((float)reading / (float)MAX_AD_COUNT) * ADC_REF_VOLTS;

float amps = volts * VOLTAGE_TO_CURRENT_RATIO;

//DEBUG!!

//Serial.printf("GetAmps(%d): reading, volts, amps = %d, %3.2f, %3.2f\n",

// pin_number, reading, volts, amps);

//DEBUG!!

return amps;

}

float GetVoltage(uint8_t volt_pin)

{

//02/18/17 get corrected battery voltage. Voltage reading is 1/3 actual Vbatt value

int analog_batt_reading = analogRead(volt_pin);//analogReadAveraging(8) in setup() does internal averaging

float calc_volts = ZENER_VOLTAGE_OFFSET + ADC_REF_VOLTS * (float)analog_batt_reading / (float)MAX_AD_COUNT;

//DEBUG!!

//for(int i = 0; i < 8;i++)

//{

// int analog_batt_reading = analogRead(BATT_MON_PIN);//analogReadAveraging(8) in setup() does internal averaging

// float calc_volts = ZENER_VOLTAGE_OFFSET + ADC_REF_VOLTS * (float)analog_batt_reading / (float)MAX_AD_COUNT;

// gl_pSerPort->printf("a/d = %d, calc = %2.2f\n", analog_batt_reading,calc_volts);

// delay(100);

//}

//DEBUG!!

return calc_volts;

}

float UpdateIMUHdgValDeg()

{

//Purpose: Get latest yaw (heading) value from IMU

//Inputs: None. This function should only be called after mpu.dmpPacketAvailable() returns TRUE

//Outputs:

// returns true if successful, otherwise false

// IMUHdgValDeg updated on success

//Plan:

//Step1: check for overflow and reset the FIFO if it occurs. In this case, wait for new packet

//Step2: read all available packets to get to latest data

//Step3: update IMUHdgValDeg with latest value

//Notes:

// 10/08/19 changed return type to boolean

// 10/08/19 no longer need mpuIntStatus

// 10/21/19 completely rewritten to use Homer's algorithm

// 05/05/20 changed return type to float vs bool.

// 06/13/23 added code to update gl_HdgHistoryArray for 'spinning' condx detection

int flag = GetCurrentFIFOPacket(fifoBuffer, packetSize, MAX_GETPACKET_LOOPS); //get the latest mpu packet

if (flag != 0) //0 = error exit, 1 = normal exit, 2 = recovered from an overflow

{

// display Euler angles in degrees

mpu.dmpGetQuaternion(&q, fifoBuffer);

mpu.dmpGetGravity(&gravity, &q);

mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

//compute the yaw value

IMUHdgValDeg = ypr[0] * 180 / M_PI;

}

//06/13/23 added to update gl_HdgHistoryArray for 'spinning' condx detection

//all array entries bumped down one, with most recent value at i = HEADING_HISTORY_ARRAY_SIZE-1

for (uint16_t i = 1; i < HEADING_HISTORY_ARRAY_SIZE; i++)

{

gl_HdgHistoryArray[i - 1] = gl_HdgHistoryArray[i];

//gl_pSerPort->printf("gl_HdgHistoryArray[%d] = %2.2f\n", i, gl_HdgHistoryArray[i]);

}

gl_HdgHistoryArray[HEADING_HISTORY_ARRAY_SIZE - 1] = IMUHdgValDeg;

return IMUHdgValDeg;//05/05/20 now returns updated value for use convenience

}

uint8_t GetCurrentFIFOPacket(uint8_t* data, uint8_t length, uint16_t max_loops)

{

mpu.resetFIFO();

delay(1);

//int countloop = 0;

fifoCount = mpu.getFIFOCount();

GetPacketLoopCount = 0;

//gl_pSerPort->printf("In GetCurrentFIFOPacket: before loop fifoC = %d\t", fifoCount);

while (fifoCount < packetSize && GetPacketLoopCount < max_loops)

{

GetPacketLoopCount++;

fifoCount = mpu.getFIFOCount();

delay(2);

}

//gl_pSerPort->printf("In GetCurrentFIFOPacket: after loop fifoC = %d, loop count = %d\n", fifoCount, GetPacketLoopCount);

if (GetPacketLoopCount >= max_loops)

{

return 0;

}

//if we get to here, there should be exactly one packet in the FIFO

mpu.getFIFOBytes(data, packetSize);

return 1;

}

And here’s the output showing all three current sensors, the battery voltage, and the rear-facing VL53L1X LIDAR output (variations in the rear distanced are due to my waving my hand in front of the sensor).

Opening port
Port open
VL53L1X detected on Wire1 and initialized
Msec	BattV	Top_I	Bot_I	Chg_I	Rear_mm
1056	7.77	0.33	0.00	0.23	3607
1158	7.81	0.30	0.00	0.23	3622
1259	7.81	0.31	0.00	0.23	3658
1360	7.81	0.30	0.00	0.23	3639
1461	7.84	0.46	0.00	0.23	3659
1562	7.99	0.29	0.00	0.23	3606
1663	7.89	0.30	0.00	0.23	481
1764	7.86	0.34	0.00	0.23	48
1865	7.92	0.30	0.00	0.23	60
1966	7.92	0.30	0.00	0.22	62
2067	7.94	0.29	0.00	0.23	75
2169	7.92	0.29	0.00	0.22	2400
2270	7.92	0.30	0.00	0.22	3675
2371	7.90	0.31	0.00	0.22	3761
2472	7.92	0.30	0.00	0.22	3739
2573	7.90	0.30	0.00	0.22	28
2674	7.91	0.31	0.00	0.22	0
2775	7.91	0.31	0.00	0.22	61
2876	7.89	0.30	0.00	0.22	59
2977	7.90	0.30	0.00	0.22	2406
3078	7.89	0.34	0.00	0.22	3657
3180	7.93	0.30	0.00	0.20	3688
3281	7.89	0.30	0.00	0.23	3619
3382	7.89	0.30	0.00	0.22	13
3483	7.88	0.31	0.00	0.22	0
3584	7.94	0.29	0.00	0.23	85
3685	7.90	0.30	0.00	0.22	3533
3786	7.91	0.31	0.00	0.22	3713

Opening port

Port open

VL53L1X detected on Wire1 and initialized

Msec BattV Top_I Bot_I Chg_I Rear_mm

1056 7.77 0.33 0.00 0.23 3607

1158 7.81 0.30 0.00 0.23 3622

1259 7.81 0.31 0.00 0.23 3658

1360 7.81 0.30 0.00 0.23 3639

1461 7.84 0.46 0.00 0.23 3659

1562 7.99 0.29 0.00 0.23 3606

1663 7.89 0.30 0.00 0.23 481

1764 7.86 0.34 0.00 0.23 48

1865 7.92 0.30 0.00 0.23 60

1966 7.92 0.30 0.00 0.22 62

2067 7.94 0.29 0.00 0.23 75

2169 7.92 0.29 0.00 0.22 2400

2270 7.92 0.30 0.00 0.22 3675

2371 7.90 0.31 0.00 0.22 3761

2472 7.92 0.30 0.00 0.22 3739

2573 7.90 0.30 0.00 0.22 28

2674 7.91 0.31 0.00 0.22 0

2775 7.91 0.31 0.00 0.22 61

2876 7.89 0.30 0.00 0.22 59

2977 7.90 0.30 0.00 0.22 2406

3078 7.89 0.34 0.00 0.22 3657

3180 7.93 0.30 0.00 0.20 3688

3281 7.89 0.30 0.00 0.23 3619

3382 7.89 0.30 0.00 0.22 13

3483 7.88 0.31 0.00 0.22 0

3584 7.94 0.29 0.00 0.23 85

3685 7.90 0.30 0.00 0.22 3533

3786 7.91 0.31 0.00 0.22 3713

I wasn’t quite sure what to make of the zero readings for Bot_I, but then I realized that when the USB cable is plugged into the Teensy 3.5, it provides all the current needed for the first deck peripherals, meaning the current through the Bot_I INA169 sensor is essentially zero.

Next I ported the MPU-6050 support code from the VisionEnhanced4WheelRobot project into my little demo program and put some code in ‘loop()’ to add the IMU heading output to the sensor line. This didn’t work at first, but with Grok’s help I traced the problem to a disconnected power/ground lead from the main power distribution bus to the MPU6050. Once this was connected, the demo program started working right away:

Opening port
Port open
VL53L1X detected on Wire1 and initialized
Checking for MPU6050 IMU at I2C Addr 0x68
MPU6050 connection successful
Initializing DMP...
Enabling DMP...
DMP ready! Waiting for MPU6050 drift rate to settle...
Calibrating...Retrieving Calibration Values
...............1800.00000, 1415.00000, 772.00000, -65.00000, 18.00000, 22.00000
Msec Hdg
2084 0.106 0.105
MPU6050 Ready at 2.08 Sec with delta = 0.002
Msec BattV Top_I Bot_I Chg_I Rear_mm HdgDeg
3094 8.06 0.26 0.00 0.01 3082 0.12
3206 8.06 0.27 0.00 0.00 2987 0.12
3320 8.06 0.26 0.00 0.02 3154 0.13
3432 8.06 0.27 0.00 0.00 3021 0.13
3544 8.07 0.26 0.00 0.00 3117 0.13
3656 8.06 0.27 0.00 0.02 3010 0.13
3767 8.05 0.29 0.00 0.02 3208 0.13
...
Rotated robot by hand...
7468 8.05 0.28 0.00 0.02 3210 0.87
7580 8.05 0.26 0.00 0.02 3211 1.50
7691 8.06 0.27 0.00 0.00 2740 2.80
7803 8.06 0.29 0.00 0.00 3078 5.25
7917 8.06 0.29 0.00 0.02 3039 7.86
8029 8.07 0.26 0.00 0.00 3166 11.13
8141 8.06 0.28 0.00 0.00 3177 13.85
8253 8.07 0.26 0.00 0.02 3130 15.74
8364 8.05 0.26 0.00 0.02 2720 15.77
8476 8.05 0.28 0.00 0.02 2926 15.56
8588 8.07 0.27 0.00 0.00 3193 14.39
8702 8.05 0.30 0.00 0.00 2921 11.06
8814 8.06 0.26 0.00 0.03 2501 7.14
8926 8.05 0.29 0.00 0.00 2724 4.01
9037 8.04 0.27 0.00 0.00 2883 -0.51
9149 8.04 0.28 0.00 0.02 3158 -5.95
9261 8.05 0.27 0.00 0.02 3199 -10.29
9373 8.04 0.29 0.00 0.02 2951 -13.32
9487 8.05 0.28 0.00 0.00 2966 -15.82
9599 8.05 0.27 0.00 0.02 2856 -17.55
9710 8.06 0.28 0.00 0.03 2790 -17.41

Opening port

Port open

VL53L1X detected on Wire1 and initialized

Checking for MPU6050 IMU at I2C Addr 0x68

MPU6050 connection successful

Initializing DMP...

Enabling DMP...

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

Calibrating...Retrieving Calibration Values

...............1800.00000, 1415.00000, 772.00000, -65.00000, 18.00000, 22.00000

Msec Hdg

2084 0.106 0.105

MPU6050 Ready at 2.08 Sec with delta = 0.002

Msec BattV Top_I Bot_I Chg_I Rear_mm HdgDeg

3094 8.06 0.26 0.00 0.01 3082 0.12

3206 8.06 0.27 0.00 0.00 2987 0.12

3320 8.06 0.26 0.00 0.02 3154 0.13

3432 8.06 0.27 0.00 0.00 3021 0.13

3544 8.07 0.26 0.00 0.00 3117 0.13

3656 8.06 0.27 0.00 0.02 3010 0.13

3767 8.05 0.29 0.00 0.02 3208 0.13

...

Rotated robot by hand...

7468 8.05 0.28 0.00 0.02 3210 0.87

7580 8.05 0.26 0.00 0.02 3211 1.50

7691 8.06 0.27 0.00 0.00 2740 2.80

7803 8.06 0.29 0.00 0.00 3078 5.25

7917 8.06 0.29 0.00 0.02 3039 7.86

8029 8.07 0.26 0.00 0.00 3166 11.13

8141 8.06 0.28 0.00 0.00 3177 13.85

8253 8.07 0.26 0.00 0.02 3130 15.74

8364 8.05 0.26 0.00 0.02 2720 15.77

8476 8.05 0.28 0.00 0.02 2926 15.56

8588 8.07 0.27 0.00 0.00 3193 14.39

8702 8.05 0.30 0.00 0.00 2921 11.06

8814 8.06 0.26 0.00 0.03 2501 7.14

8926 8.05 0.29 0.00 0.00 2724 4.01

9037 8.04 0.27 0.00 0.00 2883 -0.51

9149 8.04 0.28 0.00 0.02 3158 -5.95

9261 8.05 0.27 0.00 0.02 3199 -10.29

9373 8.04 0.29 0.00 0.02 2951 -13.32

9487 8.05 0.28 0.00 0.00 2966 -15.82

9599 8.05 0.27 0.00 0.02 2856 -17.55

9710 8.06 0.28 0.00 0.03 2790 -17.41

I also installed and tested a front-facing red laser diode and exercised the on-board speaker. At this point I believe I have exercised/tested all the sensors, the laser diode and the speaker. I haven’t run the motors with this program as I believe that will happen when I get back to the ‘mainline’ VisionEnhanced4WheelRobot.ino program.

16 August 2026 Update:

I back-ported the code from my VL53L1X_Demo program back to my VisionEnhanced4WheelRobot program and after the normal number of mistakes, got it going. Here’s a small bit of output from loop(), with me rotating the robot back and forth to show variation in the ‘Hdg’ and ‘RearCm’ values:

Opening port
Port open
In setup() just before EnableAllRearLEDs
Serial port active
gl_pSerPort now points to active Serial (USB or Wixel)
VL53L1X detected on Wire1 and initialized

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

>...............1800.00000,	1415.00000,	772.00000,	-71.00000,	17.00000,	25.00000


Msec	Hdg
8325	0.091	0.026
MPU6050 Ready at 8.33 Sec with delta = 0.065
Time	BattV	TopI	BotI	ChgI	RearCm	Hdg
9338	8.15	0.24	0.00	0.23	32.10	0.67
9546	8.16	0.24	0.00	0.23	33.80	0.61
9754	8.15	0.26	0.00	0.24	34.20	0.54
9962	8.14	0.23	0.00	0.23	34.40	0.50
10170	8.16	0.23	0.00	0.23	34.50	0.49
10377	8.14	0.25	0.00	0.23	34.30	0.48
10587	8.15	0.24	0.00	0.23	34.20	0.85
10795	8.15	0.23	0.00	0.23	34.20	6.13
11003	8.15	0.23	0.00	0.23	33.60	20.67
11211	8.14	0.24	0.00	0.23	36.20	34.39
11419	8.14	0.24	0.00	0.24	40.10	46.03
11627	8.14	0.26	0.00	0.23	46.60	59.08
11837	8.15	0.25	0.00	0.24	55.20	72.07
12045	8.14	0.24	0.00	0.23	60.50	82.54
12253	8.15	0.24	0.00	0.23	69.50	85.32
12460	8.15	0.25	0.00	0.24	63.50	80.64
12668	8.13	0.25	0.00	0.23	59.00	73.99
12876	8.14	0.23	0.00	0.23	55.60	65.12
13084	8.15	0.24	0.00	0.23	47.50	53.90
13294	8.13	0.25	0.00	0.23	43.10	43.33
13502	8.15	0.23	0.00	0.23	38.20	28.72
13710	8.14	0.26	0.00	0.23	36.50	16.64
13918	8.15	0.25	0.00	0.24	35.70	6.20
14126	8.14	0.24	0.00	0.23	36.00	-4.59
14334	8.15	0.24	0.00	0.23	38.00	-16.53
14544	8.15	0.24	0.00	0.23	25.10	-25.39
14751	8.13	0.26	0.00	0.23	25.20	-32.30
14959	8.13	0.27	0.00	0.23	27.80	-39.25
15167	8.15	0.24	0.00	0.23	43.50	-44.88
15375	8.14	0.24	0.00	0.22	41.80	-48.53
15583	8.13	0.24	0.00	0.23	41.90	-46.46
15793	8.15	0.25	0.00	0.23	42.60	-40.06
16001	8.16	0.24	0.00	0.23	22.50	-31.43
16209	8.15	0.24	0.00	0.23	28.40	-20.93
16417	8.13	0.24	0.00	0.23	35.30	-10.16
16624	8.13	0.28	0.00	0.24	34.40	0.62
16832	8.13	0.24	0.00	0.23	34.20	8.56
17042	8.14	0.27	0.00	0.24	34.20	12.15
17250	8.15	0.23	0.00	0.23	34.20	11.33
17458	8.14	0.25	0.00	0.23	34.10	11.22
17666	8.12	0.27	0.00	0.22	34.10	11.25
17874	8.13	0.23	0.00	0.24	3.60	11.22
18082	8.15	0.24	0.00	0.24	3.80	11.24
18292	8.11	0.31	0.00	0.23	2.60	11.24
18500	8.14	0.25	0.00	0.23	54.30	11.23
18707	8.13	0.25	0.00	0.23	34.20	11.21
18915	8.14	0.25	0.00	0.23	34.30	11.20
19123	8.15	0.24	0.00	0.23	34.30	11.19
19331	8.15	0.24	0.00	0.23	34.10	11.17
19541	8.17	0.24	0.00	0.23	34.20	11.16
19749	8.14	0.24	0.00	0.23	33.90	11.13
19957	8.14	0.24	0.00	0.22	34.30	11.11
20165	8.14	0.25	0.00	0.23	34.40	11.10
20373	8.15	0.25	0.00	0.24	34.10	11.08
20581	8.14	0.25	0.00	0.23	34.30	11.07
20789	8.14	0.25	0.00	0.23	34.20	11.04
20999	8.15	0.25	0.00	0.23	34.30	11.03

Port closed

Opening port

Port open

In setup() just before EnableAllRearLEDs

Serial port active

gl_pSerPort now points to active Serial (USB or Wixel)

VL53L1X detected on Wire1 and initialized

Checking for MPU6050 IMU at I2C Addr 0x68

MPU6050 connection successful

Initializing DMP...

Enabling DMP...

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

Calibrating...Retrieving Calibration Values

>...............1800.00000, 1415.00000, 772.00000, -71.00000, 17.00000, 25.00000

Msec Hdg

8325 0.091 0.026

MPU6050 Ready at 8.33 Sec with delta = 0.065

Time BattV TopI BotI ChgI RearCm Hdg

9338 8.15 0.24 0.00 0.23 32.10 0.67

9546 8.16 0.24 0.00 0.23 33.80 0.61

9754 8.15 0.26 0.00 0.24 34.20 0.54

9962 8.14 0.23 0.00 0.23 34.40 0.50

10170 8.16 0.23 0.00 0.23 34.50 0.49

10377 8.14 0.25 0.00 0.23 34.30 0.48

10587 8.15 0.24 0.00 0.23 34.20 0.85

10795 8.15 0.23 0.00 0.23 34.20 6.13

11003 8.15 0.23 0.00 0.23 33.60 20.67

11211 8.14 0.24 0.00 0.23 36.20 34.39

11419 8.14 0.24 0.00 0.24 40.10 46.03

11627 8.14 0.26 0.00 0.23 46.60 59.08

11837 8.15 0.25 0.00 0.24 55.20 72.07

12045 8.14 0.24 0.00 0.23 60.50 82.54

12253 8.15 0.24 0.00 0.23 69.50 85.32

12460 8.15 0.25 0.00 0.24 63.50 80.64

12668 8.13 0.25 0.00 0.23 59.00 73.99

12876 8.14 0.23 0.00 0.23 55.60 65.12

13084 8.15 0.24 0.00 0.23 47.50 53.90

13294 8.13 0.25 0.00 0.23 43.10 43.33

13502 8.15 0.23 0.00 0.23 38.20 28.72

13710 8.14 0.26 0.00 0.23 36.50 16.64

13918 8.15 0.25 0.00 0.24 35.70 6.20

14126 8.14 0.24 0.00 0.23 36.00 -4.59

14334 8.15 0.24 0.00 0.23 38.00 -16.53

14544 8.15 0.24 0.00 0.23 25.10 -25.39

14751 8.13 0.26 0.00 0.23 25.20 -32.30

14959 8.13 0.27 0.00 0.23 27.80 -39.25

15167 8.15 0.24 0.00 0.23 43.50 -44.88

15375 8.14 0.24 0.00 0.22 41.80 -48.53

15583 8.13 0.24 0.00 0.23 41.90 -46.46

15793 8.15 0.25 0.00 0.23 42.60 -40.06

16001 8.16 0.24 0.00 0.23 22.50 -31.43

16209 8.15 0.24 0.00 0.23 28.40 -20.93

16417 8.13 0.24 0.00 0.23 35.30 -10.16

16624 8.13 0.28 0.00 0.24 34.40 0.62

16832 8.13 0.24 0.00 0.23 34.20 8.56

17042 8.14 0.27 0.00 0.24 34.20 12.15

17250 8.15 0.23 0.00 0.23 34.20 11.33

17458 8.14 0.25 0.00 0.23 34.10 11.22

17666 8.12 0.27 0.00 0.22 34.10 11.25

17874 8.13 0.23 0.00 0.24 3.60 11.22

18082 8.15 0.24 0.00 0.24 3.80 11.24

18292 8.11 0.31 0.00 0.23 2.60 11.24

18500 8.14 0.25 0.00 0.23 54.30 11.23

18707 8.13 0.25 0.00 0.23 34.20 11.21

18915 8.14 0.25 0.00 0.23 34.30 11.20

19123 8.15 0.24 0.00 0.23 34.30 11.19

19331 8.15 0.24 0.00 0.23 34.10 11.17

19541 8.17 0.24 0.00 0.23 34.20 11.16

19749 8.14 0.24 0.00 0.23 33.90 11.13

19957 8.14 0.24 0.00 0.22 34.30 11.11

20165 8.14 0.25 0.00 0.23 34.40 11.10

20373 8.15 0.25 0.00 0.24 34.10 11.08

20581 8.14 0.25 0.00 0.23 34.30 11.07

20789 8.14 0.25 0.00 0.23 34.20 11.04

20999 8.15 0.25 0.00 0.23 34.30 11.03

Port closed

Then I fired up the VS Code connection to the pi5, started WallE_5.py supervisor program and verified that I could send motor commands in the WallE_5.py window and have the robot respond appropriately – yay. I also verified that ‘tail -f ./logs/telemetry.log’ in a separate VS Code terminal window produced the same telemetry output as above.

17 August 2026 Update:

I finally have the ‘VisionEnhanced4WheelRobot’ program running again with a Teensy 3.5. The last little bit was undoing the change in motor control pin assignments for the right-hand motor driver. Going from Teenys 3.5 to 4.1 I had to change the pin assignments to place the ‘speed’ pin on a PWM-able output pin, and going back I didn’t initially undo this pin assignment – all fixed now.

At this point the robot firmware is pretty much complete – it correctly monitors all three current sensors, the battery voltage, the current heading, and the rear distance reading. I think the next step is to work on the python side to add more navigation and obstacle-avoidance ‘smarts’.

Stay tuned!

Frank

Generating Pull Request for Vin’s MissChanger Klipper-Toolchanger Repo

Leave a reply

Posted 29 July 2026

In a prior post, Grok help me figure out why I was having so much trouble trying to change my Voron 2.4 printer from ‘dock’ to ‘no_dock’ configuration. Solving this problem exposed a pretty significant flaw in the way that config_switch.py managed the process of changing the printer.cfg file to reflect the new configuration. After getting everything to work, I suggested we generate a pull request to Vin’s Klipper-toolchanger repo to make the config swap process a bit more dependable. This post is intended to document this effort.

We started out by making sure we had all the preparatory work done.

I copied yet another set of config files to my PC
1. Update my clone of Vin-y’s klipper-toolchanger repo on my printer, so all I had to do was update it with: cd ~/klipper-toolchanger, git pull. Now my local repo is the same as Vin-y’s main branch

cd ~/klipper-toolchanger git status # just to see where you are git pull origin main
Created a new branch so we can make our changes without touching the main branch.Just type this one command and show me the result:bash

git checkout -b fix-config-switch-in-place
In VS Code on my PC, I navigated to /home/pi/klipper-toolchanger/klipper/extras/config_switch.py and replaced its contents with the file generated by Grok.

# Copyright (C) 2024 Chinh Nhan Vo <nhanvo29@proton.me>
# Improved 2026 by Frank Paynter / Grok – fix section replacement, marker handling,
# and prevent progressive corruption of printer.cfg
#
# This file may be distributed under the terms of the GNU GPLv3 license.
#
import os
import logging
import shutil
from datetime import datetime

class ConfigSwitch:
    def __init__(self, config):
        self.printer = config.get_printer()
        self.gcode = self.printer.lookup_object('gcode')
        ## Register Commands
        self.gcode.register_command('SAVE_CONFIG_MODE',
                                    self.cmd_SAVE_CONFIG_MODE,
                                    desc=self.cmd_SAVE_CONFIG_MODE_help)
        self.gcode.register_command('TOGGLE_CONFIG_MODE',
                                    self.cmd_TOGGLE_CONFIG_MODE,
                                    desc=self.cmd_TOGGLE_CONFIG_MODE_help)

    # ------------------------------------------------------------------
    # Helpers
    # ------------------------------------------------------------------
    def _get_paths(self):
        home_dir = os.path.expanduser("~")
        printer_config = os.path.join(home_dir, "printer_data/config/printer.cfg")
        config_dir = os.path.join(home_dir, "printer_data/config/config")
        return {
            "printer": printer_config,
            "dir": config_dir,
            "wt": os.path.join(config_dir, "config_wt_dock.cfg"),
            "no": os.path.join(config_dir, "config_no_dock.cfg"),
            "temp": os.path.join(config_dir, "printer.temp"),
        }

    def _ensure_config_dir(self, config_dir):
        if not os.path.exists(config_dir):
            os.makedirs(config_dir, exist_ok=True)

    def _read_lines(self, path):
        with open(path, "r") as f:
            return f.readlines()

    def _write_lines(self, path, lines):
        with open(path, "w") as f:
            f.writelines(lines)

    def _find_markers(self, lines):
        """Return (start_idx, end_idx) inclusive, or raise."""
        start = end = None
        for i, line in enumerate(lines):
            stripped = line.strip()
            if "#;<" in stripped:
                if start is not None:
                    raise self.gcode.error("Multiple '#;<' markers found in printer.cfg")
                start = i
            if "#;>" in stripped:
                if end is not None:
                    raise self.gcode.error("Multiple '#;>' markers found in printer.cfg")
                end = i
        if start is None or end is None:
            raise self.gcode.error(
                "Missing '#;<' or '#;>' marker in printer.cfg – "
                "the swappable section is not defined"
            )
        if end <= start:
            raise self.gcode.error("'#;>' appears before '#;<' – markers are inverted")
        return start, end

    def _get_current_dock_state(self, lines):
        for line in lines:
            if "variable_dock:" in line.strip():
                if "True" in line:
                    return True
                if "False" in line:
                    return False
        raise self.gcode.error(
            "Could not find a valid 'variable_dock: True/False' line inside the marked section"
        )

    # ------------------------------------------------------------------
    # SAVE_CONFIG_MODE
    # ------------------------------------------------------------------
    cmd_SAVE_CONFIG_MODE_help = "Save the marked session-variable section to the appropriate config_*.cfg"
    def cmd_SAVE_CONFIG_MODE(self, gcmd):
        paths = self._get_paths()
        self._ensure_config_dir(paths["dir"])

        lines = self._read_lines(paths["printer"])
        start, end = self._find_markers(lines)
        dock = self._get_current_dock_state(lines[start:end+1])

        dest = paths["wt"] if dock else paths["no"]
        section = lines[start:end+1]          # inclusive – keeps both markers

        self._write_lines(dest, section)

        name = "config_wt_dock.cfg" if dock else "config_no_dock.cfg"
        self.gcode.respond_info(f"Session variables (incl. markers) saved to config/{name}")

    # ------------------------------------------------------------------
    # TOGGLE_CONFIG_MODE
    # ------------------------------------------------------------------
    cmd_TOGGLE_CONFIG_MODE_help = "Replace the marked section with the opposite config_*.cfg (in-place)"
    def cmd_TOGGLE_CONFIG_MODE(self, gcmd):
        paths = self._get_paths()
        self._ensure_config_dir(paths["dir"])

        lines = self._read_lines(paths["printer"])
        start, end = self._find_markers(lines)
        dock = self._get_current_dock_state(lines[start:end+1])

        # Choose the opposite file
        source = paths["no"] if dock else paths["wt"]
        if not os.path.exists(source):
            raise self.gcode.error(f"Missing source file: {source}")

        new_section = self._read_lines(source)
        # Sanity-check the incoming section has the markers
        if not any("#;<" in l for l in new_section) or not any("#;>" in l for l in new_section):
            raise self.gcode.error(
                f"{os.path.basename(source)} is missing '#;<' or '#;>' – "
                "run SAVE_CONFIG_MODE first on a clean printer.cfg"
            )

        # Optional safety backup
        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
        backup = paths["printer"] + f".bak_{timestamp}"
        shutil.copy2(paths["printer"], backup)
        self.gcode.respond_info(f"Backup created: {os.path.basename(backup)}")

        # Build the new file: everything before start + new section + everything after end
        new_lines = lines[:start] + new_section + lines[end+1:]

        self._write_lines(paths["printer"], new_lines)
        self.gcode.respond_info(
            f"Toggled to {'no-dock' if dock else 'with-dock'} configuration "
            f"(section replaced in-place)"
        )


def load_config(config):
    return ConfigSwitch(config)

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

# Improved 2026 by Frank Paynter / Grok – fix section replacement, marker handling,

# and prevent progressive corruption of printer.cfg

# This file may be distributed under the terms of the GNU GPLv3 license.

import os

import logging

import shutil

from datetime import datetime

class ConfigSwitch:

def __init__(self, config):

self.printer = config.get_printer()

self.gcode = self.printer.lookup_object('gcode')

## Register Commands

self.gcode.register_command('SAVE_CONFIG_MODE',

self.cmd_SAVE_CONFIG_MODE,

desc=self.cmd_SAVE_CONFIG_MODE_help)

self.gcode.register_command('TOGGLE_CONFIG_MODE',

self.cmd_TOGGLE_CONFIG_MODE,

desc=self.cmd_TOGGLE_CONFIG_MODE_help)

# ------------------------------------------------------------------

# Helpers

# ------------------------------------------------------------------

def _get_paths(self):

home_dir = os.path.expanduser("~")

printer_config = os.path.join(home_dir, "printer_data/config/printer.cfg")

config_dir = os.path.join(home_dir, "printer_data/config/config")

return {

"printer": printer_config,

"dir": config_dir,

"wt": os.path.join(config_dir, "config_wt_dock.cfg"),

"no": os.path.join(config_dir, "config_no_dock.cfg"),

"temp": os.path.join(config_dir, "printer.temp"),

}

def _ensure_config_dir(self, config_dir):

if not os.path.exists(config_dir):

os.makedirs(config_dir, exist_ok=True)

def _read_lines(self, path):

with open(path, "r") as f:

return f.readlines()

def _write_lines(self, path, lines):

with open(path, "w") as f:

f.writelines(lines)

def _find_markers(self, lines):

"""Return (start_idx, end_idx) inclusive, or raise."""

start = end = None

for i, line in enumerate(lines):

stripped = line.strip()

if "#;<" in stripped:

if start is not None:

raise self.gcode.error("Multiple '#;<' markers found in printer.cfg")

start = i

if "#;>" in stripped:

if end is not None:

raise self.gcode.error("Multiple '#;>' markers found in printer.cfg")

end = i

if start is None or end is None:

raise self.gcode.error(

"Missing '#;<' or '#;>' marker in printer.cfg – "

"the swappable section is not defined"

)

if end <= start:

raise self.gcode.error("'#;>' appears before '#;<' – markers are inverted")

return start, end

def _get_current_dock_state(self, lines):

for line in lines:

if "variable_dock:" in line.strip():

if "True" in line:

return True

if "False" in line:

return False

raise self.gcode.error(

"Could not find a valid 'variable_dock: True/False' line inside the marked section"

)

# ------------------------------------------------------------------

# SAVE_CONFIG_MODE

# ------------------------------------------------------------------

cmd_SAVE_CONFIG_MODE_help = "Save the marked session-variable section to the appropriate config_*.cfg"

def cmd_SAVE_CONFIG_MODE(self, gcmd):

paths = self._get_paths()

self._ensure_config_dir(paths["dir"])

lines = self._read_lines(paths["printer"])

start, end = self._find_markers(lines)

dock = self._get_current_dock_state(lines[start:end+1])

dest = paths["wt"] if dock else paths["no"]

section = lines[start:end+1] # inclusive – keeps both markers

self._write_lines(dest, section)

name = "config_wt_dock.cfg" if dock else "config_no_dock.cfg"

self.gcode.respond_info(f"Session variables (incl. markers) saved to config/{name}")

# ------------------------------------------------------------------

# TOGGLE_CONFIG_MODE

# ------------------------------------------------------------------

cmd_TOGGLE_CONFIG_MODE_help = "Replace the marked section with the opposite config_*.cfg (in-place)"

def cmd_TOGGLE_CONFIG_MODE(self, gcmd):

paths = self._get_paths()

self._ensure_config_dir(paths["dir"])

lines = self._read_lines(paths["printer"])

start, end = self._find_markers(lines)

dock = self._get_current_dock_state(lines[start:end+1])

# Choose the opposite file

source = paths["no"] if dock else paths["wt"]

if not os.path.exists(source):

raise self.gcode.error(f"Missing source file: {source}")

new_section = self._read_lines(source)

# Sanity-check the incoming section has the markers

if not any("#;<" in l for l in new_section) or not any("#;>" in l for l in new_section):

raise self.gcode.error(

f"{os.path.basename(source)} is missing '#;<' or '#;>' – "

"run SAVE_CONFIG_MODE first on a clean printer.cfg"

)

# Optional safety backup

timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")

backup = paths["printer"] + f".bak_{timestamp}"

shutil.copy2(paths["printer"], backup)

self.gcode.respond_info(f"Backup created: {os.path.basename(backup)}")

# Build the new file: everything before start + new section + everything after end

new_lines = lines[:start] + new_section + lines[end+1:]

self._write_lines(paths["printer"], new_lines)

self.gcode.respond_info(

f"Toggled to {'no-dock' if dock else 'with-dock'} configuration "

f"(section replaced in-place)"

)

def load_config(config):

return ConfigSwitch(config)

Then we restarted the printer. Klipper came up cleanly, so that’s a good sign!

After testing the new script with some round-trip ‘TOGGLE_CONFIG_MODE‘ commands, the new script looks like it is performing correctly.

Grok then guided me through the process of updating the local repo:

pi@mainsailos:~/printer_data $ cd ~/klipper-toolchanger
pi@mainsailos:~/klipper-toolchanger $ git status
On branch main
Your branch is behind 'origin/main' by 31 commits, and can be fast-forwarded.
  (use "git pull" to update your local branch)

nothing to commit, working tree clean
pi@mainsailos:~/klipper-toolchanger $ git pull
Updating 4082d29..e7be868
Fast-forward
 klipper/extras/tools_calibrate.py | 44 +++++++++++++-------------------------------
 macros/homing.cfg                 |  5 +++--
 macros/print_time_default.cfg     | 69 +++++++++++++++++++++++++++++++++++++--------------------------------
 macros/tool_calibrate.cfg         | 25 +++++++++++++++++--------
 macros/toolchanger.cfg            |  2 +-
 scripts/misschanger_settings.cfg  | 17 ++++++++++-------
 6 files changed, 81 insertions(+), 81 deletions(-)
pi@mainsailos:~/klipper-toolchanger $ git status
On branch main
Your branch is up to date with 'origin/main'.

nothing to commit, working tree clean
pi@mainsailos:~/klipper-toolchanger $

pi@mainsailos:~/printer_data $ cd ~/klipper-toolchanger

pi@mainsailos:~/klipper-toolchanger $ git status

On branch main

Your branch is behind 'origin/main' by 31 commits, and can be fast-forwarded.

(use "git pull" to update your local branch)

nothing to commit, working tree clean

pi@mainsailos:~/klipper-toolchanger $ git pull

Updating 4082d29..e7be868

Fast-forward

klipper/extras/tools_calibrate.py | 44 +++++++++++++-------------------------------

macros/homing.cfg | 5 +++--

macros/print_time_default.cfg | 69 +++++++++++++++++++++++++++++++++++++--------------------------------

macros/tool_calibrate.cfg | 25 +++++++++++++++++--------

macros/toolchanger.cfg | 2 +-

scripts/misschanger_settings.cfg | 17 ++++++++++-------

6 files changed, 81 insertions(+), 81 deletions(-)

pi@mainsailos:~/klipper-toolchanger $ git status

On branch main

Your branch is up to date with 'origin/main'.

nothing to commit, working tree clean

pi@mainsailos:~/klipper-toolchanger $

Create a new branch to make the changes without changing the ‘main’ branch:

git checkout -b fix-config-switch-in-place

1	git checkout -b fix-config-switch-in-place

Then in VS Code we overwrote the existing klipper-toolchanger / klipper / extras / config_switch.py with the new version (see above), saved the file and then restarted Klipper with “sudo systemctl restart klipper”

Testing:

At the start of the test program, ‘printer.cfg’ contains the ‘no_dock’ configuration. Then I ran SAVE_CONFIG_MODE fro the mainsail console, and got the following output:

1:02 PM
Session variables (incl. markers) saved to config/config_no_dock.cfg
1:02 PM
SAVE_CONFIG_MODE

1:02 PM

Session variables (incl. markers) saved to config/config_no_dock.cfg

1:02 PM

SAVE_CONFIG_MODE

Most notably, now the ‘config_no_dock.cfg’ file has both the ‘#;<‘ and ‘#;>’ markers – yay!

#;<    # Section Variable marker
#---------------------------------------------------------------

#---------------------------------------------------------------
#07/23/26 Updated to include current T0/T1 cfg files
[include 251103_T0-Nitehawk-Revo-LDO.cfg]
#[include 251103_T1-Nitehawk-Revo-LDO.cfg]
#---------------------------------------------------------------

####################################################################################
##   10/19/25 [gcode_macro _limits] section added
##   See 19 October blog update for details
####################################################################################

[gcode_macro _limits]
variable_effective_min_y: 0.0 #this value gets changed to '120.0' in 'config_wt_dock'
gcode:  # Empty, just for variables


																												
[gcode_macro PRINT_END]
#   Use PRINT_END for the slicer ending script - please customise for your slicer of choice
gcode:
    # safe anti-stringing move coords
    {% set th = printer.toolhead %}
    {% set x_safe = th.position.x + 20 * (1 if th.axis_maximum.x - th.position.x > 20 else -1) %}
    {% set y_safe = th.position.y + 20 * (1 if th.axis_maximum.y - th.position.y > 20 else -1) %}
    {% set z_safe = [th.position.z + 2, th.axis_maximum.z]|min %}
    
    SAVE_GCODE_STATE NAME=STATE_PRINT_END
    
    M400                           ; wait for buffer to clear
    G92 E0                         ; zero the extruder
    G1 E-5.0 F1800                 ; retract filament
    
    TURN_OFF_HEATERS
    
    G90                                      ; absolute positioning
    G0 X{x_safe} Y{y_safe} Z{z_safe} F20000  ; move nozzle to remove stringing
    ;G0 X{th.axis_maximum.x//2} Y{th.axis_maximum.y - 2} F3600  ; park nozzle at rear
    G0 X{th.axis_maximum.x//2} Y{th.axis_maximum.y - 10} F3600  ; park nozzle at rear 06/17/25 gfp adj to avoid 'clunk' at end
    #G0 Z25 F3600  #10/17/25 gfp No dock 
    G0 Z100 F3600  #07/27/26 gfp moved 'No dock' extruder end positin higher for easier print-plate removal/replacement
    M107                                     ; turn off fan
    
    BED_MESH_CLEAR
    RESTORE_GCODE_STATE NAME=STATE_PRINT_END
    
#---------------------------------------------------------------
#gfp 09/19/25 edited for 300x300 bed
[gcode_macro _home]
variable_xh: 150.0
variable_yh: 150.0 #Dock NOT installed
# Dock is installed: True or False
variable_dock: False
gcode:
#---------------------------------------------------------------

#05/30/25 added gfp
[bed_mesh]
speed: 120
horizontal_move_z: 5
mesh_min: 30, 35 #07/05/25 gfp: 30 was not enough for Y
mesh_max: 275, 250

probe_count: 5, 5 #06/01/25 chg to 5,5
zero_reference_position: 150,150 #for use with stock z endstop. Added 06/01/25 gfp

					

#gfp 09/19/25 moved here per MissChanger setup instructions
[quad_gantry_level]

#--------------------------------------------------------------------   
##  Gantry Corners for 300mm Build
##  Uncomment for 300mm build
gantry_corners:
   -60,-10
   360,370
##  Probe points

				
points: #Dock NOT installed
   50,25
   50,225
   250,225
   250,25
#;> # End of Session Variables section

#;< # Section Variable marker

#---------------------------------------------------------------

#07/23/26 Updated to include current T0/T1 cfg files

[include 251103_T0-Nitehawk-Revo-LDO.cfg]

#[include 251103_T1-Nitehawk-Revo-LDO.cfg]

#---------------------------------------------------------------

####################################################################################

## 10/19/25 [gcode_macro _limits] section added

## See 19 October blog update for details

####################################################################################

[gcode_macro _limits]

variable_effective_min_y: 0.0 #this value gets changed to '120.0' in 'config_wt_dock'

gcode: # Empty, just for variables

[gcode_macro PRINT_END]

# Use PRINT_END for the slicer ending script - please customise for your slicer of choice

gcode:

# safe anti-stringing move coords

{% set th = printer.toolhead %}

{% set x_safe = th.position.x + 20 * (1 if th.axis_maximum.x - th.position.x > 20 else -1) %}

{% set y_safe = th.position.y + 20 * (1 if th.axis_maximum.y - th.position.y > 20 else -1) %}

{% set z_safe = [th.position.z + 2, th.axis_maximum.z]|min %}

SAVE_GCODE_STATE NAME=STATE_PRINT_END

M400 ; wait for buffer to clear

G92 E0 ; zero the extruder

G1 E-5.0 F1800 ; retract filament

TURN_OFF_HEATERS

G90 ; absolute positioning

G0 X{x_safe} Y{y_safe} Z{z_safe} F20000 ; move nozzle to remove stringing

;G0 X{th.axis_maximum.x//2} Y{th.axis_maximum.y - 2} F3600 ; park nozzle at rear

G0 X{th.axis_maximum.x//2} Y{th.axis_maximum.y - 10} F3600 ; park nozzle at rear 06/17/25 gfp adj to avoid 'clunk' at end

#G0 Z25 F3600 #10/17/25 gfp No dock

G0 Z100 F3600 #07/27/26 gfp moved 'No dock' extruder end positin higher for easier print-plate removal/replacement

M107 ; turn off fan

BED_MESH_CLEAR

RESTORE_GCODE_STATE NAME=STATE_PRINT_END

#---------------------------------------------------------------

#gfp 09/19/25 edited for 300x300 bed

[gcode_macro _home]

variable_xh: 150.0

variable_yh: 150.0 #Dock NOT installed

# Dock is installed: True or False

variable_dock: False

gcode:

#---------------------------------------------------------------

#05/30/25 added gfp

[bed_mesh]

speed: 120

horizontal_move_z: 5

mesh_min: 30, 35 #07/05/25 gfp: 30 was not enough for Y

mesh_max: 275, 250

probe_count: 5, 5 #06/01/25 chg to 5,5

zero_reference_position: 150,150 #for use with stock z endstop. Added 06/01/25 gfp

#gfp 09/19/25 moved here per MissChanger setup instructions

[quad_gantry_level]

#--------------------------------------------------------------------

## Gantry Corners for 300mm Build

## Uncomment for 300mm build

gantry_corners:

-60,-10

360,370

## Probe points

points: #Dock NOT installed

50,25

50,225

250,225

250,25

#;> # End of Session Variables section

To go the other way, we edited the ‘config_wt_dock.cfg’ file to add the closing marker ‘#;>’ to the end of the file, and then ran TOGGLE_CONFIG_MODE again to swap out the ‘no_dock’ “Session Variables” with teh ‘with_dock’ ones and then checked to make sure the Voron would reboot successfully (it did – yay!) and the “session variables” and ‘SAVE_CONFIG’ sections were correct.

Pull Request:

First we committed all the changes (principally the revised ‘config_switch.py’ script) to the local repo:

cd ~/klipper-toolchanger
git status
git add klipper/extras/config_switch.py
git commit -m "Fix CONFIG_TOGGLE: replace section in-place instead of appending

Previously the toggle stripped the marked block and appended the new
config at the end of printer.cfg. This caused progressive corruption,
lost the SAVE_CONFIG position, and dropped the closing #;> marker.

The new implementation:
- Strictly replaces only the text between (and including) the #;< / #;> markers
- Leaves all other content (including SAVE_CONFIG) untouched and in order
- Always writes both markers into the config_*.cfg files
- Creates a timestamped backup before every toggle
- Adds clear error checking for missing/duplicate markers

Tested with multiple dock / no-dock round-trips on a Voron 2.4 + MissChanger."

cd ~/klipper-toolchanger

git status

git add klipper/extras/config_switch.py

git commit -m "Fix CONFIG_TOGGLE: replace section in-place instead of appending

Previously the toggle stripped the marked block and appended the new

config at the end of printer.cfg. This caused progressive corruption,

lost the SAVE_CONFIG position, and dropped the closing #;> marker.

The new implementation:

- Strictly replaces only the text between (and including) the #;< / #;> markers

- Leaves all other content (including SAVE_CONFIG) untouched and in order

- Always writes both markers into the config_*.cfg files

- Creates a timestamped backup before every toggle

- Adds clear error checking for missing/duplicate markers

Tested with multiple dock / no-dock round-trips on a Voron 2.4 + MissChanger."

Then before we do anything else, we need to fork Vin-y’s klipper-toolchanger repo to my Github account with

Navigate to Vin-y’s Github klipper-toolchanger repo with: https://github.com/VIN-y/klipper-toolchanger
Create a fork
Push the local repo’s ‘fix-config-switch-in-place’ branch to the newly created fork with:

When the push is performed, Github creates a link that can be used for the pull request:

pi@mainsailos:~/klipper-toolchanger $ git remote add myfork https://github.com/paynterf/klipper-toolchanger.git
git push -u myfork fix-config-switch-in-place
Enumerating objects: 9, done.
Counting objects: 100% (9/9), done.
Delta compression using up to 4 threads
Compressing objects: 100% (4/4), done.
Writing objects: 100% (5/5), 2.45 KiB | 2.45 MiB/s, done.
Total 5 (delta 2), reused 0 (delta 0), pack-reused 0
remote: Resolving deltas: 100% (2/2), completed with 2 local objects.
remote: 
remote: Create a pull request for 'fix-config-switch-in-place' on GitHub by visiting:
remote:      https://github.com/paynterf/klipper-toolchanger/pull/new/fix-config-switch-in-place
remote: 
To https://github.com/paynterf/klipper-toolchanger.git
 * [new branch]      fix-config-switch-in-place -> fix-config-switch-in-place
branch 'fix-config-switch-in-place' set up to track 'myfork/fix-config-switch-in-place'.
pi@mainsailos:~/klipper-toolchanger $

pi@mainsailos:~/klipper-toolchanger $ git remote add myfork https://github.com/paynterf/klipper-toolchanger.git

git push -u myfork fix-config-switch-in-place

Enumerating objects: 9, done.

Counting objects: 100% (9/9), done.

Delta compression using up to 4 threads

Compressing objects: 100% (4/4), done.

Writing objects: 100% (5/5), 2.45 KiB | 2.45 MiB/s, done.

Total 5 (delta 2), reused 0 (delta 0), pack-reused 0

remote: Resolving deltas: 100% (2/2), completed with 2 local objects.

remote:

remote: Create a pull request for 'fix-config-switch-in-place' on GitHub by visiting:

remote: https://github.com/paynterf/klipper-toolchanger/pull/new/fix-config-switch-in-place

remote:

To https://github.com/paynterf/klipper-toolchanger.git

* [new branch] fix-config-switch-in-place -> fix-config-switch-in-place

branch 'fix-config-switch-in-place' set up to track 'myfork/fix-config-switch-in-place'.

pi@mainsailos:~/klipper-toolchanger $

Here’s what the pull request looks like on Github, just before clicking the ‘Create Pull Request button:

And here’s what it looks like after creating the pull request:

It feels good to be able to contribute something of worth to the MISSChanger community, as I am certain that others have run into similar problems with TOGGLE_CONFIG_MODE.

Stay Tuned,

Frank

New Current Sense & Step-Down Regulator Board

Leave a reply

In my old 4-wheel robot setup, I had a 5V regulator built around an LF50CV low-voltage dropout 3-pin regulator. Unfortunately, with the added current drain of the Pi5 and OAKD-Lite Camera, the LF50CV started dropping the output voltage to the point where the Pi5 started to get flaky. In discussing this with Grok, he suggested The Pololu D24V50F5 step-down regulator shown below:

I did some initial testing on the module:

At 9.1V at the ‘Switched Battery input, the Vbatt monitor voltage was 3.3V – the upper limit for inputs to a Teensy 4.1. Since 9.1 is well above even the high charging limit (~8.4V), this seems pretty safe.
At a measured Irun = 0.2A, Irun_out measured 90mV
At a measured Irun = 1.5A, Irun_out measured 0.4V
at a measured run = 1.14A, Irun_out measured 0.255V

Then attached a USB Type C male pigtail to the regulator output and then plugged it directly into the Raspberry Pi5. The Pi5 booted right up and I was able to access it from my PC using VS Code. Regulator output voltage with Pi5 connected was 4.99V at a measured input current of about 0.55A.

Here’s a photo showing the current sense/regulator module already hot-glued to its 3D-printed mounting plate.

30 July 2026 Update:

Yesterday the plan was for the existing LF50CV-based LDO 5V voltage regulator to be replaced with the above D24V50F5 DC-DC step-down regulator. However, as I was waiting for sleep last night, I was thinking about the increased 5V current level required by the Raspberry Pi5 and the OAKD-Lite camera and the fact that in the current (no pun intended) design, that 5V power comes from a module on the lower deck. So even with the higher current capacity (nominally 5A max) provided by the D24V50F5 regulator, there is still going to be a significant voltage drop in the approximately 2 meters of 24-guage stranded hookup wire and the TE Connectivity ‘MATE-N-LOK inter-deck connector. As Grok reminded me, there is not only the 1m line TO the Pi5/Camera, there is also the ground wire coming BACK for a total of approx 2m and two times through the connector. Grok noted that the total round-trip IR drop at 3A would be about 0.5V, which would place the input voltage at the Pi5 at about 4.5V, way too low for reliable operation.

That made me realize that even if I replaced the LF50CV-based LDO 5V voltage regulator with the D24V50F5 5A module, it *still* wouldn’t provide a clean 5V to the Pi5. It was at that point that the way to skin this cat was to leave the LF50CV-based LDO 5V voltage regulator in place on the bottom deck and move the D24V50F5 5A module to the top deck where the wire length to the Pi5 would be more like 0.1m instead of 2m – problem solved!

Well, not entirely, as I still have to get +Vbatt (nominally 8V) up to (and back from) the second deck, with the same 2m + 2x through the connector as before. But now that same 0.5V IR drop is applied to Vbatt instead of +5V, and that makes the problem go away entirely – yay!

Looking at a recent system schematic:

I see that +5V from the LF50CV goes through the interdeck connector on pin 1 (red), and the return path is on pin 2 (black). I believe all I would have to do is move the red wire from the output to the input of the LF50CV module. No, not that simple – the LF50CV output goes directly to the Teensy main controller 5V buss, and a separate wire goes from that 5V buss up through the interdeck connector to a terminal strip. That wire will have to be disconnected from the Teensy 5V buss and re-connected to Vbatt at the input to the LF50CV module. At the other end, the wire will have to be disconnected from the terminal strip and routed instead to the ‘Batt’ terminal of the D24V50F0 module. The output (both +5 and GND) from the D24V50F0 module must be routed back to the terminal strip. At this point, the two 5V regulator modules will be tied to the same (GND) reference, but supply entirely different circuits. Here’s a photo of the D24V50F0 module installed on the upper deck.

With this setup I was able to run the Pi5 for several hours before the +5V supply dropped to low to continue.

I need at least four additional lines from the Teensy 4.1 to the 2nd deck. Two lines for Serial1-Pi5 comms and two for Vbatt/Irun sensors on the 2nd deck 5V regulator board. The existing Brn/Wht wire pair has already been designated on the schematic for Serial1-Pi5 comms, and the existing but unassigned Blu/Gry pair can be used for the sensor lines. Note that the robot now sports *three* 1NA169 current sensor modules (the third one is in the base with the battery pack), and two of those also provide (Vbatt – 6V) voltage outputs. The pre-existing gray wire on the second deck was used to reset the Teensy 3.5 used to control the VL531LX LIDAR sensors but since the Teensy 3.5 (and all but one of the VL53L1X sensors) was removed, the Gry wire is no longer used. Here’s the updated schematic:

Building an Improved Under-monitor Accessories Tray

Leave a reply

Posted 25 July 2026

For quite a while I have used the accessories tray shown below, but I have become increasingly unhappy with the quality with it; in particular the small tray sections aren’t quite big enough to handle all my USB sticks, micro-SD cards and holders, spring-clips, etc. So, after trying in vain to find a better model on-line, I decided I would design and 3D print one for myself, using my 300×300 Voron 3D printer in the ‘no_dock’ configuration.

Here’s where I am at the moment:

preliminary design for under-monitor accessory tray. Note that this is only 1/2 of the finished product

I started out with the idea of using removable partitions to fix the problem of having too-small or too-large bins for whatever I wanted to store. The available free distance under the riser is about 270mm so I made the width (the ‘Y’ axis above) slightly wider than original at 260mm. The above model is about 237mm long (the ‘X’ direction above), but is only 1/2 length, as the full length won’t fit on the Voron’s 300x300mm print bed, so I’ll have to figure out some way of connecting the two halves.

26 July 2026 Update:

The idea I came up with to connect the two halves of the accessory tray was to place holes in the ‘front’ half of the tray, and matching posts in the ‘back’ half of the tray, as shown in the screenshot below:

Now that I have my ‘no_dock’ printer configuration working properly, I printed up the small section of the back half of the tray, along with just a small section of the mating end of the front half of the tray, just to make sure the mating idea would work. It seemed to work pretty well, as shown in the following photos:

Mating parts shown; posts on the back half, holes on the front

The posts on the back half came out a little scruffy due to small diameter and their length, but still worked well after a bit of cleanup. In the final version I think I will increase the diameter a bit and reduce the length. The gap shown in the mated photo above I believe was due to the way I printed the part, and shouldn’t be a factor in the finished product – we’ll see!

The next step was to create the rear section sketch. First I tried do this by simply copy/pasting the entire ‘2-bin_Tray’ section, but the result was ‘too many’ broken references, as shown below:

But after some consultation with Grok about using the ‘use’ feature, I simply created a new sketch on the same plane as the original ‘2-bin Tray’ sketch, then activated the ‘Use’ feature and dragged a rectangle to select the entire original sketch. This produced a new, unlinked sketch which I moved to a non-interfering location with the transform tool, as shown below:

A better ‘copy/paste’ using the ‘Use’ tool, selecting everything, and then using the ‘Transform’ tool to move it

The rear section of the tray will only be 120mm long instead of 240, as the total length of the tray is 360mm. Here are some photos of the constituent parts:

Assembled tray, waiting on ‘PET-G Gloop’

09 August 2026 Update:

After the ‘Gloop’ arrived I was able to glue the front & back sections together with a liberal application of ‘gloop’ to both sides of the junction and to the top and bottom of the tray once the sections were joined. Then transferred all my junk from my old tray to the new one and installed it under my monitor riser, as shown in the following photos:

Moving Vision Processing to My 4-Wheel Robot, Part 1

Leave a reply

Posted 08 July 2026

In this previous post, I describes my work with Grok Code to successfully demonstrate the use of vision processing with a Luxonis OAKD-Lite camera and a Raspberry Pi5 to generate ‘best clear direction’ advisories for robot motion control.

After achieving the above milestone, I started thinking about the next steps. Clearly I want to move on to achieve actual robot navigation utilizing the above ‘best direction’ information, but the current 2-wheel robot battery pack doesn’t really provide enough power for the Pi5, the OAKD-Lite camera, and the wheel motors. Consequently, I started thinking about moving the vision processing project from the 2-wheel robot onto my existing 4-wheel platform to take advantage of the latter’s larger battery pack. Looking at the pinouts for the Teensy 3.6 and Teensy 4.1 along with the 2-wheel and 4-wheel schematics, I developed the following pin assignment spreadsheet:

Pin assignments for the 2-wheel to 4-wheel transfer

In the above (hopefully self-explanatory) spreadsheet there was only one significant conflict. On the current 2-wheel robot pins 29-32 are occupied by the four Hall-effect wheel encoders, and on the current 4-wheel robot these pins are used by four of the six battery charge indicator LED’s, and that function carries over into the new vision-enhanced robot. Easy enough resolution, just move the wheel encoder pins to 7-10 (actually even easier, I don’t need to move the encoders over until I actually have software/firmware that needs them, and that might be a while).

I started out thinking I would 3D print another ‘2nd deck’ plate so I wouldn’t have to disassemble the original one. Then I came to my senses and realized that I had originally constructed the 2nd deck expressly so it *could* be disassembled – duh! Here’s the mostly disassembled 2nd deck plate:

Top. Terminal strip and rear LIDAR are reused

And the bottom contains all the motor hardware and drivers, the battery and charging circuitry. The ‘deck’ of the bottom deck already has a 5V regulator module (more on that below), the MPU5060 MPU and the charger unit homing module. Why re-invent when it is already there.

15 July 2026 Update:

I launched DipTrace, opened the schematic file for my current 4 wheel robot, then saved a copy as ‘Vision-Enhanced Robot Schematic’ and then edited it to reflect the following changes:

removed the Wixel OTA module; OTA is now handled through the Pi5 wifi link.
removed all but the rear-facing element of the 7-element VL53L1X LIDAR array and the Teensy 3.5 that managed it.
added the Raspberry Pi5 and the OAKD-Lite camera to the second deck, utilizing pins freed up by the removal of the Teensy 3.5 LIDAR array manager
Replaced the Teensy 3.6 main system controller with a Teensy 4.1 for additional processing power.

I also replaced the approximately 45cm long USB cable that came with the camera with a 15cm one as the extra length was just adding mass (and mess) to the installation. When the new USB cable arrived, I hooked it and the Pi5 stock power cube up to the Pi5 and used the previously developed Python script and associated shell scripts to confirm that the cable worked.

20 July 2026 Update:

In the process of moving the Pi5 and OAKD-Lite camera to the 4-wheel robot, I managed somehow to lose the Pi5’s SD card. I tried my best to find it – including dumpster-diving with the hope that I could find in my last trash bag – but no luck. Attempting to make lemonade out of lemons I decided to try and document the process of building up a new SD card from scratch.

RaspberryPi Imager:

20 July 2026 Update:

RaspberryPi Imager:

Choose Ubuntu Server 24.04 LTS (64-bit) (Server version is recommended — Desktop images sometimes cause issues with the customization wizard). I actually had no problem with the ‘desktop’ version, but I think it included a lot more ‘crapware’ than the ‘server’ version. Also, when the ‘server’ version is chosen, the imager includes the ‘Customization’ page for wifi setup, username/password, etc.

Hostname: RobotPi5.
Capital City: Washington D.C
Time Zone: America/New York
Username: Pi
Wifi: Choose ‘Secure Network, fil in SSID & password.
SSH: Choose ‘Enable SSH’ and ‘Use Password authentication.
Password: xxxx (note that on subsequent passes of the imager, it will offer to use a saved password)
Write: The image below shows the situation just before writing the image to the SD card

Write to the SD card (takes a few minutes).

First Boot (Monitor + Keyboard):

Insert the SD card into the Pi 5 and power it on.Complete the initial Ubuntu setup prompts (this can take a few minutes).Log in with username pi and the password you set. I had to reboot the Pi a couple of times to get it to go all the way through the startup sequence to present the login screen.

Connect from Your PC via SSH:

To confirm that the Pi can see the wifi network, run ‘ssh pi@RobotPi5’ from a Windows Command Window and enter your password. This should connect right away. If it doesn’t, then check the Pi’s IP address (DHCP can assign it a different one) with the ‘ip addr’ command on the Pi. If the address was changed (like from 192.168.1.200 to 192.168.1.201), you can change it back by editing your local router to give the Pi5 a static IP address reservation as shown below, and then rebooting the Pi. When the Pi comes back up it will get the ‘reserved’ address from the router, and life will be good again, except…

When you try to connect from your PC using ssh, you will probably get the following error message:

This occurs because the new Ubuntu installation generated a new SSH key, and the on one in your PC’s ‘host’ file no longer matches. To fix this, you must first remove the old key, with the following in a command window on your PC:

Then try to connect to the Pi from the PC with ‘ssh RobotPi5’. You should now see this:

Enter ‘yes’ (can’t enter just ‘y’) to accept the new key. Then you should be able to connect to the Pi with your username and password, and you should see this:

Connect using VS Code:

Now you should also be able to connect to the PI from VS Code with Cmd – Shift – P -> Remote SSH: Connect to host ->’RobotPi5′. This should result in the following screenshot:

Ignore the misleading warning in the blue field above and enter the password for the Pi5. This should log you in, but it may still take a while for VS Code to connect the first time (you will see ‘opening RobotPi5’ with the spinner in the bottom left-hand corner, and then this will change to ‘SSH: RobotPi5’ as shown below:

or this:

At this point you have a fully functional Ubuntu ‘server’ installation on the Pi, but none of the software packages (ROS2 Jazzy) needed to actually use the OAKD-Lite camera, and none of the already-developed user software. The user software can be retrieved from your GitHub repo (you DID create a GitHub repo and save your software, right?).

Install ROS 2 Jazzy (Jazzy is a version nickname):

From a VS Code Terminal Window, run these commands in order:

sudo apt update && sudo apt upgrade -y
sudo apt install software-properties-common curl -y
sudo add-apt-repository universe -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update
sudo apt install ros-jazzy-desktop -y

You will probably need Grok’s help with getting through the entire package installs. We tried the Ubuntu 24.04LTS ‘server’ edition because it has less bloatware, but ros2-Jazzy would not install. Finally went back to Ubuntu 24.04LTS ‘desktop’ edition, and this worked – yay!!

After flashing the SD card with Ubuntu 24.04LTS ‘desktop’ edition, I booted it up on the Pi5 and went through the initial setup process. The ‘desktop’ edition skips all the customization stuff in the SD card load because it can be done by the user in the desktop.

The first hurdle to overcome was the inability to connect to the Pi with SSH, even though I ‘enabled’ SSH in the desktop settings.

sudo apt update
sudo apt install openssh-server -y

sudo systemctl enable --now ssh

sudo systemctl status ssh

sudo apt update

sudo apt install openssh-server -y

sudo systemctl enable --now ssh

sudo systemctl status ssh

the first two commands above succeeded, but the stataus was still ‘unable’. However after some tangential stumbling around with (almost) trying to kill some interfering processes, it turned out that just waiting a minute or two did the trick and SSH started working – Yay!

At this point we started the process of getting ros2 ‘Jazzy’ installed, as follows:

sudo apt update && sudo apt upgrade -y
sudo apt install software-properties-common curl -y
sudo add-apt-repository universe -y

#Then add the ROS 2 repository:

sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key | sudo gpg --dearmor -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update

#Finally, install ROS 2 (we'll use the lighter ros-base version to avoid the previous dependency issues):

sudo apt install ros-jazzy-ros-base -y
source /opt/ros/jazzy/setup.bash

#So you don’t have to source it manually every time, run:

echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc

#Then either log out and back in, or run:
source ~/.bashrc

#Then see if ros2 is actually installed and running
pi@RobotPi5:~$ ros2 --help
usage: ros2 [-h] [--use-python-default-buffering] Call ros2 <command> -h for more detailed usage. ...ros2 is an extensible command-line tool for ROS 2.options:
  -h, --help            show this help message and exit
  --use-python-default-buffering
                        Do not force line buffering in stdout and instead use the python default buffering, which might be affected by
                        PYTHONUNBUFFERED/-u and depends on whatever stdout is interactive or notCommands:
  action     Various action related sub-commands
  bag        Various rosbag related sub-commands
  component  Various component related sub-commands
  daemon     Various daemon related sub-commands
  doctor     Check ROS setup and other potential issues
  interface  Show information about ROS interfaces
  launch     Run a launch file
  lifecycle  Various lifecycle related sub-commands
  multicast  Various multicast related sub-commands
  node       Various node related sub-commands
  param      Various param related sub-commands
  pkg        Various package related sub-commands
  plugin     Various plugin related sub-commands
  run        Run a package specific executable
  security   Various security related sub-commands
  service    Various service related sub-commands
  topic      Various topic related sub-commands
  wtf        Use wtf as alias to doctor

# Install and initialize rosdep (I have no idea what this does...)

sudo apt install python3-rosdep -y
sudo rosdep init
rosdep update

#Clone your repository (if you haven’t already)

cd ~
git clone https://github.com/paynterf/my_vision_robot.git
cd my_vision_robot
git checkout vision-to-motion-demo

sudo apt update && sudo apt upgrade -y

sudo apt install software-properties-common curl -y

sudo add-apt-repository universe -y

#Then add the ROS 2 repository:

sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key | sudo gpg --dearmor -o /usr/share/keyrings/ros-archive-keyring.gpg

echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

sudo apt update

#Finally, install ROS 2 (we'll use the lighter ros-base version to avoid the previous dependency issues):

sudo apt install ros-jazzy-ros-base -y

source /opt/ros/jazzy/setup.bash

#So you don’t have to source it manually every time, run:

echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc

#Then either log out and back in, or run:

source ~/.bashrc

#Then see if ros2 is actually installed and running

pi@RobotPi5:~$ ros2 --help

usage: ros2 [-h] [--use-python-default-buffering] Call ros2 <command> -h for more detailed usage. ...ros2 is an extensible command-line tool for ROS 2.options:

-h, --help show this help message and exit

--use-python-default-buffering

Do not force line buffering in stdout and instead use the python default buffering, which might be affected by

PYTHONUNBUFFERED/-u and depends on whatever stdout is interactive or notCommands:

action Various action related sub-commands

bag Various rosbag related sub-commands

component Various component related sub-commands

daemon Various daemon related sub-commands

doctor Check ROS setup and other potential issues

interface Show information about ROS interfaces

launch Run a launch file

lifecycle Various lifecycle related sub-commands

multicast Various multicast related sub-commands

node Various node related sub-commands

param Various param related sub-commands

pkg Various package related sub-commands

plugin Various plugin related sub-commands

run Run a package specific executable

security Various security related sub-commands

service Various service related sub-commands

topic Various topic related sub-commands

wtf Use wtf as alias to doctor

# Install and initialize rosdep (I have no idea what this does...)

sudo apt install python3-rosdep -y

sudo rosdep init

rosdep update

#Clone your repository (if you haven’t already)

cd ~

git clone https://github.com/paynterf/my_vision_robot.git

cd my_vision_robot

git checkout vision-to-motion-demo

So at this point ros2-Jazzy is installed and active, and my working software has been restored from Github – WHEW!

Tomorrow we will continue through to getting the ‘clearest_direction_node.py’ program running (yet again)

While I can’t really say that losing that micro-SD card was much fun, it *was* educational, and now I think I have a better understanding of how the startup process works, and much better documentation.

22 July 2026 Update:

I renamed the Grok session to “260722 Pi5 Boot ROS and OAKD-Lite Initialization” to be more recognizable in the future.

OK, back to the effort to recover from my disastrous loss of my original micro-SD card. When we left it last night, we had gotten to the point of successfully flashing the ‘desktop’ version of Ubuntu 24.04, gotten ros2-Jazzy installed, cloned the ‘my_vision_robot’ GitHub repo and switched to the ‘vision-to-motion-demo’ branch.

2 Wheel Robot with Vision Processing, Part IV

Leave a reply

Posted 06 July 2026

After wrapping up the Wifi_OTA project described in this post, Grok Code and I moved on to the next big step – integrating the Luxonis OAKD-Lite camera integrated into ROS (Robot Operating System) so the robot could take advantage of the ROS ‘topic/subscriber’ strategy where a sensor like the OAKD-Lite camera can ‘publish’ formatted sensor data to a ‘topic’ that programs can ‘subscribe’ to, thereby making sensor data available to inform robot navigation. Amazingly, this trick actually worked, and now the robot can ‘see’ the best direction to move. Below is a summary of the work involved getting to this point, produced by Grok Code at my request. (note: When Grok uses ‘I’ below, he really means ‘we’, as I was very much the junior partner in this adventure. Think of me more as somewhat intelligent clerk,just barely smart enough to follow direction and report results ;).

From Grok Code: Summary for “2 Wheel Robot with Vision Processing, Part IV”

After struggling for a while with custom ROS 2 package structures and running into repeated setup.py issues, I decided to take a step back and follow a known working example as closely as possible. I came across Jesse Stevens’ blog post about setting up the OAK-D Lite with ROS 2 Jazzy on a Raspberry Pi 5, and it looked very close to what I was trying to achieve. Rather than continuing to fight with my own over-engineered layout, I made the deliberate choice to start fresh using his structure and naming.

We began by backing up my existing src/ folder and creating a clean myrobot package exactly as described in the blog. This meant using the standard ros2 pkg create command, adding a config/ directory, and modifying setup.py to properly install YAML files using data_files and glob. Following the blog’s oak_run.yaml example, I configured the camera for a Depth-only pipeline with conservative settings (no subpixel, RGB disabled, HIGH_ACCURACY preset initially).

Getting the camera running wasn’t entirely smooth — we hit the familiar X_LINK_DEVICE_ALREADY_IN_USE error a few times — but once we established a reliable restart process, the camera would come up and begin publishing depth data. However, early captures showed suspicious results: the minimum and maximum depth values were often stuck at the same numbers across different scenes. This told us the depth data wasn’t yet reliable enough for motion planning.

To make progress visible and testable, we built a capture utility (save_depth_frame.py) that could save both raw depth values as CSV and, later, visual PNG heatmaps. Being able to generate side-by-side comparisons between the depth map and the actual scene was incredibly helpful. Over time, with adjustments to the YAML (switching to the DEFAULT preset, enabling subpixel, and using only the bottom half of the image for processing), the depth maps started to look structurally meaningful. Features in the real world began appearing clearly in the heatmaps.

With usable depth data in hand, we shifted focus toward the higher-level goal: using vision to help the robot decide where to go. We created a new node called clearest_direction_node.py that divides the lower half of the depth image into seven sectors, labeled intuitively as clock positions from 9:00 to 3:00. The node calculates the average valid depth in each sector and identifies the “clearest” direction — the one with the highest average depth. It then suggests a turn angle toward that direction.

This node proved to be surprisingly effective right from the start. When I moved objects into different parts of the camera’s view, the recommended direction updated immediately and logically. Seeing the output change in real time gave me confidence that the depth data was now good enough to build motion logic on top of.

Throughout this process, we also invested time in workflow improvements. I created several small shell scripts (restart_camera.sh, capture_depth.sh, start_viewer.sh) to reduce friction when restarting the camera or capturing data. These scripts, along with the analysis node, were eventually moved into my my_vision_robot GitHub repository under a clean structure (software/ros2/ for the ROS package and software/scripts/ for the utilities). We also updated the project .gitignore to properly exclude ROS 2 build artifacts and temporary test data.

By the end of this phase, I had moved well beyond simply getting the camera to publish data. I now had a working system that could look at a depth image and suggest a sensible direction for the robot to face — all running on the Pi 5 and reacting to real changes in the environment. While there’s still work ahead to turn these direction suggestions into actual motor commands on the Teensy, this felt like a genuine milestone: the vision processing side had gone from “not yet reliable” to “usable for basic navigation decisions.”

Because I know I won’t remember the details of how we got from the start to a functioning “Turn this way” demo, I asked Grok for a short description of the relevant files, based on the latest GitHub file structure for the ‘vision-to-motion-demo’ branch, as shown in the following screenshot:

1. Camera Launch & Configuration (the foundation)

software/ros2/my_vision_robot/config/oak_run.yaml The main parameter file passed to depthai_ros_driver. Defines the Depth pipeline, 480P stereo resolution, HIGH_ACCURACY (or DEFAULT) preset, bottom-half-friendly settings, RGB disabled, and all the i_* parameters that make depth actually usable.
software/ros2/my_vision_robot/launch/oak_camera.launch.py Simple launch file that starts depthai_ros_driver’s camera_node and loads oak_run.yaml via –params-file. This is what restart_camera.sh calls.

2. Core Depth Capture Script

software/scripts/save_depth_frame.py The workhorse script. Subscribes to /camera/stereo/image_raw, converts the 16UC1 depth image with CvBridge, saves the raw depth values as a timestamped CSV, and generates a matplotlib heatmap PNG (with stats in the title). This is what produces the nice side-by-side depth maps you’ve been using for validation.

3. Convenience / Workflow Scripts

software/scripts/capture_depth.sh Tiny wrapper that sources both ROS 2 environments (/opt/ros/jazzy + your workspace) and then runs save_depth_frame.py. Prevents the classic “No module named ‘rclpy’” error.
software/scripts/restart_camera.sh One-command way to kill any old camera processes, source the environments, and relaunch the camera via the launch file above. You use this every time you reboot the robot.
software/scripts/start_viewer.sh Sources the environments and launches rqt_image_view pointed at the depth topic so you can visually confirm the camera is streaming.

4. Depth Analysis / “Clearest Direction” Node

software/scripts/clearest_direction_node.py The first real vision-to-motion node. Subscribes to the depth image, looks only at the bottom half (rows 240–479), divides it into 7 clock-position sectors (9:00–3:00), computes average valid depth per sector, identifies the “clearest” direction, and prints a live table + recommended turn angle. This is the node that actually reacts when you move obstacles in front of the camera.

5. Supporting Package Files (mostly standard but customized)

software/ros2/my_vision_robot/package.xml & setup.py Standard ament_python package files. We updated the package name from myrobot → my_vision_robot and made sure config/ and launch/ get installed correctly.
software/ros2/my_vision_robot/my_vision_robot/__init__.py & resource/my_vision_robot Required empty marker files so the package is recognized by ROS 2.
software/ros2/my_vision_robot/setup.cfg & test/ files Boilerplate that came with ros2 pkg create; left mostly untouched.

Summary of the “depth measurement stack”

restart_camera.sh → starts the camera with the tuned oak_run.yaml
capture_depth.sh (or save_depth_frame.py directly) → grabs a frame and saves CSV + pretty PNG
start_viewer.sh → lets you watch the live depth image
clearest_direction_node.py → turns the raw depth into actionable “which way is clearest?” information

All of these now live under version control in the repo, so “future Frank” (or anyone else) can clone the branch and have a working depth pipeline in a few minutes.

Additions by the not-so-smart human:

This was the part of the project that transformed my somewhat unrealistic idea of enhancing my robot’s navigation capabilities with vision processing into a real, concrete capability even in its current nascent state. The Raspberry Pi5 combined with the OAKD-Lite camera easily processed 30 frames/sec of depth information to generate ‘turn this much in this direction’ outputs for the robot. This part of the overall project didn’t actually send commands to the Teensy for movement, but the ability to do so was demonstrated with an earlier ‘end-to-end-serial’ project and the earlier Wifi_OTA project. There is still A LOT more to do to get to the point where I can set the robot loose on the world (or at least the house), but getting even the most basic vision processing going was a HUGE milestone.

In addition to the above progress, I have been learning a huge amount about the OAKD-Lite camera, the ROS (Robot Operating System) environment, Visual Studio Code, and Git repository creation & maintenance. Early on in this phase of the project, Grok Code recommended VS Code for working with the Raspberry Pi in general, and for Python programming in general. I have always used Visual Studio on my PC, and by adding the Python extension I could easily generate Python code on my PC. However, this project required that the actual code has to run on the Raspberry Pi5 on the robot. In addition, there is a lot that has to be done at the Linux (bash) command line on the Pi5. What I needed was a way of doing all the work on my dual-screen PC setup without having to keep transferring files and programs back and forth between the PC and the Pi5. It turns out that ‘that something’ is VS Code, and now that I have had some time to work my way up the learning curve, it is WONDERFUL!

Once VS Code is setup properly to connect via SSH over Wifi to my Pi5, it does so every time it is launched, and from then on, the environment is very much like using Visual Studio. Files can be created, edited, moved etc using a File Explorer and editor window very similar to VS and it is easy to lose track of the fact that all the action is actually taking place on that small Pi5 board over there on my robot.

Another milestone of sorts that was achieved with this project was making a Git repository a central part of the overall project. Although I have Tortoise Git on my PC and have used it on and off for years, this is the first time I decided to set up a logical repo layout and keep it up-to-date as the project moved along. The Git repo has three different pieces – a local repo on the Pi5 for all the shell scripts, Python scripts and ROS-specific configuration files, a local repo on the PC for the teensy stuff, and a GitHub remote repo that ties the two together. Here’s a sketch of the repo layout (produced in about 3 seconds by Grok Code):

I had to insert this as an image, as WordPress doesn’t handle the 3rd level indent properly

This same layout exists in all three places (Pi5, PC, Github), but obviously the pi5 repo deals with the ‘Software’ side and the PC deals with the ‘Firmware side. Both ‘local’ repos are clones of the same repo on Github, so both local repos also contain all the material produced by the other one. So a typical work flow on either the PC or the Pi5 would be:

Git status – to see if anything unusual is happening
Git pull origin [branch] <– started out on ‘main’, now working on ‘vision-to-motion-demo’. This synchs the local repo with the GitHub repo, and any commits from the ‘other local’ repo
Git status – again to see what changed during this session
Git commit -m “[Message]”
Git status – again to catch any problems
Git push origin [branch] <- synchs the GitHub repo with the local repo

06 August 2026 Update:

Here is a ‘structured index of the Grok chat that produced most of the progress shown above:

Structured Index – OAK-D Lite + ROS 2 Jazzy on Pi 5

Chat focus: Getting reliable depth data and a “clearest direction” node working, then organizing everything into a proper Git repo.

1. Project Context & Philosophy

Goal: Vision-enhanced 2-wheel (later 4-wheel) robot using OAK-D Lite + Raspberry Pi 5 + Teensy 4.1
Key decision: Stop fighting custom structures and follow Jesse Stevens’ blog post exactly as a known-working template
Philosophy: “BIG RED BUTTON” approach — get something known to work first, then adapt

2. Initial Clean Setup (July 5–6)

Backed up and cleared ~/ros2_ws/src
Created package: ros2 pkg create –build-type ament_python myrobot –dependencies launch_ros rclpy
Added config/ folder and oak_run.yaml
Modified setup.py to install launch files and YAML configs
Successfully launched camera with: ros2 run depthai_ros_driver camera_node –ros-args –params-file …
Confirmed depth topic /camera/stereo/image_raw was publishing at ~30 Hz (16UC1 encoding)

3. Key Files Created

File	Purpose	Final Location
oak_run.yaml	Camera configuration (Depth pipeline, resolution, stereo settings)	software/ros2/my_vision_robot/config/
oak_camera.launch.py	Launch file for the camera node	software/ros2/my_vision_robot/launch/
restart_camera.sh	Kill old processes + source environments + launch camera	software/scripts/
start_viewer.sh	Launch rqt_image_view (requires monitor)	software/scripts/
capture_depth.sh	Wrapper that sources ROS and runs the capture script	software/scripts/
save_depth_frame.py	Captures one depth frame → CSV + PNG heatmap	software/scripts/
clearest_direction_node.py	Analyzes bottom half of depth image into 7 clock sectors and recommends turn direction	software/scripts/

4. Depth Data Validation

Early problem: Fixed min/max values (358 mm / ~34 m) that didn’t change with scene
Solution path:
- Enabled subpixel, changed depth preset to DEFAULT
- Restricted analysis to bottom half of the image (camera is highest point on robot)
- Confirmed with side-by-side photo + heatmap that real structure was visible (cup handle, toolbox edge, doorframe, etc.)
Capture method: Press Enter in save_depth_frame.py to grab one frame on demand

5. Clearest Direction Node Design Decisions

7 sectors labeled as clock positions: 9:00 → 3:00
Only processes the bottom half of the depth image (rows 240–479)
Scoring: Average valid depth per sector
Output: Live table showing valid pixel count, average depth, best sector, and recommended turn angle
First successful test: Correctly switched from 3:00 to 1:00 when a cardboard box was moved into the view

6. Git Repository Organization

Repo: paynterf/my_vision_robot
Branch used: vision-to-motion-demo
Final structure:textmy_vision_robot/ ├── software/ │ ├── ros2/ │ │ └── my_vision_robot/ ← ROS 2 package │ └── scripts/ ← All utility scripts ├── docs/ ├── firmware/ └── .gitignore
Important: Scripts now live inside the repo. Old copies in ~/ were cleaned up.
.gitignore updated to exclude ROS 2 build artifacts, CSVs, PNGs, and backup folders

7. Verification After Moving to 4-Wheel Robot

Switched from ~18″ USB cable to a short 6″ high-speed cable
Camera still launched cleanly and produced usable depth heatmaps
Confirmed the entire stack still works on the new platform

8. Current Recommended Workflow

Bash

# Start camera
cd ~/my_vision_robot/software/scripts
./restart_camera.sh

# Capture depth frames (in another terminal)
./capture_depth.sh

# Run direction analysis
python3 clearest_direction_node.py

9. Open Items / Next Logical Steps

Clean residual old myrobot package from ~/ros2_ws/src/ once fully confident

Send recommended turn commands to the Teensy

Add hysteresis / minimum valid-pixel thresholds

Possibly improve sector scoring