Closed-Loop Discrete PID Control & Inverted Pendulum Balancing

Stabilize a two-wheeled inverted pendulum mechanism at its inherently unstable vertical equilibrium position ($\theta = 0$). Maintain robustness against continuous external torque impulses while preventing actuator saturation windup.

Sensor Fusion Pipeline:
[MPU-6050 6-DOF IMU] --(I2C Fast Mode 400kHz)--> [STM32F401 ARM Cortex-M4]
       |
       v
[Complementary Filter (100 Hz)] --> [Discrete PID with Anti-Windup Clamping]
                                           |
                                           v
[DRV8833 Dual H-Bridge Driver] <--(Hardware Timer PWM @ 20 kHz)--+

// Discrete PID Loop with Anti-Windup Clamping (ANSI C)
#include <math.h>

typedef struct {
    float kp, ki, kd;
    float integrator;
    float prev_error;
    float max_output;
    float min_output;
    float dt;
} PIDController;

float pid_compute(PIDController *pid, float setpoint, float measured_val) {
    float error = setpoint - measured_val;
    
    // Proportional term
    float p_term = pid->kp * error;
    
    // Integral term with forward Euler integration
    float new_integrator = pid->integrator + (pid->ki * error * pid->dt);
    
    // Derivative term (on error or process variable)
    float d_term = pid->kd * (error - pid->prev_error) / pid->dt;
    pid->prev_error = error;
    
    // Raw output before saturation
    float raw_output = p_term + new_integrator + d_term;
    float clamped_output = raw_output;
    
    // Clamp actuator output boundaries
    if (clamped_output > pid->max_output) {
        clamped_output = pid->max_output;
    } else if (clamped_output < pid->min_output) {
        clamped_output = pid->min_output;
    }
    
    // Anti-Windup: Only accumulate integrator if not in severe saturation
    if (raw_output == clamped_output || (error * raw_output) < 0.0f) {
        pid->integrator = new_integrator;
    }
    
    return clamped_output;
}