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

Revert "homing.c: internal updates

Browse source 
This reverts commit e5be0730d1.
based on user problem:

https://forum.linuxcnc.org/49-basic-configuration/45798-weird-homing-behavior-y-y2-out-of-sync#242056

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

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

@ -4,22 +4,9 @@ description """
Example of a homing module buildable with halcompile.
Demonstrates required code for #includes, function definitions, etc.
If \\fBHOMING_BASE\\fR is #defined and points to a valid homing.c file,
an example of a customized homing module is built. This module
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).
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
implementation that includes all functions for the default \\fBhomemod\\fR
@ -48,245 +35,40 @@ option homemod;
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 "homing.h"
static char *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() {
if (!home_parms) {home_parms = "no_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;
}
//=====================================================================
#ifdef HOMING_BASE // { begin CUSTOM example
#define USE_HOMING_BASE XSTR(HOMING_BASE)
// NOTE: CUSTOM_HOMEMODULE: disables duplicate symbols sourced from homing.c
#define CUSTOM_HOMEMODULE
#include USE_HOMING_BASE
typedef struct {
bool request_custom_homing;
bool is_custom_homing;
} 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)
{
int jno,retval;
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
// retrieved from motmod.so:
static emcmot_joint_t *joints;
//========================================================
// motmod function ptrs for functions called BY homecomp:
static void(*SetRotaryUnlock)(int,int);
static int (*GetRotaryIsUnlocked)(int);
//========================================================
// functions ptrs received from motmod:
void homeMotFunctions(void(*pSetRotaryUnlock)(int,int)
,int (*pGetRotaryIsUnlocked)(int)
)
{
SetRotaryUnlock = *pSetRotaryUnlock;
GetRotaryIsUnlocked = *pGetRotaryIsUnlocked;
}
//========================================================
// data for per-joint homing-specific hal pins:
typedef struct {
hal_bit_t *home_sw; // home switch input
@ -332,11 +114,10 @@ static int makepins(int id,int njoints)
}
return retval;
}
// All (skeleton) functions required for homing api follow:
void homeMotFunctions(void(*pSetRotaryUnlock)(int,int)
,int (*pGetRotaryIsUnlocked)(int)
)
{ return; }
//========================================================
// All functions required for homing api
// For homecomp.comp: most are skeleton
int homing_init(int id,
double servo_period,
@ -377,9 +158,8 @@ void update_joint_homing_params (int jno,
int home_sequence
) {return;}
void write_homing_out_pins(int njoints) {return;}
#endif // } end SKELETON example minimal api implementation
//=====================================================================
//========================================================
// all home functions for homing api
EXPORT_SYMBOL(homeMotFunctions);
@ -402,9 +182,3 @@ EXPORT_SYMBOL(set_unhomed);
EXPORT_SYMBOL(set_joint_homing_params);
EXPORT_SYMBOL(update_joint_homing_params);
EXPORT_SYMBOL(write_homing_out_pins);
#undef XSTR
#undef STR
#undef HOMING_BASE
#undef USE_HOMING_BASE
#undef CUSTOM_HOMEMODULE