⚡ AV Elec Stack

ROS 2 Electronics Stack for a 4-Wheel Mecanum Drive Robot

Complete bridge between ROS 2 navigation software and embedded motor controllers for an autonomous vehicle — inverse/forward kinematics, serial handshake protocol, and IMU integration.

📋 Table of Contents

Overview
System Architecture
Hardware
ROS 2 Nodes
ROS Topics
Serial Communication Protocol
Firmware
Directory Structure
Getting Started
Usage
Maintainer

Overview

The AV Elec Stack is the electronics and software backbone for a mecanum-wheeled autonomous vehicle. It provides:

Inverse kinematics — converts cmd_vel (Twist) into individual wheel angular velocities (ω)
Forward kinematics — converts encoder feedback back into body velocities for odometry
MCU serial bridge — reliable handshake-based communication with Teensy 4.1 motor controllers
ESP32 camera/Z-axis bridge — serial interface for a Z-axis stepper + theta motor subsystem
IMU integration — BNO055 9-DOF IMU data (roll, pitch, yaw + linear acceleration)

System Architecture

graph LR
    subgraph "ROS 2 (Host PC)"
        JOY["🎮 Joystick\n(joy_node)"]
        NAV["🧭 Nav Stack\n(Software)"]

        V2O["vel_to_omega\n(Inverse Kinematics)"]
        MCU["mcu_feedback\n(Serial Bridge)"]
        FWD["forward_kinematics\n(Forward Kinematics)"]

        JOY -- "/cmd_vel" --> V2O
        NAV -- "/cmd_vel" --> V2O
        V2O -- "/omegas" --> MCU
        MCU -- "/omega" --> FWD
        FWD -- "/fdb_cmd_vel" --> NAV
        MCU -- "/imu/data" --> NAV
    end

    subgraph "Hardware"
        T41["Teensy 4.1\n(Motor + IMU)"]
        ESP["ESP32\n(Z-axis + Theta)"]
        M1["Motor 1 (FL)"]
        M2["Motor 2 (FR)"]
        M3["Motor 3 (RL)"]
        M4["Motor 4 (RR)"]
        IMU["BNO055 IMU"]
        ZAX["Z-Axis Stepper"]
    end

    MCU <-- "USB Serial\n9600 baud" --> T41
    T41 -- "Modbus ASCII\n(Serial1-8)" --> M1
    T41 --> M2
    T41 --> M3
    T41 --> M4
    T41 <--> IMU

    ESP <-- "USB Serial" --> ESP_NODE["esp_feedback\n(Serial Bridge)"]
    ESP --> ZAX

Hardware

Component	Model	Role
Main MCU	Teensy 4.1	4-motor control via Modbus ASCII + BNO055 IMU
Aux MCU	ESP32	Z-axis stepper + theta motor (camera positioning)
IMU	Adafruit BNO055	9-DOF orientation & linear acceleration (I²C on Wire1)
Motor Drivers	Modbus ASCII VFDs	4× BLDC/induction motors at 9600 baud
Wheels	Mecanum (100mm radius)	4-wheel omnidirectional drive

Robot Dimensions

Parameter	Value
Wheel radius (`a`)	0.10 m
Wheelbase length (`L`)	0.444 m
Track half-width (`D`)	0.184 m

ROS 2 Nodes

`vel_to_omega` — Inverse Kinematics

Subscribes to /cmd_vel (Twist) and computes individual wheel angular velocities using mecanum inverse kinematics:

ω_FL = (1/a) × (Vx - Vy - (L+D) × ωz)
ω_FR = (1/a) × (Vx + Vy + (L+D) × ωz)
ω_RL = (1/a) × (Vx + Vy - (L+D) × ωz)
ω_RR = (1/a) × (Vx - Vy + (L+D) × ωz)

Includes configurable velocity scaling factors (vx_multiplier = 22.03, etc.) to match the real robot's capabilities.

Publishes: Float32MultiArray on /omegas — [FL, FR, RL, RR]

`mcu_feedback` — Teensy Serial Bridge

