diff --git a/src/emc/motion/homing.c b/src/emc/motion/homing.c index cfe4895ddfc..579d4b3c107 100644 --- a/src/emc/motion/homing.c +++ b/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 */ diff --git a/tests/home-delay-final-move/checkresult b/tests/home-delay-final-move/checkresult new file mode 100755 index 00000000000..2d54500fd3d --- /dev/null +++ b/tests/home-delay-final-move/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# Success or failure is decided by test-ui.py. +exit 0 diff --git a/tests/home-delay-final-move/home-delay-final-move.hal b/tests/home-delay-final-move/home-delay-final-move.hal new file mode 100644 index 00000000000..7bc44fb6d88 --- /dev/null +++ b/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 diff --git a/tests/home-delay-final-move/test-ui.py b/tests/home-delay-final-move/test-ui.py new file mode 100755 index 00000000000..259de5e0307 --- /dev/null +++ b/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) diff --git a/tests/home-delay-final-move/test.ini b/tests/home-delay-final-move/test.ini new file mode 100644 index 00000000000..0dba9aae31c --- /dev/null +++ b/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 diff --git a/tests/home-delay-final-move/test.sh b/tests/home-delay-final-move/test.sh new file mode 100755 index 00000000000..f5cfb134143 --- /dev/null +++ b/tests/home-delay-final-move/test.sh @@ -0,0 +1,2 @@ +#!/bin/bash +linuxcnc -r test.ini