Home

linorobot2_hardware with esp32 and pico support

This is a fork of the linorobot/linorobot2_hardware project which builds firmware for micro-controllers to control mobile robots based on micro-ROS. The esp32/esp32-s2/esp32-s3 and pico support has been merged to linorobot upstream. This fork has more drivers/features not available in the upstream yet.

Why esp32

The WIFI capability of the esp32 can be used to build low-cost mobile robots with navigation/slam on ROS2.

remote development - develop on your desktop with powerful PlatformIO IDE for VSCode.
remote firmware update - with ArduinoOTA.
remote debug messages - with remote syslog.
remote lidar server - lidar raw data is forwarded to a server to publish laser scan data. Ony LDlidar LD19 is supported.
remote ROS2/NAV2 stack - with micro-ROS wifi transport.
no robot computer is required - a single esp32 will serve the robot.

In traditional robot builds, there are robot computers (such as raspberry pi, jetson nono or mini PC) running the build tools and ROS2/NAV2 navigation stack. With the WIFI capability of the esp32, the robot firmware can be developed on your laptop or desktop PC. After the firmware is written to the esp32 using USB cable, the following firmware updates can be performed remotely using ArduinoOTA. The debug messages can be read using remote syslog server. Using the WIFI transport of the micro-ROS, the navigation packages NAV2 / SLAM and visualization tool RVIZ2 can be served on your laptop or desktop PC. This is a more comfortable development environment than the more restricted robot computers. No robot computer is required on the robot now. Just an esp32 micro-controller will serve. And the cost of the robot will be a little cheaper.

If you are new to esp32, you may check the guide to esp32.

Workflow

Install the software.
Test micro-ROS agent connection.
Design your robot and customize configuration file.
Build the robot.
Test the motors and encoder. Test the sensors. Test the Lidar.
Launch robot bringup.
Use a keyboard to move the robot. Use a gamepad to move the robot.
Calibrate the magnetometer.
Launch slam and save the map.
Launch navigation with the map.

Suggested robot configuration for beginners

If you are new to ROS/micro-ROS, you should start from the basic. The first robot should be simple, compact (<30cm), light weight (<3Kg) and most importantly FUN. Since it is light weight, you won't need high power motors and battery for this first robot.

Lidar: Ldlidar LD19
Controller: esp32 or esp32-s3 with wifi transport. No robot computer. Run ROS2/NAV2 stack on your PC remotely.
IMU: MPU6050
Magnetometer: QMC5883L - optional
Battery: 12.6V 3S Li-ion battery
Motor: 12V 150-200RPM brushed gear motor with hall encoder
Motor driver: TB6612
Robot type: mecanum
Chassis: MC200 Mecanum robot chassis. It includes four gear motors with encoders and Mecanum wheels.

Or for differential drive,

Robot type: 2wd
Chassis: TP200 robot tank chassis. It includes two gear motors with encoders.

A Lidar is required to run navigation and SLAM. LD19 is suggested for its ease of use, cost and performance. Only LD19/LD06/STL27L are supported in my lidar wifi forwarder for esp32. The laser scan data is forwarded from esp32 to Lidar driver on host PC using UDP protocol. The ROS2 navigation/SLAM stack will run your PC. No robot computer is required on the robot. There will be fewer configuration issues. If you use the other Lidar, you will need a robot computer (such as a rpi or laptop) on the robot to connect the Lidar. In this case, Pico or esp32-s3 are preferred for the controller. The USB CDC (ttyACM0) on them are faster than the serial USB bridge (ttyUSB0) on esp32.

An IMU (with or without magnetometer) is recommended. An IMU is often used to improve odometry accuracy. However, an inaccurate IMU or mounting issues may cause problem to beginners. You should install an IMU, but you may disable it during initial SLAM tests. You can enable the IMU later. The cheap MPU6050 will serve well. There is no need to use more expensive ones in most cases.

Sample robot build

Mecanum drive robot built with rpi5, pico, MC200 chassis and ld19 lidar

1714318119811

Video of linorobot2 Mecanum drive with Ros2 obstacle avoidance on Raspi5 and pico

Robot tank built with Waveshare General Driver for Robot esp32 wifi, TP200 chassis and ld19 lidar

IMG_20240203_224554 IMG_20240203_224519

Video of A ROS2 robot tank built on esp32. Navigating..

Supported ROS2 distribution - Humble

Supported Linux distribution - Ubuntu 22.04

Supported micro-controllers

teensy - The teensy dose not support WIFI.
pico - The pico does not support micro-ROS WIFI transport.
esp32 - support both serial and wifi transport. There are many variants. Only esp32 esp32-s2 and esp32-s3 are supported. Some variants, such as esp32-c3, are not supported.

Supported robot types

2wd - 2 wheel drive robot.
4wd - 4 wheel drive robot.
mecanum - Mecanum drive robot.

The PID algorithm is used to control the motors speed.

Install the software

Install essential build tools. Remove brltty package which interferes with CH340 USB serial on some esp32 boards.

sudo apt remove brltty -y
sudo apt install python3-venv build-essential cmake git curl -y

Install ROS2. Make sure the ROS2 setup is sourced.

sudo apt update && sudo apt install -y curl
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 -y ros-humble-desktop ros-dev-tools python3-colcon-common-extensions python3-pip
sudo apt update
sudo rosdep init
rosdep update
source /opt/ros/humble/setup.bash
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc

Install linorobot2. Enter 'y' to the questions. This will build the micro-ROS agent and install the lidar driver.

source /opt/ros/humble/setup.bash
cd /tmp
wget https://github.com/hippo5329/linorobot2/raw/humble/install_linorobot2.bash
bash install_linorobot2.bash 2wd ld19
source ~/.bashrc

Install PlatformIO IDE for VSCode (which will install PlatformIO CLI automatically). Or install only PlatformIO CLI with the following,

curl -fsSL -o get-platformio.py https://raw.githubusercontent.com/platformio/platformio-core-installer/master/get-platformio.py
python3 get-platformio.py

Add platformio to $PATH in .bashrc.

echo "PATH=\"\$PATH:\$HOME/.platformio/penv/bin\"" >> ~/.bashrc
source ~/.bashrc

Install PlatformIO udev rules.

curl -fsSL https://raw.githubusercontent.com/platformio/platformio-core/develop/platformio/assets/system/99-platformio-udev.rules | sudo tee /etc/udev/rules.d/99-platformio-udev.rules
sudo service udev restart
sudo usermod -a -G dialout $USER
sudo usermod -a -G plugdev $USER

Clone the source.

git clone https://github.com/hippo5329/linorobot2_hardware.git

Try build to verify the installation.

cd linorobot2_hardware/firmware
pio run -e esp32

Configuration examples

The firmware is configured with a configuration file in the linorobot2_hardware/config/custom/ directory. There is a configuration file for each robot. The configuration file contains various C macro to define the options. The following are examples which you may use as a template to create your own.

esp32_config.h - serial transport, 2WD, esp32 dev board.
esp32_wifi_config.h - wifi transport, 2WD, esp32 dev board.
esp32s2_config.h - serial transport, 2WD, esp32-s2 dev board.
esp32_wifi_config.h - wifi transport, 2WD, esp32-s2 dev board.
esp32s3_config.h - serial transport, 2WD, esp32-s3 dev board.
esp32s3_wifi_config.h - wifi transport, 2WD, esp32-s3 dev board.
pico_config.h - serial transport, 2WD, pico.
gendrv_config.h - serial transport, 2WD, esp32 waveshare general driver for robots is an all-in-one esp32 board. It can save some work if you are new to esp32 and hardware stuff.
gendrv_wifi_config.h - wifi transport, 2WD, esp32 waveshare general driver for robots.

Only gendrv and gendrv_wifi have the IMU and magnetometer configured. The other examples do not have IMU configured.

Test micro-ROS agent connection

Choose the configuration you will use to build your robot.

esp32 with micro-ROS serial transport

Build and upload with USB cable connected to the esp32 board.

cd firmware
pio run -e esp32 -t upload

After upload and reset, it will try to connect to the micro-ROS agent with serial transport.

# Open another terminal. Run the micro-ROS agent
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 --baudrate 921600

After it is connected to the agent, you may check the topics it published.

ros2 node list
ros2 node info /esp32
ros2 topic list
ros2 topic echo /imu/data
ros2 topic hz /imu/data

