nx_robot_control.nx source
↩ module page · 59 lines · 2919 B
1// nx_robot_control.nx -- SOVEREIGN closed-loop motion CONTROL (the essence of "true control":
2// read sensor -> compute -> drive actuator, in a loop, until the goal is reached). This is what
3// makes a robot/sensor/actuator actually CONTROL something -- the same loop the printer and the IoT
4// devices run, generalized. Fixed-point / NO FLOAT throughout (gains scaled by /100). A simple,
5// provably-stable first-order plant (a motor+load: position advances by (command - load)/inertia
6// per tick) stands in for real hardware so the loop is gate-verifiable without a machine.
7// NEVER-BRICK (#26) BY CONSTRUCTION: the controller output is CLAMPED to +/-UMAX, so no matter how
8// large the error, the actuator can never be commanded past a safe limit -- a hardware-safety
9// invariant proven mechanically by the gate, not promised. (The real hardware-write driver inherits
10// this clamp + a watchdog + safe-state-on-fault.) license_tier: ORIGINAL expect_exit: 0
11import "nx_syscalls.nx"
12
13// proportional controller with a hard safety clamp. e = setpoint - measured (the sensor feedback).
14// u = Kp*e/100, then CLAMPED to [-umax, +umax] (the never-brick bound).
15func ctl_p(e: i64, kp: i64, umax: i64) -> i64 {
16 var u: i64 = kp * e / 100
17 if u > umax { u = umax }
18 if u < (0 - umax) { u = 0 - umax }
19 return u
20}
21
22// the raw (UNCLAMPED) command -- used only to PROVE the clamp is doing real work
23func ctl_p_raw(e: i64, kp: i64) -> i64 { return kp * e / 100 }
24
25// first-order plant tick: position advances by (command - load)/inertia. Stable for sane Kp.
26func plant_step(p: i64, u: i64, load: i64, inertia: i64) -> i64 {
27 return p + (u - load) / inertia
28}
29
30// CLOSED LOOP: sensor(p) -> controller -> actuator -> plant, repeated. Returns final position.
31// out_maxu[0] <- the maximum |command| issued over the run (for the never-brick clamp proof).
32func run_closed(setpoint: i64, kp: i64, umax: i64, load: i64, inertia: i64, ticks: i64, out_maxu: *i64) -> i64 {
33 var p: i64 = 0
34 var maxu: i64 = 0
35 var t: i64 = 0
36 while t < ticks {
37 let e: i64 = setpoint - p // <-- sensor feedback (measured position)
38 let u: i64 = ctl_p(e, kp, umax) // <-- compute actuator command (clamped)
39 var au: i64 = u
40 if au < 0 { au = 0 - au }
41 if au > maxu { maxu = au }
42 p = plant_step(p, u, load, inertia) // <-- drive the actuator / plant responds
43 t = t + 1
44 }
45 out_maxu[0] = maxu
46 return p
47}
48
49// OPEN LOOP (negative control): a fixed feed-forward command, NO sensor feedback. Returns final pos.
50// Proves that without feedback the goal is NOT reached -> feedback is doing the real "control".
51func run_open(setpoint: i64, fixed_u: i64, load: i64, inertia: i64, ticks: i64) -> i64 {
52 var p: i64 = 0
53 var t: i64 = 0
54 while t < ticks {
55 p = plant_step(p, fixed_u, load, inertia)
56 t = t + 1
57 }
58 return p
59}