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" 12import "nx_vecmath.nx" 13 14const MP_TRAPEZOID: i64 = 0 15const MP_TRIANGLE: i64 = 1 16const MP_BAD_INPUT: i64 = 2 // defensive boundary: zero/negative dist, vmax, or accel 17 18// integer sqrt (Newton, floor) 19func mp_isqrt(x: i64) -> i64 { return vm_isqrt(x) } 20 21// plan one move. out[0]=shape out[1]=v_peak(um/s) out[2]=d_acc(um) out[3]=d_cruise(um) 22// out[4]=t_acc(ms) out[5]=t_cruise(ms) out[6]=t_total(ms). returns shape / MP_BAD_INPUT. 23func mp_plan(dist_um: i64, vmax: i64, acc: i64, out: *i64) -> i64 { 24 if dist_um <= 0 { out[0] = MP_BAD_INPUT; return MP_BAD_INPUT } 25 if vmax <= 0 { out[0] = MP_BAD_INPUT; return MP_BAD_INPUT } 26 if acc <= 0 { out[0] = MP_BAD_INPUT; return MP_BAD_INPUT } 27 let d_acc_full: i64 = (vmax * vmax) / (2 * acc) 28 if 2 * d_acc_full <= dist_um { 29 // trapezoid 30 out[0] = MP_TRAPEZOID 31 out[1] = vmax 32 out[2] = d_acc_full 33 out[3] = dist_um - 2 * d_acc_full 34 out[4] = (vmax * 1000) / acc // t_acc ms 35 out[5] = (out[3] * 1000) / vmax // t_cruise ms 36 out[6] = 2 * out[4] + out[5] 37 return MP_TRAPEZOID 38 } 39 // triangle: accelerate to v_peak over half the distance, decelerate 40 out[0] = MP_TRIANGLE 41 let vp: i64 = mp_isqrt(acc * dist_um) 42 out[1] = vp 43 out[2] = dist_um / 2 44 out[3] = 0 45 out[4] = (vp * 1000) / acc 46 out[5] = 0 47 out[6] = 2 * out[4] 48 return MP_TRIANGLE 49} 50 51// whole steps for a distance at steps_per_mm (fractional carry = flagged next rung) 52func mp_steps(dist_um: i64, steps_per_mm: i64) -> i64 { 53 return (dist_um * steps_per_mm) / 1000 54} 55 56// average step interval in MICROSECONDS during cruise at v (um/s), steps_per_mm: 57// steps/s = v * steps_per_mm / 1000 ; interval_us = 1e6 / steps_per_s 58func mp_cruise_interval_us(v_um_s: i64, steps_per_mm: i64) -> i64 { 59 let sps: i64 = (v_um_s * steps_per_mm) / 1000 60 if sps <= 0 { return 0 } 61 return 1000000 / sps 62}