nx_robot_control_gate.nx source
↩ module page · 102 lines · 5062 B
1// nx_robot_control_gate.nx -- R-ROBO-2 GATE: closed-loop "true control" + never-brick safety clamp.
2// Drives a plant from position 0 to setpoint 1000 (milli-units) with a constant load disturbance,
3// using only the sovereign no-float P-controller reading the (simulated) position sensor. VERIFIES:
4// (1) CLOSED LOOP CONVERGES to the setpoint within tolerance (it actually controls).
5// (2) it climbed from 0 (reached the target region).
6// (3) OPEN LOOP (fixed feed-forward, no feedback) does NOT converge -> feedback does the real work.
7// (4) NEVER-BRICK clamp: even at a huge error the commanded actuator output never exceeds UMAX.
8// (5) clamp has teeth: the UNCLAMPED command at that error WOULD exceed UMAX.
9// (6) HONEST: P-control leaves a small, measured steady-state error (~ load*100/Kp) -- not "perfect".
10// 100% sovereign, integer-only. license_tier: ORIGINAL expect_exit: 0
11import "nx_syscalls.nx"
12import "nx_robot_control.nx"
13
14func sw(s: *u8) -> i64 { var n: i64 = 0; while s[n] != (0 as u8) { n = n + 1 } sys_write(1, s, n); return 0 }
15func sn(v: i64) -> i64 {
16 if v == 0 { sys_write(1, "0" as *u8, 1); return 0 }
17 var m: i64 = v
18 if m < 0 { sys_write(1, "-" as *u8, 1); m = 0 - m }
19 let d: *u8 = sys_mmap(24); var k: i64 = 0
20 while m > 0 { d[k] = (48 + (m % 10)) as u8; m = m / 10; k = k + 1 }
21 let o: *u8 = sys_mmap(24); var i: i64 = 0
22 while i < k { o[i] = d[k - 1 - i]; i = i + 1 }
23 sys_write(1, o, k); return 0
24}
25
26func check(name: *u8, cond: i64, tot: *i64) -> i64 {
27 if cond == 1 { sw(" ok " as *u8); tot[0] = tot[0] + 1 }
28 else { sw(" FAIL " as *u8); tot[1] = tot[1] + 1 }
29 sw(name); sw("\n" as *u8)
30 return 0
31}
32
33func abs_i(v: i64) -> i64 { if v < 0 { return 0 - v } return v }
34
35func main() -> i64 {
36 let tot: *i64 = sys_mmap(16) as *i64
37 tot[0] = 0; tot[1] = 0
38
39 sw("=== nx_robot_control_gate R-ROBO-2 -- closed-loop true control + never-brick clamp ===\n" as *u8)
40
41 let setpoint: i64 = 1000 // target position (milli-units)
42 let kp: i64 = 200 // proportional gain (/100); high enough that the integer-truncation
43 // deadband ((load+inertia)*100/kp) sits well under tolerance
44 let umax: i64 = 300 // never-brick actuator clamp
45 let load: i64 = 3 // constant disturbance force
46 let inertia: i64 = 4 // plant divisor (loop gain kp/(100*inertia)=0.5 -> stable, monotone)
47 let ticks: i64 = 400
48 let tol: i64 = 30
49
50 let mu: *i64 = sys_mmap(8) as *i64
51 let pc: i64 = run_closed(setpoint, kp, umax, load, inertia, ticks, mu)
52 let ec: i64 = abs_i(setpoint - pc)
53 sw(" closed-loop final pos=" as *u8); sn(pc); sw(" (setpoint=1000) error=" as *u8); sn(ec); sw("\n" as *u8)
54
55 // (1) converges
56 var ok1: i64 = 0
57 if ec <= tol { ok1 = 1 }
58 check("closed loop CONVERGES to setpoint (|err| <= 30)" as *u8, ok1, tot)
59
60 // (2) climbed from 0 to the target region
61 var ok2: i64 = 0
62 if pc > (setpoint - tol) { ok2 = 1 }
63 check("closed loop CLIMBED from 0 into the target region" as *u8, ok2, tot)
64
65 // (3) open loop (feed-forward = load, no feedback) does NOT converge
66 let po: i64 = run_open(setpoint, load, load, inertia, ticks)
67 let eo: i64 = abs_i(setpoint - po)
68 sw(" open-loop (no feedback) final pos=" as *u8); sn(po); sw(" error=" as *u8); sn(eo); sw("\n" as *u8)
69 var ok3: i64 = 0
70 if eo > tol { ok3 = 1 }
71 check("NEG-CONTROL: open loop FAILS to reach setpoint (feedback does the work)" as *u8, ok3, tot)
72
73 // (4) never-brick clamp under huge error
74 let mu2: *i64 = sys_mmap(8) as *i64
75 let big: i64 = 1000000
76 let pbig: i64 = run_closed(big, kp, umax, load, inertia, 50, mu2)
77 sw(" huge-setpoint run max|command|=" as *u8); sn(mu2[0]); sw(" UMAX=" as *u8); sn(umax); sw("\n" as *u8)
78 var ok4: i64 = 0
79 if mu2[0] <= umax { ok4 = 1 }
80 check("NEVER-BRICK: actuator command never exceeds UMAX (clamped by construction)" as *u8, ok4, tot)
81
82 // (5) clamp has teeth: unclamped command at that error would blow past UMAX
83 let raw: i64 = ctl_p_raw(big, kp)
84 sw(" unclamped command at huge error=" as *u8); sn(raw); sw(" (clamp prevented this)\n" as *u8)
85 var ok5: i64 = 0
86 if raw > umax { ok5 = 1 }
87 check("clamp has teeth: unclamped command WOULD exceed UMAX" as *u8, ok5, tot)
88
89 // (6) control efficacy + honesty: closed-loop error is >=10x smaller than open-loop, and we
90 // report the integer-truncation deadband floor (honest: integer P-control is not "perfect").
91 let deadband: i64 = (load + inertia) * 100 / kp
92 sw(" integer-deadband floor~=" as *u8); sn(deadband); sw(" measured closed err=" as *u8); sn(ec)
93 sw(" (open err=" as *u8); sn(eo); sw(")\n" as *u8)
94 var ok6: i64 = 0
95 if eo >= 10 * ec { ok6 = 1 }
96 check("control EFFICACY: closed-loop error >= 10x smaller than open-loop" as *u8, ok6, tot)
97
98 sw("=== VERDICT pass=" as *u8); sn(tot[0]); sw(" fail=" as *u8); sn(tot[1]); sw(" ===\n" as *u8)
99 if tot[1] == 0 { sys_exit(0) }
100 sys_exit(1)
101 return 1
102}