Since we didn't connect an IMU to the esp32, the IMU reading will be zero.

pico with micro-ROS serial transport

cd firmware
pio run -e pico -t upload
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0

esp32 with micro-ROS wifi transport

Update the configuration file with your wifi keys and servers IP.

config/custom/esp32_wifi_config.h

#define AGENT_IP { 192, 168, 1, 100 }  // eg IP of the desktop computer
#define WIFI_AP_LIST {{"WIFI_SSID", "WIFI_PASSWORD"}, {NULL}}
#define SYSLOG_SERVER { 192, 168, 1, 100 }  // eg IP of the desktop computer
#define LIDAR_SERVER { 192, 168, 1, 100 }  // eg IP of the desktop computer

Build and upload with USB cable connected to the esp32 board.

cd firmware
pio run -e esp32_wifi -t upload
# Open another terminal. Run the micro-ROS agent
ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888

After upload and reset, it will try to connect to the wifi and micro-ROS agent. The IP address of esp32 will be displayed in the micro-ROS agent message.

[1686587945.807239] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 8888
[1686587945.807411] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1686588315.310850] info     | Root.cpp           | create_client            | create                 | client_key: 0x0C7BC5A9, session_id: 0x81
[1686588315.310892] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0x0C7BC5A9, address: 192.168.1.101:47138
[1686588315.327314] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0x0C7BC5A9, participant_id: 0x000(1)
[1686588315.332840] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0x0C7BC5A9, topic_id: 0x000(2), participant_

Serial transport baud rate requirement

For linorobot2, the serial baud rate needs to be high as 921600 (default) for esp32 to support serial transport micro-ROS messages running the control loop at 50Hz. While teensy, pico and esp32-s3 do not have such issue with their build-in USB CDC protocol (ttyACM0). The wifi transport does not have such issue unless the wifi signal is too weak and lossy. If the data transfer rate or the robot computer is too slow, the robot may become unstable. This can be mandated by lower the rate of the control loop.

The rate of control loop can be monitored with the following command. It should be close the 50.

ros2 topic hz /imu/data

USB ports on esp32-s2 and esp32-s3

The esp32-s2 and esp32-s3 have onchip USB CDC devices, which will be faster than serial USB bridge like CP2102. They will become default serial port /dev/ttyACM0 when enabled with the ARDUINO_USB_CDC_ON_BOOT build flag. There may be two USB ports on the dev boards, one marked as "USB" (the onchip CDC) and the other marked as "COM" (the usb uart bridge). You should use the one marked as "USB. If there is only one USB port, you should check the schematic to find out which is connected.

esp32-s3

USB - /dev/ttyACM0 default, enabled with the ARDUINO_USB_CDC_ON_BOOT build flag
COM - /dev/ttyACM0

esp32-s2

USB - /dev/ttyACM0 default, enabled with the ARDUINO_USB_CDC_ON_BOOT build flag
COM - /dev/ttyUSB0

Build flag,

build_flags =
    -I ../config
    -D ARDUINO_USB_CDC_ON_BOOT
    -D USE_ESP32S3_CONFIG

Custom configuration file

It is recommended to create a custom configuration for each of your robots. This will minimize the future merge conflicts from the upstream. There are barriers in system configuration files to protect user configuration from maintainers configuration. User configurations should be added between these barriers. The following is a custom configuration copied from esp32 example configuration, which built with serial transport, 2WD, esp32 dev board.

Take the included example configuration as a template and add your custom configuration file to config/custom/myrobot_config.h.
Add an entry to config/config.h between the user barriers near the end.
Add an entry to the end of firmware/platformio.ini, below the user barrier.
Configure the URDF and velocity smoother param in the linorobot2 workspace.

Copy and edit the following files. Take esp32 as example. Use the other example configuration matches your target.

cp config/custom/esp32_config.h config/custom/myrobot_config.h

Configure the i/o pins assignments, motors, drivers, encoder, IMU magnetometer sensors (if any), robot size. If you use wifi, wifi keys and agent ip.

config/custom/myrobot_config.h // replace esp32 with myrobot.

#ifndef MYROBOT_CONFIG_H
#define MYROBOT_CONFIG_H
...
#define USE_MPU6050_IMU
...
#define DEVICE_HOSTNAME "myrobot"
...
#define NODE_NAME "myrobot"
// #define TOPIC_PREFIX "myrobot/"
...
#endif

config/config.h // add myrbot

// add maintainer configurations above this line
// this barrier helps to reduce user merge conflict
// add user configurations below this line

// Copy esp32 and add myrobot here
#ifdef USE_MYROBOT_CONFIG
    #include "custom/myrobot_config.h"
#endif

// add user configurations above this line
// this should be the last one
#ifndef LINO_BASE

firmware/platformio.ini // add myrobot

; add maintainer configurations above this line
; this barrier helps to reduce user merge conflict
; add user configurations below this line

; Copy esp32 and add myrobot here
[env:myrobot]
platform = espressif32
...
build_flags =
    -I ../config
    -D __PGMSPACE_H_
    -D USE_MYROBOT_CONFIG

Build and upload with USB cable connected to the esp32 board.

cd firmware
pio run -e myrobot -t upload

Git is a very important development management tool. You should stage your changes to git and commit to save your changes. So that you can sync with upstream with pull, merge or rebase. You may create your own fork on github. Check this Git Guide and Fork a repository.

Configure the linorobot2

Configure the URDF xarco which matches your target. eg.

ros2_ws/src/linorobot2/linorobot2_description/urdf/2wd_properties.urdf.xacro

Supported motor drivers

2 Direction pins(INA, INB) and 1 PWM(ENABLE) pin - L298, L293, VNH5019, TB6612
1 Direction pin(INA) and 1 PWM(ENABLE) pin
2 PWM pins (INA, INB) - BTS7960, L298, DRV8871, A4950 (<40V), L9110 DRV8833 (<10V), TB6612 (<15V) with PWM pins pull-high
ESC - brush-less motor

The BTS7960_MOTOR_DRIVER (2 PWM pins INA, INB) is preferred to reduce the number of I/O pins used. If you use TB6612, you may pull high the PWM and STBY pins to use this mode (INA, INB). The preferred PWM rate is 20KHz.

The PCA9685 i2c 16ch pwm controller can be used as pwm/output extender. But the PWM rate is limited to 100Hz. It is too slow as we run the control loop at 50Hz.

L298 is bipolar device, which has higher energy loss and generates more heat. Modern MOSFET devices such as TB6612 are preferred.

The 6 pins headers on motors with encoders and the controller PCBs may have different pins assignments. Check the specifications before wiring.

Some encoders are 5V logic level. The esp32 and pico are 3.3V tolerant. Logic level shifter should be used in this case.

Supported sensors

IMU - GY85, MPU6050, MPU9150, MPU9250, QMI8658.
magnetometer - HMC5883L, AK8963, AK8975, AK09918, QMC5883L.
encoder - PCNT (hardware pulse counter on esp32), PIO on pico, interrupt driven on Teensy
battery - on-chip ADC, INA219
ultrasonic - HC-SR04
lidar forwarder - ldlidar LD19/LD06/STL27L

A simple battery voltage monitor built with resistors voltage divider and on-chip ADC is very helpful to diagnosis motors and battery issus. When motors start running, there will be a strong current which causes the battery voltage dropped. If the voltage drops too much, it might be a system problem.

Most sensors have on board LM1117 5V to 3.3V LDO. Some do not. Check out carefully before connect the power.

The IMU and magnetometer senors are directional. They must be mounted in correct orientation. Some IMU performs self-calibration after power on or reset. It needs to be hold still during self-calibration. Incorrect self-calibration may cause the fake rotation or shifting in laser scan even when the robot does not move.

If you use MPU9250/MPU9150, you will need to connect EDA to SDA and ECL to SCL on the module, to access the magnetometer. The firmware, test_motors and test_sensors utilities will run i2cdetect and output to serial after reset. You may check the I2C devices detected.

MPU6050 no mag, i2c addr 68
MPU9150 = MPU6050 + AK8975, i2c addr 68 + 0c
MPU6500 no mag, i2c addr 68
MPU9250 = MPU6500 + AK8963, i2c addr 68 + 0c

These MPU9250/MPU6500/MPU9150 modules look the same. Check the chip carefully. Some do not have the magnetometers inside.

