Micro-ROS Teensy 4.1 GPS + Motor Controller

Firmware for a Teensy 4.1 that publishes GPS and battery telemetry over micro-ROS and drives six motors with differential drive commands from cmd_vel. Built with PlatformIO and micro_ros_arduino (Humble) using serial transport.

Features

• Publishes std_msgs/Int32 counter, std_msgs/Float32 latitude, longitude, and battery voltage at 1 Hz.
• Subscribes to geometry_msgs/Twist (cmd_vel) and converts linear/angular velocity to left/right PWM.
• GPS fix, satellite count, and coordinates are reported over USB serial for debugging.
• Battery voltage and current sensing using Teensy ADC (voltage divider + ACS712 current sensor).
• Blinks built-in LED each publish cycle as a heartbeat.

Hardware

• MCU: Teensy 4.1
• GPS: Adafruit GPS (NMEA, 9600 baud)
• Battery sensing: voltage divider to pin 28; ACS712 current sensor on pin 8
• Motors: six PWM outputs with shared direction pins for differential drive

Pin map (from include/config.hpp)

Signal	Pin	Notes
GPS RX	0	From GPS TX
GPS TX	1	To GPS RX
Voltage sense	28	Divider ratio 8.85, 3.3 V ref
Current sense	8	ACS712-5A, 185 mV/A, 1.65 V offset
Left motors PWM	2, 3, 4	Shared direction DIR_L_PIN (26)
Right motors PWM	5, 6, 7	Shared direction DIR_R_PIN (27)
LED heartbeat	LED_BUILTIN	Toggles every publish

ROS topics

Name	Type	Direction
/teensy_counter	std_msgs/Int32	Publish
/teensy_latitude	std_msgs/Float32	Publish when GPS fix available
/teensy_longitude	std_msgs/Float32	Publish when GPS fix available
/teensy_voltage	std_msgs/Float32	Publish
/cmd_vel	geometry_msgs/Twist	Subscribe

Build & Flash (PlatformIO)

1. Install VS Code + PlatformIO extension.
2. Connect the Teensy 4.1 via USB.
3. Open this folder in VS Code (Microros_test.code-workspace optional).
4. Build: PlatformIO: Build (target env teensy41).
5. Upload: PlatformIO: Upload.
6. Serial monitor (optional): PlatformIO: Monitor at 115200 baud.

Key config: platformio.ini sets board_microros_distro = humble, board_microros_transport = serial, and depends on micro_ros_arduino + Adafruit GPS Library.

Run the micro-ROS agent on the host

Start the agent before powering the robot so the node can connect. Example (Linux):

# Adjust serial device and baud as needed
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0 -b 115200

On Windows, use the COM port shown in Device Manager, e.g. --dev COM5.

Operate

1. Power the robot and wait for "All modules initialized" on serial.
2. Verify topics:
   • ros2 topic list
   • ros2 topic echo /teensy_counter
3. Drive with teleop keyboard:
   • ros2 run teleop_twist_keyboard teleop_twist_keyboard cmd_vel:=/cmd_vel
4. Watch GPS data on serial; latitude/longitude publish once a fix is acquired.

Tuning & Calibration

• include/config.hpp holds voltage divider ratio, current sensor sensitivity, max velocities, PWM limits, and wheel separation.
• Update WHEEL_SEPARATION, MAX_LINEAR_VEL, and MAX_ANGULAR_VEL to match your chassis.
• If your voltage readings are off, re-measure the divider and adjust VOLTAGE_DIVIDER_RATIO.

Repository layout

• src/ main firmware modules: GPS, motors, battery monitor, ROS setup, callbacks.
• include/ matching headers.
• platformio.ini PlatformIO environment and dependencies.

Troubleshooting

• Fast blinking LED on boot indicates a micro-ROS initialization error (error_loop). Check agent connection and USB serial port.
• If no GPS data, ensure antenna lock and wiring to pins 0/1; baud set to 9600.
• If motors spin backwards, swap motor leads or invert direction pins in motor_control.cpp.