Two-Wheel Self-Balancing Robot - ESP32 and Dynamixel

Overview

This is a small two-wheeled robot that balances on its own, like a mini Segway. It uses Dynamixel Motors with ESP32. The control board, and how the ESP32 talks to the motors over one wire, have their own guide: Single-Wire Half-Duplex UART on the ESP32.

The design aims to be simple to build:

• No custom PCB. The electronics are ready-made modules soldered onto a piece of perfboard.
• The frame is two 3D-printed plates (“decks”).
• No motor drivers or encoders. The motors are two ROBOTIS DYNAMIXEL XL430-W250-T smart servos running in speed mode, so they measure and control their own speed.

How it balances: an MPU6050 motion sensor measures how far the robot is leaning. An ESP32-S3 reads it 250 times per second and drives the wheels under the robot to catch it before it falls.

What’s included: firmware for the ESP32 (written with ESP-IDF), a Python control app with keyboard driving, live PID tuning and live plots, and test programs that check the wiring before you try to balance.

The finished robot.

How the control works

The controller is three loops, one inside the other:

Target speed (W/S keys)
  → SPEED LOOP (Speed P, I) ............ decides how far to lean
    → BALANCE LOOP (P, I, D) ............ moves the wheels to hold that lean
      → MOTOR SPEED LOOP (inside each XL430) ... makes the wheels reach that speed

• The tilt angle comes from the MPU6050: gyro and accelerometer readings are combined with a complementary filter (98 % gyro, 2 % accelerometer).
• At power-up the robot calibrates the gyro. Keep it completely still for a moment after switching on. If it moves, the log shows “robot moved - retrying” and it waits.

Built-in safety:

• Balancing only starts if the robot is within 15° of upright.
• The motors switch off if it tilts beyond 35° (fallen).
• If the app stops sending for 0.5 s (Wi-Fi lost), the robot stops driving.

Parts list

Electronics

Power

Wheels and frame

The two DYNAMIXEL XL430 motors with their TurtleBot3 wheels.

Tools

• Everything for the control board (see the half-duplex UART guide)
• Small Phillips screwdriver (M2), hex keys or screwdriver for M3
• Small pliers or a 5.5 mm nut driver for the standoffs
• 3D printer (PLA or PETG)
• Hot glue or tape for tidying wires (not for the sensor)
• USB-C data cable and a PC (Windows, Linux or Mac) with Wi-Fi

Build guide

Step 1: Build the control board

The control board (ESP32-S3, level converter, regulator, MPU6050 and motor sockets) and the motor setup have their own guide: Single-Wire Half-Duplex UART on the ESP32. Follow it up to the end of its Step 4 (connection checks), then come back here.

For this robot, set the motors up as follows:

• Left motor = ID 2, right motor = ID 1
• Baud rate 1 Mbps (register value 3)
• Operating mode 1 (velocity mode)

Fit the GY-521 (MPU6050); the robot needs it to measure its tilt.

The control board, built with the half-duplex UART guide.

Step 2: Run the hardware test

This test checks the whole board, the sensor and both motors before you assemble the robot.

1. Switch off. Plug in all modules (check orientation) and both motors.
2. Switch the battery on, then connect the ESP32 to the PC by USB.
3. Flash the hw_test firmware (see Step 5) and open the monitor.

It prints a PASS / WARN / FAIL report covering the ESP32, the MPU6050, the bus echo (TX–RX bridge), both motors, the battery voltage, and each wheel (it spins them slowly, so keep the wheels off the table). You want 0 FAIL. A WARN about the accelerometer offset is normal; software calibration fixes it.

Step 3: Print the frame

• Export Deck1_MotorBase and Deck2_Battery from Autodesk Inventor as STL (File → Save As → Save Copy As → STL).
• Material: PETG or PLA. Settings: 4 walls, 40 % infill, 0.2 mm layers.
• Deck 1: print flat with the side skirts pointing up. No supports.
• Deck 2: print flat. The switch pocket hangs underneath, so print it upside down or with supports under the pocket. Deck 2 can also be laser-cut from 3 mm acrylic or plywood (without the pocket).
• If the screws don’t fit, clean the holes by hand with a 2 mm and 3 mm drill bit.

Deck 1, the motor base with side skirts (top), and Deck 2, the battery deck (bottom).

Step 4: Assemble the robot

1. Set up the motor IDs first (Step 1: left = 2, right = 1, 1 Mbps, velocity mode).
2. Motors to Deck 1. The two XL430s hang under Deck 1 with the horn end down and the horn facing outward (left motor’s horn faces left, right motor’s faces right). Slide them between the skirts and fix them with M2 × 6 screws through the skirts (8 per motor).
3. Wheels. Screw each TB3 wheel onto its motor horn with 4 × M2 × 4 screws.

4. Motor cables. Plug a ROBOTIS cable into each motor and run it up through the cable hole in Deck 1. Zip-tie the cables to the middle bar so they can’t touch the wheels.

   Motors and wheels mounted on Deck 1.

5. Control board. Mount it on top of Deck 1 with 4 × M3 × 6 spacers and M3 screws. Plug the motor cables into J1 and J2 (each motor has its own ID, so either socket works).
6. MPU6050 placement (very important for balancing):
   • Mount it flat, directly above the wheel axle, centred front-to-back.
   • Line one of its edges up parallel to the axle.
   • Note which printed arrow (X or Y) points along the axle; you’ll choose the axis in the app later.
   • Fix it firmly with screws or stiff spacers. Not foam tape: foam lets it vibrate and the robot will shake.

   Control board mounted on Deck 1.

