⚡ 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

Loading

---

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

1. Teensy 4.1: Open firmware/teensy/FinalTeensy.ino in Arduino IDE / PlatformIO with Teensyduino. Select Board → Teensy 4.1, upload.
2. 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.