controller.go raw
1 // Package pid provides a generic PID controller implementation with filtered derivative.
2 //
3 // This package implements a Proportional-Integral-Derivative controller suitable
4 // for various dynamic adjustment scenarios:
5 // - Rate limiting (memory/load-based throttling)
6 // - PoW difficulty adjustment (block time targeting)
7 // - Temperature control
8 // - Motor speed control
9 // - Any system requiring feedback-based regulation
10 //
11 // The controller features:
12 // - Low-pass filtered derivative to suppress high-frequency noise
13 // - Anti-windup on the integral term to prevent saturation
14 // - Configurable output clamping
15 // - Thread-safe operation
16 //
17 // # Control Theory Background
18 //
19 // The PID controller computes an output based on the error between the current
20 // process variable and a target setpoint:
21 //
22 // output = Kp*error + Ki*∫error*dt + Kd*d(filtered_error)/dt
23 //
24 // Where:
25 // - Proportional (P): Immediate response proportional to current error
26 // - Integral (I): Accumulated error to eliminate steady-state offset
27 // - Derivative (D): Rate of change to anticipate future error (filtered)
28 //
29 // # Filtered Derivative
30 //
31 // Raw derivative amplifies high-frequency noise. This implementation applies
32 // an exponential moving average (low-pass filter) before computing the derivative:
33 //
34 // filtered_error = α*current_error + (1-α)*previous_filtered_error
35 // derivative = (filtered_error - previous_filtered_error) / dt
36 //
37 // Lower α values provide stronger filtering (recommended: 0.1-0.3).
38 package pid
39
40 import (
41 "math"
42 "sync"
43 "time"
44
45 pidif "next.orly.dev/pkg/interfaces/pid"
46 )
47
48 // Controller implements a PID controller with filtered derivative.
49 // It is safe for concurrent use.
50 type Controller struct {
51 // Configuration (protected by mutex for dynamic updates)
52 mu sync.Mutex
53 tuning pidif.Tuning
54
55 // Internal state
56 integral float64
57 prevError float64
58 prevFilteredError float64
59 lastUpdate time.Time
60 initialized bool
61 }
62
63 // Compile-time check that Controller implements pidif.Controller
64 var _ pidif.Controller = (*Controller)(nil)
65
66 // output implements pidif.Output
67 type output struct {
68 value float64
69 clamped bool
70 pTerm float64
71 iTerm float64
72 dTerm float64
73 }
74
75 func (o output) Value() float64 { return o.value }
76 func (o output) Clamped() bool { return o.clamped }
77 func (o output) Components() (p, i, d float64) { return o.pTerm, o.iTerm, o.dTerm }
78
79 // New creates a new PID controller with the given tuning parameters.
80 func New(tuning pidif.Tuning) *Controller {
81 return &Controller{tuning: tuning}
82 }
83
84 // NewWithGains creates a new PID controller with specified gains and defaults for other parameters.
85 func NewWithGains(kp, ki, kd, setpoint float64) *Controller {
86 tuning := pidif.DefaultTuning()
87 tuning.Kp = kp
88 tuning.Ki = ki
89 tuning.Kd = kd
90 tuning.Setpoint = setpoint
91 return &Controller{tuning: tuning}
92 }
93
94 // NewDefault creates a new PID controller with default tuning.
95 func NewDefault() *Controller {
96 return &Controller{tuning: pidif.DefaultTuning()}
97 }
98
99 // Update computes the controller output based on the current process variable.
100 func (c *Controller) Update(pv pidif.ProcessVariable) pidif.Output {
101 c.mu.Lock()
102 defer c.mu.Unlock()
103
104 now := pv.Timestamp()
105 value := pv.Value()
106
107 // Initialize on first call
108 if !c.initialized {
109 c.lastUpdate = now
110 c.prevError = value - c.tuning.Setpoint
111 c.prevFilteredError = c.prevError
112 c.initialized = true
113 return output{value: 0, clamped: false}
114 }
115
116 // Calculate time delta
117 dt := now.Sub(c.lastUpdate).Seconds()
118 if dt <= 0 {
119 dt = 0.001 // Minimum 1ms to avoid division by zero
120 }
121 c.lastUpdate = now
122
123 // Calculate current error (positive when above setpoint)
124 err := value - c.tuning.Setpoint
125
126 // Proportional term
127 pTerm := c.tuning.Kp * err
128
129 // Integral term with anti-windup
130 c.integral += err * dt
131 c.integral = clamp(c.integral, c.tuning.IntegralMin, c.tuning.IntegralMax)
132 iTerm := c.tuning.Ki * c.integral
133
134 // Derivative term with low-pass filter
135 alpha := c.tuning.DerivativeFilterAlpha
136 if alpha <= 0 {
137 alpha = 0.2 // Default if not set
138 }
139 filteredError := alpha*err + (1-alpha)*c.prevFilteredError
140
141 var dTerm float64
142 if dt > 0 {
143 dTerm = c.tuning.Kd * (filteredError - c.prevFilteredError) / dt
144 }
145
146 // Update previous values
147 c.prevError = err
148 c.prevFilteredError = filteredError
149
150 // Compute total output
151 rawOutput := pTerm + iTerm + dTerm
152 clampedOutput := clamp(rawOutput, c.tuning.OutputMin, c.tuning.OutputMax)
153
154 return output{
155 value: clampedOutput,
156 clamped: rawOutput != clampedOutput,
157 pTerm: pTerm,
158 iTerm: iTerm,
159 dTerm: dTerm,
160 }
161 }
162
163 // UpdateValue is a convenience method that takes a raw float64 value.
164 func (c *Controller) UpdateValue(value float64) pidif.Output {
165 return c.Update(pidif.NewProcessVariable(value))
166 }
167
168 // Reset clears all internal state.
169 func (c *Controller) Reset() {
170 c.mu.Lock()
171 defer c.mu.Unlock()
172
173 c.integral = 0
174 c.prevError = 0
175 c.prevFilteredError = 0
176 c.initialized = false
177 }
178
179 // SetSetpoint updates the target value.
180 func (c *Controller) SetSetpoint(setpoint float64) {
181 c.mu.Lock()
182 defer c.mu.Unlock()
183 c.tuning.Setpoint = setpoint
184 }
185
186 // Setpoint returns the current setpoint.
187 func (c *Controller) Setpoint() float64 {
188 c.mu.Lock()
189 defer c.mu.Unlock()
190 return c.tuning.Setpoint
191 }
192
193 // SetGains updates the PID gains.
194 func (c *Controller) SetGains(kp, ki, kd float64) {
195 c.mu.Lock()
196 defer c.mu.Unlock()
197 c.tuning.Kp = kp
198 c.tuning.Ki = ki
199 c.tuning.Kd = kd
200 }
201
202 // Gains returns the current PID gains.
203 func (c *Controller) Gains() (kp, ki, kd float64) {
204 c.mu.Lock()
205 defer c.mu.Unlock()
206 return c.tuning.Kp, c.tuning.Ki, c.tuning.Kd
207 }
208
209 // SetOutputLimits updates the output clamping limits.
210 func (c *Controller) SetOutputLimits(min, max float64) {
211 c.mu.Lock()
212 defer c.mu.Unlock()
213 c.tuning.OutputMin = min
214 c.tuning.OutputMax = max
215 }
216
217 // SetIntegralLimits updates the anti-windup limits.
218 func (c *Controller) SetIntegralLimits(min, max float64) {
219 c.mu.Lock()
220 defer c.mu.Unlock()
221 c.tuning.IntegralMin = min
222 c.tuning.IntegralMax = max
223 }
224
225 // SetDerivativeFilter updates the derivative filter coefficient.
226 // Lower values provide stronger filtering (0.1-0.3 recommended).
227 func (c *Controller) SetDerivativeFilter(alpha float64) {
228 c.mu.Lock()
229 defer c.mu.Unlock()
230 c.tuning.DerivativeFilterAlpha = alpha
231 }
232
233 // Tuning returns a copy of the current tuning parameters.
234 func (c *Controller) Tuning() pidif.Tuning {
235 c.mu.Lock()
236 defer c.mu.Unlock()
237 return c.tuning
238 }
239
240 // SetTuning updates all tuning parameters at once.
241 func (c *Controller) SetTuning(tuning pidif.Tuning) {
242 c.mu.Lock()
243 defer c.mu.Unlock()
244 c.tuning = tuning
245 }
246
247 // State returns the current internal state for monitoring/debugging.
248 func (c *Controller) State() (integral, prevError, prevFilteredError float64, initialized bool) {
249 c.mu.Lock()
250 defer c.mu.Unlock()
251 return c.integral, c.prevError, c.prevFilteredError, c.initialized
252 }
253
254 // clamp restricts a value to the range [min, max].
255 func clamp(value, min, max float64) float64 {
256 if math.IsNaN(value) {
257 return 0
258 }
259 if value < min {
260 return min
261 }
262 if value > max {
263 return max
264 }
265 return value
266 }
267