If the IMU is not detected, the firmware will stop and flash the LED 3 times.

I2C bus pull up requirement

The esp32/pico require external 2K Ohm resisters pull up on the I2C SDA SCL line to the 3.3V output of the esp32/pico. Because I2C interfaces of esp32/pico can be assign to other I/O pins, there are no I2C pull up on the esp32/pico modules.

controller boards

all-in-one: e.g. Waveshare General Driver for Robots is a nice robot control board for beginners. It integrates the esp32, TB6612, IMU QMI8658, magnetometer AK09918 and 4 pins Ldlidar connector. Use gendrv_wifi configuration. Update the wifi keys and agent ip. Add a jumper wire from LIDAR to IO4 on the middle header. Plug and play.
build-it-yourself: you may build controller on prototyping boards or design your own PCBs. Please check the esp32 pinout for available pins. The popular esp32 modules may have 30 pins or 38 pins. Some have external WIFI antenna connectors, which will be helpful to extended ranges or when the controller is covered by metal chassis.

Build the robot

For teensy, please check teensy build.

If you have robot computer (such as pi or laptop) on the robot, power the esp32/pico from the USB and power the IMU from the 5V output of esp32/pico. The motors are very noisy. And the IMU, which contains precision ADCs, is subject to power noise.
Use a step down converter or LDO to 3.3V for the motor drivers and encoders. Do not use the 3.3V output of esp32/pico for them. The I/O pins on the esp32/pico are 3.3V tolerant. Use logic level shifter to other logic level.
Add 220-470uF low ESR capacitors to every power line to reduce the power noise. Add the capacitors for motor power close to the motor drivers.
Mount the IMU and magnetometer in correct orientation. Mount the magnetometer away from the motors, Lidar and battery as far as possible. Use plastic mount for magnetometer.
Power the motors from battery. Do not use power regulator to motors. In special cases, when the battery voltage is higher than the rating of motors, eg 24V battery to 12V motors, reduce the PWM_MAX to 1/2 the value in the configuration file. Avoid PWM controller such as PCA9685, though it is supported. The PWM frequency limit of 100Hz on PCA9685 is too slow while we run the control loop at 50Hz. The default/preferred PWM frequency is 20KHz.

Robot orientation

Robot Orientation:

-------------FRONT-------------

WHEEL1 WHEEL2 (2WD)

WHEEL3 WHEEL4 (4WD)

--------------BACK--------------

Test the motors and sensors

You should test the motors and all the sensors before and after the robot hardware is assembled. There are diagnosis utilities to test the motors and sensors, including encoder, IMU, MAG, battery and ultrasonic range sensors. The syslog and OTA are supported in these utilities. You may switch between firmware and utilities with OTA and read the result from syslog without an USB cable connected.

Test the motors and encoders

Before proceeding, ensure that your robot is elevated and the wheels aren't touching the ground.

The test_motors utility will spin the motors forward and backward alternately every 10 seconds without user intervention. The motors will run one by one after power on. Then it will display the motors linear velocity, angular velocity and distance to full stop. Make sure the motors are running at the correct direction and the encoders get the correct speed reading, which is the maximum speed of the motors. You should calibrate the rpm reading with a digital tachometer and adjust the COUNTS_PER_REV1..4. If you have enough space, you may put the robot on the ground to watch it spins. The test_motors utility will include the firmware/platformio.ini. There is no need to edit the test_motors/platformio.ini for your custom configuration.

Build and upload with

cd test_motors
pio run -e myrobot -t upload
pio device monitor -e myrobot

Result with USE_SHORT_BRAKE

MOTOR1 FWD RPM    155.2     -8.3      0.0      0.0
MOTOR1 FWD RPM    160.9      0.0      0.0      0.0
MOTOR1 FWD RPM    160.6      0.0      0.0      0.0
MOTOR1 FWD RPM    161.2      0.0      0.0      0.0
MOTOR1 FWD RPM    160.9      0.0      0.0      0.0
MOTOR1 FWD RPM    160.7      0.0      0.0      0.0
MOTOR1 FWD RPM    158.6      0.0      0.0      0.0
MOTOR1 FWD RPM    157.8      0.0      0.0      0.0
MOTOR1 SPEED   0.46 m/s   4.13 rad/s STOP  0.014 m
MOTOR2 FWD RPM      4.8    143.2      0.0      0.0
MOTOR2 FWD RPM      0.0    166.9      0.0      0.0
MOTOR2 FWD RPM      0.0    166.9      0.0      0.0
MOTOR2 FWD RPM      0.0    166.8      0.0      0.0
MOTOR2 FWD RPM      0.0    166.8      0.0      0.0
MOTOR2 FWD RPM      0.0    166.8      0.0      0.0
MOTOR2 FWD RPM      0.0    166.8      0.0      0.0
MOTOR2 FWD RPM      0.0    166.5      0.0      0.0
MOTOR2 SPEED   0.49 m/s   4.36 rad/s STOP  0.024 m
MOTOR1 REV RPM   -153.1      8.2      0.0      0.0
MOTOR1 REV RPM   -159.8      0.0      0.0      0.0
MOTOR1 REV RPM   -159.4      0.0      0.0      0.0
MOTOR1 REV RPM   -159.9      0.0      0.0      0.0
MOTOR1 REV RPM   -159.8      0.0      0.0      0.0
MOTOR1 REV RPM   -159.8      0.0      0.0      0.0
MOTOR1 REV RPM   -159.8      0.0      0.0      0.0
MOTOR1 REV RPM   -159.9      0.0      0.0      0.0
MOTOR1 SPEED  -0.47 m/s  -4.19 rad/s STOP -0.014 m
MOTOR2 REV RPM     -4.7   -122.6      0.0      0.0
MOTOR2 REV RPM      0.0   -162.1      0.0      0.0
MOTOR2 REV RPM      0.0   -162.0      0.0      0.0
MOTOR2 REV RPM      0.0   -162.0      0.0      0.0
MOTOR2 REV RPM      0.0   -162.2      0.0      0.0
MOTOR2 REV RPM      0.0   -162.7      0.0      0.0
MOTOR2 REV RPM      0.0   -162.4      0.0      0.0
MOTOR2 REV RPM      0.0   -162.3      0.0      0.0
MOTOR2 SPEED  -0.48 m/s  -4.25 rad/s STOP -0.023 m
MOTOR1 FWD RPM    154.6     -7.8      0.0      0.0

Result without USE_SHORT_BRAKE

MOTOR1 SPEED   0.46 m/s   4.10 rad/s STOP  0.135 m
MOTOR2 SPEED   0.49 m/s   4.40 rad/s STOP  0.171 m

You can see that the distance to full stop is much longer without USE_SHORT_BRAKE. The robot might collide and it can be dangerous in some cases. The short brake mode (aka slow decay) also provides better speed control. Check out this current decay mode.

Test the other sensors

The test_sensors utility will display IMU, MAG, battery and range sensors data every second, in x y z sequence. The test_sensors utility will include the firmware/platformio.ini. There is no need to edit the test_sensors/platformio.ini for your custom configuration.

Build and upload with

cd test_sensors
pio run -e myrobot -t upload
pio device monitor -e myrobot

Output

ACC -0.25 -0.32  9.01 GYR  0.00  0.00  0.00 MAG 12.45 42.45 -52.65
 BAT 11.10V RANGE  0.00m

Make sure the ACC X (front) Y (left) Z (up) direction of th IMU is correct. The average gravitational pull of the Earth is 9.8 meters per second squared (m/s2). In normal stand, the Z reading should be the largest positive around 9-10, while the X Y are small. Flip the robot front up 90 degrees, the X reading should be the largest positive. Flip the robot left up 90 degrees, the Y reading should be the largest positive. Shake the robot forward and backward quickly. And rotate the robot in all direction to see the IMU reading changed. The MAG reading should be in the range of +-50 uTesla. Pointing the front to north, the reading of x should be positive and close to the maximum. Pointing left side to north, the reading of y should be positive and close to the maximum. The opposite sides should be negative. Make sure the IMU and MAG sensors reading are in the correct orientation.

If you mount the IMU in different orientation (eg upside down), you will need to modify the imu link in URDF.

Test the robot at maximum velocity and acceleration with full load

