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