nx_pid_test.nx source

1// nx_pid_test.nx -- smoke for nx_pid. 2 3import "syscalls.nx" 4import "nx_pid.nx" 5 6func main() -> i64 { 7 // === Test 1: P-only controller reduces error === 8 // Kp = 0.5, Ki = 0, Kd = 0. Setpoint=100, start measured=0. 9 // Update should yield control = 0.5 * (100 - 0) = 50. 10 let pid: *PID = (sys_mmap(56)) as *PID 11 nx_pid_init(pid, 8192, 0, 0, 10000) // Kp=0.5, Ki=Kd=0 12 let u: i64 = nx_pid_update(pid, 100, 0) 13 if u != 50 { return 1 } 14 15 // === Test 2: P-only on zero error returns 0 === 16 nx_pid_reset(pid) 17 if nx_pid_update(pid, 100, 100) != 0 { return 2 } 18 19 // === Test 3: I term accumulates === 20 // Kp=0, Ki=1.0, Kd=0. 3 steps with error=10 each: 21 // integral becomes 10, 20, 30. I-term = 30. 22 nx_pid_init(pid, 0, 16384, 0, 10000) 23 nx_pid_update(pid, 10, 0) 24 nx_pid_update(pid, 10, 0) 25 let u3: i64 = nx_pid_update(pid, 10, 0) 26 if u3 != 30 { return 10 } 27 28 // === Test 4: I anti-windup clamps integral === 29 nx_pid_init(pid, 0, 16384, 0, 50) // i_limit = 50 30 var i: i64 = 0 31 while i < 100 { 32 nx_pid_update(pid, 10, 0) 33 i = i + 1 34 } 35 // Integral should be capped at 50, so I-term = 50. 36 if pid.integral != 50 { return 20 } 37 let u4: i64 = nx_pid_update(pid, 10, 0) 38 // Integral stays at 50 (still error=10, +10 -> 60 clamped to 50) 39 if u4 != 50 { return 21 } 40 41 // === Test 5: D term reacts to error change === 42 // Kp=0, Ki=0, Kd=1.0. First call: no prev -> D = 0. 43 // Second call with same error: derivative = 0. 44 // Third call with bigger error: derivative > 0. 45 nx_pid_init(pid, 0, 0, 16384, 10000) 46 nx_pid_update(pid, 100, 0) // error = 100 47 let u5a: i64 = nx_pid_update(pid, 100, 0) // error = 100, deriv = 0 48 if u5a != 0 { return 30 } 49 let u5b: i64 = nx_pid_update(pid, 100, 50) // error = 50, deriv = -50 50 if u5b != -50 { return 31 } 51 52 // === Test 6: full PID toward setpoint converges === 53 // Simulated plant: x_{t+1} = x_t + 0.1 * u 54 // Kp=1, Ki=0.1, Kd=0.05. Setpoint=100. Initial x=0. 55 nx_pid_init(pid, 16384, 1638, 819, 10000) 56 var x: i64 = 0 57 var step: i64 = 0 58 while step < 100 { 59 let u: i64 = nx_pid_update(pid, 100, x) 60 // plant integrates control: x += u/10 61 x = x + u / 10 62 step = step + 1 63 } 64 // x should be near 100 65 let d6: i64 = x - 100 66 var ad6: i64 = d6 67 if ad6 < 0 { ad6 = -ad6 } 68 if ad6 > 20 { return 40 } // generous tolerance 69 70 return 0 71}