This webpage provides the recommended procedure for tuning the PID controller which balances the reaction wheel inverted pendulum. For more general information about PID controllers, see:
You cannot build a controller with a measurement of the pendulum's tilt angle. Start by implementing a complementary filter of the accelerometer and gyroscope measurements to estimate tilt angle. Small angle approximations are fine.
The proportional term of the PID controller sets the voltage (or PWM duty cycle) applied to the motor by scaling the error between the estimated tilt angle (from the complementary filter) and the desired tilt angle (0 degrees) by a constant $K_p$.
If you are starting from the PWM demo in the course demo code repository, then the PWM channel can be assigned a duty cycle in the range [0, 5000]. We want for the controller to work over a range of +/- 3-4 degrees off the vertical, so start with a proportional gain of approximately 1500. Here is some pseudocode:
// Compute the error
error = (desired_angle - complementary_angle) ;
// Compute duty cycle with P controller
duty_cycle = Kp * error ;
Use your hand to gently move the pendulum back and forth. Do you see the reaction wheel speed increase in one direction when you move the arm right, and in the other direction when you move it left? Do you feel the arm fighting against your rotation, or trying to make the arm rotate faster? If it feels like the wheel is trying to pull the arm faster, then switch the sign on $K_p$.
Your calculation of the duty cycle may yield a negative number. Remember that the sign of the duty cycle just indicates the direction that the motor should turn. In other words, the sign tells you which of the two PWM channels which drive the H-bridge you should command, and which you should set to zero.
Once proportional control is working as expected, add some integral control. The integral term of the PID controller computes a control input to the motors by integrating the error over time. To prevent integrator windup, we will clamp the integral error at a maximum value. This maximum value is a tunable parameter, but start at about a 50% duty cycle.
Recall that the proportional term of the PID controller only scales the instantaneous error, while the integral term scales the integral of the error. The integral of the error may be orders of magnitude larger than the instantaneous error! We'd like for the P term and the I term of the PID controller to both contribute to the control input, so we'll make $K_i$ a few orders of magnitude smaller than $K_p$. Start with 7-8.
If you then start the pendulum balanced, you will likely see it correct itself once, then slowly increase its rotation speed until it tips over (as shown below). We'll solve that problem in the next step.
// Compute the error
error = (desired_angle - complementary_angle) ;
// Integrate the error
error_accumulation += error ;
// Clamp the integrated error (start with Imax = max_duty_cycle/2)
if (error_accumulation>Imax) error_accumulation=Imax ;
if (error_accumulation<(-Imax)) error_accumulation=-Imax ;
// Compute duty cycle with PI controller
duty_cycle = (Kp * error) + (Ki * error_accumulation) ;
Recall that we only get torque from a reaction wheel from changes in its angular momentum. That is to say, when its rotation rate is increasing or decreasing. If the inverted pendulum is just off from its desired tilt angle, then the integral term of the PID controller will cause the wheel to slowly ramp up in speed. A slow ramp up means very little torque, which means it continues to tip over!
A simple way to solve this problem is with dithering. We constantly move the desired tilt angle very slightly away from the measured tilt angle, as shown in the code below. This way, the integrated error increases more quickly, so the I contribution to the PID controller increases more quickly, and the pendulum corrects itself.
Once you add dithering, you may be able to get the pendulum to balance (as shown below)! Now you'll want to continue to tune your controller to make it as snappy and robust as possible.
// Compute the error
error = (desired_angle - complementary_angle) ;
// Start with angle_increment = 0.0001
if (error < 0) {
desired_angle -= angle_increment ;
}
else {
desired_angle += angle_increment ;
}
// Integrate the error
error_accumulation += error ;
// Clamp the integrated error (start with Imax = max_duty_cycle/2)
if (error_accumulation>Imax) error_accumulation=Imax ;
if (error_accumulation<(-Imax)) error_accumulation=-Imax ;
// Approximate the rate of change of the error
error_deriv = (error - prev_error) ;
// Compute duty cycle with PID controller
duty_cycle = (Kp * error) + (Ki * error_accumulation) + (Kd * error_deriv) ;
// Update prev_error
prev_error = error ;
The way to make your system faster is to increase the values for $K_p$ and $K_i$. But! This will destabilize your system. If you make these gains too large, the arm will overcorrect and fall over.
You can mitigate overcorrection by adding the derivative term of the PID controller. This term scales the rate of change of the error. If the error is getting smaller very quickly (i.e. the arm is moving quickly toward vertical), the D term of the PID controller slows it down. Likewise, if the error is increasing quickly (the arm is falling over), then the D term speeds it up.
You can increase the snappiness and responsiveness of your PID controller by increasing $K_p$ and $K_i$, and combat the instability that this causes by adding a derivative term. Remember that the rate of change of the error will be orders of magnitude smaller than the instantaneous error, so $K_d$ will be orders of magnitude larger than $K_p$. Start with about 10000. Look out for sign errors!
// Compute the error
error = (desired_angle - complementary_angle) ;
// Start with angle_increment = 0.0001
if (error < 0) {
desired_angle -= angle_increment ;
}
else {
desired_angle += angle_increment ;
}
// Integrate the error
error_accumulation += error ;
// Clamp the integrated error (start with Imax = max_duty_cycle/2)
if (error_accumulation>Imax) error_accumulation=Imax ;
if (error_accumulation<(-Imax)) error_accumulation=-Imax ;
// Compute duty cycle with PID controller
duty_cycle = (Kp * error) + (Ki * error_accumulation) ;
// Update prev_error
prev_error = error ;