7. Standoffs. Screw the 4 × M3 × 30 standoffs onto Deck 1 with M3 × 6 screws from below.
8. Deck 2. Fit the power switch into its pocket (pins down through the slot). Screw Deck 2 onto the standoffs with 4 × M3 × 6 screws. Put the battery on Deck 2, centred front-to-back, and strap it down through the slots. Pass the battery lead through the lead slot.
9. Plug the LiPo voltage alarm into the battery’s balance lead.
10. Final check: spin each wheel by hand. Nothing should rub and no wire should touch a wheel. Gently shake the robot; nothing should rattle.

Keep the battery centred front-to-back. Any offset makes the robot lean and drift.

Fully assembled.

Step 5: Install the software and flash the firmware

The software has these parts:

Install the tools (once):

1. Install ESP-IDF v6.1 using Espressif’s getting-started guide (docs.espressif.com → ESP-IDF → Get Started → ESP32-S3).
2. Install Python 3.10 or newer. On Windows, tick “Add Python to PATH”.

3. Install the app’s library:

   pip install PyQt6
   

Flash the hardware test. Switch the battery on first, then plug the ESP32 into the PC by USB (battery and USB together is safe). Find the port (Windows: Device Manager → Ports, e.g. COM5; Linux: /dev/ttyACM0). In the ESP-IDF terminal:

cd Software/hw_test
idf.py build
idf.py -p COM5 flash monitor

Replace COM5 with your port. You want 0 FAIL. Exit the monitor with Ctrl+].

Flash the balance firmware:

cd Software/balance
idf.py build
idf.py -p COM5 flash

Unplug USB. The robot now runs from the battery alone. Wi-Fi settings (network name, password, or joining your home Wi-Fi) can be changed with idf.py menuconfig → “Balance robot”.

Step 6: Connect the control app

1. Switch the robot on and keep it still for a second.
2. On the PC, join the robot’s Wi-Fi network: BalanceBot, password balance123 (no internet). Change these in menuconfig for your own build.

3. Start the app:

   cd Software/ui
   python robot_ui.py
   

4. Click Find robot (or Connect to 192.168.4.1). The app reads the robot’s saved settings into the boxes.
5. If it can’t connect, allow Python through the Windows firewall (private networks).

Modes

Step 7: Tune the robot

Tune from the inside out: setup → Balance P → Balance D → Speed P → Speed I → balance angle. Hold the robot over something soft (a bed or cushion). Space stops everything at any time.

1. Setup (mode Off, every session).
   • Tilt the robot forward by hand: “Pitch” must go positive. If it goes negative, tick “invert”. If it barely changes, switch the “IMU axis”.
   • Keep it perfectly still and press Calibrate gyro.
   • Hold it where it feels weightless (balanced over the wheels) and press Set balance angle = now.
   • Enter the start values: Balance P = 60, I = 0, D = 3; Speed P = 0, Speed I = 0; Motor vel. P = 400, I = 3840.
2. Balance P. Hold the robot upright, press Balance, and raise P in steps of about 20. Too low: it tips over slowly. Right: it catches itself but rocks quickly. Too high: violent shaking. Settle about 20 % below the point where shaking starts.
3. Balance D. Raise in steps of 0.5–1 until the rocking dies out quickly. If the motors buzz, back off. After this you can often raise P a little more.
4. Balance I. Leave it at 0. Only try 0.5–2 if the robot stands at a constant lean after the speed loop is tuned.
5. Speed P. The robot now stands but slowly rolls away. Start at 0.05 and raise in steps of 0.02 until it stays roughly in place and responds to W/S. A slow sway means it’s too high.
6. Speed I. Start at 0.02 and raise in steps of 0.01 until it holds its spot and returns after a gentle push. A growing sway means it’s too high.
7. Fine-tune the balance angle. If it always rolls one way: drives forward → lower the angle by 0.2–0.5°; drives backward → raise it.
8. Save. Press Save on robot; unsaved values are lost at power off.

Values that worked on this robot (reference only; every build differs): Balance P = 140, Balance D = 8, Speed P = 0.08, Speed I = 0.02.

If values that worked once feel worse later: the balance angle may have been set slightly differently, the gyro may have been calibrated while the robot moved, or the battery is low (charge it below ~10.5 V).

Driving speed

• The motors top out at about 55 rpm (~0.2 m/s with these wheels).
• Balancing needs spare wheel speed to catch the robot, so keep the drive speed around 30–35 rpm. Raise the Speed slider step by step (15, 20, 25, 30).
• Watch the orange line on the speed plot: if it hits ±55, the motors are maxed out and the robot will fall.
• Reaches speed slowly → raise Speed P by 0.02. Never quite reaches it → raise Speed I a little.

Troubleshooting

Safety

• LiPo batteries can catch fire if shorted, punctured or over-discharged. Always use the fuse, the voltage alarm and a proper balance charger. Never leave a charging LiPo unattended.
• A fully charged 3S pack (12.6 V) is slightly above the XL430’s 12 V rating. This is common practice (ROBOTIS TurtleBot3 does the same), but never use a 4S pack.
• Always let the robot sit still for a second after switching on (gyro calibration).
• Keep fingers clear of the wheels in Balance and Manual drive modes.

Credits: DYNAMIXEL protocol by ROBOTIS; firmware built on ESP-IDF by Espressif.