The main communication bridge between ROS and the Teensy 4.1.

Subscribes to /omegas → sends #ω1 ω2 ω3 ω4* to Teensy over USB serial
Receives $rpm1 rpm2 rpm3 rpm4 imu_data& from Teensy
Publishes wheel speeds on /omega and IMU data on /imu/data
Implements a handshake protocol on startup (see Serial Protocol)
Auto-reconnects on serial failure

Serial Port: /dev/serial/by-id/usb-Teensyduino_USB_Serial_18632580-if00

`forward_kinematics` — Odometry Feedback

Converts encoder-measured wheel angular velocities back into body-frame velocities:

Vx = (r/4) × (ωFL + ωFR + ωRL + ωRR)
Vy = (r/4) × (-ωFL + ωFR + ωRL - ωRR)
ωz = (r / 4(L+W)) × (-ωFL + ωFR - ωRL + ωRR)

Includes a complementary filter (α = 0.85) for noise smoothing.

Publishes: Twist on /fdb_cmd_vel

ROS Topics

Topic	Type	Direction	Description
`/cmd_vel`	`geometry_msgs/Twist`	Input	Commanded body velocities
`/omegas`	`std_msgs/Float32MultiArray`	Internal	Wheel ω commands → MCU
`/omega`	`std_msgs/Float32MultiArray`	Internal	Wheel ω feedback ← MCU
`/imu/data`	`std_msgs/Float32MultiArray`	Output	IMU data from BNO055
`/fdb_cmd_vel`	`geometry_msgs/Twist`	Output	Feedback body velocities
`/camera_pose`	`std_msgs/Float32MultiArray`	Input	Z-axis + theta commands
`/Z_axis_height`	`std_msgs/Float32MultiArray`	Internal	Height commands to ESP32
`/Z_axis_software`	`std_msgs/Float32MultiArray`	Input	Height setpoint from software

Serial Communication Protocol

Handshake (Startup)

Teensy → PC:  "Started\n"          (repeated every 500ms)
   PC → Teensy:  "++\n"            (acknowledgement)
Teensy → PC:  "Received acknowledgement"
                                    → data exchange begins

If data arrives before handshake: PC sends "--\n" to request restart.

Data Framing

Direction	Format	Example
PC → Teensy	`#val1 val2 val3 val4*\n`	`#120.5 -85.3 120.5 -85.3*`
Teensy → PC	`$val1 val2 val3 val4 imu...&\n`	`$100 95 100 95 0.5 -0.3 9.8&`

Motor Control (Teensy → VFD)

Uses Modbus ASCII protocol at 9600 baud over Serial1/2/7/8:

Command	Modbus Hex	Description
Enable CW	`010600020101`	Clockwise rotation
Enable CCW	`010600020109`	Counter-clockwise rotation
Brake	`010600020103`	Active braking
Disable	`010600020100`	Motor off
Set Frequency	`01060006XXXX`	Set speed (freq = 14×4×RPM/60)

Firmware

`firmware/teensy/FinalTeensy.ino`

Production firmware for the Teensy 4.1:

Controls 4 motors via Modbus ASCII on Serial1, Serial2, Serial7, Serial8
Reads BNO055 IMU over I²C (Wire1)
Implements the handshake protocol for reliable startup
Receives wheel ω commands, sets motor direction and RPM
Sends back motor RPM feedback + IMU orientation data
Supports software reboot command ('R')

`firmware/esp32/ESP32Code.ino`

Production firmware for the ESP32 Z-axis controller:

Stepper motor control for vertical (Z) positioning
28BYJ-48 stepper for theta (angle) rotation
Limit switch homing with ISR-based crash detection
Accepts height + angle commands via serial
Auto-homes on power-up and after crash events

Directory Structure

