nx_motion_plan.nx source

1// nx_motion_plan.nx -- SOVEREIGN PRINTER ARC rung 1a: trapezoidal motion planning (operator is 2// building our OWN printer; the QIDI is inferior). Every motion controller (Klipper/Marlin/GRBL 3// class) reduces a G-code move to an accel/cruise/decel velocity profile and then step pulses. 4// This is that math, integer-exact: distances um, velocities um/s, accel um/s^2, times ms. 5// d_acc = v^2 / (2a). If 2*d_acc <= dist -> TRAPEZOID (accel, cruise, decel). 6// Else -> TRIANGLE: v_peak = isqrt(a * dist) (from v^2 = 2a*(dist/2)). 7// Steps: steps = dist_um * steps_per_mm / 1000 (integer; fractional step carry = next rung). 8// HONEST SCOPE: single-move planning. Junction velocity (lookahead between moves), jerk limits, 9// and the step-pulse interval table for the MCU ISR are the flagged next rungs. 10// LAWS: struct-free, integer-only. license_tier: ORIGINAL 11import "nx_syscalls.nx" 12 13const MP_TRAPEZOID: i64 = 0 14const MP_TRIANGLE: i64 = 1 15const MP_BAD_INPUT: i64 = 2 // defensive boundary: zero/negative dist, vmax, or accel 16 17// integer sqrt (Newton, floor) 18func mp_isqrt(x: i64) -> i64 { 19 if x <= 0 { return 0 } 20 var r: i64 = x 21 if r > 1 { r = x / 2 } 22 var p: i64 = 0 23 var n: i64 = 0 24 while n < 64 { 25 p = r 26 r = (r + x / r) / 2 27 if r >= p { n = 64 } else { n = n + 1 } 28 } 29 return p 30} 31 32// plan one move. out[0]=shape out[1]=v_peak(um/s) out[2]=d_acc(um) out[3]=d_cruise(um) 33// out[4]=t_acc(ms) out[5]=t_cruise(ms) out[6]=t_total(ms). returns shape / MP_BAD_INPUT. 34func mp_plan(dist_um: i64, vmax: i64, acc: i64, out: *i64) -> i64 { 35 if dist_um <= 0 { out[0] = MP_BAD_INPUT; return MP_BAD_INPUT } 36 if vmax <= 0 { out[0] = MP_BAD_INPUT; return MP_BAD_INPUT } 37 if acc <= 0 { out[0] = MP_BAD_INPUT; return MP_BAD_INPUT } 38 let d_acc_full: i64 = (vmax * vmax) / (2 * acc) 39 if 2 * d_acc_full <= dist_um { 40 // trapezoid 41 out[0] = MP_TRAPEZOID 42 out[1] = vmax 43 out[2] = d_acc_full 44 out[3] = dist_um - 2 * d_acc_full 45 out[4] = (vmax * 1000) / acc // t_acc ms 46 out[5] = (out[3] * 1000) / vmax // t_cruise ms 47 out[6] = 2 * out[4] + out[5] 48 return MP_TRAPEZOID 49 } 50 // triangle: accelerate to v_peak over half the distance, decelerate 51 out[0] = MP_TRIANGLE 52 let vp: i64 = mp_isqrt(acc * dist_um) 53 out[1] = vp 54 out[2] = dist_um / 2 55 out[3] = 0 56 out[4] = (vp * 1000) / acc 57 out[5] = 0 58 out[6] = 2 * out[4] 59 return MP_TRIANGLE 60} 61 62// whole steps for a distance at steps_per_mm (fractional carry = flagged next rung) 63func mp_steps(dist_um: i64, steps_per_mm: i64) -> i64 { 64 return (dist_um * steps_per_mm) / 1000 65} 66 67// average step interval in MICROSECONDS during cruise at v (um/s), steps_per_mm: 68// steps/s = v * steps_per_mm / 1000 ; interval_us = 1e6 / steps_per_s 69func mp_cruise_interval_us(v_um_s: i64, steps_per_mm: i64) -> i64 { 70 let sps: i64 = (v_um_s * steps_per_mm) / 1000 71 if sps <= 0 { return 0 } 72 return 1000000 / sps 73}