Industrial PID controller with anti-windup and derivative filtering.
| 144 | |
| 145 | |
| 146 | class PIDController: |
| 147 | """Industrial PID controller with anti-windup and derivative filtering.""" |
| 148 | |
| 149 | def __init__( |
| 150 | self, |
| 151 | kp: float, |
| 152 | ki: float, |
| 153 | kd: float, |
| 154 | setpoint: float, |
| 155 | output_min: float = 0, |
| 156 | output_max: float = MOTOR_MAX_RPM, |
| 157 | ): |
| 158 | self.kp = kp |
| 159 | self.ki = ki |
| 160 | self.kd = kd |
| 161 | self.setpoint = setpoint |
| 162 | self.output_min = output_min |
| 163 | self.output_max = output_max |
| 164 | |
| 165 | self.integral = 0.0 |
| 166 | self.last_error = 0.0 |
| 167 | self.last_derivative = 0.0 |
| 168 | self.derivative_filter = 0.1 # Low-pass filter coefficient |
| 169 | |
| 170 | def update(self, measured: float, dt: float) -> float: |
| 171 | """Calculate PID output with filtering.""" |
| 172 | error = self.setpoint - measured |
| 173 | |
| 174 | # Proportional |
| 175 | p_term = self.kp * error |
| 176 | |
| 177 | # Integral with anti-windup (only integrate when not saturated) |
| 178 | self.integral += error * dt |
| 179 | self.integral = clamp(self.integral, -200, 200) |
| 180 | i_term = self.ki * self.integral |
| 181 | |
| 182 | # Filtered derivative (reduces noise sensitivity) |
| 183 | if dt > 0: |
| 184 | raw_derivative = (error - self.last_error) / dt |
| 185 | self.last_derivative += self.derivative_filter * ( |
| 186 | raw_derivative - self.last_derivative |
| 187 | ) |
| 188 | d_term = self.kd * self.last_derivative |
| 189 | self.last_error = error |
| 190 | |
| 191 | # Output centered around rated RPM |
| 192 | output = MOTOR_RATED_RPM + p_term + i_term + d_term |
| 193 | return clamp(output, self.output_min, self.output_max) |
| 194 | |
| 195 | def reset(self): |
| 196 | """Reset controller state.""" |
| 197 | self.integral = 0.0 |
| 198 | self.last_error = 0.0 |
| 199 | self.last_derivative = 0.0 |
| 200 | |
| 201 | |
| 202 | # ========================================================================= |