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

homing.c: internal updates, adapt homecomp.comp

Browse source 
no api changes (homing.h)
no functional changes intended

homing.c:
1) Allow a user-built homing module to source homing.c
   and use or augment its base_*() functions
2) isolate sync final home move (negative HOME_SEQUENE)
   to local functions: sync_ready(),sync_reset()
3) support sync to a reqd_state
   (sync_final_move: HOME_FINAL_MOVE_START)
4) update 'seen' var in appropriate switch case
5) var renames for consistency/readability:
  home_sequence --> current_sequence
  n_joints --> njoints
  n_extrajoints --> nextrajoints
6) rm superfluous special_case provision

homecomp.comp:
1) adapt use of base_* funcs to demonstrate:
   a) skeleton module with all api functions
   b) customized module using homing.c base_*() functions
      augmented with support for alternate joint homing
      state machines
This commit is contained in:
Dewey Garrett 2022-05-02 10:23:18 -07:00
parent bb1f2a8760
commit e5be0730d1
2 changed files with 1056 additions and 782 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -19,8 +19,8 @@
static double servo_freq; static double servo_freq;
static emcmot_joint_t * joints; static emcmot_joint_t * joints;
static int all_joints; static int all_joints; // motmod num_joints (typ ini file: [KINS]JOINTS)
static int extra_joints; static int extra_joints; // motmod num_extrajoints
#define ABS(x) (((x) < 0) ? -(x) : (x)) #define ABS(x) (((x) < 0) ? -(x) : (x))
@ -69,9 +69,8 @@ typedef enum {
HOME_SEQUENCE_WAIT_JOINTS, // internal usage HOME_SEQUENCE_WAIT_JOINTS, // internal usage
} home_sequence_state_t; } home_sequence_state_t;
// home sequences (some states are required)
static home_sequence_state_t sequence_state; static home_sequence_state_t sequence_state;
static int home_sequence = -1; static int current_sequence = 0;
static bool homing_active; static bool homing_active;
/* internal states for homing */ /* internal states for homing */
@ -113,7 +112,6 @@ typedef struct {
bool homed; // OUT pin bool homed; // OUT pin
bool home_sw; // IN pin bool home_sw; // IN pin
bool index_enable; // IO pin bool index_enable; // IO pin
bool sync_final_move; // joints with neg sequence
bool joint_in_sequence; bool joint_in_sequence;
int pause_timer; int pause_timer;
double home_offset; // intfc, updateable double home_offset; // intfc, updateable
@ -214,7 +212,7 @@ static void update_home_is_synchronized(void) {
for (jno = 0; jno < all_joints; jno++) { for (jno = 0; jno < all_joints; jno++) {
H[jno].home_is_synchronized = 0; H[jno].home_is_synchronized = 0;
if (H[jno].home_sequence < 0) { if (H[jno].home_sequence < 0) {
// neg: sync all joints with same ABS(home_sequence): // neg: sync all joints with same ABS(H[jno].home_sequence):
for (jj = 0; jj < all_joints; jj++) { for (jj = 0; jj < all_joints; jj++) {
if (ABS(H[jj].home_sequence) == ABS(H[jno].home_sequence)) { if (ABS(H[jj].home_sequence) == ABS(H[jno].home_sequence)) {
H[jj].home_sequence = H[jno].home_sequence; H[jj].home_sequence = H[jno].home_sequence;
@ -238,7 +236,7 @@ static void update_home_is_synchronized(void) {
} }
} }
static int make_joint_home_pins(int id,int njoints) static int base_make_joint_home_pins(int id,int njoints)
{ {
//NOTE: motmod supplies the component id //NOTE: motmod supplies the component id
int jno,retval; int jno,retval;
@ -266,7 +264,7 @@ static int make_joint_home_pins(int id,int njoints)
"joint.%d.index-enable", jno); "joint.%d.index-enable", jno);
} }
return retval; return retval;
} // make_joint_home_pins() } // base_make_joint_home_pins()
static void do_home_all(void) static void do_home_all(void)
{ {
@ -277,7 +275,7 @@ static void do_home_all(void)
static void do_home_one_joint(int jno) static void do_home_one_joint(int jno)
{ {
//NOTE: if home_sequence neg, home all joints in sequence //NOTE: if H[jno].home_sequence neg, home all joints in sequence
int jj; int jj;
if (H[jno].home_sequence < 0) { //neg: home all joints in sequence if (H[jno].home_sequence < 0) { //neg: home all joints in sequence
sequence_state = HOME_SEQUENCE_DO_ONE_SEQUENCE; sequence_state = HOME_SEQUENCE_DO_ONE_SEQUENCE;
@ -342,18 +340,13 @@ static void set_all_unhomed(int unhome_method, motion_state_t motstate)
static void do_homing_sequence(void) static void do_homing_sequence(void)
{ {
int i,ii; int i,ii;
int special_case_sync_all; int seen;
int seen = 0;
emcmot_joint_t *joint; emcmot_joint_t *joint;
int sequence_is_set = 0; int sequence_is_set = 0;
/* first pass init */
if(home_sequence == -1) {
sequence_state = HOME_SEQUENCE_IDLE;
home_sequence = 0;
}
switch( sequence_state ) { switch( sequence_state ) {
case HOME_SEQUENCE_IDLE: case HOME_SEQUENCE_IDLE:
current_sequence = 0;
/* nothing to do */ /* nothing to do */
break; break;
@ -362,38 +355,34 @@ static void do_homing_sequence(void)
for (i=0; i < all_joints; i++) { for (i=0; i < all_joints; i++) {
if (H[i].home_state == HOME_START) { if (H[i].home_state == HOME_START) {
H[i].joint_in_sequence = 1; H[i].joint_in_sequence = 1;
home_sequence = ABS(H[i].home_sequence); current_sequence = ABS(H[i].home_sequence);
} else {
if (H[i].joint_in_sequence) {
// it may already be running, leave alone
} else { } else {
H[i].joint_in_sequence = 0; H[i].joint_in_sequence = 0;
} }
} }
}
sequence_is_set = 1; sequence_is_set = 1;
//drop through----drop through----drop through----drop through //drop through----drop through----drop through----drop through
case HOME_SEQUENCE_DO_ONE_SEQUENCE: case HOME_SEQUENCE_DO_ONE_SEQUENCE:
// Expect multiple joints with home_state==HOME_START // Expect multiple joints with home_state==HOME_START
// specified by a negative sequence // specified by a negative sequence
// Determine home_sequence and set H[i].joint_in_sequence // Determine current_sequence and set H[i].joint_in_sequence
// based on home_state[i] == HOME_START // based on home_state[i] == HOME_START
if (!sequence_is_set) { if (!sequence_is_set) {
for (i=0; i < all_joints; i++) { for (i=0; i < all_joints; i++) {
if (H[i].home_state == HOME_START) { if (H[i].home_state == HOME_START) {
if ( sequence_is_set if ( sequence_is_set
&& (ABS(H[i].home_sequence) != home_sequence)) { && (ABS(H[i].home_sequence) != current_sequence)) {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR,
_("homing.c Unexpected joint=%d jsequence=%d seq=%d\n") _("homing.c Unexpected joint=%d jseq=%d current_seq=%d\n")
,i,H[i].home_sequence,home_sequence); ,i,H[i].home_sequence,current_sequence);
} }
home_sequence = ABS(H[i].home_sequence); current_sequence = ABS(H[i].home_sequence);
sequence_is_set = 1; sequence_is_set = 1;
} }
H[i].joint_in_sequence = 1; //disprove H[i].joint_in_sequence = 1; //disprove
if ( (H[i].home_state != HOME_START) if ( (H[i].home_state != HOME_START)
|| (home_sequence != ABS(H[i].home_sequence)) || (current_sequence != ABS(H[i].home_sequence))
) { ) {
H[i].joint_in_sequence = 0; H[i].joint_in_sequence = 0;
} }
@ -410,7 +399,7 @@ static void do_homing_sequence(void)
// sequence_is_set not otherwise established: home-all // sequence_is_set not otherwise established: home-all
for (i=0; i < EMCMOT_MAX_JOINTS; i++) { for (i=0; i < EMCMOT_MAX_JOINTS; i++) {
H[i].joint_in_sequence = 1; H[i].joint_in_sequence = 1;
// unspecified joints have an unrealizable home_sequence: // unspecified joints have an unrealizable H[i].home_sequence:
if (H[i].home_sequence >100) { if (H[i].home_sequence >100) {
// docs: 'If HOME_SEQUENCE is not specified then this joint // docs: 'If HOME_SEQUENCE is not specified then this joint
// will not be homed by the HOME ALL sequence' // will not be homed by the HOME ALL sequence'
@ -418,12 +407,9 @@ static void do_homing_sequence(void)
} }
} }
sequence_is_set = 1; sequence_is_set = 1;
home_sequence = 0; current_sequence = 0;
} }
/* Initializations */ /* Initializations */
for(i=0; i < MAX_HOME_SEQUENCES; i++) {
H[i].sync_final_move = 0; //reset to allow a rehome
}
for(i=0; i < all_joints; i++) { for(i=0; i < all_joints; i++) {
if (!H[i].joint_in_sequence) continue; if (!H[i].joint_in_sequence) continue;
if ( (H[i].home_flags & HOME_NO_REHOME) if ( (H[i].home_flags & HOME_NO_REHOME)
@ -443,16 +429,6 @@ static void do_homing_sequence(void)
} }
} }
} }
/* special_case_sync_all: if home_sequence == -1 for all joints
* synchronize all joints final move
*/
special_case_sync_all = 1; // disprove
for(i=0; i < all_joints; i++) {
if (H[i].home_sequence != -1) {special_case_sync_all = 0;}
}
if (special_case_sync_all) {
home_sequence = 1;
}
for(i=0; i < all_joints; i++) { for(i=0; i < all_joints; i++) {
if (!H[i].joint_in_sequence) continue; if (!H[i].joint_in_sequence) continue;
if ( H[i].home_state != HOME_IDLE && H[i].home_state != HOME_START) { if ( H[i].home_state != HOME_IDLE && H[i].home_state != HOME_START) {
@ -466,10 +442,11 @@ static void do_homing_sequence(void)
//drop through----drop through----drop through----drop through //drop through----drop through----drop through----drop through
case HOME_SEQUENCE_START_JOINTS: case HOME_SEQUENCE_START_JOINTS:
/* start all joints whose sequence number matches home_sequence */ seen = 0;
/* start all joints whose sequence number matches H[i].home_sequence */
for(i=0; i < all_joints; i++) { for(i=0; i < all_joints; i++) {
joint = &joints[i]; joint = &joints[i];
if(ABS(H[i].home_sequence) == home_sequence) { if(ABS(H[i].home_sequence) == current_sequence) {
if (!H[i].joint_in_sequence) continue; if (!H[i].joint_in_sequence) continue;
/* start this joint */ /* start this joint */
joint->free_tp.enable = 0; joint->free_tp.enable = 0;
@ -477,7 +454,7 @@ static void do_homing_sequence(void)
seen++; seen++;
} }
} }
if (seen || home_sequence == 0) { if (seen || current_sequence == 0) {
sequence_state = HOME_SEQUENCE_WAIT_JOINTS; sequence_state = HOME_SEQUENCE_WAIT_JOINTS;
} else { } else {
/* no joints have this sequence number, we're done */ /* no joints have this sequence number, we're done */
@ -488,10 +465,11 @@ static void do_homing_sequence(void)
break; break;
case HOME_SEQUENCE_WAIT_JOINTS: case HOME_SEQUENCE_WAIT_JOINTS:
seen = 0;
for(i=0; i < all_joints; i++) { for(i=0; i < all_joints; i++) {
if (!H[i].joint_in_sequence) continue; if (!H[i].joint_in_sequence) continue;
// negative H[i].home_sequence means sync final move // negative H[i].home_sequence means sync final move
if(ABS(H[i].home_sequence) != home_sequence) { if(ABS(H[i].home_sequence) != current_sequence) {
/* this joint is not at the current sequence number, ignore it */ /* this joint is not at the current sequence number, ignore it */
continue; continue;
} }
@ -503,7 +481,7 @@ static void do_homing_sequence(void)
} }
if(!seen) { if(!seen) {
/* all joints at this step have finished, move on to next step */ /* all joints at this step have finished, move on to next step */
home_sequence ++; current_sequence++;
sequence_state = HOME_SEQUENCE_START_JOINTS; sequence_state = HOME_SEQUENCE_START_JOINTS;
} }
break; break;
@ -518,19 +496,15 @@ static void do_homing_sequence(void)
} }
} // do_homing_sequence() } // do_homing_sequence()
/*********************************************************************** static int base_homing_init(int id,
* PUBLIC FUNCTIONS *
************************************************************************/
int homing_init(int id,
double servo_period, double servo_period,
int n_joints, int njoints,
int n_extrajoints, int nextrajoints,
emcmot_joint_t* pjoints) emcmot_joint_t* pjoints)
{ {
int i; int i;
all_joints = n_joints; all_joints = njoints;
extra_joints = n_extrajoints; extra_joints = nextrajoints;
joints = pjoints; joints = pjoints;
if (servo_period < 1e-9) { if (servo_period < 1e-9) {
@ -539,8 +513,8 @@ int homing_init(int id,
servo_period); servo_period);
return -1; return -1;
} }
if (make_joint_home_pins(id,all_joints)) { if (base_make_joint_home_pins(id,all_joints)) {
rtapi_print_msg(RTAPI_MSG_ERR,"%s: make_joint_home_pins fail\n", rtapi_print_msg(RTAPI_MSG_ERR,"%s: base_make_joint_home_pins fail\n",
__FUNCTION__); __FUNCTION__);
return -1; return -1;
} }
@ -561,7 +535,7 @@ int homing_init(int id,
return 0; return 0;
} }
void read_homing_in_pins(int njoints) static void base_read_homing_in_pins(int njoints)
{ {
int jno; int jno;
one_joint_home_data_t *addr; one_joint_home_data_t *addr;
@ -572,7 +546,7 @@ void read_homing_in_pins(int njoints)
} }
} }
void write_homing_out_pins(int njoints) static void base_write_homing_out_pins(int njoints)
{ {
int jno; int jno;
one_joint_home_data_t *addr; one_joint_home_data_t *addr;
@ -585,21 +559,22 @@ void write_homing_out_pins(int njoints)
} }
} }
void do_home_joint(int jno) { static void base_do_home_joint(int jno) {
if (jno == -1) { if (jno == -1) {
H[0].homed = 0; // ensure at least one unhomed
do_home_all(); do_home_all();
} else { } else {
do_home_one_joint(jno); // apply rules if home_sequence negative do_home_one_joint(jno); // apply rules if home_sequence negative
} }
} }
void do_cancel_homing(int jno) { static void base_do_cancel_homing(int jno) {
if (H[jno].homing) { if (H[jno].homing) {
H[jno].home_state = HOME_ABORT; H[jno].home_state = HOME_ABORT;
} }
} }
void set_unhomed(int jno, motion_state_t motstate) { static void base_set_unhomed(int jno, motion_state_t motstate) {
// Note: negative jno ==> unhome multiple joints // Note: negative jno ==> unhome multiple joints
emcmot_joint_t *joint; emcmot_joint_t *joint;
if (jno < 0) { set_all_unhomed(jno,motstate); return; } if (jno < 0) { set_all_unhomed(jno,motstate); return; }
@ -634,9 +609,9 @@ void set_unhomed(int jno, motion_state_t motstate) {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR,
_("Cannot unhome inactive joint %d\n"), jno); _("Cannot unhome inactive joint %d\n"), jno);
} }
} // set_unhomed() } // base_set_unhomed()
void set_joint_homing_params(int jno, static void base_set_joint_homing_params(int jno,
double offset, double offset,
double home, double home,
double home_final_vel, double home_final_vel,
@ -658,7 +633,7 @@ void set_joint_homing_params(int jno,
update_home_is_synchronized(); update_home_is_synchronized();
} }
void update_joint_homing_params (int jno, static void base_update_joint_homing_params (int jno,
double offset, double offset,
double home, double home,
int home_sequence int home_sequence
@ -670,7 +645,7 @@ void update_joint_homing_params (int jno,
update_home_is_synchronized(); update_home_is_synchronized();
} }
bool get_allhomed() { static bool base_get_allhomed(void) {
int joint_num; int joint_num;
emcmot_joint_t *joint; emcmot_joint_t *joint;
@ -687,71 +662,80 @@ bool get_allhomed() {
} }
/* return true if all active joints are homed*/ /* return true if all active joints are homed*/
return 1; return 1;
} // get_allhomed() } // base_get_allhomed()
bool get_homing_is_active() { static bool base_get_homing_is_active(void) {
return homing_active; return homing_active;
} }
int get_home_sequence(int jno) { static int base_get_home_sequence(int jno) {
return H[jno].home_sequence; return H[jno].home_sequence;
} }
bool get_homing(int jno) { static bool base_get_homing(int jno) {
return H[jno].homing; return H[jno].homing;
} }
bool get_homed(int jno) { static bool base_get_homed(int jno) {
return H[jno].homed; return H[jno].homed;
} }
bool get_index_enable(int jno) { static bool base_get_index_enable(int jno) {
return H[jno].index_enable; return H[jno].index_enable;
} }
bool get_home_needs_unlock_first(int jno) { static bool base_get_home_needs_unlock_first(int jno) {
return (H[jno].home_flags & HOME_UNLOCK_FIRST) ? 1 : 0; return (H[jno].home_flags & HOME_UNLOCK_FIRST) ? 1 : 0;
} }
bool get_home_is_idle(int jno) { static bool base_get_home_is_idle(int jno) {
return H[jno].home_state == HOME_IDLE ? 1 : 0; return H[jno].home_state == HOME_IDLE ? 1 : 0;
} }
bool get_home_is_synchronized(int jno) { static bool base_get_home_is_synchronized(int jno) {
return H[jno].home_is_synchronized; return H[jno].home_is_synchronized;
} }
bool get_homing_at_index_search_wait(int jno) { static bool base_get_homing_at_index_search_wait(int jno) {
return H[jno].home_state == HOME_INDEX_SEARCH_WAIT ? 1 : 0; return H[jno].home_state == HOME_INDEX_SEARCH_WAIT ? 1 : 0;
} }
// HOMING management static bool sync_now = 0;
bool do_homing(void) static void sync_reset(void) { sync_now=0; return; }
static bool sync_ready(int joint_num,home_state_t reqd_state)
{
// defer a move until all joints in sequence are at 'reqd_state'
if ( ( ABS(H[joint_num].home_sequence) == current_sequence)
&& !sync_now) {
int jno;
for (jno = 0; jno < all_joints; jno++) {
if (!H[jno].joint_in_sequence) {continue;}
if (ABS(H[jno].home_sequence) != current_sequence) {continue;}
if (H[jno].home_flags & HOME_ABSOLUTE_ENCODER) {continue;}
if (H[jno].home_state != reqd_state) {
sync_now = 0; return 0; // not ready
}
}
sync_now = 1;
}
return 1; // ready
} // sync_ready()
static int base_1joint_state_machine(int joint_num)
{ {
int allhomed = 0;
int joint_num;
emcmot_joint_t *joint; emcmot_joint_t *joint;
double offset, tmp; double offset, tmp;
int home_sw_active, homing_flag; int home_sw_active, homing_flag;
do_homing_sequence();
homing_flag = 0; homing_flag = 0;
/* loop thru joints, treat each one individually */
for (joint_num = 0; joint_num < all_joints; joint_num++) {
if (!H[joint_num].joint_in_sequence) continue;
/* point to joint struct */
joint = &joints[joint_num]; joint = &joints[joint_num];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
/* if joint is not active, skip it */
continue;
}
home_sw_active = H[joint_num].home_sw; home_sw_active = H[joint_num].home_sw;
if (H[joint_num].home_state != HOME_IDLE) { if (H[joint_num].home_state != HOME_IDLE) {
homing_flag = 1; /* at least one joint is homing */ homing_flag = 1; /* at least one joint is homing */
} }
/* when an joint is homing, 'check_for_faults()' ignores its limit /* when a joint is homing, 'check_for_faults()' ignores its limit
switches, so that this code can do the right thing with them. Once switches, so that this code can do the right thing with them. Once
the homing process is finished, the 'check_for_faults()' resumes the homing process is finished, the 'check_for_faults()' resumes
checking */ checking */
@ -793,10 +777,9 @@ bool do_homing(void)
H[joint_num].homing = 1; H[joint_num].homing = 1;
H[joint_num].homed = 0; H[joint_num].homed = 0;
} }
/* stop any existing motion */ joint->free_tp.enable = 0; /* stop any existing motion */
joint->free_tp.enable = 0; sync_reset(); /* stop any interrupted/canceled sync */
/* reset delay counter */ H[joint_num].pause_timer = 0; /* reset delay counter */
H[joint_num].pause_timer = 0;
/* figure out exactly what homing sequence is needed */ /* figure out exactly what homing sequence is needed */
if (H[joint_num].home_flags & HOME_ABSOLUTE_ENCODER) { if (H[joint_num].home_flags & HOME_ABSOLUTE_ENCODER) {
H[joint_num].home_flags &= ~HOME_IS_SHARED; // shared not applicable H[joint_num].home_flags &= ~HOME_IS_SHARED; // shared not applicable
@ -1159,7 +1142,7 @@ bool do_homing(void)
if (H[joint_num].home_flags & HOME_NO_FINAL_MOVE) { if (H[joint_num].home_flags & HOME_NO_FINAL_MOVE) {
H[joint_num].home_state = HOME_FINISHED; H[joint_num].home_state = HOME_FINISHED;
immediate_state = 1; immediate_state = 1;
H[joint_num].homed = 1; H[joint_num].homed = 1; // finished absolute encoder
break; break;
} }
} }
@ -1285,40 +1268,17 @@ bool do_homing(void)
if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) {
/* no, update timer and wait some more */ /* no, update timer and wait some more */
H[joint_num].pause_timer++; H[joint_num].pause_timer++;
if (H[joint_num].home_sequence < 0) {
if (!H[home_sequence].sync_final_move) break;
} else {
break;
}
}
// negative H[joint_num].home_sequence means sync final move
// defer final move until all joints in sequence are ready
if ( (H[joint_num].home_sequence < 0)
&& ( ABS(H[joint_num].home_sequence) == home_sequence)
) {
if (!H[home_sequence].sync_final_move) {
int jno;
emcmot_joint_t *jtmp;
H[home_sequence].sync_final_move = 1; //disprove
for (jno = 0; jno < all_joints; jno++) {
jtmp = &joints[jno];
if (!H[jno].joint_in_sequence) continue;
if (ABS(H[jno].home_sequence) != home_sequence) {continue;}
if (H[jno].home_flags & HOME_ABSOLUTE_ENCODER) {continue;}
if ( (H[jno].home_state != HOME_FINAL_MOVE_START)
||
(jtmp->free_tp.active)
) {
H[home_sequence].sync_final_move = 0;
break;
}
}
if (!H[home_sequence].sync_final_move) break;
}
} }
H[joint_num].pause_timer = 0; H[joint_num].pause_timer = 0;
/* plan a move to home position */
// neg home sequence: sync final move
// all joints must be in *this* home_state:
if ( (H[joint_num].home_sequence < 0)
&& !sync_ready(joint_num, H[joint_num].home_state) ) {
break; // not all joints at *this* state, wait for them
}
/* plan a final move to home position */
joint->free_tp.pos_cmd = H[joint_num].home; joint->free_tp.pos_cmd = H[joint_num].home;
/* if home_vel is set (>0) then we use that, otherwise we rapid there */ /* if home_vel is set (>0) then we use that, otherwise we rapid there */
if (H[joint_num].home_final_vel > 0) { if (H[joint_num].home_final_vel > 0) {
@ -1339,6 +1299,10 @@ bool do_homing(void)
move to the home position. It terminates when the machine move to the home position. It terminates when the machine
arrives at the final location. If the move hits a limit arrives at the final location. If the move hits a limit
before it arrives, the home is aborted. */ before it arrives, the home is aborted. */
// if enabled, a sync for final move has now started, so reset:
sync_reset();
/* have we arrived (and stopped) at home? */ /* have we arrived (and stopped) at home? */
if (!joint->free_tp.active) { if (!joint->free_tp.active) {
/* yes, stop motion */ /* yes, stop motion */
@ -1382,16 +1346,10 @@ bool do_homing(void)
case HOME_FINISHED: case HOME_FINISHED:
H[joint_num].homing = 0; H[joint_num].homing = 0;
H[joint_num].homed = 1; H[joint_num].homed = 1; // finished
H[joint_num].home_state = HOME_IDLE; H[joint_num].home_state = HOME_IDLE;
immediate_state = 1; immediate_state = 1;
H[joint_num].joint_in_sequence = 0; H[joint_num].joint_in_sequence = 0;
// This joint just finished homing. See if this is the
// final one and all joints are now homed, and switch to
// Teleop mode if so.
if (get_allhomed()) {
allhomed = 1;
}
break; break;
case HOME_ABORT: case HOME_ABORT:
@ -1413,24 +1371,113 @@ bool do_homing(void)
break; break;
} /* end of switch(H[joint_num].home_state) */ } /* end of switch(H[joint_num].home_state) */
} while (immediate_state); } while (immediate_state);
} /* end of loop through all joints */
if ( homing_flag ) { return homing_flag;
/* at least one joint is homing, set global flag */ } // base_1joint_state_machine()
static bool base_do_homing(void)
{
int joint_num;
int homing_flag = 0;
bool beginning_allhomed = get_allhomed();
do_homing_sequence();
/* loop thru joints, treat each one individually */
for (joint_num = 0; joint_num < all_joints; joint_num++) {
if (!H[joint_num].joint_in_sequence) { continue; }
if (!GET_JOINT_ACTIVE_FLAG(&joints[joint_num])) { continue; }
// DEFAULT joint homing state machine:
homing_flag += base_1joint_state_machine(joint_num);
}
if ( homing_flag > 0 ) { /* one or more joint is homing */
homing_active = 1; homing_active = 1;
} else { } else { /* is a homing sequence in progress? */
/* is a homing sequence in progress? */
if (sequence_state == HOME_SEQUENCE_IDLE) { if (sequence_state == HOME_SEQUENCE_IDLE) {
/* no, single joint only, we're done */ /* no, single joint only, we're done */
homing_active = 0; homing_active = 0;
} }
} }
if (allhomed) {homing_active = 0; return 1;} // return 1 if homing completed this period
if (!beginning_allhomed && get_allhomed()) {homing_active=0; return 1;}
return 0; return 0;
} // do_homing() } // base_do_homing()
/***********************************************************************
* PUBLIC FUNCTIONS *
************************************************************************/
//======================================================== //========================================================
// all home functions for homing api #ifndef CUSTOM_HOMEMODULE // {
/*
** Default homing module (homemod) uses base_* functions
** A user-built homing module can set CUSTOM_HOMEMODULE
** and source this file (homing.c) to selectively use or
** override any of the base_* functions
*/
int homing_init(int id,
double servo_period,
int njoints,
int nextrajoints,
emcmot_joint_t* pjoints)
{
return base_homing_init(id,
servo_period,
njoints,
nextrajoints,
pjoints);
}
bool do_homing(void) { return base_do_homing(); }
bool get_allhomed(void) { return base_get_allhomed(); }
bool get_homed(int jno) { return base_get_homed(jno); }
bool get_home_is_idle(int jno) { return base_get_home_is_idle(jno); }
bool get_home_is_synchronized(int jno) { return base_get_home_is_synchronized(jno); }
bool get_home_needs_unlock_first(int jno) { return base_get_home_needs_unlock_first(jno); }
int get_home_sequence(int jno) { return base_get_home_sequence(jno); }
bool get_homing(int jno) { return base_get_homing(jno); }
bool get_homing_at_index_search_wait(int jno) { return base_get_homing_at_index_search_wait(jno); }
bool get_homing_is_active(void) { return base_get_homing_is_active(); }
bool get_index_enable(int jno) { return base_get_index_enable(jno); }
void read_homing_in_pins(int njoints) { base_read_homing_in_pins(njoints); }
void do_home_joint(int jno) { base_do_home_joint(jno); }
void do_cancel_homing(int jno) { base_do_cancel_homing(jno); }
void set_unhomed(int jno, motion_state_t motstate) { base_set_unhomed(jno,motstate); }
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
)
{ base_set_joint_homing_params(jno,
offset,
home,
home_final_vel,
home_search_vel,
home_latch_vel,
home_flags,
home_sequence,
volatile_home);
}
void update_joint_homing_params(int jno,
double offset,
double home,
int home_sequence
)
{
base_update_joint_homing_params (jno,
offset,
home,
home_sequence
);
}
void write_homing_out_pins(int njoints) {base_write_homing_out_pins(njoints); }
// all home functions for homing api follow:
EXPORT_SYMBOL(homeMotFunctions); EXPORT_SYMBOL(homeMotFunctions);
EXPORT_SYMBOL(homing_init); EXPORT_SYMBOL(homing_init);
@ -1452,3 +1499,4 @@ EXPORT_SYMBOL(set_unhomed);
EXPORT_SYMBOL(set_joint_homing_params); EXPORT_SYMBOL(set_joint_homing_params);
EXPORT_SYMBOL(update_joint_homing_params); EXPORT_SYMBOL(update_joint_homing_params);
EXPORT_SYMBOL(write_homing_out_pins); EXPORT_SYMBOL(write_homing_out_pins);
#endif // }

272
src/hal/components/homecomp.comp
View file

@ -4,9 +4,22 @@ description """
Example of a homing module buildable with halcompile. Example of a homing module buildable with halcompile.
Demonstrates required code for #includes, function definitions, etc. Demonstrates required code for #includes, function definitions, etc.
An actual homing scheme is \\fBnot\\fR implemented but all necessary If \\fBHOMING_BASE\\fR is #defined and points to a valid homing.c file,
functions are included as skeleton code. (All joints are an example of a customized homing module is built. This module
effectively homed at all times and cannot be unhomed). creates input hal pins joint.n.request-custom-homing that enable an
alternate joint homing state machine for requested joints. A hal output
pin joint.N.is_custom-homing verifies selection"
The customized homing module utilizes many of the base homing api
routines from homing.c without modification but augments other base
functions to add support for custom hal pins and custom joint homing
state machines. A user-built module will likely replace additional
api functions or augment them with other customizations.
If \\fBHOMING_BASE\\fR Is not #defined, an actual homing scheme is
\\fBnot\\fR implemented but all necessary functions are included as
skeleton code. (All joints are effectively homed at all times and
cannot be unhomed).
See the source code file: src/emc/motion/homing.c for the baseline See the source code file: src/emc/motion/homing.c for the baseline
implementation that includes all functions for the default \\fBhomemod\\fR implementation that includes all functions for the default \\fBhomemod\\fR
@ -35,39 +48,244 @@ option homemod;
option extra_setup; option extra_setup;
;; ;;
/* To incorporate default homing.c file from a local git src tree:
** enable #define HOMING_BASE set to the path to the current homing.c file.
** (Note: CUSTOM_HOMEMODULE precludes duplicate api symbols)
** (Edit myname as required for valid path)
*/
// #define HOMING_BASE /home/myname/linuxcnc-dev/src/emc/motion/homing.c
#define STR(s) #s
#define XSTR(s) STR(s)
#include "motion.h" #include "motion.h"
#include "homing.h" #include "homing.h"
static char *home_parms; static char *home_parms;
RTAPI_MP_STRING(home_parms,"Example home parms"); RTAPI_MP_STRING(home_parms,"Example home parms");
// rtapi_app_main() supplied by halcompile
// EXTRA_SETUP is executed before rtapi_app_main() // EXTRA_SETUP is executed before rtapi_app_main()
EXTRA_SETUP() { EXTRA_SETUP() {
if (!home_parms) {home_parms = "no_home_parms";} if (!home_parms) {home_parms = "no_home_parms";}
rtapi_print("@@@%s:%s: home_parms=%s\n",__FILE__,__FUNCTION__,home_parms); rtapi_print("@@@%s:%s: home_parms=%s\n",__FILE__,__FUNCTION__,home_parms);
#ifndef HOMING_BASE
rtapi_print("\n!!!%s: Skeleton Homing Module\n\n",__FILE__);
#else
rtapi_print("\n!!!%s: HOMING_BASE=%s\n"
"!!!Customize using hal pin(s): joint.N.request-custom-homing\n"
,__FILE__,XSTR(HOMING_BASE));
#endif
return 0; return 0;
} }
// retrieved from motmod.so: //=====================================================================
static emcmot_joint_t *joints; #ifdef HOMING_BASE // { begin CUSTOM example
#define USE_HOMING_BASE XSTR(HOMING_BASE)
//======================================================== // NOTE: CUSTOM_HOMEMODULE: disables duplicate symbols sourced from homing.c
// motmod function ptrs for functions called BY homecomp: #define CUSTOM_HOMEMODULE
static void(*SetRotaryUnlock)(int,int); #include USE_HOMING_BASE
static int (*GetRotaryIsUnlocked)(int);
//======================================================== typedef struct {
// functions ptrs received from motmod: bool request_custom_homing;
void homeMotFunctions(void(*pSetRotaryUnlock)(int,int) bool is_custom_homing;
,int (*pGetRotaryIsUnlocked)(int) } custom_home_local_data;
)
static custom_home_local_data customH[EMCMOT_MAX_JOINTS];
// data for per-joint custom-homing-specific hal pins:
typedef struct {
hal_bit_t *request_custom_homing; // input requests custom homing
hal_bit_t *is_custom_homing; // output verifies custom homing
} custom_one_joint_home_data_t;
typedef struct {
custom_one_joint_home_data_t custom_jhd[EMCMOT_MAX_JOINTS];
} custom_all_joints_home_data_t;
static custom_all_joints_home_data_t *custom_joint_home_data = 0;
static int custom_makepins(int id,int njoints)
{ {
SetRotaryUnlock = *pSetRotaryUnlock; int jno,retval;
GetRotaryIsUnlocked = *pGetRotaryIsUnlocked; custom_one_joint_home_data_t *addr;
custom_joint_home_data = hal_malloc(sizeof(custom_all_joints_home_data_t));
if (custom_joint_home_data == 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HOMING: custom_all_joints_home_data_t malloc failed\n");
return -1;
}
retval = 0;
for (jno = 0; jno < njoints; jno++) {
addr = &(custom_joint_home_data->custom_jhd[jno]);
retval += hal_pin_bit_newf(HAL_IN, &(addr->request_custom_homing), id,
"joint.%d.request-custom-homing", jno);
retval += hal_pin_bit_newf(HAL_OUT, &(addr->is_custom_homing), id,
"joint.%d.is-custom-homing", jno);
}
return retval;
} // custom_makepins()
static void custom_read_homing_in_pins(int njoints)
{
int jno;
custom_one_joint_home_data_t *addr;
for (jno = 0; jno < njoints; jno++) {
addr = &(custom_joint_home_data->custom_jhd[jno]);
customH[jno].request_custom_homing = *(addr->request_custom_homing); // IN
// echo for verificaion:
customH[jno].is_custom_homing = customH[jno].request_custom_homing;
}
} }
//======================================================== static void custom_write_homing_out_pins(int njoints)
{
int jno;
custom_one_joint_home_data_t *addr;
for (jno = 0; jno < njoints; jno++) {
addr = &(custom_joint_home_data->custom_jhd[jno]);
*(addr->is_custom_homing) = customH[jno].is_custom_homing; // OUT
}
}
static int custom_1joint_state_machine(int joint_num)
{
// custom: always homed
H[joint_num].homing = 0;
H[joint_num].homed = 1;
H[joint_num].home_state = HOME_IDLE;
immediate_state = 1;
H[joint_num].joint_in_sequence = 0;
return 0;
} // custom_1joint_state_machine()
// api functions below augment base_*() functions with custom code
int homing_init(int id,
double servo_period,
int n_joints,
int n_extrajoints,
emcmot_joint_t* pjoints)
{
int retval;
retval = base_homing_init(id,
servo_period,
n_joints,
n_extrajoints,
pjoints);
retval += custom_makepins(id,n_joints);
return retval;
} // homing_init()
void read_homing_in_pins(int njoints)
{
base_read_homing_in_pins(njoints);
custom_read_homing_in_pins(njoints);
}
void write_homing_out_pins(int njoints)
{
base_write_homing_out_pins(njoints);
custom_write_homing_out_pins(njoints);
}
/* do_homing() is adapted from homing.c:base_do_homing() augmented
** with support for custom homing as specified on hal input pin:
** joint.n.request-custom-homing and echoed on hal output pin
** joint.n.is-custom-homing
*/
bool do_homing(void)
{
int joint_num;
int homing_flag = 0;
bool beginning_allhomed = get_allhomed();
do_homing_sequence();
/* loop thru joints, treat each one individually */
for (joint_num = 0; joint_num < all_joints; joint_num++) {
if (!H[joint_num].joint_in_sequence) { continue; }
if (!GET_JOINT_ACTIVE_FLAG(&joints[joint_num])) { continue; }
if (customH[joint_num].is_custom_homing) {
// CUSTOM joint homing state machine:
homing_flag += custom_1joint_state_machine(joint_num);
} else {
// DEFAULT joint homing state machine:
homing_flag += base_1joint_state_machine(joint_num);
}
}
if ( homing_flag > 0 ) { /* one or more joint is homing */
homing_active = 1;
} else { /* is a homing sequence in progress? */
if (sequence_state == HOME_SEQUENCE_IDLE) {
/* no, single joint only, we're done */
homing_active = 0;
}
}
// return 1 if homing completed this period
if (!beginning_allhomed && get_allhomed()) {homing_active=0; return 1;}
return 0;
}
//===============================================================================
// functions below use unmodified base_*() implementation
bool get_allhomed(void) { return base_get_allhomed(); }
bool get_homed(int jno) { return base_get_homed(jno); }
bool get_home_is_idle(int jno) { return base_get_home_is_idle(jno); }
bool get_home_is_synchronized(int jno) { return base_get_home_is_synchronized(jno); }
bool get_home_needs_unlock_first(int jno) { return base_get_home_needs_unlock_first(jno); }
int get_home_sequence(int jno) { return base_get_home_sequence(jno); }
bool get_homing(int jno) { return base_get_homing(jno); }
bool get_homing_at_index_search_wait(int jno) { return base_get_homing_at_index_search_wait(jno); }
bool get_homing_is_active(void) { return base_get_homing_is_active(); }
bool get_index_enable(int jno) { return base_get_index_enable(jno); }
void do_home_joint(int jno) { base_do_home_joint(jno); }
void do_cancel_homing(int jno) { base_do_cancel_homing(jno); }
void set_unhomed(int jno, motion_state_t motstate) { base_set_unhomed(jno,motstate); }
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
)
{
base_set_joint_homing_params(jno,
offset,
home,
home_final_vel,
home_search_vel,
home_latch_vel,
home_flags,
home_sequence,
volatile_home);
}
void update_joint_homing_params(int jno,
double offset,
double home,
int home_sequence
)
{
base_update_joint_homing_params (jno,
offset,
home,
home_sequence
);
}
// end CUSTOM example
//=====================================================================
#else // } { begin SKELETON example minimal api implementation
static emcmot_joint_t *joints;
// data for per-joint homing-specific hal pins: // data for per-joint homing-specific hal pins:
typedef struct { typedef struct {
@ -114,10 +332,11 @@ static int makepins(int id,int njoints)
} }
return retval; return retval;
} }
// All (skeleton) functions required for homing api follow:
//======================================================== void homeMotFunctions(void(*pSetRotaryUnlock)(int,int)
// All functions required for homing api ,int (*pGetRotaryIsUnlocked)(int)
// For homecomp.comp: most are skeleton )
{ return; }
int homing_init(int id, int homing_init(int id,
double servo_period, double servo_period,
@ -158,8 +377,9 @@ void update_joint_homing_params (int jno,
int home_sequence int home_sequence
) {return;} ) {return;}
void write_homing_out_pins(int njoints) {return;} void write_homing_out_pins(int njoints) {return;}
#endif // } end SKELETON example minimal api implementation
//=====================================================================
//========================================================
// all home functions for homing api // all home functions for homing api
EXPORT_SYMBOL(homeMotFunctions); EXPORT_SYMBOL(homeMotFunctions);
@ -182,3 +402,9 @@ EXPORT_SYMBOL(set_unhomed);
EXPORT_SYMBOL(set_joint_homing_params); EXPORT_SYMBOL(set_joint_homing_params);
EXPORT_SYMBOL(update_joint_homing_params); EXPORT_SYMBOL(update_joint_homing_params);
EXPORT_SYMBOL(write_homing_out_pins); EXPORT_SYMBOL(write_homing_out_pins);
#undef XSTR
#undef STR
#undef HOMING_BASE
#undef USE_HOMING_BASE
#undef CUSTOM_HOMEMODULE