Thursday, June 20, 2024

Calibrating the vl5310x TOF laser distance sensor.

Here is a good starting point, however, it might need a polynomial regression.

#include <Wire.h>
#include <VL53L0X.h>
#include <Preferences.h>

VL53L0X sensor;
Preferences laserPrefs;

const int numMeasurements = 100;  // Number of measurements to average
const int numCalPoints = 7;  // Number of calibration points
uint16_t measuredDistances[numCalPoints];
uint16_t actualDistances[numCalPoints] = {10, 100, 300, 600, 900, 1000, 1050}; // Replace with your actual distances in mm

// Calibration parameters
float offset = 0.0;
float scale = 1.0;

void setup()

  // Initialize Preferences for laser calibration data (read-write)
  laserPrefs.begin("laser_calibration", false); // Initialize for writing
  if (!laserPrefs.isKey("offset")) {
    Serial.println("No laser calibration data found. Please calibrate and store offsets.");
    // Perform laser sensor calibration and store offsets if necessary
  } else {
    Serial.println("Laser calibration data loaded.");
    // Load laser calibration offsets if they exist

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

  // Start continuous back-to-back mode (take readings as
  // fast as possible).  To use continuous timed mode
  // instead, provide a desired inter-measurement period in
  // ms (e.g. sensor.startContinuous(100)).
  // Perform calibration if no stored data found
  for (int i = 0; i < numCalPoints; i++) {
    Serial.print("Place the sensor at ");
    Serial.println(" mm");
    delay(5000); // Wait for user to place the sensor

    measuredDistances[i] = measureDistance();
    Serial.print("Measured distance: ");
    // Prompt for user confirmation of measured distance
    if (!confirmDistance(actualDistances[i], measuredDistances[i])) {
      Serial.println("Measurement rejected. Please adjust sensor position and retry.");
      i--; // Retry measurement for the same distance
    } else {
      Serial.println("Measurement accepted.");

  // Calculate calibration parameters
  offset = calculateOffset();
  scale = calculateScale();

  // Store calibration data
  laserPrefs.putFloat("offset", offset);
  laserPrefs.putFloat("scale", scale);
  Serial.println("Calibration data stored in laserPrefs");
  Serial.print("Calibration offset: ");
  Serial.print("Calibration scale: ");

  Serial.println("Entering main loop...");

void loop()
  int tofDist = sensor.readRangeContinuousMillimeters();
  if(tofDist < 2000 && tofDist != 8191){
    float calibratedDist = tofDist * scale + offset;
    Serial.print("Raw Distance: ");
    Serial.print(" mm, Calibrated Distance: ");
    Serial.println(" mm");
    delay(1000); // Adjust delay as necessary
  //if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }


uint16_t measureDistance() {
  uint32_t sum = 0;  // Use uint32_t to avoid overflow if the sum gets large
  int validCount = 0;

  for (int i = 0; i < numMeasurements; i++) {
    uint16_t distance = sensor.readRangeContinuousMillimeters();
    if (distance < 4000) {  // Check if the distance is valid (assuming valid distances are < 4000 mm)
      sum += distance;
      Serial.print("VM: ");
      Serial.print(" ");
    } else {
      // Handle invalid measurements if necessary
      Serial.print("VM: ");
      Serial.print(" Invalid measurement: ");
    delay(50); // Small delay between measurements

  Serial.print("SUM: ");
  Serial.print("Valid measurements: ");

  if (validCount > 0) {
    return sum / validCount;  // Return the average of valid measurements
  } else {
    return 0;  // Return 0 or handle this case appropriately if no valid measurements were recorded

float calculateOffset() {
  float sum = 0;
  for (int i = 0; i < numCalPoints; i++) {
    sum += (actualDistances[i] - measuredDistances[i]);
  return sum / numCalPoints;

float calculateScale() {
  float sum = 0;
  for (int i = 0; i < numCalPoints; i++) {
    sum += (actualDistances[i] / measuredDistances[i]);
  return sum / numCalPoints;
bool confirmDistance(uint16_t actual, uint16_t measured) {
  Serial.print("Measured distance is ");
  Serial.print(" mm. Is this correct for ");
  Serial.println(" mm? (y/n)");

  while (true) {
    if (Serial.available() > 0) {
      char response =;
      if (response == 'y' || response == 'Y') {
        return true;
      } else if (response == 'n' || response == 'N') {
        return false;
    delay(100); // Wait for user input

Sunday, June 2, 2024

Pinky's Lighthouse

 After getting communication set up with the Raspberry Pi controller in this post, we are off to send and interpret sensor data with ROS2.

Cartographer is a good starting point. In my ROS2 docker container:
sudo apt install ros-$ROS_DISTRO-cartographer

It installs, but it looks like CARTOGRAPHER IS NOT SUPPORTED ON IRON...    I don't know why this package is available...

Moving on to nav2.

From inside the container, I can't launch GUIs. My working container must be committed to create a new image and re-run with the following:

sudo docker run -it \

    --privileged \

    -p 8888:8888/udp \

    --name c5cont \

    --env DISPLAY=$DISPLAY \

    --env QT_X11_NO_MITSHM=1 \

    --volume /tmp/.X11-unix:/tmp/.X11-unix \

    --cpus="3" \

    c5 /bin/bash

Then create a run file that has the following:
sudo docker start <container name>
xhost +local:docker
sudo docker exec -it <container name> /bin/bash

Also look into creating a dockerfile:

I'll have to reformat the data sent from the lighthouse IMU and laser distance sensor to mimic a lidar array.

Friday, February 16, 2024

RL training using Nvidia Omniverse

 How to run Isaac gym on my computer:

C:\Users\jynx4\AppData\Local\ov\pkg\isaac_sim-2023.1.1\isaac-sim.gym.bat --ext-folder D:\isaac\

This includes default robots. In order to run training on Pinky, many files must be created along with importing a .xacro or .urdf robot description into a USD stage.

Modify this file:

by adding "omni.isaac.sensor" = {} at the end.

Gait phases:
  1. Stance: the foot is in contact with the ground
    1. Loading: the foot transitions from non-contact to contact
    2. Travel: foot follows the opposite vector of the command
    3. Pre-swing: as the foot nears the end of its range of motion, it waits for 4 feet to be in contact with the ground
  2. Swing: the food is in the air
    1. Unloading: the foot transitions from non-contact to contact
    2. Position: the foot lifts to a minimum height for clearance while maintaining minimal distance from the center of the robot.
    3. Travel: moves to maximize its stride
    4. Pre-stance: waits until only 3 feet are in contact with the ground

Friday, February 9, 2024

Micro-ROS adventure

 PS C:\Program Files\uagent_superbuild\bin> .\MicroXRCEAgent.exe udp4 -p 2018 -v 6

Monday, January 15, 2024

Pinky: ROSberry pi controller


Raspberry Pi 4 setup:
sudo apt update
sudo apt upgrade
sudo raspi-config
    enable VNC 1080p resolution
    expand filesystem
    enable SPI
#Increasing Swap because this RPI has only 2gb of memory\
sudo dphys-swapfile swapoff
sudo nano /etc/dphys-swapfile
sudo dphys-swapfile setup
sudo dphys-swapfile swapon
sudo reboot
Setting up Docker:
curl -fsSL -o
sudo sh ./ 
sudo docker run -it  ros:iron-ros-core #use --privileged to use spi and gpio
# UPDATE ^ to add port forwarding for micro ros
sudo docker run -it --privileged -p 8888:8888/udp --name c4 control4 /bin/bash

sudo docker rename e8b9c16b0544 ROS-TEST
sudo docker exec -ti ROS-TEST /bin/bash
apt update
apt install apt-utils nano whiptail apt install python3-colcon-common-extensions python3-rosdep git
python3 -m pip install gpiozero
apt upgrade
mkdir -p ~/colcon_venv/src
cd ~/colcon_venv/
# Make a virtual env and activate it
#apt install python3.10-venv #from raspberry pi
#python3 -m venv env #from raspberry pi
#Modified from ROS
virtualenv -p python3 --system-site-packages ./rosvenv
source ./rosvenv/bin/activate
#Make sure that colcon doesn’t try to build the venv
touch ./rosvenv/COLCON_IGNORE
pip install gpiozero adafruit-circuitpython-mcp3xxx

Building a package:
~/colcon_venv/src# ros2 pkg create --build-type ament_python --license Apache-2.0 CustomController
#Warnings thrown:/usr/lib/python3/dist-packages/setuptools/command/ SetuptoolsDeprecationWarning: install is deprecated. Use build and pip and other standards-based tools.
#setuptools version 59.6.0

ROS Build
#Modify /root/colcon_venv/src/control_package/control_package/ with codes below.
colcon build --packages-select control_package
source install/local_setup.bash
ros2 run control_package control_node

Docker reentry
  1. sudo docker ps -a
  2. sudo docker start existing_container_ID_or_name
    1. sudo docker start c4
  3. sudo docker exec -it existing_container_ID_or_name /bin/bash
    1. sudo docker exec -it c4 /bin/bash
  4. check version with "printenv ROS_DISTRO"
ROS2 reentry:
  1. cd ~/colcon_venv/
  2. source ./rosvenv/bin/activate
  3. MicroRos
    1. source install/local_setup.bash
    2. ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888

3D printed Controller on the workbench:

Custom Pi Hat:
The joystick has 3 analog outputs. These run through an MCP3008


  • Up/down is chan1, 
  • Left/Right is chan2
  • Twist is chan0. 
#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from std_msgs.msg import String

import RPi.GPIO
#from gpiozero import MCP3008, Button
import adafruit_mcp3xxx.mcp3008 as MCP
from adafruit_mcp3xxx.analog_in import AnalogIn
import busio
import digitalio
import board

import time

def gpio_callback(data, channels, pub):
    # Process GPIO inputs here

    analog_values = [mcp[i].value for i in range(3)]
    # Display the input values
    print(f"Analog Inputs (MCP3008): {analog_values}")

def main():

    # Set up MCP3008 on SPI0 with CE0 as the chip select
    # create the spi bus
    spi = busio.SPI(clock=board.SCK, MISO=board.MISO, MOSI=board.MOSI)

    # create the cs (chip select)
    cs = digitalio.DigitalInOut(board.D24)

    # create the mcp object
    mcp = MCP.MCP3008(spi, cs)
    #gpio0 mcp = [MCP3008(channel=i) for i in range(3)]

    # define the channel pins
    channel_pins = [MCP.P0, MCP.P1, MCP.P2]

    # create an array of analog input channels
    channels = [AnalogIn(mcp, pin) for pin in channel_pins]

    # iterate over the channels
    for channel, chan in enumerate(channels):
        print(f'Raw ADC Value (Channel {channel}): ', chan.value)
        print(f'ADC Voltage (Channel {channel}): {chan.voltage}V')

    node = rclpy.create_node('joy_publisher')

    # create a Joy publisher
    joy_publisher = node.create_publisher(Joy, 'joy', 10)

    # set the publishing rate
    publish_rate = 1  # 1 Hz
    timer_period = 1.0 / publish_rate

    def publish_joy():
        # create a Joy message
        joy_msg = Joy()

        # iterate over the channels and add readings to the axes
        for channel, chan in enumerate(channels):

        # publish the Joy message

    timer = node.create_timer(timer_period, publish_joy)


    # shutdown when done


if __name__ == '__main__':
This was the first input challenge. It took all day to get the RPI/Docker/ROS to cooperate. It looks like my twist is wired backward and it tampers with other readings when i bring it high.


  • Joystick button
    • GPIO 12
  • D-Pad
    • Up
    • Down
    • Left
    • Right
    • Center
    • Rotary Encoder
import RPi.GPIO as GPIO
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from std_msgs.msg import Header
from builtin_interfaces.msg import Time
from adafruit_mcp3xxx.analog_in import AnalogIn
import adafruit_mcp3xxx.mcp3008 as MCP
import busio
import digitalio
import board
import time

class JoyPublisherNode(Node):
    def __init__(self):

        # Set up MCP3008 on SPI0 with CE0 as the chip select
        spi = busio.SPI(clock=board.SCK, MISO=board.MISO, MOSI=board.MOSI)
        cs = digitalio.DigitalInOut(board.D24)
        mcp = MCP.MCP3008(spi, cs)

        # Define the channel pins
        channel_pins = [MCP.P0, MCP.P1, MCP.P2]
        self.channels = [AnalogIn(mcp, pin) for pin in channel_pins]

        # Define GPIO pins for buttons
        self.button_pins = [12, 26, 17, 27, 22, 4]
        GPIO.setup(self.button_pins, GPIO.IN, pull_up_down=GPIO.PUD_UP)

        # Define GPIO pins for rotary encoder
        self.encoder_pin_a = 19  # CW
        self.encoder_pin_b = 13  # DT - these might need to be switched
        GPIO.setup(self.encoder_pin_a, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        GPIO.setup(self.encoder_pin_b, GPIO.IN, pull_up_down=GPIO.PUD_UP)
        GPIO.add_event_detect(self.encoder_pin_a, GPIO.BOTH, callback=self.encoder_callback)

        # Create a Joy publisher
        self.joy_publisher = self.create_publisher(Joy, 'joy', 10)

        # Set the publishing rate
        publish_rate = 1  # 1 Hz
        timer_period = 1.0 / publish_rate
        self.timer = self.create_timer(timer_period, self.publish_joy)

        self.last_encoder_state = GPIO.input(self.encoder_pin_a)
        self.encoder_counter = 0 # Initialize the encoder counter

    def publish_joy(self):
        # Create a Joy message
        joy_msg = Joy()
        joy_msg.header = Header()
        joy_msg.header.stamp = self.get_clock().now().to_msg()
        joy_msg.header.frame_id = 'base_link'

        # Read analog inputs
        joy_msg.axes.extend([chan.value for chan in self.channels])

        # Read button states
        joy_msg.buttons.extend(GPIO.input(button_pin) for button_pin in self.button_pins)

        # No rotation initially

        # Publish the Joy message

    def encoder_callback(self, channel):
        # Callback function for rotary encoder
        a_state = GPIO.input(self.encoder_pin_a)
        b_state = GPIO.input(self.encoder_pin_b)

        if a_state != self.last_encoder_state:
            if b_state != a_state:
                self.last_encoder_state = a_state
                # Counter-clockwise rotation
                self.last_encoder_state = a_state
                # Clockwise rotation

    def publish_encoder_rotation(self, rotation):
        # Create a Joy message for rotary encoder
        joy_msg = Joy()
        joy_msg.header = Header()
        joy_msg.header.stamp = self.get_clock().now().to_msg()
        joy_msg.header.frame_id = 'base_link'

        # Read analog inputs
        joy_msg.axes.extend([chan.value for chan in self.channels])

        # Read button states
        joy_msg.buttons.extend(GPIO.input(button_pin) for button_pin in self.button_pins)

       # Update the encoder counter based on rotation direction
        if rotation > 0:
            self.encoder_counter += 15
        elif rotation < 0:
            self.encoder_counter -= 15

        # Set axes for rotary encoder

        # Publish the Joy message

def main():
    joy_publisher_node = JoyPublisherNode()

if __name__ == '__main__':

I had problems with the twist joystick impacting the other potentiometers. This was because my particular stick had the colors off from what I expected.
As you can see, the resistance across the red-black was 5k, not 10k.

Below you can see, that I found the full resistance. I used red as 3.3V, white to ground, and black as the signal.

Setting up a second network adapter as a wifi access point for Pinky's onboard microcontrollers to connect to:

I plugged in a TP-link TL-WN725N v2 USB dongle. Some problems I ran into:
  • It is coming up as WLAN0 after reboots, this is unwanted behavior. So let's make it WLAN1 and the onboard should be WLAN0. 
    • Persistent Interface Naming

      If your built-in WiFi adapter is being labeled wlan1 instead of wlan0 and you want to ensure consistent naming, you can create a custom udev rule to assign the names based on the MAC address.

      1. Find the MAC Address:

        Identify the MAC address of your built-in WiFi adapter using the steps above.

      2. Create a udev Rule:

        Create a new udev rule file:

        sudo nano /etc/udev/rules.d/70-persistent-net.rules

        Add a rule to assign a specific name based on the MAC address. Replace xx:xx:xx:xx:xx:xx with the MAC address of your built-in WiFi adapter:

        SUBSYSTEM=="net", ACTION=="add", ATTR{address}=="xx:xx:xx:xx:xx:xx", NAME="wlan0"

        You can also add a rule for your USB adapter if needed:

        SUBSYSTEM=="net", ACTION=="add", ATTR{address}=="yy:yy:yy:yy:yy:yy", NAME="wlan1"
      3. Reload udev Rules and Reboot:

        Reload the udev rules and reboot the system:

        sudo udevadm control --reload-rules sudo reboot

      After rebooting, your built-in WiFi adapter should be consistently named wlan0.

  • The desktop network manager is telling me that AP mode is not possible with the new dongle. I may have to switch to having the onboard be the AP and the new adapter be my internet connection.
    • I switched to an edimax wifi dongle and then did this:

      Step-by-Step Setup

      1. Update and Install Required Packages:

        sudo apt-get update sudo apt-get upgrade sudo apt-get install hostapd dnsmasq iptables
      2. Configure Static IP for wlan1:

        Edit /etc/dhcpcd.conf:

        sudo nano /etc/dhcpcd.conf


        interface wlan1 static ip_address= nohook wpa_supplicant
      3. Configure dnsmasq:

        Backup existing configuration and create a new one:

        sudo mv /etc/dnsmasq.conf /etc/dnsmasq.conf.orig sudo nano /etc/dnsmasq.conf


        interface=wlan1 dhcp-range=,,,24h
      4. Configure hostapd:

        Create or edit the configuration file:

        sudo nano /etc/hostapd/hostapd.conf


        interface=wlan1 driver=nl80211 ssid=MyNetwork hw_mode=g channel=7 wmm_enabled=0 macaddr_acl=0 auth_algs=1 ignore_broadcast_ssid=0 wpa=2 wpa_passphrase=MyPassword wpa_key_mgmt=WPA-PSK rsn_pairwise=CCMP

        Edit /etc/default/hostapd to set the configuration file path:

      5. Enable and Start Services:

        sudo systemctl unmask hostapd sudo systemctl enable hostapd sudo systemctl start hostapd sudo systemctl restart dnsmasq
      6. Enable IP Forwarding:

        Edit /etc/sysctl.conf:

        sudo nano /etc/sysctl.conf

        Uncomment or add:


        Apply the changes:

        sudo sysctl -p
      7. Configure NAT with iptables:

        sudo iptables -t nat -A POSTROUTING -o wlan0 -j MASQUERADE sudo sh -c "iptables-save > /etc/iptables.ipv4.nat"

        Add to /etc/rc.local to restore iptables rules on boot:

        iptables-restore < /etc/iptables.ipv4.nat
  • Now for testing with my ros2 in docker and micropython on my microcontroller...
    • Build micro-ros for this install
    • I can see my ESP32 connected with: 
      • cat /var/lib/misc/dnsmasq.leases
    • I can see packets over port 8888 with:
      • sudo tcpdump -v -i wlan1 udp port 8888
    • ESP32 devkit and esp32CAM enter error loop after the memory allocator is initialized.
    • Find port conflicts with netstat and lsof
    • SOLVED:The micropython agent on the controller must be started before the microcontrollers are powered on. Otherwise, they will go into an error loop when initializing support.
  • Getting display to work from a container
    • export DISPLAY=:0