controller_test.go raw
1 package pid
2
3 import (
4 "testing"
5 "time"
6
7 pidif "next.orly.dev/pkg/interfaces/pid"
8 )
9
10 func TestController_BasicOperation(t *testing.T) {
11 ctrl := New(RateLimitWriteTuning())
12
13 // First call should return 0 (initialization)
14 out := ctrl.UpdateValue(0.5)
15 if out.Value() != 0 {
16 t.Errorf("expected 0 on first call, got %v", out.Value())
17 }
18
19 // Sleep a bit to ensure dt > 0
20 time.Sleep(10 * time.Millisecond)
21
22 // Process variable below setpoint (0.5 < 0.85) should return 0 or negative (clamped to 0)
23 out = ctrl.UpdateValue(0.5)
24 if out.Value() != 0 {
25 t.Errorf("expected 0 when below setpoint, got %v", out.Value())
26 }
27
28 // Process variable above setpoint should return positive output
29 time.Sleep(10 * time.Millisecond)
30 out = ctrl.UpdateValue(0.95) // 0.95 > 0.85 setpoint
31 if out.Value() <= 0 {
32 t.Errorf("expected positive output when above setpoint, got %v", out.Value())
33 }
34 }
35
36 func TestController_IntegralAccumulation(t *testing.T) {
37 tuning := pidif.Tuning{
38 Kp: 0.5,
39 Ki: 0.5, // High Ki
40 Kd: 0.0, // No Kd
41 Setpoint: 0.5,
42 DerivativeFilterAlpha: 0.2,
43 IntegralMin: -10,
44 IntegralMax: 10,
45 OutputMin: 0,
46 OutputMax: 1.0,
47 }
48 ctrl := New(tuning)
49
50 // Initialize
51 ctrl.UpdateValue(0.5)
52 time.Sleep(10 * time.Millisecond)
53
54 // Continuously above setpoint should accumulate integral
55 for i := 0; i < 10; i++ {
56 time.Sleep(10 * time.Millisecond)
57 ctrl.UpdateValue(0.8) // 0.3 above setpoint
58 }
59
60 integral, _, _, _ := ctrl.State()
61 if integral <= 0 {
62 t.Errorf("expected positive integral after sustained error, got %v", integral)
63 }
64 }
65
66 func TestController_FilteredDerivative(t *testing.T) {
67 tuning := pidif.Tuning{
68 Kp: 0.0,
69 Ki: 0.0,
70 Kd: 1.0, // Only Kd
71 Setpoint: 0.5,
72 DerivativeFilterAlpha: 0.5, // 50% filtering
73 IntegralMin: -10,
74 IntegralMax: 10,
75 OutputMin: 0,
76 OutputMax: 1.0,
77 }
78 ctrl := New(tuning)
79
80 // Initialize with low value
81 ctrl.UpdateValue(0.5)
82 time.Sleep(10 * time.Millisecond)
83
84 // Second call with same value - derivative should be near zero
85 ctrl.UpdateValue(0.5)
86 _, _, prevFiltered, _ := ctrl.State()
87
88 time.Sleep(10 * time.Millisecond)
89
90 // Big jump - filtered derivative should be dampened
91 out := ctrl.UpdateValue(1.0)
92
93 // The filtered derivative should cause some response, but dampened
94 if out.Value() < 0 {
95 t.Errorf("expected non-negative output, got %v", out.Value())
96 }
97
98 _, _, newFiltered, _ := ctrl.State()
99 // Filtered error should have moved toward the new error but not fully
100 if newFiltered <= prevFiltered {
101 t.Errorf("filtered error should increase with rising process variable")
102 }
103 }
104
105 func TestController_AntiWindup(t *testing.T) {
106 tuning := pidif.Tuning{
107 Kp: 0.0,
108 Ki: 1.0, // Only Ki
109 Kd: 0.0,
110 Setpoint: 0.5,
111 DerivativeFilterAlpha: 0.2,
112 IntegralMin: -1.0, // Tight integral bounds
113 IntegralMax: 1.0,
114 OutputMin: 0,
115 OutputMax: 10.0, // Wide output bounds
116 }
117 ctrl := New(tuning)
118
119 // Initialize
120 ctrl.UpdateValue(0.5)
121
122 // Drive the integral to its limit
123 for i := 0; i < 100; i++ {
124 time.Sleep(1 * time.Millisecond)
125 ctrl.UpdateValue(1.0) // Large positive error
126 }
127
128 integral, _, _, _ := ctrl.State()
129 if integral > 1.0 {
130 t.Errorf("integral should be clamped at 1.0, got %v", integral)
131 }
132 }
133
134 func TestController_Reset(t *testing.T) {
135 ctrl := New(RateLimitWriteTuning())
136
137 // Build up some state
138 ctrl.UpdateValue(0.5)
139 time.Sleep(10 * time.Millisecond)
140 ctrl.UpdateValue(0.9)
141 time.Sleep(10 * time.Millisecond)
142 ctrl.UpdateValue(0.95)
143
144 // Reset
145 ctrl.Reset()
146
147 integral, prevErr, prevFiltered, initialized := ctrl.State()
148 if integral != 0 || prevErr != 0 || prevFiltered != 0 || initialized {
149 t.Errorf("expected all state to be zero after reset, got integral=%v, prevErr=%v, prevFiltered=%v, initialized=%v",
150 integral, prevErr, prevFiltered, initialized)
151 }
152
153 // Next call should behave like first call
154 out := ctrl.UpdateValue(0.9)
155 if out.Value() != 0 {
156 t.Errorf("expected 0 on first call after reset, got %v", out.Value())
157 }
158 }
159
160 func TestController_SetGains(t *testing.T) {
161 ctrl := New(RateLimitWriteTuning())
162
163 // Change gains
164 ctrl.SetGains(1.0, 0.5, 0.1)
165
166 kp, ki, kd := ctrl.Gains()
167 if kp != 1.0 || ki != 0.5 || kd != 0.1 {
168 t.Errorf("gains not updated correctly: kp=%v, ki=%v, kd=%v", kp, ki, kd)
169 }
170 }
171
172 func TestController_SetSetpoint(t *testing.T) {
173 ctrl := New(RateLimitWriteTuning())
174
175 ctrl.SetSetpoint(0.7)
176
177 if ctrl.Setpoint() != 0.7 {
178 t.Errorf("setpoint not updated, got %v", ctrl.Setpoint())
179 }
180 }
181
182 func TestController_OutputClamping(t *testing.T) {
183 tuning := pidif.Tuning{
184 Kp: 10.0, // Very high Kp
185 Ki: 0.0,
186 Kd: 0.0,
187 Setpoint: 0.5,
188 DerivativeFilterAlpha: 0.2,
189 IntegralMin: -10,
190 IntegralMax: 10,
191 OutputMin: 0,
192 OutputMax: 1.0, // Strict output max
193 }
194 ctrl := New(tuning)
195
196 // Initialize
197 ctrl.UpdateValue(0.5)
198 time.Sleep(10 * time.Millisecond)
199
200 // Very high error should be clamped
201 out := ctrl.UpdateValue(2.0) // 1.5 error * 10 Kp = 15, should clamp to 1.0
202 if out.Value() > 1.0 {
203 t.Errorf("output should be clamped to 1.0, got %v", out.Value())
204 }
205 if !out.Clamped() {
206 t.Errorf("expected output to be flagged as clamped")
207 }
208 }
209
210 func TestController_Components(t *testing.T) {
211 tuning := pidif.Tuning{
212 Kp: 1.0,
213 Ki: 0.5,
214 Kd: 0.1,
215 Setpoint: 0.5,
216 DerivativeFilterAlpha: 0.2,
217 IntegralMin: -10,
218 IntegralMax: 10,
219 OutputMin: -100,
220 OutputMax: 100,
221 }
222 ctrl := New(tuning)
223
224 // Initialize
225 ctrl.UpdateValue(0.5)
226 time.Sleep(10 * time.Millisecond)
227
228 // Get components
229 out := ctrl.UpdateValue(0.8)
230 p, i, d := out.Components()
231
232 // Proportional should be positive (0.3 * 1.0 = 0.3)
233 expectedP := 0.3
234 if p < expectedP*0.9 || p > expectedP*1.1 {
235 t.Errorf("expected P term ~%v, got %v", expectedP, p)
236 }
237
238 // Integral should be small but positive (accumulated over ~10ms)
239 if i <= 0 {
240 t.Errorf("expected positive I term, got %v", i)
241 }
242
243 // Derivative should be non-zero (error changed)
244 // The sign depends on filtering and timing
245 _ = d // Just verify it's accessible
246 }
247
248 func TestPresets(t *testing.T) {
249 // Test that all presets create valid controllers
250 tests := []struct {
251 name string
252 tuning pidif.Tuning
253 }{
254 {"RateLimitWrite", RateLimitWriteTuning()},
255 {"RateLimitRead", RateLimitReadTuning()},
256 {"DifficultyAdjustment", DifficultyAdjustmentTuning()},
257 {"TemperatureControl", TemperatureControlTuning(25.0)},
258 {"MotorSpeed", MotorSpeedTuning()},
259 }
260
261 for _, tt := range tests {
262 t.Run(tt.name, func(t *testing.T) {
263 ctrl := New(tt.tuning)
264 if ctrl == nil {
265 t.Error("expected non-nil controller")
266 return
267 }
268
269 // Basic sanity check
270 out := ctrl.UpdateValue(tt.tuning.Setpoint)
271 if out == nil {
272 t.Error("expected non-nil output")
273 }
274 })
275 }
276 }
277
278 func TestFactoryFunctions(t *testing.T) {
279 // Test convenience factory functions
280 writeCtrl := NewRateLimitWriteController()
281 if writeCtrl == nil {
282 t.Error("NewRateLimitWriteController returned nil")
283 }
284
285 readCtrl := NewRateLimitReadController()
286 if readCtrl == nil {
287 t.Error("NewRateLimitReadController returned nil")
288 }
289
290 diffCtrl := NewDifficultyAdjustmentController()
291 if diffCtrl == nil {
292 t.Error("NewDifficultyAdjustmentController returned nil")
293 }
294
295 tempCtrl := NewTemperatureController(72.0)
296 if tempCtrl == nil {
297 t.Error("NewTemperatureController returned nil")
298 }
299
300 motorCtrl := NewMotorSpeedController()
301 if motorCtrl == nil {
302 t.Error("NewMotorSpeedController returned nil")
303 }
304 }
305
306 func TestController_ProcessVariableInterface(t *testing.T) {
307 ctrl := New(RateLimitWriteTuning())
308
309 // Test using the full ProcessVariable interface
310 pv := pidif.NewProcessVariableAt(0.9, time.Now())
311 out := ctrl.Update(pv)
312
313 // First call returns 0
314 if out.Value() != 0 {
315 t.Errorf("expected 0 on first call, got %v", out.Value())
316 }
317
318 time.Sleep(10 * time.Millisecond)
319
320 pv2 := pidif.NewProcessVariableAt(0.95, time.Now())
321 out2 := ctrl.Update(pv2)
322
323 // Above setpoint should produce positive output
324 if out2.Value() <= 0 {
325 t.Errorf("expected positive output above setpoint, got %v", out2.Value())
326 }
327 }
328
329 func TestController_NewWithGains(t *testing.T) {
330 ctrl := NewWithGains(1.0, 0.5, 0.1, 0.7)
331
332 kp, ki, kd := ctrl.Gains()
333 if kp != 1.0 || ki != 0.5 || kd != 0.1 {
334 t.Errorf("gains not set correctly: kp=%v, ki=%v, kd=%v", kp, ki, kd)
335 }
336
337 if ctrl.Setpoint() != 0.7 {
338 t.Errorf("setpoint not set correctly, got %v", ctrl.Setpoint())
339 }
340 }
341
342 func TestController_SetTuning(t *testing.T) {
343 ctrl := NewDefault()
344
345 newTuning := RateLimitWriteTuning()
346 ctrl.SetTuning(newTuning)
347
348 tuning := ctrl.Tuning()
349 if tuning.Kp != newTuning.Kp || tuning.Ki != newTuning.Ki || tuning.Setpoint != newTuning.Setpoint {
350 t.Errorf("tuning not updated correctly")
351 }
352 }
353
354 func TestController_SetOutputLimits(t *testing.T) {
355 ctrl := NewDefault()
356 ctrl.SetOutputLimits(-5.0, 5.0)
357
358 tuning := ctrl.Tuning()
359 if tuning.OutputMin != -5.0 || tuning.OutputMax != 5.0 {
360 t.Errorf("output limits not updated: min=%v, max=%v", tuning.OutputMin, tuning.OutputMax)
361 }
362 }
363
364 func TestController_SetIntegralLimits(t *testing.T) {
365 ctrl := NewDefault()
366 ctrl.SetIntegralLimits(-2.0, 2.0)
367
368 tuning := ctrl.Tuning()
369 if tuning.IntegralMin != -2.0 || tuning.IntegralMax != 2.0 {
370 t.Errorf("integral limits not updated: min=%v, max=%v", tuning.IntegralMin, tuning.IntegralMax)
371 }
372 }
373
374 func TestController_SetDerivativeFilter(t *testing.T) {
375 ctrl := NewDefault()
376 ctrl.SetDerivativeFilter(0.5)
377
378 tuning := ctrl.Tuning()
379 if tuning.DerivativeFilterAlpha != 0.5 {
380 t.Errorf("derivative filter alpha not updated: %v", tuning.DerivativeFilterAlpha)
381 }
382 }
383
384 func TestDefaultTuning(t *testing.T) {
385 tuning := pidif.DefaultTuning()
386
387 if tuning.Kp <= 0 || tuning.Ki <= 0 || tuning.Kd <= 0 {
388 t.Error("default tuning should have positive gains")
389 }
390
391 if tuning.DerivativeFilterAlpha <= 0 || tuning.DerivativeFilterAlpha > 1.0 {
392 t.Errorf("default derivative filter alpha should be in (0, 1], got %v", tuning.DerivativeFilterAlpha)
393 }
394
395 if tuning.OutputMin >= tuning.OutputMax {
396 t.Error("default output min should be less than max")
397 }
398
399 if tuning.IntegralMin >= tuning.IntegralMax {
400 t.Error("default integral min should be less than max")
401 }
402 }
403