motion: a kinematics switch reads the joints where the planner put them

src/emc/motion/control.c

@@ -347,12 +347,13 @@ static void handle_kinematicsSwitch(void) {
 
     emcmot_joint_t *jointKinsSwitch;
     double joint_posKinsSwitch[EMCMOT_MAX_JOINTS] = {0,};
-    /* copy joint position feedback to local array */
+    /* the joints where the planner has put them: pos_cmd is the cubic
+       interpolation behind it, still a few servo cycles short of the end
+       of a move motion reports done, and a pose read off it would hold
+       the joints there */
     for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
-        /* point to joint struct */
         jointKinsSwitch = &joints[joint_num];
-        /* copy feedback */
-        joint_posKinsSwitch[joint_num] = jointKinsSwitch->pos_cmd;
+        joint_posKinsSwitch[joint_num] = jointKinsSwitch->coarse_pos;
     }
 
     /* a module refuses a type it does not provide and goes on running the

tests/kins-switch/test-ui.py

@@ -164,6 +164,15 @@ def mdi(cmd):
     print("W ran to %.4f, the other joints held to %.2e"
           % (w_reached, max(strayed[j] for j in CARRIED)))
 
+# the G0 W0 ends on W0 although the switch after it comes as soon as motion
+# reports the move done, while the joints are still a few servo cycles
+# short of its end: a switch that read the pose there held W short
+w_home = settled()[5]
+if abs(w_home) > 1e-9:
+    error("W stopped at %.9f, not on the 0 of the G0 W0 the switch came after" % w_home)
+else:
+    print("W came back to 0 before the switch")
+
 # the program walks 0, 1, 0, 1 and the line after each selection reports
 # what the interpreter holds and what motion runs; the pin read there is
 # the check that the selection went through before the next line, which

tests/kins-switch/test.ini

@@ -38,7 +38,9 @@ COMM_TIMEOUT = 4
 
 [TASK]
 TASK = milltask
-CYCLE_TIME = 0.010
+# a short cycle: the switch after the program's G0 W0 reaches motion within
+# a few servo cycles of the move's end
+CYCLE_TIME = 0.001
 
 [EMCIO]
 TOOL_TABLE = tool.tbl