jepler/linuxcnc
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

homing.h interface api (new file)

Browse source 
0) clarify interface with new homing.h
1) provide for initialization (homing_init())
2) localize per-joint homing-specific hal pins:
      use: export_joint_home_pins() (one-time)
           read_homing_in_pins()    (servo-rate)
           write_homing_out_pins()  (servo-rate)
3) localize internal homing state machine (home_state_t)
   'hiding' state variables from other motion components
4) use simple set_/get_ functions for overall and per-joint
   queries and settings (instead of macros)
5) maintains use of existing logic and core routines:
      do_homing_sequence()
      do_homing()
6) use rtapi_bool.h
7) uses sequence states (home_sequence_state_t) per existing
   design

No interface or functional changes are intended.

Rebases:
baaf887f1 Wed Jul 17, 2019

Related earlier work:
   4aa4791cd1 2008-02-27 (provisional)
   0a7f2d34fc 2009-12-24 (enabled)
This commit is contained in:
Dewey Garrett 2019-02-27 18:25:52 -07:00
parent baaf887f1e
commit 84d5758ed7
9 changed files with 679 additions and 459 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

15
src/emc/motion-logger/motion-logger.c
View file

@ -149,8 +149,6 @@ static int init_comm_buffers(void) {
joint->acc_limit = 1.0; joint->acc_limit = 1.0;
joint->min_ferror = 0.01; joint->min_ferror = 0.01;
joint->max_ferror = 1.0; joint->max_ferror = 1.0;
joint->home_final_vel = -1;
joint->home_sequence = -1;
joint->comp.entry = &(joint->comp.array[0]); joint->comp.entry = &(joint->comp.array[0]);
/* the compensation code has -DBL_MAX at one end of the table /* the compensation code has -DBL_MAX at one end of the table
@ -239,20 +237,17 @@ void update_joint_status(void) {
joint_status->min_pos_limit = joint->min_pos_limit; joint_status->min_pos_limit = joint->min_pos_limit;
joint_status->min_ferror = joint->min_ferror; joint_status->min_ferror = joint->min_ferror;
joint_status->max_ferror = joint->max_ferror; joint_status->max_ferror = joint->max_ferror;
joint_status->home_offset = joint->home_offset;
} }
} }
static void mark_joint_homed(int joint_num) { static void mark_joint_homed(int joint_num) {
emcmot_joint_t *joint; emcmot_joint_status_t *joint_status;
joint = &joints[joint_num]; joint_status = &emcmotStatus->joint_status[joint_num];
joint_status->homing = 0;
SET_JOINT_HOMING_FLAG(joint, 0); joint_status->homed = 1;
SET_JOINT_HOMED_FLAG(joint, 1); return;
SET_JOINT_AT_HOME_FLAG(joint, 1);
joint->home_state = HOME_IDLE;
} }
void maybe_reopen_logfile() { void maybe_reopen_logfile() {

129
src/emc/motion/command.c
View file

@ -64,6 +64,7 @@
#include "mot_priv.h" #include "mot_priv.h"
#include "rtapi_math.h" #include "rtapi_math.h"
#include "motion_types.h" #include "motion_types.h"
#include "homing.h"
#include "tp_debug.h" #include "tp_debug.h"
@ -75,19 +76,18 @@
static int rehomeAll; static int rehomeAll;
/* loops through the active joints and checks if any are not homed */ /* loops through the active joints and checks if any are not homed */
int checkAllHomed(void) bool checkAllHomed(void)
{ {
int joint_num; int joint_num;
emcmot_joint_t *joint; emcmot_joint_t *joint;
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint data */
joint = &joints[joint_num]; joint = &joints[joint_num];
if (!GET_JOINT_ACTIVE_FLAG(joint)) { if (!GET_JOINT_ACTIVE_FLAG(joint)) {
/* if joint is not active, don't even look at its limits */ /* if joint is not active, don't even look at its limits */
continue; continue;
} }
if (!GET_JOINT_HOMED_FLAG(joint)) { if (!get_homed(joint_num) ) {
/* if any of the joints is not homed return false */ /* if any of the joints is not homed return false */
return 0; return 0;
} }
@ -153,7 +153,7 @@ STATIC int joint_jog_ok(int joint_num, double vel)
joint_num); joint_num);
return 0; return 0;
} }
refresh_jog_limits(joint); refresh_jog_limits(joint,joint_num);
if ( vel > 0.0 && (joint->pos_cmd > joint->max_jog_limit) ) { if ( vel > 0.0 && (joint->pos_cmd > joint->max_jog_limit) ) {
reportError(_("Can't jog joint %d further past max soft limit."), reportError(_("Can't jog joint %d further past max soft limit."),
joint_num); joint_num);
@ -172,12 +172,14 @@ STATIC int joint_jog_ok(int joint_num, double vel)
or not. If not homed, the limits are relative to the current or not. If not homed, the limits are relative to the current
position by +/- the full range of travel. Once homed, they position by +/- the full range of travel. Once homed, they
are absolute. are absolute.
homing api requires joint_num
*/ */
void refresh_jog_limits(emcmot_joint_t *joint) void refresh_jog_limits(emcmot_joint_t *joint, int joint_num)
{ {
double range; double range;
if (GET_JOINT_HOMED_FLAG(joint)) { if (get_homed(joint_num) ) {
/* if homed, set jog limits using soft limits */ /* if homed, set jog limits using soft limits */
joint->max_jog_limit = joint->max_pos_limit; joint->max_jog_limit = joint->max_pos_limit;
joint->min_jog_limit = joint->min_pos_limit; joint->min_jog_limit = joint->min_pos_limit;
@ -290,31 +292,27 @@ STATIC int inRange(EmcPose pos, int id, char *move_type)
return in_range; return in_range;
} }
/* clearHomes() will clear the homed flags for joints that have moved /* legacy note:
clearHomes() will clear the homed flags for joints that have moved
since homing, outside coordinated control, for machines with no since homing, outside coordinated control, for machines with no
forward kinematics. This is used in conjunction with the rehomeAll forward kinematics. This is used in conjunction with the rehomeAll
flag, which is set for any coordinated move that in general will flag, which is set for any coordinated move that in general will
result in all joints moving. The flag is consulted whenever a joint result in all joints moving. The flag is consulted whenever a joint
is jogged in joint mode, so that either its flag can be cleared if is jogged in joint mode, so that either its flag can be cleared if
no other joints have moved, or all have to be cleared. */ no other joints have moved, or all have to be cleared.
NOTE: dubious usefulness (inverse-only kins etc.)
*/
void clearHomes(int joint_num) void clearHomes(int joint_num)
{ {
int n; int n;
emcmot_joint_t *joint;
if (emcmotConfig->kinType == KINEMATICS_INVERSE_ONLY) { if (emcmotConfig->kinType == KINEMATICS_INVERSE_ONLY) {
if (rehomeAll) { if (rehomeAll) {
for (n = 0; n < emcmotConfig->numJoints; n++) { for (n = 0; n < emcmotConfig->numJoints; n++) {
/* point at joint data */ set_joint_homed(joint_num,0);
joint = &(joints[n]);
/* clear flag */
SET_JOINT_HOMED_FLAG(joint, 0);
} }
} else { } else {
/* point at joint data */ set_joint_homed(joint_num,0);
joint = &joints[joint_num];
/* clear flag */
SET_JOINT_HOMED_FLAG(joint, 0);
} }
} }
} }
@ -494,18 +492,18 @@ void emcmotCommandHandler(void *arg, long period)
|| emcmotCommand->command == EMCMOT_JOG_ABS || emcmotCommand->command == EMCMOT_JOG_ABS
) )
&& !(GET_MOTION_TELEOP_FLAG()) && !(GET_MOTION_TELEOP_FLAG())
&& (joint->home_sequence < 0) && (get_home_sequence(joint_num) < 0)
&& !emcmotStatus->homing_active && !get_homing_is_active()
) { ) {
if (emcmotConfig->kinType == KINEMATICS_IDENTITY) { if (emcmotConfig->kinType == KINEMATICS_IDENTITY) {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR,
"Homing is REQUIRED to jog requested coordinate\n" "Homing is REQUIRED to jog requested coordinate\n"
"because joint (%d) in home_sequence is negative (%d)\n" "because joint (%d) in home_sequence is negative (%d)\n"
,joint_num,joint->home_sequence); ,joint_num,get_home_sequence(joint_num));
} else { } else {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR,
"Cannot jog joint %d because home_sequence is negative (%d)\n" "Cannot jog joint %d because home_sequence is negative (%d)\n"
,joint_num,joint->home_sequence); ,joint_num,get_home_sequence(joint_num));
} }
return; return;
} }
@ -542,8 +540,8 @@ void emcmotCommandHandler(void *arg, long period)
/* tell joint planner to stop */ /* tell joint planner to stop */
joint->free_tp.enable = 0; joint->free_tp.enable = 0;
/* stop homing if in progress */ /* stop homing if in progress */
if ( joint->home_state != HOME_IDLE ) { if ( ! get_home_is_idle(joint_num)) {
joint->home_state = HOME_ABORT; set_home_abort(joint_num);
} }
} }
} }
@ -576,8 +574,8 @@ void emcmotCommandHandler(void *arg, long period)
/* validate joint */ /* validate joint */
if (joint == 0) { break; } if (joint == 0) { break; }
/* stop homing if in progress */ /* stop homing if in progress */
if ( joint->home_state != HOME_IDLE ) { if ( !get_home_is_idle(joint_num) ) {
joint->home_state = HOME_ABORT; set_home_abort(joint_num);
} }
/* update status flags */ /* update status flags */
SET_JOINT_ERROR_FLAG(joint, 0); SET_JOINT_ERROR_FLAG(joint, 0);
@ -663,14 +661,16 @@ void emcmotCommandHandler(void *arg, long period)
if (joint == 0) { if (joint == 0) {
break; break;
} }
joint->home_offset = emcmotCommand->offset; set_joint_homing_params(joint_num,
joint->home = emcmotCommand->home; emcmotCommand->offset,
joint->home_final_vel = emcmotCommand->home_final_vel; emcmotCommand->home,
joint->home_search_vel = emcmotCommand->search_vel; emcmotCommand->home_final_vel,
joint->home_latch_vel = emcmotCommand->latch_vel; emcmotCommand->search_vel,
joint->home_flags = emcmotCommand->flags; emcmotCommand->latch_vel,
joint->home_sequence = emcmotCommand->home_sequence; emcmotCommand->flags,
joint->volatile_home = emcmotCommand->volatile_home; emcmotCommand->home_sequence,
emcmotCommand->volatile_home
);
break; break;
case EMCMOT_UPDATE_JOINT_HOMING_PARAMS: case EMCMOT_UPDATE_JOINT_HOMING_PARAMS:
@ -680,9 +680,11 @@ void emcmotCommandHandler(void *arg, long period)
if (joint == 0) { if (joint == 0) {
break; break;
} }
joint->home_offset = emcmotCommand->offset; update_joint_homing_params(joint_num,
joint->home = emcmotCommand->home; emcmotCommand->offset,
joint->home_sequence = emcmotCommand->home_sequence; emcmotCommand->home,
emcmotCommand->home_sequence
);
break; break;
case EMCMOT_OVERRIDE_LIMITS: case EMCMOT_OVERRIDE_LIMITS:
@ -790,7 +792,7 @@ void emcmotCommandHandler(void *arg, long period)
SET_JOINT_ERROR_FLAG(joint, 1); SET_JOINT_ERROR_FLAG(joint, 1);
break; break;
} }
if (emcmotStatus->homing_active) { if ( get_homing_is_active() ) {
reportError(_("Can't jog any joints while homing.")); reportError(_("Can't jog any joints while homing."));
SET_JOINT_ERROR_FLAG(joint, 1); SET_JOINT_ERROR_FLAG(joint, 1);
break; break;
@ -804,7 +806,7 @@ void emcmotCommandHandler(void *arg, long period)
/* can't do two kinds of jog at once */ /* can't do two kinds of jog at once */
break; break;
} }
if (joint->home_flags & HOME_UNLOCK_FIRST) { if (get_home_needs_unlock_first(joint_num) ) {
reportError("Can't jog locking joint_num=%d",joint_num); reportError("Can't jog locking joint_num=%d",joint_num);
SET_JOINT_ERROR_FLAG(joint, 1); SET_JOINT_ERROR_FLAG(joint, 1);
break; break;
@ -815,7 +817,7 @@ void emcmotCommandHandler(void *arg, long period)
break; break;
} }
/* set destination of jog */ /* set destination of jog */
refresh_jog_limits(joint); refresh_jog_limits(joint,joint_num);
if (emcmotCommand->vel > 0.0) { if (emcmotCommand->vel > 0.0) {
joint->free_tp.pos_cmd = joint->max_jog_limit; joint->free_tp.pos_cmd = joint->max_jog_limit;
} else { } else {
@ -886,7 +888,7 @@ void emcmotCommandHandler(void *arg, long period)
SET_JOINT_ERROR_FLAG(joint, 1); SET_JOINT_ERROR_FLAG(joint, 1);
break; break;
} }
if (emcmotStatus->homing_active) { if ( get_homing_is_active() ) {
reportError(_("Can't jog any joint while homing.")); reportError(_("Can't jog any joint while homing."));
SET_JOINT_ERROR_FLAG(joint, 1); SET_JOINT_ERROR_FLAG(joint, 1);
break; break;
@ -900,7 +902,7 @@ void emcmotCommandHandler(void *arg, long period)
/* can't do two kinds of jog at once */ /* can't do two kinds of jog at once */
break; break;
} }
if (joint->home_flags & HOME_UNLOCK_FIRST) { if (get_home_needs_unlock_first(joint_num) ) {
reportError("Can't jog locking joint_num=%d",joint_num); reportError("Can't jog locking joint_num=%d",joint_num);
SET_JOINT_ERROR_FLAG(joint, 1); SET_JOINT_ERROR_FLAG(joint, 1);
break; break;
@ -917,7 +919,7 @@ void emcmotCommandHandler(void *arg, long period)
tmp1 = joint->free_tp.pos_cmd - emcmotCommand->offset; tmp1 = joint->free_tp.pos_cmd - emcmotCommand->offset;
} }
/* don't jog past limits */ /* don't jog past limits */
refresh_jog_limits(joint); refresh_jog_limits(joint,joint_num);
if (tmp1 > joint->max_jog_limit) { if (tmp1 > joint->max_jog_limit) {
break; break;
} }
@ -988,7 +990,7 @@ void emcmotCommandHandler(void *arg, long period)
SET_JOINT_ERROR_FLAG(joint, 1); SET_JOINT_ERROR_FLAG(joint, 1);
break; break;
} }
if (emcmotStatus->homing_active) { if ( get_homing_is_active() ) {
reportError(_("Can't jog any joints while homing.")); reportError(_("Can't jog any joints while homing."));
SET_JOINT_ERROR_FLAG(joint, 1); SET_JOINT_ERROR_FLAG(joint, 1);
break; break;
@ -1011,7 +1013,7 @@ void emcmotCommandHandler(void *arg, long period)
/*! \todo FIXME-- use 'goal' instead */ /*! \todo FIXME-- use 'goal' instead */
joint->free_tp.pos_cmd = emcmotCommand->offset; joint->free_tp.pos_cmd = emcmotCommand->offset;
/* don't jog past limits */ /* don't jog past limits */
refresh_jog_limits(joint); refresh_jog_limits(joint,joint_num);
if (joint->free_tp.pos_cmd > joint->max_jog_limit) { if (joint->free_tp.pos_cmd > joint->max_jog_limit) {
joint->free_tp.pos_cmd = joint->max_jog_limit; joint->free_tp.pos_cmd = joint->max_jog_limit;
} }
@ -1457,8 +1459,8 @@ void emcmotCommandHandler(void *arg, long period)
if(joint_num == -1) { // -1 means home all if(joint_num == -1) { // -1 means home all
if(emcmotStatus->homingSequenceState == HOME_SEQUENCE_IDLE) { if(get_home_sequence_state() == HOME_SEQUENCE_IDLE) {
emcmotStatus->homingSequenceState = HOME_SEQUENCE_START; set_home_sequence_state(HOME_SEQUENCE_START);
} else { } else {
reportError(_("homing sequence already in progress")); reportError(_("homing sequence already in progress"));
} }
@ -1470,21 +1472,19 @@ void emcmotCommandHandler(void *arg, long period)
// ******************************************************** // ********************************************************
// support for other homing modes (one sequence, one joint) // support for other homing modes (one sequence, one joint)
if (joint->home_sequence < 0) { if (get_home_sequence(joint_num) < 0) {
int jj; int jj;
emcmot_joint_t *syncjoint; set_home_sequence_state(HOME_SEQUENCE_DO_ONE_SEQUENCE);
emcmotStatus->homingSequenceState = HOME_SEQUENCE_DO_ONE_SEQUENCE;
for (jj = 0; jj < emcmotConfig->numJoints; jj++) { for (jj = 0; jj < emcmotConfig->numJoints; jj++) {
syncjoint = &joints[jj]; if (ABS(get_home_sequence(jj)) == ABS(get_home_sequence(joint_num))) {
if (ABS(syncjoint->home_sequence) == ABS(joint->home_sequence)) {
// set home_state for all joints at same neg sequence // set home_state for all joints at same neg sequence
syncjoint->home_state = HOME_START; set_home_start(jj);
} }
} }
break; break;
} else { } else {
emcmotStatus->homingSequenceState = HOME_SEQUENCE_DO_ONE_JOINT; set_home_sequence_state(HOME_SEQUENCE_DO_ONE_JOINT);
joint->home_state = HOME_START; // one joint only set_home_start(joint_num); //one joint only
} }
break; break;
@ -1506,7 +1506,7 @@ void emcmotCommandHandler(void *arg, long period)
for (n = 0; n < emcmotConfig->numJoints; n++) { for (n = 0; n < emcmotConfig->numJoints; n++) {
joint = &joints[n]; joint = &joints[n];
if(GET_JOINT_ACTIVE_FLAG(joint)) { if(GET_JOINT_ACTIVE_FLAG(joint)) {
if (GET_JOINT_HOMING_FLAG(joint)) { if (get_homing(n)) {
reportError(_("Cannot unhome while homing, joint %d"), n); reportError(_("Cannot unhome while homing, joint %d"), n);
return; return;
} }
@ -1520,16 +1520,25 @@ void emcmotCommandHandler(void *arg, long period)
for (n = 0; n < emcmotConfig->numJoints; n++) { for (n = 0; n < emcmotConfig->numJoints; n++) {
joint = &joints[n]; joint = &joints[n];
if(GET_JOINT_ACTIVE_FLAG(joint)) { if(GET_JOINT_ACTIVE_FLAG(joint)) {
/* legacy notes:
4aa4791cd1 (Chris Radek 2008-02-27 21:07:02 +0000 1310)
Unhome support, partly based on a patch by Bryant. Allow unhoming one joint or
all (-1) via nml message. A special unhome mode (-2) unhomes only the joints
marked as VOLATILE_HOME in the ini. task could use this to unhome some joints,
based on policy, at various state changes. This part is unimplemented so far.
*/
/* if -2, only unhome the volatile_home joints */ /* if -2, only unhome the volatile_home joints */
if(joint_num != -2 || joint->volatile_home) { if( (joint_num != -2) || get_home_is_volatile(n) ) {
SET_JOINT_HOMED_FLAG(joint, 0); set_joint_homed(n, 0);
} }
} }
} }
} else if (joint_num < emcmotConfig->numJoints) { } else if (joint_num < emcmotConfig->numJoints) {
/* request was for only one joint */ /* request was for only one joint */
if(GET_JOINT_ACTIVE_FLAG(joint)) { if(GET_JOINT_ACTIVE_FLAG(joint)) {
if (GET_JOINT_HOMING_FLAG(joint)) { if (get_homing(joint_num) ) {
reportError(_("Cannot unhome while homing, joint %d"), joint_num); reportError(_("Cannot unhome while homing, joint %d"), joint_num);
return; return;
} }
@ -1537,7 +1546,7 @@ void emcmotCommandHandler(void *arg, long period)
reportError(_("Cannot unhome while moving, joint %d"), joint_num); reportError(_("Cannot unhome while moving, joint %d"), joint_num);
return; return;
} }
SET_JOINT_HOMED_FLAG(joint, 0); set_joint_homed(joint_num, 0);
} else { } else {
reportError(_("Cannot unhome inactive joint %d"), joint_num); reportError(_("Cannot unhome inactive joint %d"), joint_num);
} }

67
src/emc/motion/control.c
View file

@ -25,6 +25,7 @@
#include "motion_debug.h" #include "motion_debug.h"
#include "config.h" #include "config.h"
#include "motion_types.h" #include "motion_types.h"
#include "homing.h"
// Mark strings for translation, but defer translation to userspace // Mark strings for translation, but defer translation to userspace
#define _(s) (s) #define _(s) (s)
@ -149,9 +150,8 @@ static void set_operating_mode(void);
static void handle_jjogwheels(void); static void handle_jjogwheels(void);
static void handle_ajogwheels(void); static void handle_ajogwheels(void);
/* 'do_homing_sequence()' looks at emcmotStatus->homingSequenceState /* 'do_homing_sequence()' decides what, if anything, needs to be done
to decide what, if anything, needs to be done related to multi-joint related to multi-joint homing.
homing.
no prototype here, implemented in homing.c, proto in mot_priv.h no prototype here, implemented in homing.c, proto in mot_priv.h
*/ */
@ -274,6 +274,7 @@ void emcmotController(void *arg, long period)
dbg_ct++; dbg_ct++;
#endif #endif
process_inputs(); process_inputs();
read_homing_in_pins(emcmotConfig->numJoints);
do_forward_kins(); do_forward_kins();
process_probe_inputs(); process_probe_inputs();
check_for_faults(); check_for_faults();
@ -286,6 +287,7 @@ void emcmotController(void *arg, long period)
compute_screw_comp(); compute_screw_comp();
plan_external_offsets(); plan_external_offsets();
output_to_hal(); output_to_hal();
write_homing_out_pins(emcmotConfig->numJoints);
update_status(); update_status();
/* here ends the core of the controller */ /* here ends the core of the controller */
emcmotStatus->heartbeat++; emcmotStatus->heartbeat++;
@ -403,11 +405,10 @@ static void process_inputs(void)
continue; continue;
} }
/* copy data from HAL to joint structure */ /* copy data from HAL to joint structure */
joint->index_enable = *(joint_data->index_enable);
joint->motor_pos_fb = *(joint_data->motor_pos_fb); joint->motor_pos_fb = *(joint_data->motor_pos_fb);
/* calculate pos_fb */ /* calculate pos_fb */
if (( joint->home_state == HOME_INDEX_SEARCH_WAIT ) && if (( get_homing_at_index_search_wait(joint_num) ) &&
( joint->index_enable == 0 )) { ( get_index_enable(joint_num) == 0 )) {
/* special case - we're homing the joint, and it just /* special case - we're homing the joint, and it just
hit the index. The encoder count might have made a hit the index. The encoder count might have made a
step change. The homing code will correct for it step change. The homing code will correct for it
@ -463,14 +464,6 @@ static void process_inputs(void)
} else { } else {
SET_JOINT_FAULT_FLAG(joint, 0); SET_JOINT_FAULT_FLAG(joint, 0);
} }
/* read home switch input */
if (*(joint_data->home_sw)) {
SET_JOINT_HOME_SWITCH_FLAG(joint, 1);
} else {
SET_JOINT_HOME_SWITCH_FLAG(joint, 0);
}
/* end of read and process joint inputs loop */
} }
// a fault was signalled during a spindle-orient in progress // a fault was signalled during a spindle-orient in progress
@ -676,8 +669,8 @@ static void process_probe_inputs(void)
} }
// abort any homing // abort any homing
if(GET_JOINT_HOMING_FLAG(joint)) { if(get_homing(i)) {
joint->home_state = HOME_ABORT; set_home_abort(i);
aborted=1; aborted=1;
} }
@ -748,7 +741,7 @@ static void check_for_faults(void)
if ((GET_JOINT_PHL_FLAG(joint) && ! pos_limit_override ) || if ((GET_JOINT_PHL_FLAG(joint) && ! pos_limit_override ) ||
(GET_JOINT_NHL_FLAG(joint) && ! neg_limit_override )) { (GET_JOINT_NHL_FLAG(joint) && ! neg_limit_override )) {
/* joint is on limit switch, should we trip? */ /* joint is on limit switch, should we trip? */
if (GET_JOINT_HOMING_FLAG(joint)) { if (get_homing(joint_num)) {
/* no, ignore limits */ /* no, ignore limits */
} else { } else {
/* trip on limits */ /* trip on limits */
@ -811,8 +804,8 @@ static void set_operating_mode(void)
if (GET_JOINT_ACTIVE_FLAG(joint)) { if (GET_JOINT_ACTIVE_FLAG(joint)) {
SET_JOINT_INPOS_FLAG(joint, 1); SET_JOINT_INPOS_FLAG(joint, 1);
SET_JOINT_ENABLE_FLAG(joint, 0); SET_JOINT_ENABLE_FLAG(joint, 0);
SET_JOINT_HOMING_FLAG(joint, 0); set_joint_homing(joint_num,0);
joint->home_state = HOME_IDLE; set_home_idle(joint_num);
} }
/* don't clear the joint error flag, since that may signify why /* don't clear the joint error flag, since that may signify why
we just went into disabled state */ we just went into disabled state */
@ -848,8 +841,8 @@ static void set_operating_mode(void)
joint->free_tp.curr_pos = joint->pos_cmd; joint->free_tp.curr_pos = joint->pos_cmd;
if (GET_JOINT_ACTIVE_FLAG(joint)) { if (GET_JOINT_ACTIVE_FLAG(joint)) {
SET_JOINT_ENABLE_FLAG(joint, 1); SET_JOINT_ENABLE_FLAG(joint, 1);
SET_JOINT_HOMING_FLAG(joint, 0); set_joint_homing(joint_num,0);
joint->home_state = HOME_IDLE; set_home_idle(joint_num);
} }
/* clear any outstanding joint errors when going into enabled /* clear any outstanding joint errors when going into enabled
state */ state */
@ -1020,7 +1013,7 @@ static void handle_jjogwheels(void)
continue; continue;
} }
/* must not be homing */ /* must not be homing */
if (emcmotStatus->homing_active) { if (get_homing_is_active() ) {
continue; continue;
} }
/* must not be doing a keyboard jog */ /* must not be doing a keyboard jog */
@ -1031,20 +1024,20 @@ static void handle_jjogwheels(void)
/* don't jog if feedhold is on or if feed override is zero */ /* don't jog if feedhold is on or if feed override is zero */
break; break;
} }
if (joint->home_flags & HOME_UNLOCK_FIRST) { if (get_home_needs_unlock_first(joint_num) ) {
reportError("Can't wheel jog locking joint_num=%d",joint_num); reportError("Can't wheel jog locking joint_num=%d",joint_num);
continue; continue;
} }
if (joint->home_sequence < 0) { if (get_home_sequence(joint_num) < 0) {
if (emcmotConfig->kinType == KINEMATICS_IDENTITY) { if (emcmotConfig->kinType == KINEMATICS_IDENTITY) {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR,
"Homing is REQUIRED to wheel jog requested coordinate\n" "Homing is REQUIRED to wheel jog requested coordinate\n"
"because joint (%d) in home_sequence is negative (%d)\n" "because joint (%d) in home_sequence is negative (%d)\n"
,joint_num,joint->home_sequence); ,joint_num,get_home_sequence(joint_num) );
} else { } else {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR,
"Cannot wheel jog joint %d because home_sequence is negative (%d)\n" "Cannot wheel jog joint %d because home_sequence is negative (%d)\n"
,joint_num,joint->home_sequence); ,joint_num,get_home_sequence(joint_num) );
} }
continue; continue;
} }
@ -1060,7 +1053,7 @@ static void handle_jjogwheels(void)
/* calc target position for jog */ /* calc target position for jog */
pos = joint->free_tp.pos_cmd + distance; pos = joint->free_tp.pos_cmd + distance;
/* don't jog past limits */ /* don't jog past limits */
refresh_jog_limits(joint); refresh_jog_limits(joint,joint_num);
if (pos > joint->max_jog_limit) { if (pos > joint->max_jog_limit) {
continue; continue;
} }
@ -1146,7 +1139,7 @@ static void handle_ajogwheels(void)
if (!GET_MOTION_TELEOP_FLAG()) { continue; } if (!GET_MOTION_TELEOP_FLAG()) { continue; }
if (!GET_MOTION_ENABLE_FLAG()) { continue; } if (!GET_MOTION_ENABLE_FLAG()) { continue; }
if ( *(axis_data->ajog_enable) == 0 ) { continue; } if ( *(axis_data->ajog_enable) == 0 ) { continue; }
if (emcmotStatus->homing_active) { continue; } if (get_homing_is_active() ) { continue; }
if (axis->kb_ajog_active) { continue; } if (axis->kb_ajog_active) { continue; }
if (axis->locking_joint >= 0) { if (axis->locking_joint >= 0) {
@ -1225,7 +1218,7 @@ static void get_pos_cmds(long period)
if(joint->acc_limit > emcmotStatus->acc) if(joint->acc_limit > emcmotStatus->acc)
joint->acc_limit = emcmotStatus->acc; joint->acc_limit = emcmotStatus->acc;
/* compute joint velocity limit */ /* compute joint velocity limit */
if ( joint->home_state == HOME_IDLE ) { if ( get_home_is_idle(joint_num) ) {
/* velocity limit = joint limit * global scale factor */ /* velocity limit = joint limit * global scale factor */
/* the global factor is used for feedrate override */ /* the global factor is used for feedrate override */
vel_lim = joint->vel_limit * emcmotStatus->net_feed_scale; vel_lim = joint->vel_limit * emcmotStatus->net_feed_scale;
@ -1269,8 +1262,8 @@ static void get_pos_cmds(long period)
/* active TP means we're moving, so not in position */ /* active TP means we're moving, so not in position */
SET_JOINT_INPOS_FLAG(joint, 0); SET_JOINT_INPOS_FLAG(joint, 0);
SET_MOTION_INPOS_FLAG(0); SET_MOTION_INPOS_FLAG(0);
/* if we move at all, clear AT_HOME flag */ /* if we move at all, clear at_home flag */
SET_JOINT_AT_HOME_FLAG(joint, 0); set_joint_at_home(joint_num,0);
/* is any limit disabled for this move? */ /* is any limit disabled for this move? */
if ( emcmotStatus->overrideLimitMask ) { if ( emcmotStatus->overrideLimitMask ) {
emcmotDebug->overriding = 1; emcmotDebug->overriding = 1;
@ -1516,7 +1509,7 @@ static void get_pos_cmds(long period)
/* point to joint data */ /* point to joint data */
joint = &joints[joint_num]; joint = &joints[joint_num];
/* skip inactive or unhomed axes */ /* skip inactive or unhomed axes */
if ((!GET_JOINT_ACTIVE_FLAG(joint)) || (!GET_JOINT_HOMED_FLAG(joint))) { if ((!GET_JOINT_ACTIVE_FLAG(joint)) || (!get_homed(joint_num))) {
continue; continue;
} }
@ -2009,8 +2002,7 @@ static void output_to_hal(void)
*(joint_data->joint_pos_cmd) = joint->pos_cmd; *(joint_data->joint_pos_cmd) = joint->pos_cmd;
*(joint_data->joint_pos_fb) = joint->pos_fb; *(joint_data->joint_pos_fb) = joint->pos_fb;
*(joint_data->amp_enable) = GET_JOINT_ENABLE_FLAG(joint); *(joint_data->amp_enable) = GET_JOINT_ENABLE_FLAG(joint);
*(joint_data->index_enable) = joint->index_enable;
*(joint_data->homing) = GET_JOINT_HOMING_FLAG(joint);
*(joint_data->coarse_pos_cmd) = joint->coarse_pos; *(joint_data->coarse_pos_cmd) = joint->coarse_pos;
*(joint_data->joint_vel_cmd) = joint->vel_cmd; *(joint_data->joint_vel_cmd) = joint->vel_cmd;
*(joint_data->joint_acc_cmd) = joint->acc_cmd; *(joint_data->joint_acc_cmd) = joint->acc_cmd;
@ -2031,17 +2023,15 @@ static void output_to_hal(void)
*(joint_data->error) = GET_JOINT_ERROR_FLAG(joint); *(joint_data->error) = GET_JOINT_ERROR_FLAG(joint);
*(joint_data->phl) = GET_JOINT_PHL_FLAG(joint); *(joint_data->phl) = GET_JOINT_PHL_FLAG(joint);
*(joint_data->nhl) = GET_JOINT_NHL_FLAG(joint); *(joint_data->nhl) = GET_JOINT_NHL_FLAG(joint);
*(joint_data->homed) = GET_JOINT_HOMED_FLAG(joint);
*(joint_data->f_errored) = GET_JOINT_FERROR_FLAG(joint); *(joint_data->f_errored) = GET_JOINT_FERROR_FLAG(joint);
*(joint_data->faulted) = GET_JOINT_FAULT_FLAG(joint); *(joint_data->faulted) = GET_JOINT_FAULT_FLAG(joint);
*(joint_data->home_state) = joint->home_state;
// conditionally remove outstanding requests to unlock rotaries: // conditionally remove outstanding requests to unlock rotaries:
if ( !GET_MOTION_ENABLE_FLAG() && (joint_is_lockable(joint_num))) { if ( !GET_MOTION_ENABLE_FLAG() && (joint_is_lockable(joint_num))) {
*(joint_data->unlock) = 0; *(joint_data->unlock) = 0;
} }
} } //for joint_num
/* output axis info to HAL for scoping, etc */ /* output axis info to HAL for scoping, etc */
for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) {
@ -2091,6 +2081,8 @@ static void update_status(void)
} }
#endif #endif
joint_status->flag = joint->flag; joint_status->flag = joint->flag;
joint_status->homing = get_homing(joint_num);
joint_status->homed = get_homed(joint_num);
joint_status->pos_cmd = joint->pos_cmd; joint_status->pos_cmd = joint->pos_cmd;
joint_status->pos_fb = joint->pos_fb; joint_status->pos_fb = joint->pos_fb;
joint_status->vel_cmd = joint->vel_cmd; joint_status->vel_cmd = joint->vel_cmd;
@ -2102,7 +2094,6 @@ static void update_status(void)
joint_status->min_pos_limit = joint->min_pos_limit; joint_status->min_pos_limit = joint->min_pos_limit;
joint_status->min_ferror = joint->min_ferror; joint_status->min_ferror = joint->min_ferror;
joint_status->max_ferror = joint->max_ferror; joint_status->max_ferror = joint->max_ferror;
joint_status->home_offset = joint->home_offset;
} }
for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) {

692
src/emc/motion/homing.c
View file

File diff suppressed because it is too large Load diff

82
src/emc/motion/homing.h Normal file
View file

@ -0,0 +1,82 @@
#ifndef HOMING_H
#define HOMING_H
#include <rtapi_bool.h>
// SEQUENCE states
typedef enum {
HOME_SEQUENCE_IDLE = 0, // valid start state
HOME_SEQUENCE_START, // valid start state
HOME_SEQUENCE_DO_ONE_JOINT, // valid start state
HOME_SEQUENCE_DO_ONE_SEQUENCE, // valid start state
HOME_SEQUENCE_START_JOINTS, // homing.c internal usage
HOME_SEQUENCE_WAIT_JOINTS, // homing.c internal usage
} home_sequence_state_t;
//---------------------------------------------------------------------
// INTERFACE routines
// per-joint interface parameters (one-time setup)
extern void set_joint_homing_params(int jno,
double offset,
double home,
double home_final_vel,
double home_search_vel,
double home_latch_vel,
int home_flags,
int home_sequence,
bool volatile_home
);
// updateable interface params (for inihal pin changes typically):
void update_joint_homing_params (int jno,
double home_offset,
double home_home,
int home_sequence
);
//---------------------------------------------------------------------
// CONTROL routines
// one-time initialization:
extern void homing_init(void);
extern int export_joint_home_pins(int njoints,int id);
// once-per-servo-period functions:
extern void read_homing_in_pins(int njoints);
extern void do_homing_sequence(void);
extern void do_homing(void);
extern void write_homing_out_pins(int njoints);
// overall sequence control:
extern void set_home_sequence_state(home_sequence_state_t);
// per-joint control of internal state machine:
extern void set_home_start(int jno);
extern void set_home_abort(int jno);
extern void set_home_idle( int jno);
// per-joint set status items:
extern void set_joint_homing( int jno, bool value);
extern void set_joint_homed( int jno, bool value);
extern void set_joint_at_home(int jno, bool value);
//---------------------------------------------------------------------
// QUERIES
// overall status:
extern home_sequence_state_t get_home_sequence_state(void);
extern bool get_homing_is_active(void);
// per-joint information:
extern int get_home_sequence(int jno);
extern bool get_homing(int jno);
extern bool get_homed(int jno);
extern bool get_index_enable(int jno);
extern bool get_home_is_volatile(int jno);
extern bool get_home_needs_unlock_first(int jno);
extern bool get_home_is_idle(int jno);
extern bool get_homing_at_index_search_wait(int jno);
//---------------------------------------------------------------------
#endif /* HOMING_H */

32
src/emc/motion/mot_priv.h
View file

@ -98,18 +98,12 @@ typedef struct {
hal_bit_t *error; /* RPI: joint has an error */ hal_bit_t *error; /* RPI: joint has an error */
hal_bit_t *phl; /* RPI: joint is at positive hard limit */ hal_bit_t *phl; /* RPI: joint is at positive hard limit */
hal_bit_t *nhl; /* RPI: joint is at negative hard limit */ hal_bit_t *nhl; /* RPI: joint is at negative hard limit */
hal_bit_t *homing; /* RPI: joint is homing */
hal_bit_t *homed; /* RPI: joint was homed */
hal_bit_t *f_errored; /* RPI: joint had too much following error */ hal_bit_t *f_errored; /* RPI: joint had too much following error */
hal_bit_t *faulted; /* RPI: joint amp faulted */ hal_bit_t *faulted; /* RPI: joint amp faulted */
hal_bit_t *pos_lim_sw; /* RPI: positive limit switch input */ hal_bit_t *pos_lim_sw; /* RPI: positive limit switch input */
hal_bit_t *neg_lim_sw; /* RPI: negative limit switch input */ hal_bit_t *neg_lim_sw; /* RPI: negative limit switch input */
hal_bit_t *home_sw; /* RPI: home switch input */
hal_bit_t *index_enable; /* RPIO: motmod sets: request reset on index
encoder clears: index arrived */
hal_bit_t *amp_fault; /* RPI: amp fault input */ hal_bit_t *amp_fault; /* RPI: amp fault input */
hal_bit_t *amp_enable; /* WPI: amp enable output */ hal_bit_t *amp_enable; /* WPI: amp enable output */
hal_s32_t *home_state; /* WPI: homing state machine state */
hal_bit_t *unlock; /* WPI: command that axis should unlock for rotation */ hal_bit_t *unlock; /* WPI: command that axis should unlock for rotation */
hal_bit_t *is_unlocked; /* RPI: axis is currently unlocked */ hal_bit_t *is_unlocked; /* RPI: axis is currently unlocked */
@ -119,7 +113,6 @@ typedef struct {
hal_float_t *jjog_scale; /* RPI: distance to jog on each count */ hal_float_t *jjog_scale; /* RPI: distance to jog on each count */
hal_float_t *jjog_accel_fraction; /* RPI: to limit wheel jog accel */ hal_float_t *jjog_accel_fraction; /* RPI: to limit wheel jog accel */
hal_bit_t *jjog_vel_mode; /* RPI: true for "velocity mode" jogwheel */ hal_bit_t *jjog_vel_mode; /* RPI: true for "velocity mode" jogwheel */
} joint_hal_t; } joint_hal_t;
typedef struct { typedef struct {
@ -257,11 +250,6 @@ extern void emcmotAioWrite(int index, double value);
extern void emcmotSetRotaryUnlock(int axis, int unlock); extern void emcmotSetRotaryUnlock(int axis, int unlock);
extern int emcmotGetRotaryIsUnlocked(int axis); extern int emcmotGetRotaryIsUnlocked(int axis);
/* homing is no longer in control.c, make functions public */
extern void do_homing_sequence(void);
extern void do_homing(void);
// //
// Try to change the Motion mode to Teleop. // Try to change the Motion mode to Teleop.
// //
@ -272,11 +260,10 @@ extern void do_homing(void);
// //
void switch_to_teleop_mode(void); void switch_to_teleop_mode(void);
/* loops through the active joints and checks if any are not homed */ /* loops through the active joints and checks if any are not homed */
extern int checkAllHomed(void); extern bool checkAllHomed(void);
/* recalculates jog limits */ /* recalculates jog limits */
extern void refresh_jog_limits(emcmot_joint_t *joint); extern void refresh_jog_limits(emcmot_joint_t *joint,int joint_num);
/* handles 'homed' flags, see command.c for details */ /* handles 'homed' flags, see command.c for details */
extern void clearHomes(int joint_num); extern void clearHomes(int joint_num);
@ -341,21 +328,6 @@ int joint_is_lockable(int joint_num);
#define SET_JOINT_NHL_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_MIN_HARD_LIMIT_BIT; else (joint)->flag &= ~EMCMOT_JOINT_MIN_HARD_LIMIT_BIT; #define SET_JOINT_NHL_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_MIN_HARD_LIMIT_BIT; else (joint)->flag &= ~EMCMOT_JOINT_MIN_HARD_LIMIT_BIT;
#define GET_JOINT_HOME_SWITCH_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_HOME_SWITCH_BIT ? 1 : 0)
#define SET_JOINT_HOME_SWITCH_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_HOME_SWITCH_BIT; else (joint)->flag &= ~EMCMOT_JOINT_HOME_SWITCH_BIT;
#define GET_JOINT_HOMING_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_HOMING_BIT ? 1 : 0)
#define SET_JOINT_HOMING_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_HOMING_BIT; else (joint)->flag &= ~EMCMOT_JOINT_HOMING_BIT;
#define GET_JOINT_HOMED_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_HOMED_BIT ? 1 : 0)
#define SET_JOINT_HOMED_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_HOMED_BIT; else (joint)->flag &= ~EMCMOT_JOINT_HOMED_BIT;
#define GET_JOINT_AT_HOME_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_AT_HOME_BIT ? 1 : 0)
#define SET_JOINT_AT_HOME_FLAG(joint,fl) if (fl) (joint)->flag |= EMCMOT_JOINT_AT_HOME_BIT; else (joint)->flag &= ~EMCMOT_JOINT_AT_HOME_BIT;
#define GET_JOINT_FERROR_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_FERROR_BIT ? 1 : 0) #define GET_JOINT_FERROR_FLAG(joint) ((joint)->flag & EMCMOT_JOINT_FERROR_BIT ? 1 : 0)

26
src/emc/motion/motion.c
View file

@ -19,6 +19,7 @@
#include "motion_struct.h" #include "motion_struct.h"
#include "mot_priv.h" #include "mot_priv.h"
#include "rtapi_math.h" #include "rtapi_math.h"
#include "homing.h"
// Mark strings for translation, but defer translation to userspace // Mark strings for translation, but defer translation to userspace
#define _(s) (s) #define _(s) (s)
@ -460,13 +461,18 @@ static int init_hal_io(void)
rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: joint %d pin/param export failed\n"), n); rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: joint %d pin/param export failed\n"), n);
return -1; return -1;
} }
/* init axis pins and parameters */
*(joint_data->amp_enable) = 0; *(joint_data->amp_enable) = 0;
*(joint_data->home_state) = 0;
/* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now, /* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now,
because it is always supported. */ because it is always supported. */
} }
/* export joint home pins (assigned to motion comp)*/
retval = export_joint_home_pins(num_joints,mot_comp_id);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: export_joint_home_pins failed\n"));
return -1;
}
/* export axis pins and parameters */ /* export axis pins and parameters */
for (n = 0; n < EMCMOT_MAX_AXIS; n++) { for (n = 0; n < EMCMOT_MAX_AXIS; n++) {
char c = "xyzabcuvw"[n]; char c = "xyzabcuvw"[n];
@ -578,8 +584,6 @@ static int export_joint(int num, joint_hal_t * addr)
if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->motor_offset), mot_comp_id, "joint.%d.motor-offset", num)) != 0) return retval; if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->motor_offset), mot_comp_id, "joint.%d.motor-offset", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->pos_lim_sw), mot_comp_id, "joint.%d.pos-lim-sw-in", num)) != 0) return retval; if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->pos_lim_sw), mot_comp_id, "joint.%d.pos-lim-sw-in", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->neg_lim_sw), mot_comp_id, "joint.%d.neg-lim-sw-in", num)) != 0) return retval; if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->neg_lim_sw), mot_comp_id, "joint.%d.neg-lim-sw-in", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->home_sw), mot_comp_id, "joint.%d.home-sw-in", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_IO, &(addr->index_enable), mot_comp_id, "joint.%d.index-enable", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->amp_enable), mot_comp_id, "joint.%d.amp-enable-out", num)) != 0) return retval; if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->amp_enable), mot_comp_id, "joint.%d.amp-enable-out", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->amp_fault), mot_comp_id, "joint.%d.amp-fault-in", num)) != 0) return retval; if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->amp_fault), mot_comp_id, "joint.%d.amp-fault-in", num)) != 0) return retval;
if ((retval = hal_pin_s32_newf(HAL_IN, &(addr->jjog_counts), mot_comp_id, "joint.%d.jog-counts", num)) != 0) return retval; if ((retval = hal_pin_s32_newf(HAL_IN, &(addr->jjog_counts), mot_comp_id, "joint.%d.jog-counts", num)) != 0) return retval;
@ -605,10 +609,6 @@ static int export_joint(int num, joint_hal_t * addr)
if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->error), mot_comp_id, "joint.%d.error", num)) != 0) return retval; if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->error), mot_comp_id, "joint.%d.error", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->f_errored), mot_comp_id, "joint.%d.f-errored", num)) != 0) return retval; if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->f_errored), mot_comp_id, "joint.%d.f-errored", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->faulted), mot_comp_id, "joint.%d.faulted", num)) != 0) return retval; if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->faulted), mot_comp_id, "joint.%d.faulted", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->homed), mot_comp_id, "joint.%d.homed", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->homing), mot_comp_id, "joint.%d.homing", num)) != 0) return retval;
if ((retval = hal_pin_s32_newf(HAL_OUT, &(addr->home_state), mot_comp_id, "joint.%d.home-state", num)) != 0) return retval;
if ((retval = hal_pin_float_newf(HAL_IN,&(addr->jjog_accel_fraction),mot_comp_id,"joint.%d.jog-accel-fraction", num)) != 0) return retval; if ((retval = hal_pin_float_newf(HAL_IN,&(addr->jjog_accel_fraction),mot_comp_id,"joint.%d.jog-accel-fraction", num)) != 0) return retval;
*addr->jjog_accel_fraction = 1.0; // fraction of accel for wheel jjogs *addr->jjog_accel_fraction = 1.0; // fraction of accel for wheel jjogs
@ -771,14 +771,6 @@ static int init_comm_buffers(void)
joint->acc_limit = 1.0; joint->acc_limit = 1.0;
joint->min_ferror = 0.01; joint->min_ferror = 0.01;
joint->max_ferror = 1.0; joint->max_ferror = 1.0;
joint->home_search_vel = 0.0;
joint->home_latch_vel = 0.0;
joint->home_final_vel = -1;
joint->home_offset = 0.0;
joint->home = 0.0;
joint->home_flags = 0;
joint->home_sequence = -1;
joint->home_state = HOME_IDLE;
joint->backlash = 0.0; joint->backlash = 0.0;
joint->comp.entries = 0; joint->comp.entries = 0;
@ -822,6 +814,8 @@ static int init_comm_buffers(void)
cubicInit(&(joint->cubic)); cubicInit(&(joint->cubic));
} }
homing_init(); // for all joints
/*! \todo FIXME-- add emcmotError */ /*! \todo FIXME-- add emcmotError */
emcmotDebug->cur_time = emcmotDebug->last_time = 0.0; emcmotDebug->cur_time = emcmotDebug->last_time = 0.0;

