From ee80cc1330dca14da5a1301e3dcac2b3d479d5fb Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 20 Sep 2026 17:38:29 +1000 Subject: [PATCH] homing: bound the homing moves, return from a failed latch move Every homing move is planned to twice the joint's travel, so a joint with a dead switch searches until it hits a limit switch or the end of its travel, a joint whose switch is stuck on backs off the same way, and a switch that fails after its first trip sends the latch move the same way at latch speed. Operators who retry the homing move the joint further each time. Add [JOINT_n]HOME_SEARCH_DIST for the search move and HOME_LATCH_DIST for the back-off, latch and index moves, both defaulting to 0, the old unbounded behaviour. A search that runs out stops where it is and the homing is aborted; the next attempt searches on from there. When one of the other moves runs its full length the joint reports which move, moves back to where that move began at the same speed, and the homing is aborted. The other joints of the sequence stop at once; those synchronized with the joint return to the start of their own current move as well. tests/home-dist covers a dead switch, a switch stuck on, a switch that dies after its first trip, and a synchronized pair with both switches stuck on. --- docs/src/config/ini-config.adoc | 8 ++ docs/src/config/ini-homing.adoc | 24 ++++ src/emc/ini/inijoint.cc | 5 + src/emc/motion/command.c | 2 + src/emc/motion/homing.c | 192 ++++++++++++++++++++++++++----- src/emc/motion/homing.h | 6 +- src/emc/motion/motion.h | 2 + src/emc/nml_intf/emc.cc | 2 + src/emc/nml_intf/emc.hh | 1 + src/emc/nml_intf/emc_nml.hh | 4 + src/emc/task/emctaskmain.cc | 2 + src/emc/task/taskintf.cc | 9 +- src/hal/components/homecomp.comp | 6 + tests/home-dist/checkresult | 4 + tests/home-dist/home-dist.hal | 50 ++++++++ tests/home-dist/test-ui.py | 185 +++++++++++++++++++++++++++++ tests/home-dist/test.ini | 100 ++++++++++++++++ tests/home-dist/test.sh | 2 + 18 files changed, 574 insertions(+), 30 deletions(-) create mode 100755 tests/home-dist/checkresult create mode 100644 tests/home-dist/home-dist.hal create mode 100755 tests/home-dist/test-ui.py create mode 100644 tests/home-dist/test.ini create mode 100755 tests/home-dist/test.sh diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index 9f932eb01cb..72081266015 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -1206,6 +1206,14 @@ These parameters are Homing related, for a better explanation read the < Home switch/index pulse location // [JOINT_n]HOME_SEARCH_VEL Homing speed, search phase // [JOINT_n]HOME_LATCH_VEL Homing speed, latch phase +// [JOINT_n]HOME_SEARCH_DIST Bound on the search move, 0 = unbounded +// [JOINT_n]HOME_LATCH_DIST Bound on the back-off, latch and index moves, 0 = unbounded // [JOINT_n]HOME_FINAL_VEL Speed to move from HOME_OFFSET to HOME location (at the end of homing) // [JOINT_n]HOME_IS_SHARED The home switch input is shared between joints // [JOINT_n]HOME_USE_INDEX Use index pulse when homing @@ -129,6 +131,8 @@ static int loadJoint(int joint, const IniFile &ini) double search_vel = ini.findRealV("HOME_SEARCH_VEL", jointSection, 0.0); double latch_vel = ini.findRealV("HOME_LATCH_VEL", jointSection, 0.0); double final_vel = ini.findRealV("HOME_FINAL_VEL", jointSection, -1.0); + double search_dist = ini.findRealV("HOME_SEARCH_DIST", jointSection, 0.0); + double latch_dist = ini.findRealV("HOME_LATCH_DIST", jointSection, 0.0); bool is_shared = ini.findBoolV("HOME_IS_SHARED", jointSection, false); bool use_index = ini.findBoolV("HOME_USE_INDEX", jointSection, false); bool encoder_reset = ini.findBoolV("HOME_INDEX_NO_ENCODER_RESET", jointSection, false); @@ -144,6 +148,7 @@ static int loadJoint(int joint, const IniFile &ini) if (0 != emcJointSetHomingParams(joint, home, offset, final_vel, search_vel, latch_vel, + search_dist, latch_dist, use_index, encoder_reset, ignore_limits, diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index 17e9c767a37..26a103e9188 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -716,6 +716,8 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) emcmotCommand->home_final_vel, emcmotCommand->search_vel, emcmotCommand->latch_vel, + emcmotCommand->search_dist, + emcmotCommand->latch_dist, emcmotCommand->flags, emcmotCommand->home_sequence, emcmotCommand->volatile_home diff --git a/src/emc/motion/homing.c b/src/emc/motion/homing.c index 662cd2f3591..3b1fa593f2d 100644 --- a/src/emc/motion/homing.c +++ b/src/emc/motion/homing.c @@ -101,7 +101,9 @@ typedef enum { HOME_LOCK,// 22 HOME_LOCK_WAIT,// 23 HOME_FINISHED,// 24 - HOME_ABORT// 25 + HOME_ABORT,// 25 + HOME_RETURN_START,// 26 + HOME_RETURN_WAIT// 27 } home_state_t; // local per-joint data (includes hal pin data) @@ -119,10 +121,16 @@ typedef struct { double home_final_vel; // intfc double home_search_vel; // intfc double home_latch_vel; // intfc + double home_search_dist; // intfc + double home_latch_dist; // intfc int home_flags; // intfc int home_sequence; // intfc, updateable bool volatile_home; // intfc bool home_is_synchronized; + bool moved; // a move was started in this homing + double move_start; // pos_cmd when the current move began + double move_dist; // its HOME_*_DIST bound, 0 = unbounded + double move_vel; // speed of the current move } home_local_data; static home_local_data H[EMCMOT_MAX_JOINTS]; @@ -151,19 +159,24 @@ static all_joints_home_data_t *joint_home_data = 0; repeated in several different states of the homing state machine */ /* 'home_start_move()' starts a move at the specified velocity. The - length of the move is equal to twice the overall range of the joint, - but the intent is that something (like a home switch or index pulse) - will stop it before that point. */ -static void home_start_move(emcmot_joint_t * joint, double vel) + length of the move is 'dist', or twice the overall range of the joint + when 'dist' is zero, but the intent is that something (like a home + switch or index pulse) will stop it before that point. */ +static void home_start_move(int joint_num, double vel, double dist) { - double joint_range; - - /* set up a long move */ - joint_range = joint->max_pos_limit - joint->min_pos_limit; + emcmot_joint_t *joint = &joints[joint_num]; + + H[joint_num].moved = 1; + H[joint_num].move_start = joint->pos_cmd; + H[joint_num].move_dist = dist; + H[joint_num].move_vel = fabs(vel); + if (dist <= 0.0) { + dist = 2.0 * (joint->max_pos_limit - joint->min_pos_limit); + } if (vel > 0.0) { - joint->free_tp.pos_cmd = joint->pos_cmd + 2.0 * joint_range; + joint->free_tp.pos_cmd = joint->pos_cmd + dist; } else { - joint->free_tp.pos_cmd = joint->pos_cmd - 2.0 * joint_range; + joint->free_tp.pos_cmd = joint->pos_cmd - dist; } if (fabs(vel) < joint->vel_limit) { joint->free_tp.max_vel = fabs(vel); @@ -175,6 +188,45 @@ static void home_start_move(emcmot_joint_t * joint, double vel) joint->free_tp.enable = 1; } // home_start_move() +/* 'home_return()' is called when a move bounded by HOME_LATCH_DIST ran + its full length without finding what it was looking for. Every joint + of the sequence stops; the joint and the joints synchronized with it + move back to where their current move began, then the homing is + aborted. A joint that is homed repeatedly against a dead switch + therefore stays where it is instead of creeping further with every + attempt. */ +static void home_return(int jno) +{ + int i; + + for (i = 0; i < all_joints; i++) { + if (!H[i].joint_in_sequence) { continue; } + if (H[i].home_state == HOME_IDLE) { continue; } + joints[i].free_tp.enable = 0; + if ( i == jno + || ( H[jno].home_is_synchronized + && ABS(H[i].home_sequence) == ABS(H[jno].home_sequence) + && H[i].moved)) { + H[i].home_state = HOME_RETURN_START; + } else { + H[i].home_state = HOME_RETURN_WAIT; + } + } +} // home_return() + +/* true once no joint of the sequence has a return move pending */ +static bool home_returns_done(void) +{ + int i; + + for (i = 0; i < all_joints; i++) { + if (!H[i].joint_in_sequence) { continue; } + if (H[i].home_state == HOME_RETURN_START) { return 0; } + if (H[i].home_state == HOME_RETURN_WAIT && joints[i].free_tp.active) { return 0; } + } + return 1; +} // home_returns_done() + /* 'home_do_moving_checks()' is called from states where the machine is supposed to be moving. It checks to see if the machine has hit a limit, or if the move has stopped. (Normally such moves @@ -196,6 +248,37 @@ static bool home_do_moving_checks(int jno) if (! (&joints[jno])->free_tp.active) { /* reached end of move without hitting switch */ (&joints[jno])->free_tp.enable = 0; + if (H[jno].move_dist > 0.0 && H[jno].home_state == HOME_INITIAL_SEARCH_WAIT) { + /* the search stays where it got to: the next attempt + carries on from there */ + rtapi_print_msg(RTAPI_MSG_ERR, + _("j%d home switch not found within HOME_SEARCH_DIST %.4g"), + jno, H[jno].move_dist); + H[jno].home_state = HOME_ABORT; + return 1; // abort reqd + } + if (H[jno].move_dist > 0.0) { + switch (H[jno].home_state) { + case HOME_INITIAL_BACKOFF_WAIT: + case HOME_FINAL_BACKOFF_WAIT: + rtapi_print_msg(RTAPI_MSG_ERR, + _("j%d home switch did not clear within HOME_LATCH_DIST %.4g, returning to start"), + jno, H[jno].move_dist); + break; + case HOME_INDEX_SEARCH_WAIT: + rtapi_print_msg(RTAPI_MSG_ERR, + _("j%d index pulse not found within HOME_LATCH_DIST %.4g, returning to start"), + jno, H[jno].move_dist); + break; + default: + rtapi_print_msg(RTAPI_MSG_ERR, + _("j%d home switch not found within HOME_LATCH_DIST %.4g, returning to start"), + jno, H[jno].move_dist); + break; + } + home_return(jno); + return 1; // state changed + } rtapi_print_msg(RTAPI_MSG_ERR,_("j%d end of move in home state %d"),jno, H[jno].home_state); H[jno].home_state = HOME_ABORT; return 1; // abort reqd @@ -205,7 +288,6 @@ static bool home_do_moving_checks(int jno) #define ABORT_CHECK(joint_num) do { \ if (home_do_moving_checks(joint_num)) { \ - H[joint_num].home_state = HOME_ABORT; \ immediate_state = 1; \ } \ } while(0); @@ -517,6 +599,8 @@ static int base_homing_init(int id, H[i].home_state = HOME_IDLE; H[i].home_search_vel = 0; H[i].home_latch_vel = 0; + H[i].home_search_dist = 0; + H[i].home_latch_dist = 0; H[i].home_final_vel = 0; H[i].home_offset = 0; H[i].home = 0; @@ -632,19 +716,23 @@ static void base_set_joint_homing_params(int jno, double home_final_vel, double home_search_vel, double home_latch_vel, + double home_search_dist, + double home_latch_dist, int home_flags, int home_sequence, bool volatile_home ) { - H[jno].home_offset = offset; - H[jno].home = home; - H[jno].home_final_vel = home_final_vel; - H[jno].home_search_vel = home_search_vel; - H[jno].home_latch_vel = home_latch_vel; - H[jno].home_flags = home_flags; - H[jno].home_sequence = home_sequence; - H[jno].volatile_home = volatile_home; + H[jno].home_offset = offset; + H[jno].home = home; + H[jno].home_final_vel = home_final_vel; + H[jno].home_search_vel = home_search_vel; + H[jno].home_latch_vel = home_latch_vel; + H[jno].home_search_dist = fabs(home_search_dist); + H[jno].home_latch_dist = fabs(home_latch_dist); + H[jno].home_flags = home_flags; + H[jno].home_sequence = home_sequence; + H[jno].volatile_home = volatile_home; update_home_is_synchronized(); } @@ -777,6 +865,7 @@ static int base_1joint_state_machine(int joint_num) /* This state is responsible for getting the homing process started. It doesn't actually do anything, it simply determines what state is next */ + H[joint_num].moved = 0; if (H[joint_num].home_flags & HOME_IS_SHARED && home_sw_active) { rtapi_print_msg(RTAPI_MSG_ERR, _("Cannot home while shared home switch is closed j=%d"), joint_num); @@ -874,7 +963,8 @@ static int base_1joint_state_machine(int joint_num) } H[joint_num].pause_timer = 0; /* set up a move at '-search_vel' to back off of switch */ - home_start_move(joint, - H[joint_num].home_search_vel); + home_start_move(joint_num, - H[joint_num].home_search_vel, + H[joint_num].home_latch_dist); /* next state */ H[joint_num].home_state = HOME_INITIAL_BACKOFF_WAIT; break; @@ -922,7 +1012,8 @@ static int base_1joint_state_machine(int joint_num) break; } /* set up a move at 'search_vel' to find switch */ - home_start_move(joint, H[joint_num].home_search_vel); + home_start_move(joint_num, H[joint_num].home_search_vel, + H[joint_num].home_search_dist); /* next state */ H[joint_num].home_state = HOME_INITIAL_SEARCH_WAIT; break; @@ -1005,7 +1096,8 @@ static int base_1joint_state_machine(int joint_num) break; } /* set up a move at '-search_vel' to back off of switch */ - home_start_move(joint, - H[joint_num].home_search_vel); + home_start_move(joint_num, - H[joint_num].home_search_vel, + H[joint_num].home_latch_dist); /* next state */ H[joint_num].home_state = HOME_FINAL_BACKOFF_WAIT; break; @@ -1054,7 +1146,8 @@ static int base_1joint_state_machine(int joint_num) break; } /* set up a move at 'latch_vel' to locate the switch */ - home_start_move(joint, H[joint_num].home_latch_vel); + home_start_move(joint_num, H[joint_num].home_latch_vel, + H[joint_num].home_latch_dist); /* next state */ H[joint_num].home_state = HOME_RISE_SEARCH_WAIT; break; @@ -1112,7 +1205,8 @@ static int base_1joint_state_machine(int joint_num) break; } /* set up a move at 'latch_vel' to locate the switch */ - home_start_move(joint, H[joint_num].home_latch_vel); + home_start_move(joint_num, H[joint_num].home_latch_vel, + H[joint_num].home_latch_dist); /* next state */ H[joint_num].home_state = HOME_FALL_SEARCH_WAIT; break; @@ -1212,7 +1306,8 @@ static int base_1joint_state_machine(int joint_num) /* set the index enable */ H[joint_num].index_enable = 1; /* set up a move at 'latch_vel' to find the index pulse */ - home_start_move(joint, H[joint_num].home_latch_vel); + home_start_move(joint_num, H[joint_num].home_latch_vel, + H[joint_num].home_latch_dist); /* next state */ H[joint_num].home_state = HOME_INDEX_SEARCH_WAIT; break; @@ -1383,6 +1478,47 @@ static int base_1joint_state_machine(int joint_num) H[joint_num].joint_in_sequence = 0; break; + case HOME_RETURN_START: + /* A bounded move ran its full length. Once the joint has + stopped, move back to where that move began. */ + if (joint->free_tp.active) { + H[joint_num].pause_timer = 0; + break; + } + if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { + H[joint_num].pause_timer++; + break; + } + H[joint_num].pause_timer = 0; + joint->free_tp.pos_cmd = H[joint_num].move_start; + joint->free_tp.max_vel = H[joint_num].move_vel; + joint->free_tp.enable = 1; + H[joint_num].home_state = HOME_RETURN_WAIT; + break; + + case HOME_RETURN_WAIT: + /* Wait for the return move, and for the returns of the other + joints of the sequence, then abort the homing. */ + if (joint->on_pos_limit || joint->on_neg_limit) { + if (!(H[joint_num].home_flags & HOME_IGNORE_LIMITS)) { + rtapi_print_msg(RTAPI_MSG_ERR, _("j%d hit limit in home state %d"), + joint_num, H[joint_num].home_state); + H[joint_num].home_state = HOME_ABORT; + immediate_state = 1; + break; + } + } + if (joint->free_tp.active) { + break; + } + joint->free_tp.enable = 0; + if (!home_returns_done()) { + break; + } + H[joint_num].home_state = HOME_ABORT; + immediate_state = 1; + break; + case HOME_ABORT: for(int i = 0; i < all_joints; i++) { H[i].homing = 0; @@ -1482,6 +1618,8 @@ void set_joint_homing_params(int jno, double home_final_vel, double home_search_vel, double home_latch_vel, + double home_search_dist, + double home_latch_dist, int home_flags, int home_sequence, bool volatile_home @@ -1492,6 +1630,8 @@ void set_joint_homing_params(int jno, home_final_vel, home_search_vel, home_latch_vel, + home_search_dist, + home_latch_dist, home_flags, home_sequence, volatile_home); diff --git a/src/emc/motion/homing.h b/src/emc/motion/homing.h index 2b84428195c..b6a51c46c7f 100644 --- a/src/emc/motion/homing.h +++ b/src/emc/motion/homing.h @@ -24,14 +24,16 @@ // per-joint interface parameters (one-time setup) // Called once per joint with the homing values from the INI file -// [JOINT_N] section (HOME, HOME_OFFSET, velocities, HOME_FLAGS, -// HOME_SEQUENCE, VOLATILE_HOME). +// [JOINT_N] section (HOME, HOME_OFFSET, velocities, distances, +// HOME_FLAGS, HOME_SEQUENCE, VOLATILE_HOME). void set_joint_homing_params(int jno, double offset, double home, double home_final_vel, double home_search_vel, double home_latch_vel, + double home_search_dist, + double home_latch_dist, int home_flags, int home_sequence, bool volatile_home diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index b107b232811..151e758537f 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -238,6 +238,8 @@ extern "C" { double home_final_vel; /* joint velocity for moving from OFFSET to HOME */ double search_vel; /* home search velocity */ double latch_vel; /* home latch velocity */ + double search_dist; /* home search move bound, 0 = unbounded */ + double latch_dist; /* home back-off, latch and index move bound, 0 = unbounded */ int flags; /* homing config flags, other boolean args */ int home_sequence; /* order in homing sequence */ int volatile_home; /* joint should get unhomed when we get unhome -2 diff --git a/src/emc/nml_intf/emc.cc b/src/emc/nml_intf/emc.cc index fd9742562a8..dc35cb4209d 100644 --- a/src/emc/nml_intf/emc.cc +++ b/src/emc/nml_intf/emc.cc @@ -1946,6 +1946,8 @@ void EMC_JOINT_SET_HOMING_PARAMS::update(CMS * cms) cms->update(home_final_vel); cms->update(search_vel); cms->update(latch_vel); + cms->update(search_dist); + cms->update(latch_dist); cms->update(use_index); cms->update(encoder_does_not_reset); cms->update(ignore_limits); diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index dbe30abe555..2d98a5b1dbd 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -308,6 +308,7 @@ extern int emcJointSetFerror(int joint, double ferror); extern int emcJointSetMinFerror(int joint, double ferror); extern int emcJointSetHomingParams(int joint, double home, double offset, double home_vel, double search_vel, double latch_vel, + double search_dist, double latch_dist, int use_index, int encoder_does_not_reset, int ignore_limits, int is_shared, int home_sequence, int volatile_home, int locking_indexer, int absolute_encoder); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index eb42bbdbc8d..358582d08c0 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -286,6 +286,8 @@ class EMC_JOINT_SET_HOMING_PARAMS:public EMC_JOINT_CMD_MSG { home_final_vel(0.0), search_vel(0.0), latch_vel(0.0), + search_dist(0.0), + latch_dist(0.0), use_index(0), encoder_does_not_reset(0), ignore_limits(0), @@ -306,6 +308,8 @@ class EMC_JOINT_SET_HOMING_PARAMS:public EMC_JOINT_CMD_MSG { double home_final_vel; double search_vel; double latch_vel; + double search_dist; + double latch_dist; int use_index; int encoder_does_not_reset; int ignore_limits; diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 32c13a35669..8e638b08651 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1811,6 +1811,8 @@ static int emcTaskIssueCommand(NMLmsg * cmd) set_homing_params_msg->home_final_vel, set_homing_params_msg->search_vel, set_homing_params_msg->latch_vel, + set_homing_params_msg->search_dist, + set_homing_params_msg->latch_dist, set_homing_params_msg->use_index, set_homing_params_msg->encoder_does_not_reset, set_homing_params_msg->ignore_limits, diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 9b6fab24c62..c93d82ede20 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -276,13 +276,15 @@ int emcJointSetMinFerror(int joint, double ferror) int emcJointSetHomingParams(int joint, double home, double offset, double home_final_vel, double search_vel, double latch_vel, + double search_dist, double latch_dist, int use_index, int encoder_does_not_reset, int ignore_limits, int is_shared, int sequence,int volatile_home, int locking_indexer,int absolute_encoder) { #ifdef ISNAN_TRAP if (std::isnan(home) || std::isnan(offset) || std::isnan(home_final_vel) || - std::isnan(search_vel) || std::isnan(latch_vel)) { + std::isnan(search_vel) || std::isnan(latch_vel) || + std::isnan(search_dist) || std::isnan(latch_dist)) { printf("isnan error in emcJointSetHomingParams()\n"); return -1; } @@ -299,6 +301,8 @@ int emcJointSetHomingParams(int joint, double home, double offset, double home_f emcmotCommand.home_final_vel = home_final_vel; emcmotCommand.search_vel = search_vel; emcmotCommand.latch_vel = latch_vel; + emcmotCommand.search_dist = search_dist; + emcmotCommand.latch_dist = latch_dist; emcmotCommand.flags = 0; emcmotCommand.home_sequence = sequence; emcmotCommand.volatile_home = volatile_home; @@ -336,8 +340,9 @@ int emcJointSetHomingParams(int joint, double home, double offset, double home_f int retval = usrmotWriteEmcmotCommand(&emcmotCommand); if (emc_debug & EMC_DEBUG_CONFIG) { - rcs_print("%s(%d, %.4f, %.4f, %.4f, %.4f, %.4f, %d, %d, %d, %d, %d) returned %d\n", + rcs_print("%s(%d, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %d, %d, %d, %d, %d) returned %d\n", __FUNCTION__, joint, home, offset, home_final_vel, search_vel, latch_vel, + search_dist, latch_dist, use_index, ignore_limits, is_shared, sequence, volatile_home, retval); } return retval; diff --git a/src/hal/components/homecomp.comp b/src/hal/components/homecomp.comp index 4c616cc76e7..909646954cc 100644 --- a/src/hal/components/homecomp.comp +++ b/src/hal/components/homecomp.comp @@ -253,11 +253,14 @@ void set_joint_homing_params(int jno, double home_final_vel, double home_search_vel, double home_latch_vel, + double home_search_dist, + double home_latch_dist, int home_flags, int home_sequence, bool volatile_home) { (void)jno; (void)offset; (void)home; (void)home_final_vel; (void)home_search_vel; (void)home_latch_vel; + (void)home_search_dist; (void)home_latch_dist; (void)home_flags; (void)home_sequence; (void)volatile_home; } @@ -382,11 +385,14 @@ void set_joint_homing_params(int jno, double home_final_vel, double home_search_vel, double home_latch_vel, + double home_search_dist, + double home_latch_dist, int home_flags, int home_sequence, bool volatile_home) { base_set_joint_homing_params(jno, offset, home, home_final_vel, home_search_vel, home_latch_vel, + home_search_dist, home_latch_dist, home_flags, home_sequence, volatile_home); } void update_joint_homing_params(int jno, double offset, diff --git a/tests/home-dist/checkresult b/tests/home-dist/checkresult new file mode 100755 index 00000000000..b421a17968b --- /dev/null +++ b/tests/home-dist/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-dist/home-dist.hal b/tests/home-dist/home-dist.hal new file mode 100644 index 00000000000..a90a95affb5 --- /dev/null +++ b/tests/home-dist/home-dist.hal @@ -0,0 +1,50 @@ +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,backoff_j0 +loadrt conv_s32_float names=state_j0 +loadrt and2 names=mask_j0 +loadrt flipflop names=dead_j0 + +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 +addf state_j0 servo-thread +addf backoff_j0 servo-thread +addf dead_j0 servo-thread +addf mask_j0 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, positions set by test-ui.py: +# far below the travel = dead switch, far above = switch stuck on. +net j0swpos => comp_j0.in1 +net j1swpos => comp_j1.in1 +net j2swpos => comp_j2.in1 +sets j0swpos -1000 +sets j1swpos -1000 +sets j2swpos -1000 +setp comp_j0.hyst 0.2 +setp comp_j1.hyst 0.2 +setp comp_j2.hyst 0.2 + +# Joint 0's switch can be made intermittent: with j0dead-reset low the +# flipflop latches when the final back-off starts (home-state 10) and +# masks the switch from then on, so the latch move never finds it. +net j0state joint.0.home-state => state_j0.in +net j0statef state_j0.out => backoff_j0.in1 +setp backoff_j0.in0 9.5 +net j0backoff backoff_j0.out => dead_j0.set +net j0alive dead_j0.out-not => mask_j0.in1 +net j0raw comp_j0.out => mask_j0.in0 +net j0sw mask_j0.out => joint.0.home-sw-in +net j0dead-reset dead_j0.reset +sets j0dead-reset 1 +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-dist/test-ui.py b/tests/home-dist/test-ui.py new file mode 100755 index 00000000000..977f66fb5bd --- /dev/null +++ b/tests/home-dist/test-ui.py @@ -0,0 +1,185 @@ +#!/usr/bin/env python3 + +import linuxcnc +import hal + +import os +import subprocess +import sys +import time + +# how close a joint must come back to where its move began +TOLERANCE = 0.01 + +failures = [] + + +def fail(msg): + print("FAIL " + msg) + failures.append(msg) + + +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 errors(): + out = [] + while True: + error = e.poll() + if not error: + return out + print("linuxcnc error %d: %s" % (error[0], error[1])) + out.append(error[1]) + + +def machine_on(): + c.state(linuxcnc.STATE_ON) + c.wait_complete() + c.mode(linuxcnc.MODE_MANUAL) + c.wait_complete() + + +def sets(name, value): + subprocess.run(["halcmd", "sets", name, str(value)], check=True) + + +def pos(j): + return hal.get_value("joint.%d.motor-pos-cmd" % j) + + +def home(joint, label, expect): + # Home 'joint', wait for the homing to end, check that the expected + # message came and that the machine is still on and unhomed. + start = [pos(j) for j in range(3)] + errors() + c.teleop_enable(0) + c.wait_complete() + c.home(joint) + c.wait_complete() + if not wait_for(lambda: any(j["homing"] for j in s.joint[:3]), 5.0): + fail("%s: homing did not start" % label) + return start + farthest = list(start) + t0 = time.time() + while time.time() - t0 < 30.0: + s.poll() + for j in range(3): + if abs(pos(j) - start[j]) > abs(farthest[j] - start[j]): + farthest[j] = pos(j) + if os.environ.get("TRACE"): + print(" t=%.3f pos=%.3f,%.3f,%.3f state=%d,%d,%d sw=%d,%d,%d" % ( + time.time() - t0, pos(0), pos(1), pos(2), + hal.get_value("joint.0.home-state"), hal.get_value("joint.1.home-state"), + hal.get_value("joint.2.home-state"), + hal.get_value("joint.0.home-sw-in"), hal.get_value("joint.1.home-sw-in"), + hal.get_value("joint.2.home-sw-in"))) + if not any(j["homing"] for j in s.joint[:3]): + break + time.sleep(0.002) + time.sleep(0.3) + s.poll() + msgs = errors() + end = [pos(j) for j in range(3)] + print("%-28s farthest=%s end=%s homed=%d,%d,%d state=%d" % ( + label, ["%.3f" % (f - st) for f, st in zip(farthest, start)], + ["%.3f" % (en - st) for en, st in zip(end, start)], + s.homed[0], s.homed[1], s.homed[2], s.task_state)) + sys.stdout.flush() + if not any(expect in m for m in msgs): + fail("%s: no error containing '%s'" % (label, expect)) + if any(s.homed[:3]): + fail("%s: a joint reports homed" % label) + if s.task_state != linuxcnc.STATE_ON: + fail("%s: machine is no longer on" % label) + return start, farthest, end + + +def check_return(label, j, start, end): + if abs(end[j] - start[j]) > TOLERANCE: + fail("%s: joint %d ended %.3f from where it started" % (label, j, end[j] - start[j])) + + +def check_moved(label, j, start, farthest, dist): + if abs(abs(farthest[j] - start[j]) - dist) > TOLERANCE: + fail("%s: joint %d moved %.3f, expected %.3f" % (label, j, abs(farthest[j] - start[j]), dist)) + + +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() + +# Dead switch: the search runs HOME_SEARCH_DIST and stays there. +sets("j0swpos", -1000) +start, farthest, end = home(0, "dead switch", "HOME_SEARCH_DIST") +check_moved("dead switch", 0, start, farthest, 5.0) +if abs(end[0] - start[0] + 5.0) > TOLERANCE: + fail("dead switch: joint 0 ended %.3f from the start, expected -5" % (end[0] - start[0])) + +# Switch stuck on: the initial back-off runs HOME_LATCH_DIST the other +# way and comes back. +sets("j0swpos", 1000) +start, farthest, end = home(0, "switch stuck on", "did not clear") +check_moved("switch stuck on", 0, start, farthest, 2.0) +check_return("switch stuck on", 0, start, end) + +# Intermittent switch: found by the search, dead from the back-off on, +# so the latch move runs HOME_LATCH_DIST past the point where the switch +# was seen and comes back to where the latch move began. +sets("j0swpos", pos(0) - 3.0) +sets("j0dead-reset", 0) +start, farthest, end = home(0, "intermittent switch", "HOME_LATCH_DIST") +if not -3.6 < end[0] - start[0] < -2.6: + fail("intermittent switch: joint 0 ended %.3f from the start, expected about -3" % (end[0] - start[0])) +if not (farthest[0] - start[0]) < -4.9: + fail("intermittent switch: joint 0 only reached %.3f" % (farthest[0] - start[0])) +sets("j0dead-reset", 1) + +# Synchronized pair, both switches stuck on: joint 1 runs out of +# HOME_LATCH_DIST first, joint 2 stops with it, and both come back. +sets("j1swpos", 1000) +sets("j2swpos", 1000) +start, farthest, end = home(1, "pair, switches stuck on", "did not clear") +check_moved("pair, switches stuck on", 1, start, farthest, 2.0) +check_return("pair, switches stuck on", 1, start, end) +check_return("pair, switches stuck on", 2, start, end) +if abs(farthest[2] - start[2]) >= 3.0 - TOLERANCE: + fail("pair, switches stuck on: joint 2 ran its own bound instead of stopping with joint 1") + +# A working switch still homes. +sets("j0swpos", pos(0) - 3.0) +c.teleop_enable(0) +c.wait_complete() +c.home(0) +c.wait_complete() +if not wait_for(lambda: s.homed[0], 20.0): + errors() + fail("working switch: joint 0 did not home") +else: + print("%-28s homed" % "working switch") + +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-dist/test.ini b/tests/home-dist/test.ini new file mode 100644 index 00000000000..7006d59d44e --- /dev/null +++ b/tests/home-dist/test.ini @@ -0,0 +1,100 @@ +[EMC] +VERSION = 1.1 +MACHINE = home-dist +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-dist.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. +[JOINT_0] +TYPE = LINEAR +HOME = 0.0 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = -20.0 +HOME_LATCH_VEL = -5.0 +HOME_FINAL_VEL = 5.0 +HOME_SEARCH_DIST = 5.0 +HOME_LATCH_DIST = 2.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 = 0.0 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = -20.0 +HOME_LATCH_VEL = 5.0 +HOME_FINAL_VEL = 5.0 +HOME_SEARCH_DIST = 5.0 +HOME_LATCH_DIST = 2.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 = 0.0 +HOME_OFFSET = 0.0 +HOME_SEARCH_VEL = -20.0 +HOME_LATCH_VEL = 5.0 +HOME_FINAL_VEL = 5.0 +HOME_SEARCH_DIST = 5.0 +HOME_LATCH_DIST = 3.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-dist/test.sh b/tests/home-dist/test.sh new file mode 100755 index 00000000000..f5cfb134143 --- /dev/null +++ b/tests/home-dist/test.sh @@ -0,0 +1,2 @@ +#!/bin/bash +linuxcnc -r test.ini