homing: wait HOME_DELAY before the final move

src/emc/motion/homing.c

@@ -1262,19 +1262,25 @@ static int base_1joint_state_machine(int joint_num)
                 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 */
-                H[joint_num].pause_timer++;
-            }
-            H[joint_num].pause_timer = 0;
-
             // neg home sequence: sync final move
             if  (    (H[joint_num].home_sequence  < 0)
                   && !sync_ready(joint_num) ) {
                 break; // not all joints at *this* state, wait for them
             }
 
+            /* has delay timed out?  Only a joint that made a search or
+               latch move has to settle; one homed in place (both vels
+               zero, or an absolute encoder) goes on at once, as before. */
+            if (   !(H[joint_num].home_flags & HOME_ABSOLUTE_ENCODER)
+                && (   H[joint_num].home_search_vel != 0.0
+                    || H[joint_num].home_latch_vel != 0.0)
+                && H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) {
+                /* no, update timer and wait some more */
+                H[joint_num].pause_timer++;
+                break;
+            }
+            H[joint_num].pause_timer = 0;
+
             /* plan a final move to home position */
             joint->free_tp.pos_cmd = H[joint_num].home;
             /* if home_vel is set (>0) then we use that, otherwise we rapid there */

tests/home-delay-final-move/checkresult

@@ -0,0 +1,3 @@
+#!/bin/sh
+# Success or failure is decided by test-ui.py.
+exit 0

tests/home-delay-final-move/home-delay-final-move.hal

@@ -0,0 +1,27 @@
+loadrt [KINS]KINEMATICS
+loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
+loadrt comp names=comp_j0,comp_j1,comp_j2
+
+addf motion-command-handler servo-thread
+addf motion-controller servo-thread
+addf comp_j0 servo-thread
+addf comp_j1 servo-thread
+addf comp_j2 servo-thread
+
+net j0cmd joint.0.motor-pos-cmd => joint.0.motor-pos-fb => comp_j0.in0
+net j1cmd joint.1.motor-pos-cmd => joint.1.motor-pos-fb => comp_j1.in0
+net j2cmd joint.2.motor-pos-cmd => joint.2.motor-pos-fb => comp_j2.in0
+
+# Home switches on the negative end: on while the joint is below them.
+setp comp_j0.in1 -5.0
+setp comp_j1.in1 -3.0
+setp comp_j2.in1 -9.0
+setp comp_j0.hyst 0.1
+setp comp_j1.hyst 0.1
+setp comp_j2.hyst 0.1
+net j0sw comp_j0.out => joint.0.home-sw-in
+net j1sw comp_j1.out => joint.1.home-sw-in
+net j2sw comp_j2.out => joint.2.home-sw-in
+
+net estop-out <= iocontrol.0.user-enable-out
+net estop-out => iocontrol.0.emc-enable-in

tests/home-delay-final-move/test-ui.py

@@ -0,0 +1,116 @@
+#!/usr/bin/env python3
+
+# HOME_FINAL_MOVE_START must wait HOME_DELAY (0.1 s in homing.c) after
+# the joint has stopped before it plans the final move, and the final
+# moves of a negative HOME_SEQUENCE pair must still start together.
+
+import linuxcnc
+import hal
+
+import sys
+import time
+
+HOME_FINAL_MOVE_START = 20
+HOME_DELAY = 0.100
+
+failures = []
+
+
+def fail(msg):
+    print("FAIL " + msg)
+    failures.append(msg)
+
+
+def state(j):
+    return hal.get_value("joint.%d.home-state" % j)
+
+
+def watch(joints, timeout=30.0):
+    # Poll home-state; return, per joint, when it entered and left
+    # HOME_FINAL_MOVE_START and when its command last changed before it
+    # left (the joint had stopped by then).
+    enter = {}
+    leave = {}
+    stopped = {}
+    last = {j: hal.get_value("joint.%d.motor-pos-cmd" % j) for j in joints}
+    t0 = time.monotonic()
+    while time.monotonic() - t0 < timeout:
+        now = time.monotonic()
+        for j in joints:
+            st = state(j)
+            p = hal.get_value("joint.%d.motor-pos-cmd" % j)
+            if st == HOME_FINAL_MOVE_START:
+                enter.setdefault(j, now)
+                if p != last[j]:
+                    stopped[j] = now
+            elif j in enter and j not in leave:
+                leave[j] = now
+            last[j] = p
+        s.poll()
+        if all(s.homed[j] for j in joints):
+            break
+        time.sleep(0.0002)
+    return enter, leave, stopped
+
+
+h = hal.component("test-ui")
+h.ready()
+
+c = linuxcnc.command()
+s = linuxcnc.stat()
+
+c.state(linuxcnc.STATE_ESTOP_RESET)
+c.state(linuxcnc.STATE_ON)
+c.wait_complete()
+c.mode(linuxcnc.MODE_MANUAL)
+c.wait_complete()
+c.teleop_enable(0)
+c.wait_complete()
+
+# Joint 0 alone.
+c.home(0)
+c.wait_complete()
+enter, leave, stopped = watch([0])
+s.poll()
+if not s.homed[0]:
+    fail("joint 0 did not home")
+elif 0 not in leave:
+    fail("joint 0: HOME_FINAL_MOVE_START not seen")
+else:
+    wait = leave[0] - stopped.get(0, enter[0])
+    print("joint 0: waited %.3f s after stopping before the final move" % wait)
+    if wait < 0.8 * HOME_DELAY:
+        fail("joint 0: final move started %.3f s after the stop, expected >= %.3f" % (wait, HOME_DELAY))
+
+# Synchronized pair (HOME_SEQUENCE = -1), switches at different places.
+c.home(1)
+c.wait_complete()
+enter, leave, stopped = watch([1, 2])
+s.poll()
+if not (s.homed[1] and s.homed[2]):
+    fail("pair did not home")
+elif 1 not in leave or 2 not in leave:
+    fail("pair: HOME_FINAL_MOVE_START not seen")
+else:
+    for j in (1, 2):
+        print("joint %d: in HOME_FINAL_MOVE_START %.3f s" % (j, leave[j] - enter[j]))
+    skew = abs(leave[1] - leave[2])
+    late = max(enter[1], enter[2])
+    wait = min(leave[1], leave[2]) - late
+    print("pair: final moves %.3f s apart, %.3f s after the later joint arrived" % (skew, wait))
+    if skew > 0.03:
+        fail("pair: final moves started %.3f s apart" % skew)
+    if wait < 0.8 * HOME_DELAY:
+        fail("pair: final move started %.3f s after the later joint arrived, expected >= %.3f" % (wait, HOME_DELAY))
+
+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)

