Spaces:
Running
Running
File size: 2,490 Bytes
1b7bc37 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 |
import time
import math
class PIDController:
def __init__(
self, kp=1.0, ki=0.0, kd=0.0, setpoint=0.0, min_output=-100, max_output=100
):
# PID constants
self.kp = kp # Proportional gain
self.ki = ki # Integral gain
self.kd = kd # Derivative gain
# Control variables
self.setpoint = setpoint # Desired target value
self.prev_error = 0.0 # Previous error for derivative calculation
self.integral = 0.0 # Accumulated error for integral calculation
self.last_time = time.time() # Time of last update
self.last_input = float("nan") # Store the last process variable
# Output constraints
self.min_output = min_output
self.max_output = max_output
# For visualization/debugging
self.p_term = 0.0
self.i_term = 0.0
self.d_term = 0.0
def compute(self, process_variable):
"""Calculate PID output value based on current process variable"""
# Store the current input value
self.last_input = process_variable
# Handle non-numeric or invalid input
if math.isnan(process_variable):
return 0.0, 0.0, 0.0, 0.0
# Calculate time delta
current_time = time.time()
dt = current_time - self.last_time
if dt <= 0:
dt = 0.001 # Avoid division by zero
# Calculate error
error = self.setpoint - process_variable
# Proportional term
self.p_term = self.kp * error
# Integral term - with anti-windup
self.integral += error * dt
self.i_term = self.ki * self.integral
# Derivative term - on measurement, not error
if dt > 0:
self.d_term = self.kd * (error - self.prev_error) / dt
else:
self.d_term = 0.0
# Calculate output
output = self.p_term + self.i_term + self.d_term
# Constrain output
output = max(self.min_output, min(self.max_output, output))
# Save state
self.prev_error = error
self.last_time = current_time
return output, self.p_term, self.i_term, self.d_term
def reset(self):
"""Reset the controller state"""
self.prev_error = 0.0
self.integral = 0.0
self.last_time = time.time()
self.p_term = 0.0
self.i_term = 0.0
self.d_term = 0.0
self.last_input = float("nan") # Reset the last input to NaN
|