This is an arduino sketch for an inverted pendulum with two wheels (a self balancing robot). Due to the unstable nature of this platform, it must be actively stabilized to maintain balance.

The program provides stabilization through an MPU6050 IMU and two brushed DC motors driven by an H-bridge. It uses three PID loops acting simultaneously:

• Pitch stabilization: This is the dominant PID controller of the system. It uses the pitch angle of the vehicle as a proportional term to keep the vehicle upright by accelerating into gravity.

• Position stabilization: It estimates the velocity by integrating pitch angle, which is proportional to the lateral acceleration (at small angles), and this is integrated again to approximate position (the double integral of the pitch angle). Both are used as the derivative and proportional term respectively. Their combined effect prevent the vehicle from drifting laterally and counters any change in center of gravity.

$$ a_x \propto \theta_x $$

$$ v_x = \int{a_x}dt \propto \int{ \theta_x dt } $$

$$ x = \int{v_x}dt \propto \int{ \int{ \theta_x dt^2 } } $$

• Yaw stabilization: A controller that maintains heading by using as a proportional term the angular velocity of the yaw axis. It counters the open-loop nature of the motor control, wherein one motor can spin faster and cause the vehicle to turn.

Ultimately, the unstable axis is only stabilized by the measured pitch angle.

Note: Both motors are driven with pulse frequency modulation to counter magnetic cogging and stiction. The duty is cycle is long enough so that each pulse causes the motors to rotate one magnetic pole. Such a waveform provides precise control over low-speed rotation at the cost of jerky movement. This precision is necessary to reduce under and over correction when the vehicle is nearly vertical.

Driving the motors like this causes them to behave similar to a stepper motor, albeit with much less precision, larger steps, and less torque.

These are the libraries needed to compile this sketch:

• basicMPU6050
• imuFilter
• pulseInput
• TimerOne