While the test_motor utility finds the no load maximum speed without the wheels touching ground, this utility finds maximum velocity and acceleration with the robot grounded with full load. Make sure there is enough space for the robot and put the robot on the ground with full load. The robot will run forward/backward and spin counterclockwise/clockwise twice at maximum power for one second then 1/2 power and 1/4 power. It is an important test of the robot motors and battery performance. This is the hard limit. You should configure the NAV2 velocity_smoother based on the test result. Battery voltage is monitored during the tests. Battery voltage will drop due to the surge current from the motors. High voltage drop should be checked.

cd test_acc
pio run -e myrobot -t upload
pio device monitor -e myrobot

Output from moving forward and backward at full power,

MAX PWM 1023.0 -1023.0
MAX VEL   0.55   0.00 m/s    0.23 rad/s
MIN VEL  -0.54   0.00 m/s   -0.15 rad/s
MAX ACC  10.50   0.00 m/s2    7.68 rad/s2
MIN ACC -10.39   0.00 m/s2  -11.42 rad/s2
IMU ACC   3.45  -4.68 m/s2
time to 0.9x max vel   0.14 sec
distance to stop   0.02 m
BAT 11.47V MIN 10.79V

Output from spinning counterclockwise and clockwise at full power,

MAX PWM 1023.0 -1023.0
MAX VEL   0.02   0.00 m/s    4.81 rad/s
MIN VEL  -0.01   0.00 m/s   -4.66 rad/s
MAX ACC   1.26   0.00 m/s2   87.94 rad/s2
MIN ACC  -0.85   0.00 m/s2  -91.83 rad/s2
IMU ACC   1.88  -2.22 m/s2
time to 0.9x max vel   0.10 sec
distance to stop   0.20 rad
BAT 11.46V MIN 10.78V

Bring up the robot

Build and upload the firmware.

cd firmware
pio run -e myrobot -t upload

teensy, esp32-s3 or pico with serial transport and ld19 lidar on the robot computer

ros2 launch linorobot2_bringup bringup.launch.py

esp32 with micro-ROS wifi transport and ld19 lidar udp server without a robot computer

ros2 launch linorobot2_bringup bringup.launch.py micro_ros_transport:=udp4 micro_ros_port:=8888 lidar_transport:=udp_server lidar_server_port:=8889

Or, you may keep the micro-ROS wifi agent running in another terminal. And set micro_ros_transport to 'none' to skip the launch of micro-ROS agent during robot bringup.

ros2 launch linorobot2_bringup bringup.launch.py micro_ros_transport:=none lidar_transport:=udp_server lidar_server_port:=8889

esp32 with serial transport and ld19 lidar

To use serial transport for both miro-ROS and lidar on a robot computer. You will need to specify which USB serial port will be used for each purpose.

ros2 launch linorobot2_bringup bringup.launch.py base_serial_port:=/dev/ttyUSB0 lidar_serial_port:=/dev/ttyUSB1

If they have different USB ID, you may use udev rules to give them different device names to avoid confusion. If they have the same IDs, you may use by-path names.

