diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 22cbfdaf5a4..1a95e90fb6c 100644 --- a/src/emc/motion/control.c +++ b/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 diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index d245113f5ab..44b73115b3c 100755 --- a/tests/kins-switch/test-ui.py +++ b/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 diff --git a/tests/kins-switch/test.ini b/tests/kins-switch/test.ini index 59e2c6d151a..cc47ac69889 100644 --- a/tests/kins-switch/test.ini +++ b/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