tests/home-delay-final-move/test.ini

@@ -0,0 +1,96 @@
+[EMC]
+VERSION = 1.1
+MACHINE = home-delay-final-move
+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-delay-final-move.hal
+
+[TRAJ]
+COORDINATES = XYZ
+LINEAR_UNITS = mm
+ANGULAR_UNITS = degree
+DEFAULT_LINEAR_VELOCITY = 10.0
+MAX_LINEAR_VELOCITY = 100.0
+
+[KINS]
+KINEMATICS = trivkins
+JOINTS = 3
+
+[AXIS_X]
+MIN_LIMIT = -100.0
+MAX_LIMIT = 100.0
+MAX_VELOCITY = 100.0
+MAX_ACCELERATION = 500.0
+
+[AXIS_Y]
+MIN_LIMIT = -100.0
+MAX_LIMIT = 100.0
+MAX_VELOCITY = 100.0
+MAX_ACCELERATION = 500.0
+
+[AXIS_Z]
+MIN_LIMIT = -100.0
+MAX_LIMIT = 100.0
+MAX_VELOCITY = 100.0
+MAX_ACCELERATION = 500.0
+
+# Joint 0 homes alone; joints 1 and 2 are a synchronized pair whose
+# switches are at different distances, so they reach the final move at
+# different times.
+[JOINT_0]
+TYPE = LINEAR
+HOME = 2.0
+HOME_OFFSET = 0.0
+HOME_SEARCH_VEL = -20.0
+HOME_LATCH_VEL = 5.0
+HOME_FINAL_VEL = 5.0
+HOME_SEQUENCE = 0
+FERROR = 1.0
+MIN_FERROR = 1.0
+MAX_VELOCITY = 100.0
+MAX_ACCELERATION = 500.0
+MIN_LIMIT = -100.0
+MAX_LIMIT = 100.0
+
+[JOINT_1]
+TYPE = LINEAR
+HOME = 2.0
+HOME_OFFSET = 0.0
+HOME_SEARCH_VEL = -20.0
+HOME_LATCH_VEL = 5.0
+HOME_FINAL_VEL = 5.0
+HOME_SEQUENCE = -1
+FERROR = 1.0
+MIN_FERROR = 1.0
+MAX_VELOCITY = 100.0
+MAX_ACCELERATION = 500.0
+MIN_LIMIT = -100.0
+MAX_LIMIT = 100.0
+
+[JOINT_2]
+TYPE = LINEAR
+HOME = 2.0
+HOME_OFFSET = 0.0
+HOME_SEARCH_VEL = -20.0
+HOME_LATCH_VEL = 5.0
+HOME_FINAL_VEL = 5.0
+HOME_SEQUENCE = -1
+FERROR = 1.0
+MIN_FERROR = 1.0
+MAX_VELOCITY = 100.0
+MAX_ACCELERATION = 500.0
+MIN_LIMIT = -100.0
+MAX_LIMIT = 100.0

tests/home-delay-final-move/test.sh

@@ -0,0 +1,2 @@
+#!/bin/bash
+linuxcnc -r test.ini