nx_robot_firmware.nx source
↩ module page · 114 lines · 6587 B
1// nx_robot_firmware.nx -- SOVEREIGN MCU FIRMWARE GENERATOR (Omniforge ladder tier-4: "given a board,
2// auto-generate NishiLang firmware for its MCUs"). This is what makes "robots run our Nishi code"
3// LITERAL: given a board control spec (setpoint, gains, safety limits, pins), it EMITS a NishiLang
4// firmware source file implementing the closed-loop controller -- reusing the SAME proven control
5// math (nx_robot_control). The emitted firmware is NEVER-BRICK (#26) BY CONSTRUCTION: it contains an
6// output clamp, a watchdog, and a safe-state handler, so on any fault (sensor loss / watchdog
7// timeout / e-stop) it commands the actuator to a SAFE state instead of running away.
8// Also provides the fail-safe RUNTIME MODEL (fw_safe_command + fw_run host-sim) so the generated
9// firmware's behaviour -- converge / clamp / fault->safe-state -- is gate-provable WITHOUT real
10// hardware. HONEST SCOPE: the emitted .nx targets a hardware HAL (hw_read_sensor/hw_drive_step) that
11// is the next rung (the fail-safe driver); we generate + prove the logic, not flash a chip.
12// license_tier: ORIGINAL expect_exit: 0
13import "nx_syscalls.nx"
14import "nx_robot_control.nx"
15
16func fw_puts(buf: *u8, len: *i64, s: *u8) -> i64 {
17 var i: i64 = 0
18 while s[i] != (0 as u8) { buf[len[0]] = s[i]; len[0] = len[0] + 1; i = i + 1 }
19 return 0
20}
21func fw_puti(buf: *u8, len: *i64, v: i64) -> i64 {
22 if v == 0 { buf[len[0]] = 48 as u8; len[0] = len[0] + 1; return 0 }
23 var m: i64 = v
24 if m < 0 { buf[len[0]] = 45 as u8; len[0] = len[0] + 1; m = 0 - m }
25 let d: *u8 = sys_mmap(24); var k: i64 = 0
26 while m > 0 { d[k] = (48 + (m % 10)) as u8; m = m / 10; k = k + 1 }
27 var i: i64 = k - 1
28 while i >= 0 { buf[len[0]] = d[i]; len[0] = len[0] + 1; i = i - 1 }
29 return 0
30}
31
32// Emit a NishiLang firmware source for a closed-loop control board into out[]; return byte length.
33// Parameters become const declarations; the loop, clamp, watchdog and safe-state are always present.
34func firmware_emit(setpoint: i64, kp: i64, umax: i64, wd_ticks: i64,
35 pin_step: i64, pin_sensor: i64, out: *u8) -> i64 {
36 let len: *i64 = sys_mmap(8) as *i64
37 len[0] = 0
38 fw_puts(out, len, "// nx_fw_robot.nx -- GENERATED Nishi firmware (closed-loop control).\n" as *u8)
39 fw_puts(out, len, "// NEVER-BRICK #26 BY CONSTRUCTION: output clamp + watchdog + safe-state.\n" as *u8)
40 fw_puts(out, len, "// Targets the fail-safe HAL (hw_read_sensor / hw_drive_step) = next rung.\n" as *u8)
41 fw_puts(out, len, "import \"nx_syscalls.nx\"\n" as *u8)
42 fw_puts(out, len, "import \"nx_robot_hal.nx\"\n" as *u8)
43 fw_puts(out, len, "const FW_SETPOINT: i64 = " as *u8); fw_puti(out, len, setpoint); fw_puts(out, len, "\n" as *u8)
44 fw_puts(out, len, "const FW_KP: i64 = " as *u8); fw_puti(out, len, kp); fw_puts(out, len, "\n" as *u8)
45 fw_puts(out, len, "const FW_UMAX: i64 = " as *u8); fw_puti(out, len, umax); fw_puts(out, len, "\n" as *u8)
46 fw_puts(out, len, "const FW_WATCHDOG_TICKS: i64 = " as *u8); fw_puti(out, len, wd_ticks); fw_puts(out, len, "\n" as *u8)
47 fw_puts(out, len, "const FW_PIN_STEP: i64 = " as *u8); fw_puti(out, len, pin_step); fw_puts(out, len, "\n" as *u8)
48 fw_puts(out, len, "const FW_PIN_SENSOR: i64 = " as *u8); fw_puti(out, len, pin_sensor); fw_puts(out, len, "\n" as *u8)
49 // the never-brick clamp
50 fw_puts(out, len, "func fw_clamp(u: i64) -> i64 {\n" as *u8)
51 fw_puts(out, len, " if u > FW_UMAX { return FW_UMAX }\n" as *u8)
52 fw_puts(out, len, " if u < (0 - FW_UMAX) { return 0 - FW_UMAX }\n" as *u8)
53 fw_puts(out, len, " return u\n}\n" as *u8)
54 // the safe state: command actuator to 0 (motors hold/disable) -- never runaway
55 fw_puts(out, len, "func fw_safe_state() -> i64 { hw_drive_step(FW_PIN_STEP, 0); return 0 }\n" as *u8)
56 // the control loop with watchdog + fault->safe-state
57 fw_puts(out, len, "func fw_control_loop() -> i64 {\n" as *u8)
58 fw_puts(out, len, " hw_init_pin(FW_PIN_STEP); hw_init_pin(FW_PIN_SENSOR)\n" as *u8)
59 fw_puts(out, len, " var wd: i64 = 0\n" as *u8)
60 fw_puts(out, len, " while 1 == 1 {\n" as *u8)
61 fw_puts(out, len, " let s: i64 = hw_read_sensor(FW_PIN_SENSOR)\n" as *u8)
62 fw_puts(out, len, " if hw_sensor_invalid(s) { return fw_safe_state() }\n" as *u8)
63 fw_puts(out, len, " wd = wd + 1\n" as *u8)
64 fw_puts(out, len, " if wd > FW_WATCHDOG_TICKS { return fw_safe_state() }\n" as *u8)
65 fw_puts(out, len, " let e: i64 = FW_SETPOINT - s\n" as *u8)
66 fw_puts(out, len, " let u: i64 = fw_clamp(FW_KP * e / 100)\n" as *u8)
67 fw_puts(out, len, " hw_drive_step(FW_PIN_STEP, u)\n" as *u8)
68 fw_puts(out, len, " wd = 0\n" as *u8)
69 fw_puts(out, len, " }\n return 0\n}\n" as *u8)
70 return len[0]
71}
72
73// RUNTIME MODEL of the emitted firmware's control law: clamped P control, but on a FAULT
74// (sensor invalid / watchdog timeout / e-stop) it returns the SAFE command (0) -> never runaway.
75func fw_safe_command(e: i64, kp: i64, umax: i64, fault: i64) -> i64 {
76 if fault != 0 { return 0 }
77 return ctl_p(e, kp, umax)
78}
79
80// Host-sim of the emitted firmware against a plant, with a fault injected at tick fault_at.
81// safe_mode=1 -> the GENERATED firmware (detects fault -> safe-state).
82// safe_mode=0 -> a NAIVE firmware (no safe-state): on sensor loss it acts on garbage -> runaway.
83// out_maxcmd[0] <- max |command|; out_runaway[0] <- 1 if position blew past +/-5*setpoint. returns final pos.
84func fw_run(setpoint: i64, kp: i64, umax: i64, load: i64, inertia: i64, ticks: i64,
85 fault_at: i64, safe_mode: i64, out_maxcmd: *i64, out_runaway: *i64) -> i64 {
86 var p: i64 = 0
87 var maxc: i64 = 0
88 var runaway: i64 = 0
89 var t: i64 = 0
90 while t < ticks {
91 var fault: i64 = 0
92 if t >= fault_at { fault = 1 }
93 var u: i64 = 0
94 if safe_mode == 1 {
95 let e: i64 = setpoint - p
96 u = fw_safe_command(e, kp, umax, fault)
97 } else {
98 var meas: i64 = p
99 if fault == 1 { meas = 0 } // sensor lost -> reads garbage (0)
100 let e: i64 = setpoint - meas
101 u = ctl_p(e, kp, umax) // naive: keeps driving on bad data
102 }
103 var au: i64 = u
104 if au < 0 { au = 0 - au }
105 if au > maxc { maxc = au }
106 p = plant_step(p, u, load, inertia)
107 if p > setpoint * 5 { runaway = 1 }
108 if p < (0 - setpoint * 5) { runaway = 1 }
109 t = t + 1
110 }
111 out_maxcmd[0] = maxc
112 out_runaway[0] = runaway
113 return p
114}