Self-balancing robot

A two-wheel robot that falls over unless your code balances it, with its own MPU6050, TB6612 driver and encoders.

It is the body of the "balance car" kits: a tall chassis on one axle, two gear motors with Hall encoders, a motor battery, and a board carrying an MPU6050 and a TB6612FNG. You wire a microcontroller to it. Let go of it and it is an inverted pendulum: gravity tips it further every millisecond unless the wheels drive under it, and the only thing driving the wheels is your sketch. The gallery project Balance bot balances it with a complementary filter and a PID.

Pins

Pin	What it does
VCC, GND	Logic supply for the IMU, the driver's logic and the encoders, 2.7 V to 5.5 V. The motors run from the robot's own battery.
SCL, SDA	The built-in MPU6050 at 0x68, with 4.7k pull-ups on the robot's board.
STBY	The TB6612's standby: high to run. It has a 200k pull-down, so unwired it is off.
AIN1, AIN2, PWMA	The left motor.
BIN1, BIN2, PWMB	The right motor.
LA, LB, RA, RB	The two quadrature encoders. Optional: leave them unwired if you do not count them.

Driving it

The motor inputs follow the TB6612FNG truth table, read with their exact timing, so analogWrite on PWMA is measured as the duty it really is:

IN1	IN2	PWM	Motor
H	L	H	forward
L	H	H	backward
H	L	L	short brake
L	H	L	short brake
H	H	any	short brake
L	L	any	coast

On this robot, IN1 high drives that wheel forward on both sides. Leaning forward, drive forward: the wheels run under the body and stand it back up.

Reading the tilt

The MPU6050 is mounted flat on the body with X pointing forward, Y to the left and Z up, so leaning forward is a positive rotation about Y:

float accelAngle = atan2(-ax, az) * 57.3;   // degrees, from gravity
float rate = gy / 131.0;                     // deg/s at the +/-250 range
angle = 0.98 * (angle + rate * dt) + 0.02 * accelAngle;

It is the full MPU6050 model, so it wakes asleep (write 0 to PWR_MGMT_1), and WHO_AM_I reads 0x68. The accelerometer feels the robot accelerating as well as gravity, exactly as the real one does, which is why balancing code blends it with the gyroscope instead of trusting it.

Held, falling, fallen

The robot starts held upright by a hand at tilt degrees, still, for release milliseconds, then it is let go: that gives the sketch time to start, as you would hold a real one while it powers up. Past 45 degrees it is falling; at 80 degrees it lies on the floor and its wheels spin free. The window shows which.

The window's buttons: ↺ stands it up again (held for release more), and ◀ and ▶ push the top of the body back or forward with an impulse of 0.05 N·s, enough to set it turning at about 70 degrees per second.

The model

A planar wheeled inverted pendulum (the cart-pole with rolling wheels), integrated with fourth-order Runge-Kutta every 1 ms. x is how far the axle has rolled and theta the tilt:

a = M + 2m + 2Iw/r^2 + 2J/r^2
b = M l cos(theta) - 2J/r
c = I + M l^2 + 2J

a x''     + b theta'' = tau / r + M l sin(theta) theta'^2
b x''     + c theta'' = M g l sin(theta) - tau

M, l, I are the body's mass, center-of-mass height above the axle and inertia about its center; r, m, Iw = m r^2 / 2 are each wheel's radius, mass and inertia; J = rotor x ratio^2 is each motor's rotor inertia seen at the wheel. tau is the torque both gearboxes put between body and wheels. The motors are bolted to the body, so the torque that drives the wheels forward pushes the body back.

Each motor is the linear DC motor law at the gearbox output, plus friction:

tau_i = stall (u - c w / w0) - friction tanh(w / 0.2 rad/s) - damping w

w is the wheel's speed relative to the body, w0 the no-load speed, u the average drive over the step (+1 forward, -1 backward, times battery / voltage) and c the fraction of the step the winding was connected (driven or braked): a connected winding brakes with its back EMF, an open one does not.

The IMU reads the specific force at its mounting height imu, in its own axes: gravity plus the acceleration of that point, so the accelerometer sees the wheels' shoves. With noise at 1 it adds Gaussian noise at the datasheet's figures (0.004 g per accelerometer axis, 0.05 deg/s per gyro axis), from a fixed seed so a run repeats; gyro_bias adds a constant drift to the Y rate.

Properties

Property	Default	Meaning
mass	800 g	Body mass, battery and boards included, wheels not
com	70 mm	Center of mass above the axle
inertia	17 kg·cm²	Body inertia about its center of mass
height	160 mm	Body height; a push lands at the top
wheel	68 mm	Wheel diameter
wheel_mass	40 g	Each wheel
voltage	7.4 V	The motors' rated voltage
battery	7.4 V	The motor battery; below voltage the motors are weaker and slower
rpm	280	No-load speed at the wheel, at voltage
stall	4.0 kg·cm	Stall torque at the wheel, each motor, at voltage (0.39 N·m)
rotor	4 g·cm²	Rotor inertia at the motor shaft
ratio	30	Gearbox ratio
ppr	13	Encoder pulses per motor turn, each channel
friction	20 mN·m	Gearbox friction at the wheel, each motor
damping	0.5 mN·m·s	Viscous damping at the wheel, each motor, per rad/s
imu	120 mm	The MPU6050's height above the axle
noise	1	IMU noise, as a multiple of the datasheet's
gyro_bias	0 deg/s	Constant offset on the Y gyro
tilt	2°	The tilt it is held at before it is let go
release	1000 ms	How long it is held

The defaults are a small kit robot with JGA25-370 style 1:30 gear motors on a 2S battery. With them it falls from 2 degrees to the floor in about half a second if nothing drives the wheels.

Encoders

Each wheel makes ppr x ratio pulses a turn on each channel, 390 by default, and four times that many edges. LA leads LB when the left wheel turns forward relative to the body (the motor measures the wheel against the body, not the floor). They are driven at the logic supply.

What is not modeled

Turning: the two wheels share one axle, so their torques add and a difference between them does not yaw the robot. Wheel slip, an uneven floor, gearbox backlash, motor inductance, the driver's half ohm and current limit, the battery sagging or running down, and the MPU6050's INT pin, DMP and FIFO.

Readings

The probe is the tilt in degrees, forward positive. The window shows the tilt, the wheel speed in rpm and whether it is held, balancing, falling or fallen.