homing: run every leg of a synchronized home sequence together

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.

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 */

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.

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

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

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)