AV_Elec_Stack/
├── firmware/
│   ├── teensy/
│   │   └── FinalTeensy.ino          # Teensy 4.1 motor + IMU firmware
│   └── esp32/
│       └── ESP32Code.ino            # ESP32 Z-axis controller firmware
├── src/
│   └── elecstack/
│       ├── elecstack/               # Python ROS 2 nodes
│       │   ├── __init__.py
│       │   ├── mcu_feedback.py      # Teensy serial bridge node
│       │   ├── vel_to_omega.py      # Inverse kinematics node
│       │   ├── forward_kinematics.py# Forward kinematics node
│       │   ├── esp_feedback.py      # ESP32 serial bridge node
│       │   ├── Zaxis_reading.py     # Z-axis height relay node
│       │   ├── Z_axis_testing.py    # Z-axis serial test node
│       │   └── testing_node.py      # Debug/test publisher node
│       ├── launch/
│       │   ├── elec.launch.py       # Main launch (mcu_feedback + vel_to_omega)
│       │   └── joylaunch.py         # Dual joystick launch
│       ├── test/                    # Linting & style tests
│       ├── package.xml
│       ├── setup.py
│       └── setup.cfg
└── README.md

Getting Started

Prerequisites

ROS 2 Humble (Ubuntu 22.04)
Python 3.10+
pyserial (pip install pyserial)

Build

# Clone the repository
git clone https://github.com/your-username/AV_Elec_Stack.git

# Navigate to your ROS 2 workspace
cd ~/ros2_ws/src
ln -s /path/to/AV_Elec_Stack/src/elecstack .

# Build
cd ~/ros2_ws
colcon build --packages-select elecstack
source install/setup.bash

Flash Firmware

Teensy 4.1: Open firmware/teensy/FinalTeensy.ino in Arduino IDE / PlatformIO with Teensyduino. Select Board → Teensy 4.1, upload.
ESP32: Open firmware/esp32/ESP32Code.ino in Arduino IDE. Select Board → ESP32 Dev Module, upload.

Usage

Launch the Electronics Stack

# Launch MCU feedback + inverse kinematics
ros2 launch elecstack elec.launch.py

# (Optional) Launch dual joystick nodes
ros2 launch elecstack joylaunch.py

Manual Testing

# Send a velocity command
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

# Monitor wheel omegas
ros2 topic echo /omegas

# Monitor feedback velocities
ros2 topic echo /fdb_cmd_vel

Maintainer

Madhav Pradheep — madhavpradeep2207@gmail.com

Built with ❤️ for autonomous mobility

License

This project is open source. See LICENSE for details.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

⚡ AV Elec Stack

📋 Table of Contents

Overview

System Architecture

Hardware

Robot Dimensions

ROS 2 Nodes

`vel_to_omega` — Inverse Kinematics

`mcu_feedback` — Teensy Serial Bridge

`forward_kinematics` — Odometry Feedback

ROS Topics

Serial Communication Protocol

Handshake (Startup)

Data Framing

Motor Control (Teensy → VFD)

Firmware

`firmware/teensy/FinalTeensy.ino`

`firmware/esp32/ESP32Code.ino`

Directory Structure

Getting Started

Prerequisites

Build

Flash Firmware

Usage

Launch the Electronics Stack

Manual Testing

Maintainer

License

FilesExpand file tree

README.md

Latest commit

History

README.md

File metadata and controls

⚡ AV Elec Stack

📋 Table of Contents

Overview

System Architecture

Hardware

Robot Dimensions

ROS 2 Nodes

vel_to_omega — Inverse Kinematics

mcu_feedback — Teensy Serial Bridge

forward_kinematics — Odometry Feedback

ROS Topics

Serial Communication Protocol

Handshake (Startup)

Data Framing

Motor Control (Teensy → VFD)

Firmware

firmware/teensy/FinalTeensy.ino

firmware/esp32/ESP32Code.ino

Directory Structure

Getting Started

Prerequisites

Build

Flash Firmware

Usage

Launch the Electronics Stack

Manual Testing

Maintainer

License

`vel_to_omega` — Inverse Kinematics

`mcu_feedback` — Teensy Serial Bridge

`forward_kinematics` — Odometry Feedback

`firmware/teensy/FinalTeensy.ino`

`firmware/esp32/ESP32Code.ino`