ls -l /dev/serial/by-path/*
lrwxrwxrwx 1 root root 13 Mar  2 16:49 /dev/serial/by-path/pci-0000:00:14.0-usb-0:2.4.2:1.0-port0 -> ../../ttyUSB0
lrwxrwxrwx 1 root root 13 Mar  2 16:49 /dev/serial/by-path/pci-0000:00:14.0-usb-0:2.4.4:1.0-port0 -> ../../ttyUSB1

ros2 launch linorobot2_bringup bringup.launch.py \
    base_serial_port:=/dev/serial/by-path/pci-0000:00:14.0-usb-0:2.4.2:1.0-port0 \
    lidar_serial_port:=/dev/serial/by-path/pci-0000:00:14.0-usb-0:2.4.4:1.0-port0

You may specify micro-ROS baudrate, default is 921600.

ros2 launch linorobot2_bringup bringup.launch.py base_serial_port:=/dev/ttyUSB0 lidar_serial_port:=/dev/ttyUSB1 micro_ros_baudrate:=921600

Drive the robot with a keyboard

The robot listens to topic /cmdvel and move. You may run the teleop_twist_keyboard package and follow the instructions on the terminal to drive the robot:

ros2 run teleop_twist_keyboard teleop_twist_keyboard

You will see the keyboard command list.

This node takes keypresses from the keyboard and publishes them
as Twist messages. It works best with a US keyboard layout.
---------------------------
Moving around:
u    i    o
j    k    l
m    ,    .

For Holonomic mode (strafing), hold down the shift key:
---------------------------
U    I    O
J    K    L
M    <    >

t : up (+z)
b : down (-z)

anything else : stop

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%

CTRL-C to quit

currently:	speed 0.5	turn 1.0

Press 'i' key, and the robot should move forward.

Drive the robot with a gamepad

You may drive the robot with a gamepad. Check this guide on using gamepad to move the robot. You may check the buttons assignment on the gamepad with the jstest-gtk utility.

sudo usermod -aG input ubuntu
sudo apt install -y jstest-gtk

For Switch gamepad

ros2 launch teleop_twist_joy teleop-launch.py joy_config:='atk3'

On the Switch gamepad, the enable button is 'B'. Press and hold 'B' button and use the left stick to move the robot.

For Xbox gamepad

ros2 launch teleop_twist_joy teleop-launch.py joy_config:='xbox'

You can check the joy and cmdvel topic output.

ros2 topic echo /joy
ros2 topic echo /cmdvel

Enable Magnetometer

If you enable magnetometer device, you will need to enable Magdwick filter by adding "madgwick:=true" in robot bringup. The topic from imu will be named as imu/data_raw. The Madgwick filter will fuse imu/data_raw and imu/mag to imu/data. The hippo5329/linorobot2 has been updated with Madgwick filter support in the imu_tools package. You may visualize the IMU and manetometer by enable the IMU and magetometer plug-ins in RVIZ2.

ros2 launch linorobot2_bringup bringup.launch.py madgwick:=true orientation_stddev:=0.01

The ekf filter configuration will need update to enable magnetometer, as only 'vyaw' is enabled in the default configuration. Both IMU and magnetometer must be calibrated, otherwise the robot's pose will rotate.

linorobot2_ws/src/linorobot2/linorobot2_base/config/ekf.yaml

    #x     , y     , z,
    #roll  , pitch , yaw,
    #vx    , vy    , vz,
    #vroll , vpitch, vyaw,
    #ax    , ay    , az
    odom0: odom/unfiltered
    odom0_config: [false, false, false,
                   false, false, false,
                   true, true, false,
                   false, false, true,
                   false, false, false]

    imu0: imu/data
    imu0_config: [false, false, false,
                  false, false, true,
                  false, false, false,
                  false, false, true,
                  true, true, false]

Magnetometer calibration

Magnetometer calibration should be taken on board with all hardware installed, inlcuding all connectors, battery and motors. The calibration package will rotate the robot slowly for 60 sec. And compute the hard iron bias. More info here.

sudo apt-get install ros-humble-robot-calibration -y
rm -rf /tmp/magnetometer_calibration.bag
ros2 run robot_calibration magnetometer_calibration
...
mag_bias_x: -7.94713e-06
mag_bias_y: 3.49388e-05
mag_bias_z: -5.40286e-05

Set the MAG_BIAS to these values in robot configuration file. Then build and upload the firmware.

#define MAG_BIAS { -7.94713e-06, 3.49388e-05, -5.40286e-05 }

SLAM

In another terminal, start slam.

ros2 launch linorobot2_navigation slam.launch.py

Save the map

ros2 run nav2_map_server map_saver_cli -f <map_name> --ros-args -p save_map_timeout:=10000.

Navigate

In another terminal, start navigation. You may view with RVIZ2 and you can set a new goal and see the robot move to the goal.

ros2 launch linorobot2_navigation navigation.launch.py rviz:=true map:=<path_to_map_file>/<map_name>.yaml

I/O pins assignment

Most i/o pins on the micro-controllers are configurable with some limitation. The following lists are the i/o pins assignments used in configuration examples. You may modify the i/o pins assignment for different robots.

The i/o voltage of esp32 is 3.3V. It will need a logic level converter to interface to other voltage, such as 5V output from some encoders or 1.8V from some imu.

esp32 i/o pins assignment

motor 19 18 16 17 13 12 4 23, power 12V gnd, 2 PWM pins (INA, INB) driver
encoder 35 34 39 36 32 27 26 25, power 3.3V gnd
i2c scl 22 sda 21, power 3.3v gnd, need 2.2k pull high to 3.3V
battery adc 33, divider 12V-33k-10k-gnd (dac 25, used only during adc calibration)
uart0 txd 1 rxd 3
led 2
lidar 14

available i/o pins,

5 15

For 2wd configuration, unused motors pins 13 12 4 23 and encoders pins 32 27 26 25 can be used for general purpose i/o. Just change the unused motors and encoders pin number to -1 in the configuration file.

#define MOTOR3_ENCODER_A -1
#define MOTOR3_ENCODER_B -1

#define MOTOR4_ENCODER_A -1
#define MOTOR4_ENCODER_B -1

#ifdef USE_BTS7960_MOTOR_DRIVER
  #define MOTOR3_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
  #define MOTOR3_IN_A -1
  #define MOTOR3_IN_B -1

  #define MOTOR4_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
  #define MOTOR4_IN_A -1
  #define MOTOR4_IN_B -1
#endif

esp32-s2 i/o pins assignment

motor 10 11 12 13 14 15 16 17, power 12V gnd, 2 PWM pins (INA, INB) driver
encoder 4 5 6 7 39 40 41 42, power 3.3V gnd
i2c scl 9 sda 8, power 3.3v gnd, need 2.2k pull high to 3.3V
battery adc 1, divider 12V-33k-10k-gnd
uart0 txd 43 rxd 44
led 18
lidar 2

There may be two USB connectors on esp32-s2 boards. Use the one marked as COM/UART.

esp32-s3 i/o pins assignment

motor 10 11 12 13 14 15 16 17, power 12V gnd, 2 PWM pins (INA, INB) driver
encoder 4 5 6 7 39 40 41 42, power 3.3V gnd
i2c scl 9 sda 8, power 3.3v gnd, need 2.2k pull high to 3.3V
battery adc 1, divider 12V-33k-10k-gnd
uart0 txd 43 rxd 44
led 48
lidar 2

available i/o pins,

3 18 21 35 36 37 38 45 46

There may be two USB connectors on esp32-s3 boards. Use the one marked as USB.

waveshare general driver for robot esp32 i/o pins assignment

motor 25 21 17 26 22 23, power 12V gnd, driver TB6612 (PWM, INA, INB), header H3 motor1 H4 motor2, note the 6 pins headers pins assignment are different from that of motors.
encoder 34 35 16 27, power 5V gnd
i2c scl 33 sda 32, power 3.3v gnd
battery INA219
lidar 4, add a jumper wire from LIDAR to IO4 on middle header

pico i/o pins assignment

motor 12 13 14 15 16 17 18 19, power 12V gnd, 2 PWM pins (INA, INB) driver
encoder 6 7 10 11 20 21 2 3, power 3.3V gnd, encoder pins pair must be consecutive
i2c0 scl 5 sda 4 default, power 3.3v gnd, need 2.2k pull high to 3.3V
battery 28, divider 12V-33k-10k-gnd
led 25

available i/o pins,

22 26 27
uart0 txd 0 rxd 1
uart1 txd 8 rxd 9

Velocity Smoother

For Mecanum drive, the parameters of velocity_smoother must be changed to enable omni-directional move. Set the Y limits as X. Otherwise, it will move like a 2wd.

If your robot is heavy (~10Kg) or RPM is slow, try lower the velocity and acceleration limits in velocity_smoother. Otherwise, the robot may rotate in RVIZ.

ros2_ws/src/linorobot2/linorobot2_navigation/config/navigation.yaml

velocity_smoother:
ros__parameters:
    ...

For 2wd/4wd, set Y=0.

    max_velocity: [0.5, 0.0, 2.5]
    min_velocity: [-0.5, 0.0, -2.5]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]

For mecanum, set Y=X.

    max_velocity: [0.5, 0.5, 2.5]
    min_velocity: [-0.5, -0.5, -2.5]
    max_accel: [2.5, 2.5, 3.2]
    max_decel: [-2.5, -2.5, -3.2]

Then build and install.

colcon build

Fine tune covariance

The covariance of various sensors messages is important to the navigation stack. You may fine tune the covariance in the configuration file for best estimation of robot's pose.

#define ACCEL_COV { 0.01, 0.01, 0.01 }
#define GYRO_COV { 0.001, 0.001, 0.001 }
#define ORI_COV { 0.01, 0.01, 0.01 }
#define MAG_COV { 1e-12, 1e-12, 1e-12 }
#define POSE_COV { 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 }
#define TWIST_COV { 0.001, 0.001, 0.001, 0.003, 0.003, 0.003 }

If you install a magnetometer, you may adjust the covariance of the pose output from madgwick filter with the orientation_stddev parameter.

Reset and power on/off USB devices - Optional

During firmware development, you may need to reset the USB devices, such as esp32 or pic. You can use the usbreset command.

ubuntu@rpi5b:~$ usbreset
Usage:
usbreset PPPP:VVVV - reset by product and vendor id
usbreset BBB/DDD   - reset by bus and device number
usbreset "Product" - reset by product name

Devices:
Number 003/002  ID 2e8a:000a  Pico
Number 001/002  ID 05a3:9331  HD Web Camera
ubuntu@rpi5b:~$ which usbreset
/usr/bin/usbreset
ubuntu@rpi5b:~$ usbreset Pico
Resetting Pico ... ok

You can power on/off the USB devices with the uhubctl tool.

You may use a button to power up and shutdown the rpi.

sudo nano /boot/config.txt 
dtoverlay=gpio-shutdown,gpio_pin=3,active_low=1,gpio_pull=up,debounce=200

Cyclone-DDS - Optional

The Cyclone-DDS can be used instead of the default Fastrtsp-DDS.

sudo apt install ros-humble-rmw-cyclonedds-cpp
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

Use ROS2 with Fast-DDS Discovery Server - Optional

The default ROS2 setup using simple discovery is not reliable over wifi. Topics may be missing or communication stalls. It can be resolved with a fast-DDS Discovery Server.

Start by launching a discovery server with id 0, port 11811 (default port) and listening on all available interfaces. Open a new terminal and run:

fastdds discovery --server-id 0

Setup clients. In a new terminal, set the environment variable ROS_DISCOVERY_SERVER to the location of the discovery server. eg, 192.168.1.100

export ROS_DISCOVERY_SERVER=192.168.1.100:11811
ros2 ...

But the discovery server has default filters to reduce network load. The ros2 topic commands need a super client configuration to bypass the filters. The other nodes running nav2/slam/rviz do not need to become super clinets.

curl -O https://fast-dds.docs.eprosima.com/en/latest/_downloads/9f9e92b14612364b742c8ecde24b3d24/super_client_configuration_file.xml

Change the IP 127.0.0.1 to your discovery server in the xml.

<udpv4>
    <address>127.0.0.1</address>  ==> <address>192.168.1.100</address>
    <port>11811</port>
</udpv4>

Become super clients ONLY for ros2 topci and rqt_graph commands,

export FASTRTPS_DEFAULT_PROFILES_FILE=super_client_configuration_file.xml
ros2 daemon stop
ros2 daemon start
ros2 topic list
ros2 node info /esp32
ros2 topic info /imu/data
ros2 topic echo /imu/data
rqt_graph

Enable remote syslog server - Optional

Edit syslog server config on your PC. Add udp service and template to the end of this file.

sudo nano /etc/rsyslog.conf

# provides UDP syslog reception
module(load="imudp")
input(type="imudp" port="514")

$template Incoming-logs,"/var/log/%HOSTNAME%/%PROGRAMNAME%.log"
*.* ?Incoming-logs

Restart the syslog service

sudo systemctl restart rsyslog

To view the log file of myrobot,

less /var/log/myrobot/hardware.log

If WIFI is setup correctly, you should see syslog messages like this

Dec 23 16:49:48 myrobot hardware initWifis ssid labwifi rssi -51 ip 192.168.1.101

Here 192.168.1.101 is the ip address of the esp32 controller.

Switch to remote upload using ArduinoOTA - Optional

After the esp32 connected to wifi. Find the esp32 IP address from serial monitor or syslog messages. Modify the project configuration to espota protocol and the esp32 IP address.

firmware/platformio.ini

[env:myrobot]

; upload_port = /dev/ttyUSB0
; upload_protocol = esptool
upload_protocol = espota
upload_port = 192.168.1.101

Save this file. Build and upload again. Then the esp32 firmware will be updated remotely with OTA.

pio run -e myrobot -t upload

You will see upload command messages,

Configuring upload protocol...
AVAILABLE: cmsis-dap, esp-bridge, esp-prog, espota, esptool, iot-bus-jtag, jlink, minimodule, olimex-arm-usb-ocd, olimex-arm-usb-ocd-h, olimex-arm-usb-tiny-h, olimex-jtag-tiny, tumpa
CURRENT: upload_protocol = espota
Uploading .pio/build/myrobot/firmware.bin
14:34:22 [DEBUG]: Options: {'esp_ip': '192.168.1.101', 'host_ip': '0.0.0.0', 'esp_port': 3232, 'host_port': 54252, 'auth': '', 'image': '.pio/build/myrobot/firmware.bin', 'spiffs': False, 'debug': True, 'progress': True, 'timeout': 10}
14:34:22 [INFO]: Starting on 0.0.0.0:54252
14:34:22 [INFO]: Upload size: 951408
Sending invitation to 192.168.1.101
14:34:22 [INFO]: Waiting for device...

Uploading: [                                                            ] 0%
Uploading: [=                                                           ] 0%
...
Uploading: [============================================================] 99%
Uploading: [============================================================] 100% Done...

14:34:33 [INFO]: Waiting for result...
14:34:34 [INFO]: Result: OK
14:34:34 [INFO]: Success
========================= [SUCCESS] Took 20.84 seconds =========================

Improve esp32 ADC accuracy with calibration - Optional

Skip this section if you do not use the esp32 ADC to measure the battery voltage.

We use the esp32 ADC to measure the battery voltage. The esp32 ADC accuracy can be improved by calibration with a DAC. The adc_calibrate utility is used to generate the calibration data. You should do the calibration before building the robot. Please note the OTA and syslog are not supported in this utility. The adc_calibrate utility will include the firmware/platformio.ini. There is no need to edit the adc_calibrate/platformio.ini for your custom configuration.

Connect pin 25 DAC1 output to battery ADC input pin 33 with a jumper wire during calibration. Build and upload the adc_calibrate with USB cable.

cd adc_calibrate
pio run -e myrobot -t upload
pio device monitor

The serial monitor will show the generated calibration table.

Test Linearity .....
Calculate interpolated values ..
Generating LUT ..

const int16_t ADC_LUT[4096] = { 0,
65,66,68,70,72,73,75,77,79,80,81,83,84,85,86,
88,89,90,91,93,94,95,96,97,98,99,100,101,102,103,
...
4076,4077,4077,4078,4078,4079,4080,4080,4081,4082,4082,4083,4084,4084,4085,
4086,4086,4087,4088,4088,4089,4090,4091,4091,4092,4093,4093,4094,4095,4095
};

Copy the table data to your configuration file. Uncomment the line #define USE_ADC_LUT.

// battery voltage ADC pin
#define BATTERY_PIN 33
// 3.3V ref, 12 bits ADC, 33k + 10k voltage divider
#define USE_ADC_LUT
#ifdef USE_ADC_LUT
const int16_t ADC_LUT[4096] = { 0,
65,66,68,70,72,73,75,77,79,80,81,83,84,85,86,
88,89,90,91,93,94,95,96,97,98,99,100,101,102,103,
...
4076,4077,4077,4078,4078,4079,4080,4080,4081,4082,4082,4083,4084,4084,4085,
4086,4086,4087,4088,4088,4089,4090,4091,4091,4092,4093,4093,4094,4095,4095
};
#define BATTERY_ADJUST(v) (ADC_LUT[v] * (3.3 / 4096 * (33 + 10) / 10 * 1.0))
#else
#define BATTERY_ADJUST(v) ((v) * (3.3 / 4096 * (33 + 10) / 10))
#endif

Remove the DAC1 jumper wire after calibration.

PlatformIO IDE for VSCode installation - Optional

This firmware project is based on the micro-ROS for PlatformIO. We have installed the tools in the Quick start. You may check the guide VSCode and Docker. You may install the the gitlens, code-spell-checker and markdownlint extensions. They are very helpful.

Be sure to enable trailing whitespace trimming.

Ultrasonic range sensor - Optional

The cheap ultrasonic range sensor HC-SR04 can be used to provide limited range detection when a lidar is not in use.

In esp32 configuration example, we use pin 5 for TRIG and pin 32 for ECHO.

#define TRIG_PIN 5 // ultrasonic sensor HC-SR04
#define ECHO_PIN 32

Echo Ultrasonic range

ros2 topic echo /ultrasound

PCA9685 i2c 16ch pwm controller - Optional

The PCA9685 i2c 16ch pwm controller can be used as a pwm/output extender. The pwm outputs of micro-controller and PCA9685 are combined and controlled with the following functions defined in lib/pwm/pwm.h.

void setPin(int pin, int value); // analogWrite()
void setPulse(int pin, int value); // writeMicroseconds()
void setLevel(int pin, int value); // digitalWrite()
void setPWMFreq(float freq); // analogWriteFrequency()

The pwm channels on PCA9685 are mapped to PCA_BASE, which is default to 100. So the pin number of pwm0 on PCA9685 is 100 and the pin number of pwm15 on PCA9685 is 115. The PWM_BITS is fixed to 12 for PCA9685. The PCA9685 driver is enabled with PCA_BASE.

#define PWM_BITS 12                         // PWM Resolution of the microcontroller
#define PWM_FREQUENCY 100                   // PWM Frequency
#define PCA_BASE 100                        // enable PCA9685 support

During initilal debugging, you may enable debug output of the Adafruit PWM driver with the ENABLE_DEBUG_OUTPUT build_flags in the platfromio.ini. Like this,

build_flags =
    -I ../config
    -D USE_ESP32_CONFIG
    -D ENABLE_DEBUG_OUTPUT

ZIO qwiic 4 dc motor controller

Zio qwiic 4-Channel DC Motor Controller is a qwiic interface with 4 DC motors drivers. It is based on PCA9685 and TB6612. The STBY pins must be set to HIGH to enable motor outputs.

#define USE_GENERIC_2_IN_MOTOR_DRIVER
#define PWM_BITS 12                         // PWM Resolution of the microcontroller
#define PWM_FREQUENCY 100                   // PWM Frequency
#define PCA_BASE 100                        // enable PCA9685 support

#define MOTOR1_PWM  100
#define MOTOR1_IN_A 101
#define MOTOR1_IN_B 102
#define MOTOR2_PWM  106
#define MOTOR2_IN_A 104
#define MOTOR2_IN_B 105
#define MOTOR3_PWM  107
#define MOTOR3_IN_A 108
#define MOTOR3_IN_B 109
#define MOTOR4_PWM  113
#define MOTOR4_IN_A 111
#define MOTOR4_IN_B 112
#define MOTOR_STBY_1 103
#define MOTOR_STBY_2 110

#define BOARD_INIT_LATE { \
setLevel(MOTOR_STBY_1, HIGH); \
setLevel(MOTOR_STBY_2, HIGH); \
}

PCA9685  --  TB6612       PCA9685  --   TB6612
0        --  PWMA1         8       --   A2IN1
1        --  A1IN1         9       --   A2IN2
2        --  A1IN2         10      --   STBY2
3        --  STBY1         11      --   B2IN1
4        --  B1IN1         12      --   B2IN2
5        --  B1IN2         13      --   PWMB2
6        --  PWMB1         14      --   IO14
7        --  PWMA2         15      --   IO15

Waveshare pico motor driver

Waveshare pico motor driver is a 4 DC motors driver module for Raspberry Pi Pico. It is based on PCA9685 and TB6612.

#define USE_GENERIC_2_IN_MOTOR_DRIVER
#define PWM_BITS 12                         // PWM Resolution of the microcontroller
#define PWM_FREQUENCY 100                   // PWM Frequency
#define PCA_BASE 100                        // enable PCA9685 support

#define MOTOR1_PWM  100
#define MOTOR1_IN_A 101
#define MOTOR1_IN_B 102

#define MOTOR2_PWM  103
#define MOTOR2_IN_A 104
#define MOTOR2_IN_B 105

#define MOTOR3_PWM  106
#define MOTOR3_IN_A 107
#define MOTOR3_IN_B 108

#define MOTOR4_PWM  109
#define MOTOR4_IN_A 110
#define MOTOR4_IN_B 111

#define SDA_PIN 20 // specify I2C pins
#define SCL_PIN 21

Reset and read information of esp32 - optional

pip install esptool
esptool.py -p /dev/ttyUSB0 flash_id

Program the CP2102 to unique serial ID - Optional

Many serial devices use Silicon Labs CP2102. The factory programmed serial IDs are often 1. Their serial IDs can be reprogrammed with an utility to an unique ID.

Download and install the Simplicity-Studio and USBXpressHostSDK.
Create a sample project of CP2102, which will generate a configuration file.
Program the device.

cp210xsmt --device-count 1 --set-and-verify-config cp2102_gm.configuration --serial-nums GUID

Then you can use the by-id names.

ls -l /dev/serial/by-id/*
lrwxrwxrwx 1 root root 13 Mar  2 17:56 /dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_6741f43656434eb68e1b47596e44a9f7-if00-port0 -> ../../ttyUSB1
lrwxrwxrwx 1 root root 13 Mar  2 17:55 /dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_7d4f2bd23f644b619cc2a23a7102fd10-if00-port0 -> ../../ttyUSB0

Configuration details

1. Robot Settings

Open your custom configuration file. Uncomment/comment the base, motor driver and IMU you want to use for your robot. For example:

#define LINO_BASE DIFFERENTIAL_DRIVE       // 2WD and Tracked robot w/ 2 motors
#define USE_BTS7960_MOTOR_DRIVER        // BTS7970 Motor Driver using A4950 (<40V) module or DRV8833 (<10V)
#define USE_MPU6050_IMU

Constants' Meaning:

ROBOT TYPE (LINO_BASE)

DIFFERENTIAL_DRIVE - 2 wheel drive or tracked robots w/ 2 motors.
SKID_STEER - 4 wheel drive robots.
MECANUM - 4 wheel drive robots using mecanum wheels.

MOTOR DRIVERS

USE_GENERIC_2_IN_MOTOR_DRIVER - Motor drivers that have EN (pwm) pin, and 2 direction pins (usually DIRA, DIRB pins).
USE_GENERIC_1_IN_MOTOR_DRIVER - Motor drivers that have EN (pwm) pin, and 1 direction pin (usual DIR pin). These drivers usually have logic gates included to lessen the pins required in controlling the driver.
USE_BTS7960_MOTOR_DRIVER - BTS7960 motor driver.
USE_ESC_MOTOR_DRIVER - Bi-directional (forward/reverse) electronic speed controllers.

INERTIAL MEASUREMENT UNIT (IMU)

USE_GY85_IMU - GY-85 IMUs.
USE_MPU6050_IMU - MPU6060 IMUs.
USE_MPU9150_IMU - MPU9150 IMUs.
USE_MPU9250_IMU - MPU9250 IMUs.
USE_HMC5883L_IMU - HMC5883L MAGs.
USE_AK8963_MAG - AK8963 MAGs.
USE_AK8975_MAG - AK8975 MAGs.
USE_AK09918_MAG - AK09918 MAGs.
USE_QMC5883L_MAG - QMC5883L MAGs.
MAG_BIAS - Magnetometer calibration, eg { -352, -382, -10 }.

Next, fill in the robot settings accordingly:

#define K_P 0.6                             // P constant
#define K_I 0.8                             // I constant
#define K_D 0.5                             // D constant

//define your robot' specs here
#define MOTOR_MAX_RPM 150                   // motor's max RPM
#define MAX_RPM_RATIO 0.85                  // max RPM allowed for each MAX_RPM_ALLOWED = MOTOR_MAX_RPM * MAX_RPM_RATIO
#define MOTOR_OPERATING_VOLTAGE 12          // motor's operating voltage (used to calculate max RPM)
#define MOTOR_POWER_MAX_VOLTAGE 12          // max voltage of the motor's power source (used to calculate max RPM)
#define MOTOR_POWER_MEASURED_VOLTAGE 12     // current voltage reading of the power connected to the motor (used for calibration)
#define COUNTS_PER_REV1 550                 // wheel1 encoder's no of ticks per rev
#define COUNTS_PER_REV2 550                 // wheel2 encoder's no of ticks per rev
#define COUNTS_PER_REV3 550                 // wheel3 encoder's no of ticks per rev
#define COUNTS_PER_REV4 550                 // wheel4 encoder's no of ticks per rev
#define WHEEL_DIAMETER 0.0560               // wheel's diameter in meters
#define LR_WHEELS_DISTANCE 0.224            // distance between left and right wheels
#define PWM_BITS 10                         // PWM Resolution of the microcontroller
#define PWM_FREQUENCY 20000                 // PWM Frequency

Constants' Meaning:

K_P, K_I, K_D - PID constants used to translate the robot's target velocity to motor speed. These values would likely work on your build, change these only if you experience jittery motions from the robot or you'd want to fine-tune it further.
MOTOR_MAX_RPM - Motor's maximum number of rotations it can do in a minute specified by the manufacturer.
MAX_RPM_RATIO - Percentage of the motor's maximum RPM that the robot is allowed to move. This parameter ensures that the user-defined velocity will not be more than or equal the motor's max RPM, allowing the PID to have ample space to add/subtract RPM values to reach the target velocity. For instance, if your motor's maximum velocity is 0.5 m/s with MAX_RPM_RATIO set to 0.85, and you asked the robot to move at 0.5 m/s, the robot's maximum velocity will be capped at 0.425 m/s (0.85 * 0.5m/s). You can set this parameter to 1.0 if your wheels can spin way more than your operational speed.

Wheel velocity can be computed as:

MAX_WHEEL_VELOCITY = (`MOTOR_MAX_RPM` / 60.0) * PI * `WHEEL_DIAMETER`

MOTOR_OPERATING_VOLTAGE - Motor's operating voltage specified by the manufacturer (usually 5V/6V, 12V, 24V, 48V). This parameter is used to calculate the motor encoder's COUNTS_PER_REV constant during calibration and actual maximum RPM of the motors. For instance, a robot with MOTOR_OPERATING_VOLTAGE of 24V with a MOTOR_POWER_MAX_VOLTAGE of 12V, will only have half of the manufacturer's specified maximum RPM ((MOTOR_POWER_MAX_VOLTAGE / MOTOR_OPERATING_VOLTAGE) * MOTOR_MAX_RPM).
MOTOR_POWER_MAX_VOLTAGE - Maximum voltage of the motor's power source. This parameter is used to calculate the actual maximum RPM of the motors.
MOTOR_POWER_MEASURED_VOLTAGE - Measured voltage of the motor's power source. If you don't have a multimeter, it's best to fully charge your battery and set this parameter to your motor's operating voltage (MOTOR_OPERATING_VOLTAGE). This parameter is used to calculate the motor encoder's COUNTS_PER_REV constant. You can ignore this if you're using the manufacturer's specified counts per rev.
COUNTS_PER_REVX - The total number of pulses the encoder has to read to be considered as one revolution. You can either use the manufacturer's specification or the calibrated value in the next step. If you're planning to use the calibrated value, ensure that you have defined the correct values for MOTOR_OPERATING_VOLTAGE and MOTOR_POWER_MEASURED_VOLTAGE.
WHEEL_DIAMETER - Diameter of the wheels in meters.
LR_WHEELS_DISTANCE - The distance between the center of left and right wheels in meters.
PWM_BITS - Number of bits in generating the PWM signal. You can use the default value if you're unsure what to put here. More info here.
PWM_FREQUENCY - Frequency of the PWM signals used to control the motor drivers. You can use the default value if you're unsure what to put here. More info here.

WIFI related settings, only for esp32

AGENT_IP - micro-ROS agent IP. eg. { 192, 168, 1, 100 }
AGENT_PORT - micro-ROS agent port. default 8888
WIFI_AP_LIST - Enable WiFi with null terminated list of multiple APs SSID and password. eg. {{"WIFI_SSID1", "WIFI_PASSWORD1"}, {"WIFI_SSID2", "WIFI_PASSWORD2"}, {NULL}} . The AP with strongest signal will be used. When wifi signal is too low, the current AP will be disconnected and reconnect the AP with the strongest signal.
USE_ARDUINO_OTA - Arduino OTA up load protocol support
USE_SYSLOG - logging to remote syslog server.
SYSLOG_SERVER - syslog server name or IP.
SYSLOG_PORT - syslog server udp port. default 514
DEVICE_HOSTNAME - my device name to syslog. default "linorobot2"
APP_NAME - my app name to syslog. default "hardware"
USE_LIDAR_UDP - push Lidar data to UDP server, which will decode the data and publish laser scan message.
LIDAR_RXD - RXD pin for serial data from Lidar
LIDAR_BAUDRATE
LIDAR_SERVER - Lidar server IP address, eg. { 192, 168, 1, 100 }
LIDAR_PORT - Lidar server UDP port, eg. 8889

Optional settings

BAUDRATE - serial baudrate. default 921600.
NODE_NAME - ROS2 node name. default "linorobot_base_node"
SDA_PIN/SCL_PIN - I2C pins assignment
TOPIC_PREFIX - Namespace prefix to topic, eg "turtle1/". Useful when there are multiple robots running.
BATTERY_PIN - ADC pin for battery voltage measurement through a 33K/10K resistors voltage divider.
USE_ADC_LUT - use calibration table lookup to improve accuracy of esp32 ADC.
BATTERY_ADJUST - ADC reading adjustment to battery voltage.
USE_INA219 - use INA219 chip for battery voltage/current measurement.
BATTERY_DIP - If BATTERY_DIP is defined, the reading from adc will be sampled every control cycle and check for voltage dip resulted from motors current surge. When a voltage dip is deteced, a syslog warning and extra battery topic will be published. If not defined, it will sample only once per BATTERY_TIMER. Suggested value: 0.98.
TRIG_PIN/ECHO_PIN - HC-SR04 Ultrasonic sensor trigger and echo pins. The echo pin needs a 6.8K/10K voltage divider, because the esp32 I/O pins are 3.3V tolerance. The pulse width reading is hard coded timeout 5000uS in driver, so it is roughly 75cm range.
USE_SHORT_BRAKE - Short brake for shorter distance to stop, only for generic_2 BT6612 and BTS7960 like motor drivers
WDT_TIMEOUT - Hardware watchdog timeout period, only for esp32.
BOARD_INIT - board specific setup, eg I/O ports mode or extra startup delay, sleep(5)

2. Hardware Pin Assignments

The pin number described here is the I/O pin number of esp32. It is not the pin number of the module package. Only modify the pin assignments under the motor driver constant that you are using ie. #ifdef USE_BTS7960_MOTOR_DRIVER. For teensy, you can check out PJRC's pinout page for each board's pin layout.

The pin assignments found in lino_base_config.h are based on Linorobot's PCB board. You can wire up your electronic components based on the default pin assignments but you're also free to modify it depending on your setup. Just ensure that you're connecting MOTORX_PWM pins to a PWM enabled pin on the microcontroller and reserve SCL and SDA pins for the IMU, and pin 2 (built-in LED) for debugging.

// ENCODER PINS
#define MOTOR1_ENCODER_A 36
#define MOTOR1_ENCODER_B 39

#define MOTOR2_ENCODER_A 35
#define MOTOR2_ENCODER_B 34

#define MOTOR3_ENCODER_A -1
#define MOTOR3_ENCODER_B -1

#define MOTOR4_ENCODER_A -1
#define MOTOR4_ENCODER_B -1

#ifdef USE_BTS7960_MOTOR_DRIVER
#define MOTOR1_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
#define MOTOR1_IN_A 19
#define MOTOR1_IN_B 18

#define MOTOR2_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
#define MOTOR2_IN_A 16
#define MOTOR2_IN_B 17

#define MOTOR3_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
#define MOTOR3_IN_A -1
#define MOTOR3_IN_B -1

#define MOTOR4_PWM -1 //DON'T TOUCH THIS! This is just a placeholder
#define MOTOR4_IN_A -1
#define MOTOR4_IN_B -1

#define PWM_MAX pow(2, PWM_BITS) - 1
#define PWM_MIN -PWM_MAX
#endif

Constants' Meaning:

MOTORX_ENCODER_A - Microcontroller pin that is connected to the first read pin of the motor encoder. This pin is usually labelled as A pin on the motor encoder board.
MOTORX_ENCODER_B - Microcontroller pin that is connected to the second read pin of the motor encoder. This pin is usually labelled as B pin on the motor encoder board.
MOTORX_ENCODER_INV - Flag used to change the sign of the encoder value. More on that later.
MOTORX_PWM - Microcontroller pin that is connected to the PWM pin of the motor driver. This pin is usually labelled as EN or ENABLE pin on the motor driver board.
MOTORX_IN_A - Microcontroller pin that is connected to one of the motor driver's direction pins. This pin is usually labelled as DIRA or DIR1 pin on the motor driver board. On BTS7960 driver, this is one of the two PWM pins connected to the driver (RPWM/LPWM).
MOTORX_IN_B - Microcontroller pin that is connected to one of the motor driver's direction pins. This pin is usually labelled as DIRB or DIR2 pin on the motor driver board. On BTS7960 driver, this is one of the two PWM pins connected to the driver (RPWM/LPWM).
MOTORX_INV - Flag used to invert the direction of the motor. More on that later.

Parts

Waveshare General Driver for Robot 81NATUuUNKL AC_SL1500

ldlidar LD19 61KEOAQzjoL AC_SL1500

Robot tank chassis TP200 71kH5Hl2rhL AC_SL1500

URDF

Once the hardware is done, you can go back to linorobot2 package and start defining the robot's URDF.

Troubleshooting Guide

1. One of my motor isn't spinning

Check if the motors are powered.
Check if you have bad wiring.
Check if you have misconfigured the motor's pin assignment in lino_base_config.h.
Check if you uncommented the correct motor driver (ie. USE_GENERIC_2_IN_MOTOR_DRIVER)
Check if you assigned the motor driver pins under the correct motor driver constant. For instance, if you uncommented USE_GENERIC_2_IN_MOTOR_DRIVER, all the pins you assigned must be inside the ifdef USE_GENERIC_2_IN_MOTOR_DRIVER macro.

2. Wrong wheel is spinning during calibration process

Check if the motor drivers have been connected to the correct microcontroller pin.
Check if you have misconfigured the motor's pin assignment in lino_base_config.h.

3 One of my encoders has no reading (0 value)

Check if the encoders are powered.
Check if you have bad wiring.
Check if you have misconfigured the encoder's pin assignment in lino_base_config.h.

4. The wheels only spin in one direction

Check if the Teensy's GND pin is connected to the motor driver's GND pin.

5. The motor doesn't change it's direction after setting the INV to true

Check if the Teensy's GND pin is connected to the motor driver's GND pin.

6. Nothing's printing when I run the screen app

Check if you're passing the correct serial port. Run:
```
  ls /dev/ttyACM*
```
and ensure that the available serial port matches the port you're passing to the screen app.
Check if you forgot to copy the udev rule:
```
  ls /etc/udev/rules.d/00-teensy.rules
```
Remember to restart your computer if you just copied the udev rule.

7. The firmware was uploaded but nothing's happening

Check if you're assigning the correct Teensy board when uploading the firmware. If you're unsure which Teensy board you're using, take a look at the label on the biggest chip found in your Teensy board and compare it with the boards shown on PJRC's website.

8. The robot's forward motion is not straight

This happens when the target velocity is more than or equal the motor's RPM (usually happens on low RPM motors). To fix this, set the MAX_RPM_RATIO lower to allow the PID controller to compensate for errors.

9. The robot rotates after braking

This happens due to the same reason as 7. When the motor hits its maximum rpm and fails to reach the target velocity, the PID controller's error continously increases. The abrupt turning motion is due to the PID controller's attempt to further compensate the accumulated error. To fix this, set the MAX_RPM_RATIO lower to allow the PID controller to compensate for errors while moving to avoid huge accumulative errors when the robot stops.

10. PlatformIO stop building at "Downloading micro-ROS dev dependencies"

Configuring teensy40 with transport serial
Downloading micro-ROS dev dependencies
KeyError: '':
File "/home/ubuntu/.platformio/penv/lib/python3.10/site-packages/platformio/builder/main.py", line 173:
    env.SConscript("$BUILD_SCRIPT")

This happened when board_microros_distro is not correct in platformio.ini. You should source the ROS2 setup, or define "board_microros_distro = humble" in your board env in platformio.ini.