nx_robot_step.nx source
↩ module page · 79 lines · 3470 B
1// nx_robot_step.nx -- SOVEREIGN multi-axis stepper STEP GENERATION (the actuation primitive that
2// drives a real robot/printer/CNC/sensor-gantry). This is the rung nx_motion_plan.nx explicitly
3// flagged as NEXT ("multi-axis ... step-pulse table for the MCU ISR"): given a synchronized move as
4// per-axis signed step counts, emit the exact pulse train each axis must send so all axes arrive
5// together. Pure integer Bresenham/DDA -- NO FLOAT (this is WHY steppers are driven this way: a
6// motor takes discrete steps, the math is inherently integer). The dominant axis pulses every tick;
7// minor axes are Bresenham-distributed so each axis i emits EXACTLY |delta[i]| pulses, evenly.
8// This is the SAME math the printer (which the operator cited as the proof) runs, generalized to any
9// N-axis machine -> "output to real robots we can build", in Nishi.
10// NEVER-BRICK (#26): this emits a pulse PLAN (pure data); the real GPIO/STEP-pin write is a separate
11// driver that must be fail-safe by construction (bounded rate, watchdog, safe-state-on-fault).
12// license_tier: ORIGINAL expect_exit: 0
13import "nx_syscalls.nx"
14
15// Generate an N-axis synchronized step plan via integer Bresenham.
16// delta[i] = signed target step count for axis i
17// dir[i] <- +1 / -1 / 0 (filled: direction per axis)
18// puls[t*naxes + i] <- 1 if axis i pulses on tick t else 0 (filled)
19// returns n_ticks (= max_i |delta[i]|), bounded by max_ticks.
20func robot_step_plan(delta: *i64, naxes: i64, dir: *i64, puls: *i64, max_ticks: i64) -> i64 {
21 var nt: i64 = 0
22 var i: i64 = 0
23 while i < naxes {
24 var ad: i64 = delta[i]
25 if ad < 0 { ad = 0 - ad }
26 dir[i] = 0
27 if delta[i] > 0 { dir[i] = 1 }
28 if delta[i] < 0 { dir[i] = 0 - 1 }
29 if ad > nt { nt = ad }
30 i = i + 1
31 }
32 if nt == 0 { return 0 }
33 if nt > max_ticks { nt = max_ticks }
34 let err: *i64 = sys_mmap(naxes * 8) as *i64
35 i = 0
36 while i < naxes { err[i] = nt / 2; i = i + 1 }
37 var t: i64 = 0
38 while t < nt {
39 i = 0
40 while i < naxes {
41 var ad: i64 = delta[i]
42 if ad < 0 { ad = 0 - ad }
43 err[i] = err[i] + ad
44 var p: i64 = 0
45 if err[i] >= nt { err[i] = err[i] - nt; p = 1 }
46 puls[t * naxes + i] = p
47 i = i + 1
48 }
49 t = t + 1
50 }
51 return nt
52}
53
54// Bresenham correctness invariant for axis i over a plan: at every tick the emitted pulse count
55// must track the ideal ratio within one major-step, i.e. |count*nt - t*|delta_i|| <= nt for all t.
56// returns the MAX absolute deviation*... actually returns the max |count*nt - t*ad| over ticks
57// (0 <= max <= nt for a correct Bresenham axis; > nt means a broken/uneven sequence).
58func robot_step_max_dev(puls: *i64, naxes: i64, nt: i64, axis: i64, ad: i64) -> i64 {
59 var count: i64 = 0
60 var maxdev: i64 = 0
61 var t: i64 = 0
62 while t < nt {
63 if puls[t * naxes + axis] == 1 { count = count + 1 }
64 let ideal_num: i64 = (t + 1) * ad // ideal pulses-so-far * nt
65 var dev: i64 = count * nt - ideal_num
66 if dev < 0 { dev = 0 - dev }
67 if dev > maxdev { maxdev = dev }
68 t = t + 1
69 }
70 return maxdev
71}
72
73// total pulses emitted for an axis
74func robot_step_count(puls: *i64, naxes: i64, nt: i64, axis: i64) -> i64 {
75 var c: i64 = 0
76 var t: i64 = 0
77 while t < nt { if puls[t * naxes + axis] == 1 { c = c + 1 } t = t + 1 }
78 return c
79}