nx_robot_kinematics.nx source
↩ module page · 41 lines · 1922 B
1// nx_robot_kinematics.nx -- SOVEREIGN robot KINEMATICS (turn a desired pose into machine/joint
2// motion). Two real machine classes:
3// 1) CoreXY / H-bot / Cartesian -- the kinematics of most printers/CNC/gantry robots. The belt
4// equations are LINEAR -> INTEGER-EXACT, NO TRIG, perfect FK<->IK round-trip. Feeds directly
5// into nx_robot_step (motor deltas -> synchronized stepper pulses).
6// 2) 2-link planar ARM -- the canonical rotary robot arm. FORWARD kinematics (joint angles ->
7// end-effector position) via the sovereign Q10 nx_trig sin/cos. INVERSE arm IK needs acos/atan2
8// (not yet in nx_trig) -> flagged as the next rung.
9// NO FLOAT throughout (CoreXY exact integer; arm in Q10 fixed point).
10// license_tier: ORIGINAL expect_exit: 0
11import "nx_syscalls.nx"
12import "nx_trig.nx"
13
14// CoreXY inverse kinematics: cartesian (x,y) -> motor (A,B). A=x+y, B=x-y. EXACT integer.
15func corexy_ik(x: i64, y: i64, out_a: *i64, out_b: *i64) -> i64 {
16 out_a[0] = x + y
17 out_b[0] = x - y
18 return 0
19}
20
21// CoreXY forward kinematics: motor (A,B) -> cartesian (x,y). x=(A+B)/2, y=(A-B)/2.
22// EXACT when A,B come from corexy_ik (A+B=2x, A-B=2y are even).
23func corexy_fk(a: i64, b: i64, out_x: *i64, out_y: *i64) -> i64 {
24 out_x[0] = (a + b) / 2
25 out_y[0] = (a - b) / 2
26 return 0
27}
28
29// 2-link planar arm FORWARD kinematics. t1,t2 in Q10 turns (1024 = full circle, 256 = 90 deg).
30// l1,l2 link lengths (length units). out (x,y) in the same units.
31// x = (l1*cos(t1) + l2*cos(t1+t2)) / 1024
32// y = (l1*sin(t1) + l2*sin(t1+t2)) / 1024
33func arm2_fk(t1: i64, t2: i64, l1: i64, l2: i64, out_x: *i64, out_y: *i64) -> i64 {
34 let c1: i64 = nx_cos_turn_q10(t1)
35 let s1: i64 = nx_sin_turn_q10(t1)
36 let c12: i64 = nx_cos_turn_q10(t1 + t2)
37 let s12: i64 = nx_sin_turn_q10(t1 + t2)
38 out_x[0] = (l1 * c1 + l2 * c12) / 1024
39 out_y[0] = (l1 * s1 + l2 * s12) / 1024
40 return 0
41}