From 0c5eee3e0710fbc2a023b8fcedf68cb1c2bcc783 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 10 Nov 2026 17:59:45 +1100 Subject: [PATCH] motion: the S-curve single axis planner stops at the end of a move On the S-curve planner, a move of the single axis planner, which runs jogs, free mode joint moves, homing included, and external offsets, that reached its target went back and forth over it for about two seconds and stopped up to 0.8 um off. Once within its dead band of the target, simple_scurve_tp_update() set the speed to zero but still took one more step with the speed and acceleration of the cycle before; that step put it some microns back outside, the trapezoid ramp it falls back on there brought it in again, and so on, each round a little smaller. Motion reported the jog done while this went on. An incremental jog of X by 5 at 100 mm/s, 800 mm/s^2 and a jerk of 10000 arrives after 252 servo cycles, then hunted until cycle 2047, up to 7.7 um off the target, and came to rest 0.8 um off it; now it stops on the cycle it arrives, 3 nm off. At the target the planner now stops without that step. tests/jog-speed-change adds an incremental jog of joint 0 in free mode and of X in teleop mode, on both planners, and checks that each ends where it was sent and that no servo cycle runs the other way. On master the S-curve run has joint 0 run backwards in 130 servo cycles and X in 445; here none. The joint's velocity is checked in free mode only, since in teleop it comes from the cubic interpolator rather than the planner. --- src/emc/motion/simple_tp.c | 4 ++++ tests/jog-speed-change/README | 7 ++++--- tests/jog-speed-change/sim.hal | 6 ++++-- tests/jog-speed-change/test-ui.py | 28 +++++++++++++++++++++++++--- 4 files changed, 37 insertions(+), 8 deletions(-) diff --git a/src/emc/motion/simple_tp.c b/src/emc/motion/simple_tp.c index 92394bce6ac..3f117c1510e 100644 --- a/src/emc/motion/simple_tp.c +++ b/src/emc/motion/simple_tp.c @@ -205,8 +205,12 @@ void simple_scurve_tp_update(simple_tp_t *tp, double period){ } } }else{ + /* at the target: stop here, without the step the last cycle's + speed would still take, which would put it back outside */ vel_req = 0.0; tp->curr_jerk = 0; + curr_vel = 0.0; + curr_acc = 0.0; } /* a max_vel lowered under the current speed is reached by the ramp diff --git a/tests/jog-speed-change/README b/tests/jog-speed-change/README index 5653ec1fbc0..cbda3b1da56 100644 --- a/tests/jog-speed-change/README +++ b/tests/jog-speed-change/README @@ -1,6 +1,7 @@ A continuous jog asked again at a lower speed while it runs, as halui does when halui.axis.jog-speed or halui.joint.jog-speed changes while a jog pin is held: the speed comes down to the new one within the -acceleration limit. A joint jog in free mode and a world jog in teleop -mode, every servo cycle sampled, on the trapezoid and on the S-curve -planner. +acceleration limit. Then an incremental jog of each, which must end +where it was sent and not go back and forth over its end. A joint jog +in free mode and a world jog in teleop mode, every servo cycle sampled, +on the trapezoid and on the S-curve planner. diff --git a/tests/jog-speed-change/sim.hal b/tests/jog-speed-change/sim.hal index efc26461f7b..443dfbbafb2 100644 --- a/tests/jog-speed-change/sim.hal +++ b/tests/jog-speed-change/sim.hal @@ -1,8 +1,9 @@ loadrt [KINS]KINEMATICS loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS -# the commanded velocity of joint 0 and of axis X every servo cycle -loadrt sampler depth=4000 cfg=ff +# the commanded velocity of joint 0 and of axis X every servo cycle, and +# whether motion is in teleop, where the joint's comes from the cubic +loadrt sampler depth=4000 cfg=ffb addf motion-command-handler servo-thread addf motion-controller servo-thread @@ -10,6 +11,7 @@ addf sampler.0 servo-thread net VJ joint.0.vel-cmd => sampler.0.pin.0 net VX axis.x.teleop-vel-cmd => sampler.0.pin.1 +net TM motion.teleop-mode => sampler.0.pin.2 loadusr halsampler -t samples.log net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb diff --git a/tests/jog-speed-change/test-ui.py b/tests/jog-speed-change/test-ui.py index a27fef12831..5130a7a0889 100755 --- a/tests/jog-speed-change/test-ui.py +++ b/tests/jog-speed-change/test-ui.py @@ -49,7 +49,9 @@ def wait_for(what, test, timeout=30): def settled(): wait_for("the machine to stop", - lambda: s.inpos and not hal.get_value("motion.jog-is-active")) + lambda: s.inpos and not hal.get_value("motion.jog-is-active") + and hal.get_value("joint.0.vel-cmd") == 0 + and hal.get_value("axis.x.teleop-vel-cmd") == 0) def slow_down(jjog, what): # FAST, then the same jog again at SLOW while it still runs @@ -66,6 +68,19 @@ def slow_down(jjog, what): settled() drain() +def step(jjog, what): + # an incremental jog of 5, in the same direction as the others; the + # position read from HAL, live like the speeds settled() waits on + before = hal.get_value("joint.0.pos-cmd") + c.jog(linuxcnc.JOG_INCREMENT, jjog, 0, FAST, 5) + wait_for("%s to start" % what, lambda: hal.get_value("motion.jog-is-active"), timeout=5) + settled() + moved = hal.get_value("joint.0.pos-cmd") - before + print("%s: an incremental jog of 5 moved %.9f" % (what, moved)) + if abs(moved - 5) > 1e-5: + error("%s: an incremental jog of 5 moved %.9f" % (what, moved)) + drain() + h = hal.component("test-ui") h.ready() @@ -83,22 +98,25 @@ def slow_down(jjog, what): c.wait_complete(30) wait_for("free mode", lambda: s.motion_mode == linuxcnc.TRAJ_MODE_FREE) slow_down(True, "joint 0") +step(True, "joint 0") # a world jog in teleop mode c.teleop_enable(1) c.wait_complete(30) wait_for("teleop mode", lambda: s.motion_mode == linuxcnc.TRAJ_MODE_TELEOP) slow_down(False, "axis X") +step(False, "axis X") # every servo cycle: the speed comes down to SLOW within the acceleration -# limit, it does not drop at once +# limit, it does not drop at once; and every jog runs one way, so a speed +# the other way is a jog going back and forth over its end time.sleep(0.5) with open(LOG) as f: lines = f.read().split("\n") samples = [] for line in lines[:-1]: v = line.split() - if len(v) == 3: + if len(v) == 4: samples.append([float(x) for x in v[1:]]) print("%d samples" % len(samples)) for i, what in enumerate(("joint 0", "axis X")): @@ -113,6 +131,10 @@ def slow_down(jjog, what): error("%s ran at %.3f, over the %.3f asked" % (what, vpeak, FAST)) if apeak > ACC * 1.001: error("%s accelerated at %.2f, over its limit of %.2f" % (what, apeak, ACC)) + # the joint's own planner runs in free mode only + back = sum(1 for v in samples if v[i] < 0 and (i == 1 or not v[2])) + if back: + error("%s ran backwards in %d servo cycles" % (what, back)) if not samples: error("no samples") overruns = int(hal.get_value("sampler.0.overruns"))