Self-Balancing Robot Simulator: Inverted Pendulum PID

Controls

Start, Pause and Reset run the loop. Shove delivers an impulse the way a finger would, and Loop on/off disables both controllers so you can see the bare plant fall.

The most direct control is the robot itself: drag the body to any angle and let go. Whatever pose you leave it in is where the controller takes over — there is no re-arm step, and the gain sliders apply to the running simulation immediately. Turn derivative gain to zero while it is balancing and it starts wobbling in that same second.

The Tuning menu loads four presets: one that works, and three that fail in the three ways real balancers fail.

Focus the canvas and use Space, S, L, R and F for the same controls from the keyboard.

Theory

Standing a robot on two wheels means holding an inverted pendulum upright. The centre of mass sits above the axle, so vertical is an equilibrium in the same sense that a pencil balanced on its point is: mathematically real, physically hopeless without constant correction. Tip it by any angle and gravity produces a torque that tips it further. The further it goes, the faster it goes.

The robot has exactly one way to fight that torque — it can move the axle. Accelerating the wheels forward pushes the base back under the centre of mass, which rotates the body back toward vertical. This is why the correction feels inverted at first: to stop falling forward you must drive forward, the same way you move your hand under a broom you are balancing rather than away from it.

That gives the robot a hard limit. Beyond roughly 35°, the horizontal acceleration needed exceeds what the wheels can deliver before the body hits the floor, and no amount of gain helps. The dashed lines on the trace mark that boundary.

Algorithm

The controller is cascaded — two PID loops, one feeding the other.

The inner loop runs fast and holds a tilt angle. It compares the measured angle against a target and commands motor torque:

error   = measured_angle − target_angle
motor   = Kp·error + Ki·∫error + Kd·d(measured)/dt

Note the error is measured minus target, not the textbook target minus measured. Catching a forward fall means driving the wheels forward, so a positive tilt has to produce a positive motor command. This sign is the single most common reason a first balancing robot slams itself into the floor the moment it is switched on.

The outer loop runs slower and holds a position. It cannot command the motors — they are already spoken for — so instead it commands a tilt target for the inner loop:

target_angle = Kp_pos·(desired_position − position) − Kd_pos·velocity

Behind where it wants to be, it asks for a small forward lean; the inner loop then drives the wheels to hold that lean, and the robot travels forward. The lean is deliberately clamped to a few degrees. An outer loop allowed to demand a large angle will hand the inner loop a target it cannot hold, and both loops lose at once.

The integral term matters more here than in most loops, because it is what absorbs the difference between where the IMU thinks vertical is and where vertical actually is. It is also why the integral has to stop accumulating while the motors are saturated — otherwise it winds up during a fall it has no power to prevent, then fights the recovery.

Source code

// Inner loop: tilt → motors. Runs every 5 ms off a timer, not in loop().
float angleError = angle - tiltTarget;          // measured minus target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);
float command   = Kp * angleError
                + Ki * angleIntegral
                + Kd * gyroRate;                // rate straight from the gyro

command = constrain(command, -255, 255);
if (fabs(angle) > FALL_ANGLE) command = 0;      // past saving; cut the motors
driveMotors(command, command);

// Outer loop: position → tilt target. Runs ten times slower, and may only
// ever ask for a small lean.
if (++slowTick >= 10) {
  slowTick = 0;
  float positionError = 0 - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);          // radians, about ±7°
}

Two details carry over directly from this simulator. Take the derivative term from the gyro rate rather than differencing the angle: the gyro measures that rate directly and without the noise that differentiation amplifies. And cut the motors past the recovery angle rather than letting the controller keep straining — it saves the gearboxes and makes the failure obvious instead of violent.

Hardware

The sensor is an MPU6050, which contains both an accelerometer and a gyroscope, because neither alone is usable. The accelerometer knows where down is but is swamped by the robot’s own movement; the gyroscope tracks rotation cleanly but drifts within seconds. A complementary filter fuses them into an angle that is both stable and quick.

Motors need to be geared low enough for torque and quick enough to catch a fall — N20 gearmotors around 200 RPM are a common choice, driven by a TB6612FNG, whose low dropout matters when the loop is asking for small corrections thousands of times a second.

Build it for real in the self-balancing robot project, or start with the reading tilt tutorial if the IMU is new to you.

Circuit diagram

Two wiring rules decide whether this robot works.

The motor battery reaches the driver’s VM pin and nothing else. Motor current must never run through the Arduino, and every ground — board, driver, IMU, battery negative — has to be tied together, or the control signals have no shared reference and the failure is silent rather than obvious.

The encoder is powered from logic, not from the motor rail. Feeding an encoder the motor supply either destroys it or fills the counts with switching noise. Put encoder channel A on an interrupt-capable pin as well; polling it from loop() drops counts, and dropped counts mean the outer loop slowly loses track of where the robot is.

Mount the IMU close to the axle. Mounting it high multiplies the linear acceleration it sees during every correction, which makes the fusion problem harder for no benefit at all.