80
src/emc/motion/motion.h
View file

@ -79,7 +79,7 @@ to another.
#include "simple_tp.h" #include "simple_tp.h"
#include "rtapi_limits.h" #include "rtapi_limits.h"
#include <stdarg.h> #include <stdarg.h>
#include "rtapi_bool.h"
// define a special value to denote an invalid motion ID // define a special value to denote an invalid motion ID
// NB: do not ever generate a motion id of MOTION_INVALID_ID // NB: do not ever generate a motion id of MOTION_INVALID_ID
@ -350,23 +350,10 @@ Suggestion: Split this in to an Error and a Status flag register..
#define EMCMOT_JOINT_ACTIVE_BIT 0x0002 #define EMCMOT_JOINT_ACTIVE_BIT 0x0002
#define EMCMOT_JOINT_INPOS_BIT 0x0004 #define EMCMOT_JOINT_INPOS_BIT 0x0004
#define EMCMOT_JOINT_ERROR_BIT 0x0008 #define EMCMOT_JOINT_ERROR_BIT 0x0008
#define EMCMOT_JOINT_MAX_HARD_LIMIT_BIT 0x0010
#define EMCMOT_JOINT_MAX_HARD_LIMIT_BIT 0x0040 #define EMCMOT_JOINT_MIN_HARD_LIMIT_BIT 0x0020
#define EMCMOT_JOINT_MIN_HARD_LIMIT_BIT 0x0080 #define EMCMOT_JOINT_FERROR_BIT 0x0040
#define EMCMOT_JOINT_FAULT_BIT 0x0080
#define EMCMOT_JOINT_HOME_SWITCH_BIT 0x0100
#define EMCMOT_JOINT_HOMING_BIT 0x0200
#define EMCMOT_JOINT_HOMED_BIT 0x0400
/*! \todo FIXME - I'm not sure AT_HOME is being reported correctly.
AT_HOME is cleared when you jog in free mode, but not if
you do a coordinated move... perhaps that is the intended
behavior.
*/
#define EMCMOT_JOINT_AT_HOME_BIT 0x0800
#define EMCMOT_JOINT_FERROR_BIT 0x1000
#define EMCMOT_JOINT_FAULT_BIT 0x2000
/*! \todo FIXME - the terms "teleop", "coord", and "free" are poorly /*! \todo FIXME - the terms "teleop", "coord", and "free" are poorly
documented. This is my feeble attempt to understand exactly documented. This is my feeble attempt to understand exactly
@ -427,44 +414,6 @@ Suggestion: Split this in to an Error and a Status flag register..
EMCMOT_MOTION_COORD EMCMOT_MOTION_COORD
} motion_state_t; } motion_state_t;
/* states for homing */
typedef enum {
HOME_IDLE = 0,
HOME_START, // 1
HOME_UNLOCK, // 2
HOME_UNLOCK_WAIT, // 3
HOME_INITIAL_BACKOFF_START, // 4
HOME_INITIAL_BACKOFF_WAIT, // 5
HOME_INITIAL_SEARCH_START, // 6
HOME_INITIAL_SEARCH_WAIT, // 7
HOME_SET_COARSE_POSITION, // 8
HOME_FINAL_BACKOFF_START, // 9
HOME_FINAL_BACKOFF_WAIT, // 10
HOME_RISE_SEARCH_START, // 11
HOME_RISE_SEARCH_WAIT, // 12
HOME_FALL_SEARCH_START, // 13
HOME_FALL_SEARCH_WAIT, // 14
HOME_SET_SWITCH_POSITION, // 15
HOME_INDEX_ONLY_START, // 16
HOME_INDEX_SEARCH_START, // 17
HOME_INDEX_SEARCH_WAIT, // 18
HOME_SET_INDEX_POSITION, // 19
HOME_FINAL_MOVE_START, // 20
HOME_FINAL_MOVE_WAIT, // 21
HOME_LOCK, // 22
HOME_LOCK_WAIT, // 23
HOME_FINISHED, // 24
HOME_ABORT // 25
} home_state_t;
typedef enum {
HOME_SEQUENCE_IDLE = 0,
HOME_SEQUENCE_START,
HOME_SEQUENCE_START_JOINTS,
HOME_SEQUENCE_WAIT_JOINTS,
HOME_SEQUENCE_DO_ONE_JOINT,
HOME_SEQUENCE_DO_ONE_SEQUENCE,
} home_sequence_state_t;
typedef enum { typedef enum {
EMCMOT_ORIENT_NONE = 0, EMCMOT_ORIENT_NONE = 0,
@ -511,16 +460,7 @@ Suggestion: Split this in to an Error and a Status flag register..
double acc_limit; /* upper limit of joint accel */ double acc_limit; /* upper limit of joint accel */
double min_ferror; /* zero speed following error limit */ double min_ferror; /* zero speed following error limit */
double max_ferror; /* max speed following error limit */ double max_ferror; /* max speed following error limit */
double home_search_vel; /* dir/spd to look for home switch */
double home_final_vel; /* speed to travel from OFFSET to HOME position */
double home_latch_vel; /* dir/spd to latch switch/index pulse */
double home_offset; /* dir/dist from switch to home point */
double home; /* joint coordinate of home point */
int home_flags; /* flags for various homing options */
int volatile_home; /* joint should get unhomed when we get unhome -2
(generated by task upon estop, etc) */
double backlash; /* amount of backlash */ double backlash; /* amount of backlash */
int home_sequence; /* Order in homing sequence */
emcmot_comp_t comp; /* leadscrew correction data */ emcmot_comp_t comp; /* leadscrew correction data */
/* status info - changes regularly */ /* status info - changes regularly */
@ -551,11 +491,7 @@ Suggestion: Split this in to an Error and a Status flag register..
int on_pos_limit; /* non-zero if on limit */ int on_pos_limit; /* non-zero if on limit */
int on_neg_limit; /* non-zero if on limit */ int on_neg_limit; /* non-zero if on limit */
double home_sw_pos; /* latched position of home sw */
int home_pause_timer; /* used to delay between homing states */
int index_enable; /* current state of index enable pin */
home_state_t home_state; /* state machine for homing */
double motor_offset; /* diff between internal and motor pos, used double motor_offset; /* diff between internal and motor pos, used
to set position to zero during homing */ to set position to zero during homing */
int old_jjog_counts; /* prior value, used for deltas */ int old_jjog_counts; /* prior value, used for deltas */
@ -576,6 +512,9 @@ Suggestion: Split this in to an Error and a Status flag register..
*/ */
typedef struct { typedef struct {
EMCMOT_JOINT_FLAG flag; /* see above for bit details */ EMCMOT_JOINT_FLAG flag; /* see above for bit details */
bool homed;
bool homing;
double pos_cmd; /* commanded joint position */ double pos_cmd; /* commanded joint position */
double pos_fb; /* position feedback, comp removed */ double pos_fb; /* position feedback, comp removed */
double vel_cmd; /* current velocity */ double vel_cmd; /* current velocity */
@ -593,7 +532,6 @@ Suggestion: Split this in to an Error and a Status flag register..
double min_pos_limit; /* lower soft limit on joint pos */ double min_pos_limit; /* lower soft limit on joint pos */
double min_ferror; /* zero speed following error limit */ double min_ferror; /* zero speed following error limit */
double max_ferror; /* max speed following error limit */ double max_ferror; /* max speed following error limit */
double home_offset; /* dir/dist from switch to home point */
} emcmot_joint_status_t; } emcmot_joint_status_t;
@ -681,8 +619,6 @@ Suggestion: Split this in to an Error and a Status flag register..
EmcPose carte_pos_fb; /* actual Cartesian position */ EmcPose carte_pos_fb; /* actual Cartesian position */
int carte_pos_fb_ok; /* non-zero if feedback is valid */ int carte_pos_fb_ok; /* non-zero if feedback is valid */
EmcPose world_home; /* cartesean coords of home position */ EmcPose world_home; /* cartesean coords of home position */
int homing_active; /* non-zero if any joint is homing */
home_sequence_state_t homingSequenceState;
emcmot_joint_status_t joint_status[EMCMOT_MAX_JOINTS]; /* all joint status data */ emcmot_joint_status_t joint_status[EMCMOT_MAX_JOINTS]; /* all joint status data */
emcmot_axis_status_t axis_status[EMCMOT_MAX_AXIS]; /* all axis status data */ emcmot_axis_status_t axis_status[EMCMOT_MAX_AXIS]; /* all axis status data */
int spindleSync; /* spindle used for syncronised moves. -1 = none */ int spindleSync; /* spindle used for syncronised moves. -1 = none */

5
src/emc/task/taskintf.cc
View file

@ -912,8 +912,9 @@ int emcJointUpdate(EMC_JOINT_STAT stat[], int numJoints)
stat[joint_num].ferrorCurrent = joint->ferror; stat[joint_num].ferrorCurrent = joint->ferror;
stat[joint_num].ferrorHighMark = joint->ferror_high_mark; stat[joint_num].ferrorHighMark = joint->ferror_high_mark;
stat[joint_num].homing = (joint->flag & EMCMOT_JOINT_HOMING_BIT ? 1 : 0); stat[joint_num].homing = joint->homing;
stat[joint_num].homed = (joint->flag & EMCMOT_JOINT_HOMED_BIT ? 1 : 0); stat[joint_num].homed = joint->homed;
stat[joint_num].fault = (joint->flag & EMCMOT_JOINT_FAULT_BIT ? 1 : 0); stat[joint_num].fault = (joint->flag & EMCMOT_JOINT_FAULT_BIT ? 1 : 0);
stat[joint_num].enabled = (joint->flag & EMCMOT_JOINT_ENABLE_BIT ? 1 : 0); stat[joint_num].enabled = (joint->flag & EMCMOT_JOINT_ENABLE_BIT ? 1 : 0);
stat[joint_num].inpos = (joint->flag & EMCMOT_JOINT_INPOS_BIT ? 1 : 0); stat[joint_num].inpos = (joint->flag & EMCMOT_JOINT_INPOS_BIT ? 1 : 0);