diff --git a/src/emc/ini/inijoint.cc b/src/emc/ini/inijoint.cc index f9c01e4a007..f5e8a8f3cc0 100644 --- a/src/emc/ini/inijoint.cc +++ b/src/emc/ini/inijoint.cc @@ -146,6 +146,7 @@ static int loadJoint(int joint, const IniFile &ini) bool ignore_limits = ini.findBoolV("HOME_IGNORE_LIMITS", jointSection, false); bool volatile_home = ini.findBoolV("VOLATILE_HOME", jointSection, false); bool locking_idxer = ini.findBoolV("LOCKING_INDEXER", jointSection, false); + bool home_dogbone = ini.findBoolV("HOME_DOGBONE", jointSection, false); int abs_encoder = ini.findIntV("HOME_ABSOLUTE_ENCODER", jointSection, 0, 0, 2); // Sequence defaults to an unrealizable and positive sequence so that @@ -162,7 +163,8 @@ static int loadJoint(int joint, const IniFile &ini) sequence, volatile_home, locking_idxer, - abs_encoder)) { + abs_encoder, + home_dogbone)) { print_dbg_config("emcJointSetHomingParams"); return -1; } diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index c806deccf4f..c5f6f51c5a1 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -657,7 +657,8 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) emcmotCommand->latch_vel, emcmotCommand->flags, emcmotCommand->home_sequence, - emcmotCommand->volatile_home + emcmotCommand->volatile_home, + emcmotCommand->home_dogbone ); break; diff --git a/src/emc/motion/homing.c b/src/emc/motion/homing.c index 13c5e6b513b..bcc73a5c914 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_DOGBONE_SLOW_START,// 26 + HOME_DOGBONE_SLOW_WAIT// 27 } home_state_t; // local per-joint data (includes hal pin data) @@ -122,6 +124,7 @@ typedef struct { int home_sequence; // intfc, updateable bool volatile_home; // intfc bool home_is_synchronized; + bool home_dogbone; } home_local_data; static home_local_data H[EMCMOT_MAX_JOINTS]; @@ -521,6 +524,7 @@ static int base_homing_init(int id, H[i].home = 0; H[i].home_flags = 0; H[i].home_sequence = 1000; //startup: unrealizable, positive seq no. + H[i].home_dogbone = 0; H[i].volatile_home = 0; } return 0; @@ -625,7 +629,8 @@ static void base_set_joint_homing_params(int jno, double home_latch_vel, int home_flags, int home_sequence, - bool volatile_home + bool volatile_home, + bool home_dogbone ) { H[jno].home_offset = offset; @@ -636,6 +641,7 @@ static void base_set_joint_homing_params(int jno, H[jno].home_flags = home_flags; H[jno].home_sequence = home_sequence; H[jno].volatile_home = volatile_home; + H[jno].home_dogbone = home_dogbone; update_home_is_synchronized(); } @@ -846,7 +852,52 @@ static int base_1joint_state_machine(int joint_num) } } break; - + case HOME_DOGBONE_SLOW_START: + 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; + + if (!home_sw_active) { + rtapi_print_msg(RTAPI_MSG_ERR, + _("Dogbone: switch lost before slow phase j=%d"), + joint_num); + H[joint_num].home_state = HOME_ABORT; + immediate_state = 1; + break; + } + + /* Continue SAME direction, but slower */ + double vel = H[joint_num].home_latch_vel; + + if (H[joint_num].home_search_vel < 0) + vel = -fabs(vel); + else + vel = fabs(vel); + + home_start_move(joint, vel); + + H[joint_num].home_state = HOME_DOGBONE_SLOW_WAIT; + break; + case HOME_DOGBONE_SLOW_WAIT: + if (!home_sw_active) { + joint->free_tp.enable = 0; + + /* latch position at falling edge */ + H[joint_num].home_state = HOME_SET_SWITCH_POSITION; + immediate_state = 1; + break; + } + + ABORT_CHECK(joint_num); + break; case HOME_INITIAL_BACKOFF_START: /* This state is called if the homing sequence starts at a location where the home switch is already tripped. It @@ -927,8 +978,13 @@ static int base_1joint_state_machine(int joint_num) if (home_sw_active) { /* yes, stop motion */ joint->free_tp.enable = 0; + /* go to next step */ - H[joint_num].home_state = HOME_SET_COARSE_POSITION; + if (H[joint_num].home_dogbone) { + H[joint_num].home_state = HOME_DOGBONE_SLOW_START; + } else { + H[joint_num].home_state = HOME_SET_COARSE_POSITION; + } immediate_state = 1; break; } @@ -1475,7 +1531,8 @@ void set_joint_homing_params(int jno, double home_latch_vel, int home_flags, int home_sequence, - bool volatile_home + bool volatile_home, + bool home_dogbone ) { base_set_joint_homing_params(jno, offset, @@ -1485,7 +1542,8 @@ void set_joint_homing_params(int jno, home_latch_vel, home_flags, home_sequence, - volatile_home); + volatile_home, + home_dogbone); } void update_joint_homing_params(int jno, double offset, diff --git a/src/emc/motion/homing.h b/src/emc/motion/homing.h index 0c4869de6dd..8e1e8764d3d 100644 --- a/src/emc/motion/homing.h +++ b/src/emc/motion/homing.h @@ -25,7 +25,8 @@ void set_joint_homing_params(int jno, double home_latch_vel, int home_flags, int home_sequence, - bool volatile_home + bool volatile_home, + bool home_dogbone ); // updateable interface params (for inihal pin changes typically): diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 31183a18356..ead6a7111a8 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -241,6 +241,8 @@ extern "C" { int home_sequence; /* order in homing sequence */ int volatile_home; /* joint should get unhomed when we get unhome -2 (generated by task upon estop, etc) */ + int home_dogbone; /* Use dogbone homing, fast initial search, slow until switch + release, and no change of direction */ double minFerror; /* min following error */ double maxFerror; /* max following error */ int wdWait; /* cycle to wait before toggling wd */ diff --git a/src/emc/nml_intf/emc.hh b/src/emc/nml_intf/emc.hh index 0637940362f..684136f6ccd 100644 --- a/src/emc/nml_intf/emc.hh +++ b/src/emc/nml_intf/emc.hh @@ -307,7 +307,7 @@ extern int emcJointSetHomingParams(int joint, double home, double offset, double double search_vel, double latch_vel, 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); + int absolute_encoder, int home_dogbone); extern int emcJointUpdateHomingParams(int joint, double home, double offset, int sequence); extern int emcJointSetMaxVelocity(int joint, double vel); extern int emcJointSetMaxAcceleration(int joint, double acc); diff --git a/src/emc/nml_intf/emc_nml.hh b/src/emc/nml_intf/emc_nml.hh index afd051e27bc..1f18515c93a 100644 --- a/src/emc/nml_intf/emc_nml.hh +++ b/src/emc/nml_intf/emc_nml.hh @@ -313,6 +313,7 @@ class EMC_JOINT_SET_HOMING_PARAMS:public EMC_JOINT_CMD_MSG { int volatile_home; int locking_indexer; int absolute_encoder; + int home_dogbone; }; class EMC_JOINT_HALT:public EMC_JOINT_CMD_MSG { diff --git a/src/emc/task/emctaskmain.cc b/src/emc/task/emctaskmain.cc index 1d709157425..9ccc78d6e0b 100644 --- a/src/emc/task/emctaskmain.cc +++ b/src/emc/task/emctaskmain.cc @@ -1728,7 +1728,8 @@ static int emcTaskIssueCommand(NMLmsg * cmd) set_homing_params_msg->home_sequence, set_homing_params_msg->volatile_home, set_homing_params_msg->locking_indexer, - set_homing_params_msg->absolute_encoder); + set_homing_params_msg->absolute_encoder, + set_homing_params_msg->home_dogbone); break; case EMC_JOINT_SET_FERROR_TYPE: diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 7aaee75fe73..fc9afd9ff89 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -277,7 +277,8 @@ int emcJointSetHomingParams(int joint, double home, double offset, double home_f double search_vel, double latch_vel, 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) + int sequence,int volatile_home, int locking_indexer, int absolute_encoder, + int home_dogbone) { #ifdef ISNAN_TRAP if (std::isnan(home) || std::isnan(offset) || std::isnan(home_final_vel) || @@ -301,6 +302,7 @@ int emcJointSetHomingParams(int joint, double home, double offset, double home_f emcmotCommand.flags = 0; emcmotCommand.home_sequence = sequence; emcmotCommand.volatile_home = volatile_home; + emcmotCommand.home_dogbone = home_dogbone; if (use_index) { emcmotCommand.flags |= HOME_USE_INDEX; } @@ -335,9 +337,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, %d, %d, %d, %d, %d, %d) returned %d\n", __FUNCTION__, joint, home, offset, home_final_vel, search_vel, latch_vel, - use_index, ignore_limits, is_shared, sequence, volatile_home, retval); + use_index, ignore_limits, is_shared, sequence, volatile_home, home_dogbone, retval); } return retval; } diff --git a/src/hal/components/homecomp.comp b/src/hal/components/homecomp.comp index 79c4691787f..897965004ff 100644 --- a/src/hal/components/homecomp.comp +++ b/src/hal/components/homecomp.comp @@ -312,7 +312,8 @@ void set_joint_homing_params(int jno, double home_latch_vel, int home_flags, int home_sequence, - bool volatile_home + bool volatile_home, + bool home_dogbone ) { base_set_joint_homing_params(jno, @@ -323,7 +324,8 @@ void set_joint_homing_params(int jno, home_latch_vel, home_flags, home_sequence, - volatile_home); + volatile_home, + home_dogbone); } void update_joint_homing_params(int jno, double offset,