From 6a059eafdd27f8c3a81311a456306a5a3830fcb2 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 20 Sep 2026 10:40:11 +1000 Subject: [PATCH] homing: run every leg of a synchronized home sequence together A negative HOME_SEQUENCE only synchronized the final move. Search, back-off and latch ran per joint, so on a gantry one side moved while the other held, or the two moved in opposite directions, for distances the software cannot know: how deep a side sits in its switch, how far the switches are out of line. Each such leg racks the frame, and on a stepper gantry with encoders that is a following error that comes and goes with where the previous cycle left the frame (#4572). Run every leg together: back off together when any side starts on its switch, search together, finish the search at latch speed once the first side has tripped, back off at latch speed once all have tripped, latch together. The sides are then never driven apart by more than the misalignment of their switches plus the latch overshoot: 0.56 mm of strain instead of 1.02 mm with Sigma1912's settings on a simulated bridge. A stuck-on switch drives the group together to the end of travel and aborts there instead of twisting the frame. tests/home-sync-backoff couples two joints through a limit3 bridge and homes the pair from far away, from HOME with one switch active, and with both sides inside their switches; the old code peaks at 1.2 to 1.3 mm of strain against a 1.05 mm limit. --- docs/src/config/ini-homing.adoc | 6 + src/emc/motion/homing.c | 96 ++++++++++- tests/home-sync-backoff/README | 13 ++ tests/home-sync-backoff/checkresult | 4 + tests/home-sync-backoff/home-sync-backoff.hal | 49 ++++++ tests/home-sync-backoff/test-ui.py | 162 ++++++++++++++++++ tests/home-sync-backoff/test.ini | 72 ++++++++ tests/home-sync-backoff/test.sh | 2 + 8 files changed, 398 insertions(+), 6 deletions(-) create mode 100644 tests/home-sync-backoff/README create mode 100755 tests/home-sync-backoff/checkresult create mode 100644 tests/home-sync-backoff/home-sync-backoff.hal create mode 100755 tests/home-sync-backoff/test-ui.py create mode 100644 tests/home-sync-backoff/test.ini create mode 100755 tests/home-sync-backoff/test.sh diff --git a/docs/src/config/ini-homing.adoc b/docs/src/config/ini-homing.adoc index 7f99d8552de..8937a765c1c 100644 --- a/docs/src/config/ini-homing.adoc +++ b/docs/src/config/ini-homing.adoc @@ -261,6 +261,12 @@ A *negative* HOME_SEQUENCE also applies to commands to home a single joint. If the HOME_SEQUENCE value is *negative*, all joints having the same absolute value of that HOME_SEQUENCE will be *homed together with a synchronized final move*. If the HOME_SEQUENCE value is zero or positive, a command to home the joint will home only the specified joint. +The joints of a synchronized group are treated as rigidly coupled sides of one machine, so no joint moves alone for an unknown distance: +if any joint starts on its home switch, every joint of the group backs off together until every switch is clear, and then every joint searches together; +once the first joint trips its switch the others finish their search at HOME_LATCH_VEL; +the back-off from the switch runs at HOME_LATCH_VEL and starts once every joint has tripped, and the latch move starts once every joint has backed off. +The two sides are then never driven further apart than the misalignment of their switches, plus the latch-move overshoot. + Joint mode jogging of joints having a negative HOME_SEQUENCE is disallowed. In common gantry applications, such jogging can lead to misalignment (racking). Note that conventional jogging in world coordinates is always available once a machine is homed. diff --git a/src/emc/motion/homing.c b/src/emc/motion/homing.c index 662cd2f3591..7fee0dabc8f 100644 --- a/src/emc/motion/homing.c +++ b/src/emc/motion/homing.c @@ -737,6 +737,60 @@ static bool sync_ready(int joint_num) return 1; // ready } // sync_ready() +/* Joints of a negative home sequence are the coupled sides of one + machine and run every leg together: they back off together when any + side starts on its switch, search together, finish the search at latch + speed once the first side has tripped, back off at latch speed once + all have tripped, and latch together. The sides are then never driven + apart by more than the misalignment of their switches. */ +static bool sync_partner(int joint_num, int jno) +{ + if (jno == joint_num) {return 0;} + if (!H[jno].joint_in_sequence) {return 0;} + if (ABS(H[jno].home_sequence) != current_sequence) {return 0;} + if (ABS(H[joint_num].home_sequence) != current_sequence) {return 0;} + if (H[jno].home_flags & HOME_ABSOLUTE_ENCODER) {return 0;} + return 1; +} + +// a partner sits on its switch before having searched for it +static bool sync_partner_on_switch(int joint_num) +{ + int jno; + if (H[joint_num].home_sequence >= 0) {return 0;} + for (jno = 0; jno < all_joints; jno++) { + if (!sync_partner(joint_num, jno)) {continue;} + if (!H[jno].home_sw) {continue;} + if (H[jno].home_state > HOME_INITIAL_SEARCH_WAIT) {continue;} + return 1; + } + return 0; +} + +// a partner has found its switch +static bool sync_partner_tripped(int joint_num) +{ + int jno; + if (H[joint_num].home_sequence >= 0) {return 0;} + for (jno = 0; jno < all_joints; jno++) { + if (!sync_partner(joint_num, jno)) {continue;} + if (H[jno].home_state >= HOME_SET_COARSE_POSITION) {return 1;} + } + return 0; +} + +// a partner has not reached the given state yet +static bool sync_partner_before(int joint_num, home_state_t state) +{ + int jno; + if (H[joint_num].home_sequence >= 0) {return 0;} + for (jno = 0; jno < all_joints; jno++) { + if (!sync_partner(joint_num, jno)) {continue;} + if (H[jno].home_state < state) {return 1;} + } + return 0; +} + static int base_1joint_state_machine(int joint_num) { emcmot_joint_t *joint; @@ -884,8 +938,8 @@ static int base_1joint_state_machine(int joint_num) the home switch. It terminates when the switch is cleared successfully. If the move ends or hits a limit before it clears the switch, the home is aborted. */ - /* are we off home switch yet? */ - if (! home_sw_active) { + /* are we and our partners off the home switches yet? */ + if (! home_sw_active && ! sync_partner_on_switch(joint_num)) { /* yes, stop motion */ joint->free_tp.enable = 0; /* begin initial search */ @@ -914,8 +968,8 @@ static int base_1joint_state_machine(int joint_num) break; } H[joint_num].pause_timer = 0; - /* make sure we aren't already on home switch */ - if (home_sw_active) { + /* make sure we or a partner aren't already on home switch */ + if (home_sw_active || sync_partner_on_switch(joint_num)) { /* already on switch, need to back off it first */ H[joint_num].home_state = HOME_INITIAL_BACKOFF_START; immediate_state = 1; @@ -941,6 +995,12 @@ static int base_1joint_state_machine(int joint_num) immediate_state = 1; break; } + /* a partner found its switch first: finish at latch speed */ + if ( (H[joint_num].home_latch_vel != 0.0) + && (joint->free_tp.max_vel > fabs(H[joint_num].home_latch_vel)) + && sync_partner_tripped(joint_num)) { + joint->free_tp.max_vel = fabs(H[joint_num].home_latch_vel); + } ABORT_CHECK(joint_num); break; @@ -988,6 +1048,11 @@ static int base_1joint_state_machine(int joint_num) H[joint_num].pause_timer = 0; break; } + /* back off together with the partners */ + if (sync_partner_before(joint_num, HOME_SET_COARSE_POSITION)) { + H[joint_num].pause_timer = 0; + break; + } /* has delay timed out? */ if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { /* no, update timer and wait some more */ @@ -1004,8 +1069,17 @@ static int base_1joint_state_machine(int joint_num) immediate_state = 1; break; } - /* set up a move at '-search_vel' to back off of switch */ - home_start_move(joint, - H[joint_num].home_search_vel); + /* set up a move at '-search_vel' to back off of switch, at + latch_vel for a synchronized joint so that the overshoot + matches its partners' */ + if ( (H[joint_num].home_sequence < 0) + && (H[joint_num].home_latch_vel != 0.0)) { + tmp = fabs(H[joint_num].home_latch_vel); + if (H[joint_num].home_search_vel > 0.0) { tmp = -tmp; } + home_start_move(joint, tmp); + } else { + home_start_move(joint, - H[joint_num].home_search_vel); + } /* next state */ H[joint_num].home_state = HOME_FINAL_BACKOFF_WAIT; break; @@ -1038,6 +1112,11 @@ static int base_1joint_state_machine(int joint_num) H[joint_num].pause_timer = 0; break; } + /* latch together with the partners */ + if (sync_partner_before(joint_num, HOME_RISE_SEARCH_START)) { + H[joint_num].pause_timer = 0; + break; + } /* has delay timed out? */ if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { /* no, update timer and wait some more */ @@ -1095,6 +1174,11 @@ static int base_1joint_state_machine(int joint_num) H[joint_num].pause_timer = 0; break; } + /* latch together with the partners */ + if (sync_partner_before(joint_num, HOME_RISE_SEARCH_START)) { + H[joint_num].pause_timer = 0; + break; + } /* has delay timed out? */ if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { /* no, update timer and wait some more */ diff --git a/tests/home-sync-backoff/README b/tests/home-sync-backoff/README new file mode 100644 index 00000000000..06535d12142 --- /dev/null +++ b/tests/home-sync-backoff/README @@ -0,0 +1,13 @@ +Joints 0 and 1 are a gantry pair (HOME_SEQUENCE = -1) with simulated +home switches 2 mm out of line (HOME_OFFSET -1 and +1), so a homing +cycle started from HOME begins with joint 1 on its switch. + +home-sync-backoff.hal couples the joints through a bridge: each side's +feedback is the mean of the two commands plus a strain that limit3 caps +in size and rate, so a side driven too far past its partner stops +following its command, like a stepper gantry with encoders. + +test-ui.py homes the pair from far away, from HOME and with both sides +inside their switches, and fails if the peak strain exceeds half the +switch misalignment by more than the latch-move overshoot, if a +following error is raised, or if the pair does not end up homed. diff --git a/tests/home-sync-backoff/checkresult b/tests/home-sync-backoff/checkresult new file mode 100755 index 00000000000..b421a17968b --- /dev/null +++ b/tests/home-sync-backoff/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +# Success or failure of this test is handled in the test.sh script, if we +# get this far it's a success. +exit 0 diff --git a/tests/home-sync-backoff/home-sync-backoff.hal b/tests/home-sync-backoff/home-sync-backoff.hal new file mode 100644 index 00000000000..ff0d2f137c9 --- /dev/null +++ b/tests/home-sync-backoff/home-sync-backoff.hal @@ -0,0 +1,49 @@ +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS +loadrt sum2 names=bridge-mean,bridge-diff,bridge-fb0,bridge-fb1 +loadrt limit3 names=bridge-flex +loadrt comp names=comp_j0,comp_j1 + +addf motion-command-handler servo-thread +addf motion-controller servo-thread +addf bridge-mean servo-thread +addf bridge-diff servo-thread +addf bridge-flex servo-thread +addf bridge-fb0 servo-thread +addf bridge-fb1 servo-thread +addf comp_j0 servo-thread +addf comp_j1 servo-thread + +# Bridge between the two joints: feedback is the mean of the two commands +# plus a strain (half their difference) that limit3 caps in size and +# rate. A side driven past that stalls: its feedback stops following the +# command, like a stepper with an encoder. +net j0cmd joint.0.motor-pos-cmd => bridge-mean.in0 bridge-diff.in0 +net j1cmd joint.1.motor-pos-cmd => bridge-mean.in1 bridge-diff.in1 +setp bridge-mean.gain0 0.5 +setp bridge-mean.gain1 0.5 +setp bridge-diff.gain0 0.5 +setp bridge-diff.gain1 -0.5 +net bridge-pos bridge-mean.out => bridge-fb0.in0 bridge-fb1.in0 +net bridge-strain bridge-diff.out => bridge-flex.in +setp bridge-flex.min -[JOINT_0]BRIDGE_FLEX +setp bridge-flex.max [JOINT_0]BRIDGE_FLEX +setp bridge-flex.maxv [JOINT_0]BRIDGE_SLIP +setp bridge-flex.maxa 100000 +net bridge-deflection bridge-flex.out => bridge-fb0.in1 bridge-fb1.in1 +setp bridge-fb1.gain1 -1 +net j0fb bridge-fb0.out => joint.0.motor-pos-fb => comp_j0.in0 +net j1fb bridge-fb1.out => joint.1.motor-pos-fb => comp_j1.in0 + +# Home switches on the negative end, positions set by test-ui.py. +net j0swpos => comp_j0.in1 +net j1swpos => comp_j1.in1 +sets j0swpos -30 +sets j1swpos -30 +setp comp_j0.hyst 0.2 +setp comp_j1.hyst 0.2 +net j0sw comp_j0.out => joint.0.home-sw-in +net j1sw comp_j1.out => joint.1.home-sw-in + +net estop-out <= iocontrol.0.user-enable-out +net estop-out => iocontrol.0.emc-enable-in diff --git a/tests/home-sync-backoff/test-ui.py b/tests/home-sync-backoff/test-ui.py new file mode 100755 index 00000000000..6a842a36a80 --- /dev/null +++ b/tests/home-sync-backoff/test-ui.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python3 + +import linuxcnc +import hal + +import os +import subprocess +import time +import sys + +# switch misalignment, see test.ini +MISALIGNMENT = 2.0 + +# allowed strain beyond misalignment / 2: latch-move overshoot plus +# polling slack +STRAIN_TOLERANCE = 0.05 + +failures = [] + + +def print_errors(): + while True: + error = e.poll() + if not error: + return + print("linuxcnc error %d: %s" % (error[0], error[1])) + if "following error" in error[1]: + fail(error[1]) + + +def wait_for(cond, timeout=10.0): + start_time = time.time() + while time.time() - start_time < timeout: + s.poll() + if cond(): + return True + time.sleep(0.02) + s.poll() + return cond() + + +def fail(msg): + print("FAIL " + msg) + failures.append(msg) + + +def unhome(): + # a following error switches the machine off + s.poll() + if s.task_state != linuxcnc.STATE_ON: + machine_on() + c.teleop_enable(0) + c.wait_complete() + c.unhome(-1) + c.wait_complete() + if not wait_for(lambda: not (s.homed[0] or s.homed[1]), 5.0): + print_errors() + print("failed to unhome") + sys.exit(1) + + +def motor_pos(): + # motor-pos-cmd is continuous through homing; joint_position jumps + # when the switch position is set + return [hal.get_value("joint.0.motor-pos-cmd"), hal.get_value("joint.1.motor-pos-cmd")] + + +def machine_on(): + c.state(linuxcnc.STATE_ON) + c.wait_complete() + c.mode(linuxcnc.MODE_MANUAL) + c.wait_complete() + + +def set_switches(j0, j1): + # relative to where the motors sit now; they sit together after every + # homing since HOME is square + for j, rel in ((0, j0), (1, j1)): + pos = hal.get_value("joint.%d.motor-pos-cmd" % j) + rel + subprocess.run(["halcmd", "sets", "j%dswpos" % j, str(pos)], check=True) + time.sleep(0.1) + + +def home_and_watch(label, misalignment): + # Home the pair and record the peak bridge strain (half the distance + # the sides are driven apart). Touching both switches racks the machine + # by their misalignment, so the floor is misalignment / 2. + c.teleop_enable(0) + c.wait_complete() + c.home(0) + c.wait_complete() + if not wait_for(lambda: s.joint[0]["homing"] or s.joint[1]["homing"], 5.0): + fail("%s: homing did not start" % label) + return + peak = 0.0 + t0 = time.time() + while time.time() - t0 < 30.0: + s.poll() + peak = max(peak, abs(hal.get_value("bridge-strain"))) + if os.environ.get("TRACE"): + print(" t=%.3f strain=%.3f defl=%.3f fe=%.3f,%.3f pos=%.3f,%.3f state=%d,%d" % ( + time.time() - t0, hal.get_value("bridge-strain"), hal.get_value("bridge-deflection"), + hal.get_value("joint.0.f-error"), hal.get_value("joint.1.f-error"), + hal.get_value("joint.0.motor-pos-cmd"), hal.get_value("joint.1.motor-pos-cmd"), + hal.get_value("joint.0.home-state"), hal.get_value("joint.1.home-state"))) + if not (s.joint[0]["homing"] or s.joint[1]["homing"]): + break + time.sleep(0.002) + time.sleep(0.2) + s.poll() + print_errors() + limit = misalignment / 2 + STRAIN_TOLERANCE + print("%-20s homed=%d,%d pos=%.3f,%.3f peak strain=%.3f (limit %.3f)" % ( + label, s.homed[0], s.homed[1], s.joint_position[0], s.joint_position[1], peak, limit)) + sys.stdout.flush() + if not (s.homed[0] and s.homed[1]): + fail("%s: joints not homed" % label) + if peak > limit: + fail("%s: the sides were driven %.3f apart, limit %.3f" % (label, 2 * peak, 2 * limit)) + + +h = hal.component("test-ui") +h.ready() + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.wait_complete() +machine_on() + +# The switches are 2 mm out of line (test.ini), so the floor for the +# peak strain is 1 mm in every scenario; what changes is where the gantry +# sits when homing starts. + +# Far from both switches: joint 1 trips 2 mm before joint 0. +set_switches(-31, -29) +home_and_watch("far from switches", MISALIGNMENT) + +# At HOME: joint 1 starts 1 mm into its switch, joint 0 1 mm short of +# its own. +unhome() +set_switches(-1, 1) +home_and_watch("joint 1 on switch", MISALIGNMENT) + +# Both sides inside their switches. +unhome() +set_switches(1, 3) +home_and_watch("both on switch", MISALIGNMENT) + +c.state(linuxcnc.STATE_ESTOP) +c.wait_complete() + +if failures: + print("%d failure(s):" % len(failures)) + for msg in failures: + print(" " + msg) + sys.exit(1) + +print("success") +sys.exit(0) diff --git a/tests/home-sync-backoff/test.ini b/tests/home-sync-backoff/test.ini new file mode 100644 index 00000000000..21aa35f607c --- /dev/null +++ b/tests/home-sync-backoff/test.ini @@ -0,0 +1,72 @@ +[EMC] +VERSION = 1.1 +MACHINE = home-sync-backoff +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +SERVO_PERIOD = 1000000 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[HAL] +HALFILE = home-sync-backoff.hal + +[TRAJ] +COORDINATES = XX +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 10.0 +MAX_LINEAR_VELOCITY = 100.0 + +[KINS] +KINEMATICS = trivkins coordinates=XX kinstype=BOTH +JOINTS = 2 + +[AXIS_X] +MIN_LIMIT = -100.0 +MAX_LIMIT = 100.0 +MAX_VELOCITY = 100.0 +MAX_ACCELERATION = 1000.0 + +# The switches are 2 mm out of line: at HOME (square) joint 0 sits 1 mm +# short of its switch and joint 1 1 mm into its own. +[JOINT_0] +# Bridge model, see home-sync-backoff.hal: strain limit and strain rate +# limit (half the distance and half the relative velocity of the sides). +BRIDGE_FLEX = 1.2 +BRIDGE_SLIP = 12.5 +TYPE = LINEAR +HOME = 0.0 +HOME_OFFSET = -1.0 +HOME_SEARCH_VEL = -20.0 +HOME_LATCH_VEL = 5.0 +HOME_FINAL_VEL = 5.0 +HOME_SEQUENCE = -1 +FERROR = 0.1 +MIN_FERROR = 0.1 +MAX_VELOCITY = 100.0 +MAX_ACCELERATION = 1000.0 +MIN_LIMIT = -100.0 +MAX_LIMIT = 100.0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0.0 +HOME_OFFSET = 1.0 +HOME_SEARCH_VEL = -20.0 +HOME_LATCH_VEL = 5.0 +HOME_FINAL_VEL = 5.0 +HOME_SEQUENCE = -1 +FERROR = 0.1 +MIN_FERROR = 0.1 +MAX_VELOCITY = 100.0 +MAX_ACCELERATION = 1000.0 +MIN_LIMIT = -100.0 +MAX_LIMIT = 100.0 diff --git a/tests/home-sync-backoff/test.sh b/tests/home-sync-backoff/test.sh new file mode 100755 index 00000000000..f5cfb134143 --- /dev/null +++ b/tests/home-sync-backoff/test.sh @@ -0,0 +1,2 @@ +#!/bin/bash +linuxcnc -r test.ini