Home
This is a fork of the linorobot/linorobot2_hardware project which builds motor controller 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.
If you are new to esp32, you may check the guide to esp32.
I started from the publish int example and built an esp32 wifi robot. Later, I found the linorobot2 project, which has some interesting features. They use teensy. I ported my esp32 code to it. I added more devices and test programs. To help myself and others, I wrote this wiki. I was inspired and helped by other open-sources guru. I can help, too.
The micro-ROS robot controller will subscribe /cmd_vel, convert the twist msg and move the wheels with PID control. It will then publish wheels odometry to /odom/unfiltered and IMU reading to /imu/data.
On the robot computer, EKF filter will fuse /odom/unfiltered and /imu/data to /odom. Robot state, joint state and transforms will be published.
Only esp32 esp32-s2 and esp32-s3 are supported. Some variants, such as esp32-c3, are not supported.
- Install the software.
- Test micro-ROS agent connection before wiring and building the robot.
- Connect IMU and run test_sensors.
- Connect micro-ROS agent again and check /imu/data topic.
- 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.
If you are new to ROS/micro-ROS, you should start from the basic. The first robot should be simple, compact (<30cm) and light weight (<3Kg). Lesson learned from the smaller robot will help you with larger and more powerful robots. Since it is light weight, you won't need high power motors and battery for this first robot. If you skip this simple step, jump into your ambitious dream robot and want to success in the first try, it will only waste your time (and mine).
If you have a robot computer, such as RPI4/5, you can use Pico and micro-ROS serial transport. The USB CDC /dev/ttyACM0 on Pico works better than USB serial /dev/ttyUSB0 on esp32.
If you do not have a robot computer, you can use esp32 with micro-ROS wifi transport.
For differential drive robot
- Lidar: Ldlidar LD19
- Controller: esp32 on Waveshare General Driver for Robots all-in-one control board
- Battery: 12.6V 3S Li-ion battery
- Motor: 12V 150-200RPM brushed gear motor with hall encoder
- Robot type: 2wd
- Chassis: DIY or TP200 robot tank chassis. It includes two gear motors with encoders.
For Mecanum drive robot
- Lidar: Ldlidar LD19
- Controller: esp32
- Battery: 12.6V 3S Li-ion battery
- Motor: 12V 150-200RPM brushed gear motor with hall encoder
- Robot type: mecanum
- Chassis: MC200 Mecanum robot chassis. It includes four gear motors with encoders and Mecanum wheels.
- IMU: MPU6050
- Magnetometer: QMC5883L - optional
- Motor driver: TB6612
If you use serial transport with robot computer, such as Rpi4/5 or mini PC, pico is a good alternative to esp32.
A Lidar is required to run navigation and SLAM. LD19 is suggested for its ease of use, cost and performance. Though the rplidar A1 is popular, the power requirement of A1 is tighter (4.9V or higher on lidar motor) and the higher sampling frequency 8KHz (compared to LD19 4.5KHz) will require higher processing load. So A1/A2 are not suggested for the starter robot.
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.
NOTE: rpi 3B 3B+ 1GB does not have enough memory and speed to build and run linorobot2.
Waveshare General Driver for Robots is a nice robot control board for beginners. It integrates the esp32, motor driver TB6612, IMU QMI8658, magnetometer AK09918, voltage/current sensor INA219, step down power regulator, serial bus servo controller and 4 pins LD19 connector. Use gendrv_wifi example configuration. Update the wifi keys and agent ip. Add a jumper wire from LIDAR to IO4 on the middle header. Plug and play.
Waveshare General Driver for Robot Wiki Schematic. You may use the schematic as a reference to design your esp32 controller.
Follow this guide to install software tools and try out the publisher example.
Clone the firmware source into your working directory.
git clone https://github.com/hippo5329/linorobot2_hardware.git
Try build to verify the installation.
cd linorobot2_hardware/firmware
pio run -e esp32
It is recommended to test the micro-ROS agent connection with the micro-controller module alone before wiring and building the robots. Choose the configuration you will use to build your robot. DO NOT enable the IMU setting in this step. Keep the IMU setting comment out. IMU is not needed in this step. You can use a bare esp32 module to test. Enable IMU after this connection test success.
NOTE: Upload, device monitor, micro-ros agent serial transport can not run at the same time. They use the same serial port. Only one at a time. Remember to stop the micro-ROS agent with ctrl-C after connection test.
Build and upload with USB cable connected to the esp32 board. Do not connect the lidar.
cd linorobot2_hardware/firmware
pio run -e esp32 -t upload
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 --baudrate 921600
Screen output.
$ ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 --baudrate 921600
[1716860362.641698] info | TermiosAgentLinux.cpp | init | running... | fd: 3
[1716860362.641922] info | Root.cpp | set_verbose_level | logger setup | verbose_level: 4
[1716860362.959510] info | Root.cpp | create_client | create | client_key: 0x6A54364B, session_id: 0x81
[1716860362.959549] info | SessionManager.hpp | establish_session | session established | client_key: 0x6A54364B, address: 0
[1716860362.971085] info | ProxyClient.cpp | create_participant | participant created | client_key: 0x6A54364B, participant_id: 0x000(1)
[1716860362.975931] info | ProxyClient.cpp | create_topic | topic created | client_key: 0x6A54364B, topic_id: 0x000(2), participant_id: 0x000(1)
[1716860362.979719] info | ProxyClient.cpp | create_publisher | publisher created | client_key: 0x6A54364B, publisher_id: 0x000(3), participant_id: 0x000(1)
[1716860362.983797] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0x6A54364B, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1716860362.988079] info | ProxyClient.cpp | create_topic | topic created | client_key: 0x6A54364B, topic_id: 0x001(2), participant_id: 0x000(1)
[1716860362.991661] info | ProxyClient.cpp | create_publisher | publisher created | client_key: 0x6A54364B, publisher_id: 0x001(3), participant_id: 0x000(1)
[1716860362.995709] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0x6A54364B, datawriter_id: 0x001(5), publisher_id: 0x001(3)
[1716860363.000219] info | ProxyClient.cpp | create_topic | topic created | client_key: 0x6A54364B, topic_id: 0x002(2), participant_id: 0x000(1)
[1716860363.003886] info | ProxyClient.cpp | create_publisher | publisher created | client_key: 0x6A54364B, publisher_id: 0x002(3), participant_id: 0x000(1)
[1716860363.007945] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0x6A54364B, datawriter_id: 0x002(5), publisher_id: 0x002(3)
[1716860363.012393] info | ProxyClient.cpp | create_topic | topic created | client_key: 0x6A54364B, topic_id: 0x003(2), participant_id: 0x000(1)
[1716860363.016055] info | ProxyClient.cpp | create_subscriber | subscriber created | client_key: 0x6A54364B, subscriber_id: 0x000(4), participant_id: 0x000(1)
[1716860363.020072] info | ProxyClient.cpp | create_datareader | datareader created | client_key: 0x6A54364B, datareader_id: 0x000(6), subscriber_id: 0x000(4)
After it is connected to the agent, you may check the topics it published in another terminal.
ros2 node list
ros2 node info /esp32
ros2 topic list -t
ros2 topic echo /imu/data
ros2 topic hz /imu/data
Screen output.
ubuntu@cc187:~$ ros2 node list
/esp32
ubuntu@cc187:~$ ros2 node info /esp32
/esp32
Subscribers:
/cmd_vel: geometry_msgs/msg/Twist
Publishers:
/battery: sensor_msgs/msg/BatteryState
/imu/data: sensor_msgs/msg/Imu
/odom/unfiltered: nav_msgs/msg/Odometry
Service Servers:
Service Clients:
Action Servers:
Action Clients:
ubuntu@cc187:~$ ros2 topic list -t
/battery [sensor_msgs/msg/BatteryState]
/cmd_vel [geometry_msgs/msg/Twist]
/imu/data [sensor_msgs/msg/Imu]
/odom/unfiltered [nav_msgs/msg/Odometry]
/parameter_events [rcl_interfaces/msg/ParameterEvent]
/rosout [rcl_interfaces/msg/Log]
ubuntu@cc187:~$ ros2 topic echo /imu/data
header:
stamp:
sec: 1716860695
nanosec: 122109000
frame_id: imu_link
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
orientation_covariance:
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
angular_velocity:
x: 0.0
y: 0.0
z: 0.0
angular_velocity_covariance:
- 0.0010000000474974513
- 0.0
- 0.0
- 0.0
- 0.0010000000474974513
- 0.0
- 0.0
- 0.0
- 0.0010000000474974513
linear_acceleration:
x: 0.0
y: 0.0
z: 0.0
linear_acceleration_covariance:
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
...
ubuntu@cc187:~$ ros2 topic hz /imu/data
average rate: 49.999
min: 0.019s max: 0.023s std dev: 0.00095s window: 51
average rate: 49.951
min: 0.019s max: 0.023s std dev: 0.00096s window: 102
As we didn't connect an IMU to the esp32 yet, the IMU reading x,y,z,w will be all zero. The hz of /imu/data should be close to 50Hz.
cd linorobot2_hardware/firmware
pio run -e pico -t upload
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0
Update the configuration file with your wifi keys and servers IP, ie IP of your laptop or robot computer.
linorobot2_hardware/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 linorobot2_hardware/firmware
pio run -e esp32_wifi -t upload
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_
Uncomment the IMU you will use to enable the IMU driver, eg. "#define USE_MPU6050_IMU" for MPU6050, in the configuration file you will use, eg. config/custom/esp32_config.h.
Connect VCC,GND,SCL,SDA on the MPU6050 (GY521) to VIN(5V),GND,D22,D21 on the esp32 module. Add a 2K resistor between D21 and 3V3. Add another 2K resistor between D22 and 3V3.
I2C Bus Pullup Resistor Calculation Most i2c sensor modules have on-board i2c pullup resistors of 10K Ohm. When there are more than three i2c sensors on an i2c bus, some of the pullup resistors need to be adjusted.
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 linorobot2_hardware/test_sensors
pio run -e esp32 -t upload
pio device monitor -e esp32
Output
ACC -0.25 -0.32 9.01 GYR 0.00 0.00 0.00 MAG 0.00 0.00 0.00
BAT 0.10V RANGE 0.00m
Because only the IMU is connected and enabled in this step, only IMU data is valid and the other values may be zero. You should run test_sensors again when you connect the other sensors.
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 firmware, test_motors and test_sensors utilities will run i2cdetect and output to serial after reset. If IMU (eg MPU6050) is detected, serial message should be like this. If you missed the message, press RESET button on esp32 to see the messages.
# run the device monitor in platformio cli or run "upload and monitor" in vscode
pio device monitor -e esp32
0 1 2 3 4 5 6 7 8 9 a b c d e f
00: -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- --
Otherwise, the IMU is not detected. Check your I2C pins assignment and wiring.
After the IMU tests success in test_sensors, connect micro-ROS agent again and check the /imu/data topic.
cd linorobot2_hardware/firmware
pio run -e esp32 -t upload
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 --baudrate 921600
In another terminal, check /imu/data topic. In normal stand, the linear_acceleration x,y,z should close to 0,0,9.8. angular_velocity x,y,z should be all zero.
ubuntu@cc187:~$ ros2 topic echo /imu/data
header:
stamp:
sec: 1716918508
nanosec: 47000000
frame_id: imu_link
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
orientation_covariance:
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
angular_velocity:
x: 0.0
y: 0.0
z: 0.0
angular_velocity_covariance:
- 0.0010000000474974513
- 0.0
- 0.0
- 0.0
- 0.0010000000474974513
- 0.0
- 0.0
- 0.0
- 0.0010000000474974513
linear_acceleration:
x: -0.004790039267390966
y: -0.0718505890108645
z: 9.769285085843876
linear_acceleration_covariance:
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
---
Then move the IMU in all directions to see the messages changed as in the test_sensors.
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.
- lino_base_config.h - serial transport, 2WD, teensy.
The configuration file is selected with platformio [env]. eg, "pio run -e esp32" selects esp32_config.h and "pio run -e pico" selects pico_config.h. Use the env and configuration file which you will use for your robot. For teensy v4.0, use "pio run -e teensy40" and lino_base_config.h.
IMPORTANT NOTE: IMU is NOT enabled in these examples
Only gendrv and gendrv_wifi have the IMU and magnetometer configured. The other examples do not have IMU configured. After the initial micro-ROS agent connection test, you will need to uncomment the IMU you will use to enable IMU driver. eg. "#define USE_MPU6050_IMU".
- USE_GENERIC_2_IN_MOTOR_DRIVER : 2 Direction pins(INA, INB) and 1 PWM(ENABLE) pin
- USE_GENERIC_1_IN_MOTOR_DRIVER : 1 Direction pin(INA) and 1 PWM(ENABLE) pin
- USE_BTS7960_MOTOR_DRIVER : 2 PWM pins (INA, INB)
- USE_ESC_MOTOR_DRIVER : 1 ESC Servo pin for brush-less motor
For brushed DC motors, USE_BTS7960_MOTOR_DRIVER, 2 PWM pins (INA, INB) is preferred to reduce the number of I/O pins used. Most full bridge motor drivers can be used in this mode with PWM/ENABLE pins pulled high.
The choice of motor controller depends on battery voltage, motor current rating and robot/load weight.
- for 8.4V 2S li-ion battery - L9110 (12V 0.8A) DRV8833 (10.8V, 1A)
- for 12.6V 2S li-ion battery - TB6612 (15V 1.2A)
- for higher voltage, higher current - L298N A4950 VNH5019 DRV8871 BTS7960
L298N is bipolar device, which has higher energy loss and generates more heat. Modern MOSFET devices such as TB6612 are preferred.
For brush-less motors, ESC motor driver is often used. However, some BLDC motor controllers such as Odrive are more powerful than esp32. They should be connected to robot computer directly. There is no need for esp32 micro-ROS controller in such use cases.
- IMU - GY85, MPU6050 (GY521), 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 don't have the LDO. Check out carefully before connecting the power. If it has the LDO, connect the VCC on the sensor module to 5V. If it does not have the LDO, connect the VCC to 3.3V.
The 6 pins headers on motors with encoders and the controller PCBs may have different pins assignments. Check the specifications before wiring.
- M+ M- GND VCC A B
- M- VCC A B GND M+
- M- VCC GND B A M+
Most hall encoders can work with VCC = 3.3V. But some encoders output are 5V logic level. The esp32 and pico are 3.3V tolerant. Logic level shifter should be used in this case.
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.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.
If you mount the IMU in different orientation (eg upside down), you will need to modify the imu link in URDF.
The esp32/pico/teensy require external 2K Ohm resisters pull up on the I2C SDA SCL line to the 3.3V output of the esp32/pico/teensy. Because I2C interfaces of esp32/pico/teensy can be assign to other I/O pins, there are no I2C pull up on the esp32/pico/teensy modules. The pull up have to be added externally.
SDA -- 2K -- 3V3 SCL -- 2K -- 3V3
-
all-in-one: e.g. Waveshare Generat Driver for Robots.
-
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.
ESP hardware Design Guidelines
The PCA9685 i2c 16ch pwm controller can be used as pwm/output extender. But the PWM rate is limited to 1.5KHz. The default/preferred PWM frequency is 20KHz.
Most i/o pins on the micro-controllers are configurable with some constrain. The following lists are the i/o pins assignments used in examples configuration. 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.
- 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.
- 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.
- 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
- 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
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.
Robot Orientation:
-------------FRONT-------------
WHEEL1 WHEEL2 (2WD)
WHEEL3 WHEEL4 (4WD)
--------------BACK--------------
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.
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 linorobot2_hardware/test_motors
pio run -e esp32 -t upload
pio device monitor -e esp32
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.
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 might be issues in the battery or wiring.
cd linorobot2_hardware/test_acc
pio run -e esp32 -t upload
pio device monitor -e esp32
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
Install linorobot2. Enter 'y' to the questions. This will create ~/linorobot2_ws workspace and install the lidar driver. Replace 2wd and ld19 with the robot_type and laser_sensor you will use. Check linorobot2 for complete list of lidar and depth sensors.
cd /tmp
wget https://github.com/hippo5329/linorobot2/raw/rolling/install_linorobot2.bash
bash install_linorobot2.bash 2wd ld19
source ~/.bashrc
robot_type:
-
2wd- 2 wheel drive robot. -
4wd- 4 wheel drive robot. -
mecanum- Mecanum drive robot.
laser_sensor:
-
a1- RPLIDAR A1 -
a2m12- RPLIDAR A2M12 -
ld19- LD19/LD300 LIDAR -
ydlidar- YDLIDAR
Configure the URDF xarco which matches your target. eg. 2wd
ros2_ws/src/linorobot2/linorobot2_description/urdf/2wd_properties.urdf.xacro
ONLY "base_length" and "base_width" needs to be changed to the size of your robot. The other parameters can keep intact.
Build and upload the firmware.
cd linorobot2_hardware/firmware
pio run -e esp32 -t upload
ros2 launch linorobot2_bringup bringup.launch.py
Screen output.
ubuntu@cc187:~$ ros2 launch linorobot2_bringup bringup.launch.py
[INFO] [launch]: All log files can be found below /home/ubuntu/.ros/log/2024-05-28-10-22-38-917812-cc187-170377
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ekf_node-1]: process started with pid [170379]
[INFO] [micro_ros_agent-2]: process started with pid [170381]
[INFO] [joint_state_publisher-3]: process started with pid [170383]
[INFO] [robot_state_publisher-4]: process started with pid [170392]
[INFO] [ldlidar_stl_ros2_node-5]: process started with pid [170396]
[micro_ros_agent-2] [1716916960.293585] info | TermiosAgentLinux.cpp | init | running... | fd: 3
[micro_ros_agent-2] [1716916960.293837] info | Root.cpp | set_verbose_level | logger setup | verbose_level: 4
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336181034] [ld19]: LDLiDAR SDK Pack Version is: v3.0.3
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336292574] [ld19]: <product_name>: LDLiDAR_LD19
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336313025] [ld19]: <topic_name>: scan
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336327662] [ld19]: <frame_id>: laser
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336341563] [ld19]: <comm_mode>: serial
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336355283] [ld19]: <port_name>: /dev/ttyUSB0
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336369115] [ld19]: <port_baudrate>: 230400
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336382710] [ld19]: <server_ip>: 0.0.0.0
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336396143] [ld19]: <server_port>: 8889
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336409761] [ld19]: <laser_scan_dir>: Counterclockwise
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336423576] [ld19]: <enable_angle_crop_func>: false
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336437080] [ld19]: <angle_crop_min>: 135.000000
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336455058] [ld19]: <angle_crop_max>: 225.000000
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336470307] [ld19]: <bins>: 456
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336485130] [ld19]: <enable_box_crop_func>: false
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336500125] [ld19]: <x_crop_min>: 0.000000
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336518785] [ld19]: <x_crop_max>: 0.000000
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336533399] [ld19]: <y_crop_min>: 0.000000
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336547697] [ld19]: <y_crop_max>: 0.000000
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.336562007] [ld19]: <pub_rate>: 10.000000
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.338877084] [ld19]: ldlidar node start is success
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.344272276] [ld19]: ldlidar communication is normal.
[ldlidar_stl_ros2_node-5] [INFO] [1716916960.346040144] [ld19]: Publish topic message:ldlidar scan data.
[robot_state_publisher-4] [INFO] [1716916960.352140476] [robot_state_publisher]: got segment base_footprint
[robot_state_publisher-4] [INFO] [1716916960.352253461] [robot_state_publisher]: got segment base_link
[robot_state_publisher-4] [INFO] [1716916960.352273291] [robot_state_publisher]: got segment camera_depth_link
[robot_state_publisher-4] [INFO] [1716916960.352287425] [robot_state_publisher]: got segment camera_link
[robot_state_publisher-4] [INFO] [1716916960.352300992] [robot_state_publisher]: got segment front_caster_wheel_link
[robot_state_publisher-4] [INFO] [1716916960.352315075] [robot_state_publisher]: got segment imu_link
[robot_state_publisher-4] [INFO] [1716916960.352329027] [robot_state_publisher]: got segment laser
[robot_state_publisher-4] [INFO] [1716916960.352342915] [robot_state_publisher]: got segment left_wheel_link
[robot_state_publisher-4] [INFO] [1716916960.352356542] [robot_state_publisher]: got segment rear_caster_wheel_link
[robot_state_publisher-4] [INFO] [1716916960.352370202] [robot_state_publisher]: got segment right_wheel_link
[joint_state_publisher-3] [INFO] [1716916960.684028436] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...
[micro_ros_agent-2] [1716916960.783111] info | Root.cpp | create_client | create | client_key: 0x3FE028A5, session_id: 0x81
[micro_ros_agent-2] [1716916960.783159] info | SessionManager.hpp | establish_session | session established | client_key: 0x3FE028A5, address: 0
[micro_ros_agent-2] [1716916960.801364] info | ProxyClient.cpp | create_participant | participant created | client_key: 0x3FE028A5, participant_id: 0x000(1)
[micro_ros_agent-2] [1716916960.804886] info | ProxyClient.cpp | create_topic | topic created | client_key: 0x3FE028A5, topic_id: 0x000(2), participant_id: 0x000(1)
[micro_ros_agent-2] [1716916960.808010] info | ProxyClient.cpp | create_publisher | publisher created | client_key: 0x3FE028A5, publisher_id: 0x000(3), participant_id: 0x000(1)
[micro_ros_agent-2] [1716916960.811465] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0x3FE028A5, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[micro_ros_agent-2] [1716916960.815189] info | ProxyClient.cpp | create_topic | topic created | client_key: 0x3FE028A5, topic_id: 0x001(2), participant_id: 0x000(1)
[micro_ros_agent-2] [1716916960.818239] info | ProxyClient.cpp | create_publisher | publisher created | client_key: 0x3FE028A5, publisher_id: 0x001(3), participant_id: 0x000(1)
[micro_ros_agent-2] [1716916960.821501] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0x3FE028A5, datawriter_id: 0x001(5), publisher_id: 0x001(3)
[micro_ros_agent-2] [1716916960.825721] info | ProxyClient.cpp | create_topic | topic created | client_key: 0x3FE028A5, topic_id: 0x002(2), participant_id: 0x000(1)
[micro_ros_agent-2] [1716916960.828824] info | ProxyClient.cpp | create_publisher | publisher created | client_key: 0x3FE028A5, publisher_id: 0x002(3), participant_id: 0x000(1)
[micro_ros_agent-2] [1716916960.832197] info | ProxyClient.cpp | create_datawriter | datawriter created | client_key: 0x3FE028A5, datawriter_id: 0x002(5), publisher_id: 0x002(3)
[micro_ros_agent-2] [1716916960.836300] info | ProxyClient.cpp | create_topic | topic created | client_key: 0x3FE028A5, topic_id: 0x003(2), participant_id: 0x000(1)
[micro_ros_agent-2] [1716916960.839269] info | ProxyClient.cpp | create_subscriber | subscriber created | client_key: 0x3FE028A5, subscriber_id: 0x000(4), participant_id: 0x000(1)
[micro_ros_agent-2] [1716916960.842803] info | ProxyClient.cpp | create_datareader | datareader created | client_key: 0x3FE028A5, datareader_id: 0x000(6), subscriber_id: 0x000(4)
The bringup launch will pause and wait for initial pose, which will be set by navigation lauch.
[joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...
On another terminal, check the node and topic.
ubuntu@cc187:~$ ros2 node list
/ekf_filter_node
/joint_state_publisher
/ld19
/pico
/robot_state_publisher
/transform_listener_impl_610ff0739b50
ubuntu@cc187:~$ ros2 topic list -t
/battery [sensor_msgs/msg/BatteryState]
/cmd_vel [geometry_msgs/msg/Twist]
/diagnostics [diagnostic_msgs/msg/DiagnosticArray]
/imu/data [sensor_msgs/msg/Imu]
/joint_states [sensor_msgs/msg/JointState]
/odom [nav_msgs/msg/Odometry]
/odom/unfiltered [nav_msgs/msg/Odometry]
/parameter_events [rcl_interfaces/msg/ParameterEvent]
/robot_description [std_msgs/msg/String]
/rosout [rcl_interfaces/msg/Log]
/scan [sensor_msgs/msg/LaserScan]
/set_pose [geometry_msgs/msg/PoseWithCovarianceStamped]
/tf [tf2_msgs/msg/TFMessage]
/tf_static [tf2_msgs/msg/TFMessage]
Check /imu/data. linear_acceleration x,y,z should close to 0,0,9.8. angular_velocity x,y,z should be all zero. If you have a magnetometer, check /imu/data_raw instead of /imu/data. Then check the /imu/mag, magnetic_field should be in the range of +0.00005 ~ -0.00005 (Tesla), and the Madgwick filtered /imu/data.
ubuntu@cc187:~$ ros2 topic echo /imu/data
header:
stamp:
sec: 1716918508
nanosec: 47000000
frame_id: imu_link
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
orientation_covariance:
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
angular_velocity:
x: 0.0
y: 0.0
z: 0.0
angular_velocity_covariance:
- 0.0010000000474974513
- 0.0
- 0.0
- 0.0
- 0.0010000000474974513
- 0.0
- 0.0
- 0.0
- 0.0010000000474974513
linear_acceleration:
x: -0.004790039267390966
y: -0.0718505890108645
z: 9.769285085843876
linear_acceleration_covariance:
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
- 0.0
- 0.0
- 0.0
- 0.009999999776482582
---
Check /odom, which is the output of EKF filter. pose.orientation x,y,z,w should close to 0,0,0,1. pose.position, twist.linear and twist.angular should be all zero. The /odom will change when the robot moves.
ubuntu@cc187:~$ ros2 topic echo /odom
header:
stamp:
sec: 1716918599
nanosec: 390000000
frame_id: odom
child_frame_id: base_footprint
pose:
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.05265238706350413
w: 0.9986129010465041
covariance:
- 7.394473076915929
- -1.522012327097571e-22
- 0.0
- 0.0
- 0.0
- 0.0
- 1.9852334701272664e-23
- 7.394473076915929
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 4.99818971766944e-07
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 4.996382054193254e-07
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 4.996382054193254e-07
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 8.871597938581186
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: -5.0e-324
covariance:
- 0.0005247624066372413
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0005247624066372413
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 4.997285559031791e-07
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 4.989177430895882e-07
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 4.989177430895882e-07
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0003969137640789713
---
Check /joint_states. There should be left_wheel_joint and right_wheel_joint. There will be no changes in position. You will notice that the wheels do not rotate on RVIZ when the robot moves, because the robot does not send joint updates (not implemented). In simulation, the wheeels rotate becuase Gazebo generates the tf.
ubuntu@cc187:~$ ros2 topic echo /joint_states
header:
stamp:
sec: 1716920212
nanosec: 99621935
frame_id: ''
name:
- left_wheel_joint
- right_wheel_joint
position:
- 0.0
- 0.0
velocity: []
effort: []
---
Check the rate of topic. /imu/data should be close to 50. /odom should be close to 50. /joint_states should be close to 10. /scan should be close to 10 (depend on lidar).
ubuntu@cc187:~$ ros2 topic hz /imu/data
average rate: 49.911
min: 0.017s max: 0.023s std dev: 0.00086s window: 51
average rate: 50.011
min: 0.017s max: 0.023s std dev: 0.00072s window: 102
average rate: 50.007
min: 0.017s max: 0.023s std dev: 0.00084s window: 153
ubuntu@cc187:~$ ros2 topic hz /odom
average rate: 47.124
min: 0.020s max: 0.040s std dev: 0.00480s window: 49
average rate: 47.552
min: 0.020s max: 0.040s std dev: 0.00442s window: 97
average rate: 48.027
min: 0.020s max: 0.040s std dev: 0.00397s window: 146
ubuntu@cc187:~$ ros2 topic hz /joint_states
average rate: 9.999
min: 0.100s max: 0.100s std dev: 0.00010s window: 12
average rate: 9.999
min: 0.100s max: 0.100s std dev: 0.00011s window: 22
average rate: 10.000
min: 0.100s max: 0.100s std dev: 0.00011s window: 32
ubuntu@cc187:~$ ros2 topic hz /scan
average rate: 10.001
min: 0.100s max: 0.100s std dev: 0.00011s window: 12
average rate: 10.001
min: 0.100s max: 0.100s std dev: 0.00010s window: 23
average rate: 10.000
min: 0.100s max: 0.100s std dev: 0.00010s window: 33
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
It will be a bit confusing, because both the esp32 and lidar use the same interface name, ttyUSBx. The sequence of ttyUSB0 and ttyUSB1 does not persist. It will be difficult to find who is who. So we can not use ttyUSBx to identify them. The safe way is using the by-path names.
First, plug only the esp32.
$ 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
The long name, /dev/serial/by-path/pci-0000:00:14.0-usb-0:2.4.2:1.0-port0, will identify the USB port which esp32 pluged.
Next, plug the lidar.
$ 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
The long name, /dev/serial/by-path/pci-0000:00:14.0-usb-0:2.4.4:1.0-port0, will idntify the USB port which lidar pluged.
Use your by-path device names for the base_serial_port and lidar_serial_port to run the bringup launch. Do not use mine. Now we can launch bringup without mistakes.
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 add "micro_ros_baudrate" pararmeter, eg, micro_ros_baudrate:=921600.
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.
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
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.
The EKF (ekf.yaml) takes vx vy yaw of /odom/unfiltered and yaw of /imu/data to compute the /odom. 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]
After the update, run "colcon build" in linorobot2_ws dir. The launch bringup. Depend on the robot configuration, you may need additional parameters for micro-ROS and lidar.
cd linorobot2_ws
colcon build
ros2 launch linorobot2_bringup bringup.launch.py madgwick:=true orientation_stddev:=0.01
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 -y ros-$ROS_DISTRO-robot-calibration
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 }
In another terminal, start slam.
ros2 launch linorobot2_navigation slam.launch.py rviz:=true
In another terminal, save the map.
ros2 run nav2_map_server map_saver_cli -f <map_name> --ros-args -p save_map_timeout:=10000.
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
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 max/min velocity and acceleration limits in velocity_smoother. Otherwise, the robot may rotate in RVIZ.
linorobot2_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.
cd linorobot2_ws
colcon build
The covariance matrix of various sensors messages is important to the navigation stack. You may fine tune the covariance matrix in the configuration file for best estimation of robot's pose.
Imu Messsage /imu/data or /imu/data_raw
#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 }
MagneticField Message /imu/mag
#define MAG_COV { 1e-12, 1e-12, 1e-12 }
Odometry Message /odom/unfiltered
#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 orientation_covariance in /imu/data output from madgwick filter with the orientation_stddev parameter.
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
For linorobot2, the serial baud rate needs to be higher than 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
While esp32 modules with CP2102 serial USB bridges support baud rate up to 921600, those esp32 modules with CH341/CH343 and CP2102N as serial bridges work well at 2000000 baud. The micro-ROS middle ware will run even smoother at this speed.
To get firmware updates from the repository.
# save your changes
git add .
git commit -m "my changes"
# rebase the updates
git fetch
git rebase origin/master
If you cloned linorobot/linorobot2_hardware before, you can add this fork as remote and check out.
cd linorobot2_hardware/firmware
# commit your previous changes
git add .
git commit -m "my lino"
# add this fork as remote
git remote add hippo https://github.com/hippo5329/linorobot2_hardware.git
git fetch hippo
git checkout -b hippo hippo/master
To get firmware updates.
# save your changes
git add .
git commit -m "my changes"
# rebase the updates
git fetch hippo
git rebase hippo/master
Use the same procedures as jazzy and replace jazzy with foxy. However, rosdep seems failed to work. The packages dependency need to be handled manually before linorobot2 installation.
sudo apt install -y ros-foxy-imu-tools ros-foxy-robot-localization ros-foxy-joy-linux \
ros-foxy-xacro ros-foxy-depth-image-proc ros-foxy-joint-state-publisher \
ros-foxy-image-proc ros-foxy-laser-filters ros-foxy-navigation2 \
ros-foxy-nav2-bringup ros-foxy-turtlebot3-gazebo
Build the firmware with jazzy setting. It will work with micro-ROS agent on Foxy. Uncomment the line of your board env in platformio.ini.
board_microros_distro = jazzy
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.
cd linorobot2_hardware
cp config/custom/esp32_config.h config/custom/myrobot_config.h
NOTE: IMU is not configured in these examples configuration files.
Change only the IMU setting. Uncomment the IMU you will use. eg. "#define USE_MPU6050_IMU". You will update the other setting after a test build.
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 to check the initial configuration.
cd linorobot2_hardware/firmware
pio run -e myrobot
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. You may use vscode to commit or use command line.
git add ../config/custom/myrobot_config.h
git add .
git commit -m "create myrobot initial configuration"
After a successful build. You can update the other settings, such as robot size, wheels, motors, motor drivers, sensors and pins assignments. Then git commit to save your changes.
Mecanum drive robot built with rpi5, pico, MC200 chassis and ld19 lidar
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
Video of A ROS2 robot tank built on esp32. Navigating..
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.
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
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
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.
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.
linorobot2_hardware/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 =========================
The battery capacity percentage estimation in the /battery topic can enabled with the battery parameters. For Li-ion battery, the minimum voltage (empty) of a single cell is 3.0V typically, and the maximum voltage (full) is 4.2V. For 3S battery pack, the minimum voltage is 3.0V x3 = 9.0V and the maximum voltage is 4.2V x3 = 12.6V. The capacity of 18650 is around 2000mAh = 2.0Ah.
#define BATTERY_CAP 2.0 // battery capacity Ah
#define BATTERY_MIN 9.0 // battery minimal voltage
#define BATTERY_MAX 12.6 // battery maximum voltage
This is the /battery message of my 12.6V 4.9Ah battery pack.
header:
stamp:
sec: 1716351874
nanosec: 821621000
frame_id: ''
voltage: 12.357491493225098
temperature: 0.0
current: .nan
charge: .nan
capacity: 4.793232440948486
design_capacity: 4.900000095367432
percentage: 0.9782107472419739
power_supply_status: 0
power_supply_health: 0
power_supply_technology: 0
present: true
cell_voltage: []
cell_temperature: []
location: ''
serial_number: ''
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 linorobot2_hardware/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.
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
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-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 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
pip install esptool
esptool.py -p /dev/ttyUSB0 flash_id
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
To connect I2C devices through USB to laptop or rapi, the CH341T I2C dongle can be used. It costs around USD3 on Amazon.
git clone https://github.com/frank-zago/ch341-i2c-spi-gpio
cd ch341-i2c-spi-gpio
make
sudo make install
sudo su -
echo 'blacklist ch341' >> /etc/modprobe.d/blacklist.conf
groupadd ch341
adduser ubuntu ch341 ; use your user name
echo 'SUBSYSTEMS=="usb" ATTRS{idProduct}=="5512" ATTRS{idVendor}=="1a86" GROUP="ch341" MODE="0660"' > /etc/udev/rules.d/99-ch341.rules
service udev restart
exit
root@cc187:~# i2cdetect -l
i2c-0 smbus SMBus I801 adapter at 5040 SMBus adapter
i2c-1 i2c i915 gmbus dpc I2C adapter
i2c-2 i2c i915 gmbus dpb I2C adapter
i2c-3 i2c i915 gmbus dpd I2C adapter
i2c-4 i2c AUX A/DDI A/PHY A I2C adapter
i2c-5 i2c AUX D/DDI D/PHY D I2C adapter
i2c-6 i2c CH341 I2C USB bus 001 device 029 I2C adapter
root@cc187:~# i2cdetect -y 6
0 1 2 3 4 5 6 7 8 9 a b c d e f
00: -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- --
Assume the robot moves in plane, no up/down hill, then you need to decide the maximum acceleration/de-acceleration to reach the speed/time requirement. F=ma. Find out the maximum total torque requirement. The Mecanum drive is 4WD. Divided by 4 to get the torque required for single motor. Say max speed 0.5m/s, wheel diameter 10cm, time to max speed 0.5s, 10Kg load, acc = 0.5/0.5 = 1, F = 10x1=10Kg, 10/4=2.5, Torque = 2.5x5=12.5Kg/cm, 0.5x60 / (0.1x3.14)=95.54rpm. so the motor requirement is 12.5Kg/cm, ~100rpm. If you change time to max speed to 1s, the torque requirement will be 6.25Kg/cm.
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:
-
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.
-
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.
-
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_RATIOset 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_REVconstant during calibration and actual maximum RPM of the motors. For instance, a robot withMOTOR_OPERATING_VOLTAGEof 24V with aMOTOR_POWER_MAX_VOLTAGEof 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'sCOUNTS_PER_REVconstant. 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_VOLTAGEandMOTOR_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.
-
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
-
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)
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.
Waveshare General Driver for Robot
Once the hardware is done, you can go back to linorobot2 package and start defining the robot's URDF.
- 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 theifdef USE_GENERIC_2_IN_MOTOR_DRIVERmacro.
- 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.
- 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.
- Check if the Teensy's GND pin is connected to the motor driver's GND pin.
- Check if the Teensy's GND pin is connected to the motor driver's GND pin.
-
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.rulesRemember to restart your computer if you just copied the udev rule.
- 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.
- 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_RATIOlower to allow the PID controller to compensate for errors.
- 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_RATIOlower to allow the PID controller to compensate for errors while moving to avoid huge accumulative errors when the robot stops.
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 = jazzy" in your board env in platformio.ini.