cia402_homecomp.comp

component cia402_homecomp "homing module template";

description """
A homing module for cia402 devices buildable with halcompile.
This module should be used with Dominic Braun's cia402.comp Ref: https://github.com/dbraun1981/hal-cia402
which provides matching pins home, stat_homed and stat_homed

\\fBHOMING_BASE\\fR must be #defined and must point to a valid homing.c file,
an example of a customized homing module is built.  This module
creates input hal pins joint.n.request-cia402-homing that enables an
alternate joint homing state machine for requested joints.  A hal output
pin joint.N.is_cia402-homing verifies selection"


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
implementation that includes all functions for the default \\fBhomemod\\fR
module.

This cia402 homing component can be built and installed with
halcompile and then substituted for the default homing module
(\\fBhomemod\\fR) using:\n
Sor by inifile setting: \\fB[EMCMOT]HOMEMOD=user_homecomp\\fR

\\fBNote:\\fRIf using a deb install:\n
1) halcompile is provided by the package linuxcnc-uspace-dev\n
""";

pin out bit is_module=1; //one pin is required to use halcompile)

license "GPL";
author "Rod Webster";
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/rod/devel/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");

// EXTRA_SETUP is executed before rtapi_app_main()
EXTRA_SETUP() {
    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;
	bool custom_homing_finished;
} 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_cia402_homing;   // output requests homing (connect to cia402.n.home)
    hal_bit_t *is_cia402_homing;        // input verifies custom homing (connect to cia402.n.stat_homing)
  hal_bit_t *cia402_homing_finished;  // input from servo drive after homing (connect to cia402.stat_homed)
} 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_IO, &(addr->request_cia402_homing), id,
                                  "joint.%d.request-cia402-homing", jno);
        retval += hal_pin_bit_newf(HAL_IN, &(addr->is_cia402_homing), id,
                                  "joint.%d.is-cia402-homing", jno);
        retval += hal_pin_bit_newf(HAL_IN, &(addr->cia402_homing_finished), id,
                                  "joint.%d.cia402-homing-finished", jno);
    }
    return retval;
}

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_cia402_homing);   // IO
      customH[jno].is_custom_homing = *(addr->is_cia402_homing);             // IN
  		customH[jno].custom_homing_finished = *(addr->cia402_homing_finished); // IN
    }
}

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->request_cia402_homing) = customH[jno].request_custom_homing; // IO
    }
}

static int custom_1joint_state_machine(int joint_num){
	
typedef enum {
    HOME_WAIT      = 0,
    HOME_BEGIN     = 1,
    HOME_RUNNING   = 2,
	  HOME_FINISHED  = 3,
} custom_home_state_t;

static custom_home_state_t chomestate[EMCMOT_MAX_JOINTS] = {0};
       custom_home_state_t nextcstate;

    if ( H[joint_num].home_state == HOME_IDLE)
      return(0);
    if ((H[joint_num].home_state == HOME_START) && (chomestate[joint_num] == HOME_WAIT) ) {
        H[joint_num].homing = 1;
        H[joint_num].homed  = 0;
        chomestate[joint_num] = HOME_BEGIN; // set first non-idle custom_home_state
    }

    switch (chomestate[joint_num]) {
      case HOME_BEGIN:
        //rtapi_print("state machine started \n");
        customH[joint_num].request_custom_homing = 1;
        chomestate[joint_num] = HOME_RUNNING;
        break;
		
      case HOME_RUNNING:
        // rtapi_print("servo drive did hear and is now homing \n");
        // RODW - I think we should be checking that the drive has entered homing, maybe insert another state?
        // RODW - the cia402 comp sets pos_fb = pos_cmd while internal homing to prevent following errors. This could be done here.
        if(customH[joint_num].custom_homing_finished)
			    chomestate[joint_num] = HOME_FINISHED;
        break;
      case HOME_FINISHED:
    		//rtapi_print("state machine finished \n");
    		joints[joint_num].pos_cmd = 0.0;
    		joints[joint_num].free_tp.curr_pos = 0.0;
    		joints[joint_num].motor_pos_cmd = 0.0;
    		H[joint_num].homing = 0;
    		H[joint_num].homed  = 1;
    		H[joint_num].home_state = HOME_IDLE;
    		chomestate[joint_num] = HOME_WAIT;
    		customH[joint_num].request_custom_homing = 0;
    		return 0; // finished custom_home_states
        break;
      case HOME_WAIT:  // RODW -this has been set as the defult when chomestate[joint_num] was defined but should never be set during the switch()
      default:
        rtapi_print("Unhandled custom_home_state: %d\n",chomestate[joint_num]);
    }
    return 1; // return 1 if busy
}

// 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 (for CIA402 drives)
			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:
typedef struct {
    hal_bit_t *home_sw;      // home switch input
    hal_bit_t *homing;       // joint is homing
    hal_bit_t *homed;        // joint was homed
    hal_bit_t *index_enable; // motmod sets: request reset on index
                             //        encoder clears: index arrived
    hal_s32_t *home_state;   // homing state machine state
} one_joint_home_data_t;

typedef struct {
    one_joint_home_data_t jhd[EMCMOT_MAX_JOINTS];
} all_joints_home_data_t;

static all_joints_home_data_t *joint_home_data = 0;

static int makepins(int id,int njoints)
{
    // home_pins needed to work with configs expecting them:
    int jno,retval;
    one_joint_home_data_t *addr;

    joint_home_data = hal_malloc(sizeof(all_joints_home_data_t));
    if (joint_home_data == 0) {
        rtapi_print_msg(RTAPI_MSG_ERR, "HOMING: all_joints_home_data_t malloc failed\n");
        return -1;
    }

    retval = 0;
    for (jno = 0; jno < njoints; jno++) {
        addr = &(joint_home_data->jhd[jno]);

        retval += hal_pin_bit_newf(HAL_IN, &(addr->home_sw), id,
                                  "joint.%d.home-sw-in", jno);
        retval += hal_pin_bit_newf(HAL_OUT, &(addr->homing), id,
                                  "joint.%d.homing", jno);
        retval += hal_pin_bit_newf(HAL_OUT, &(addr->homed), id,
                                  "joint.%d.homed", jno);
        retval += hal_pin_s32_newf(HAL_OUT, &(addr->home_state), id,
                                  "joint.%d.home-state", jno);
        retval += hal_pin_bit_newf(HAL_IO, &(addr->index_enable), id,
                                  "joint.%d.index-enable", jno);
    }
    return retval;
}
// All (skeleton) functions required for homing api follow:
void homeMotFunctions(void(*pSetRotaryUnlock)(int,int)
                     ,int (*pGetRotaryIsUnlocked)(int)
                     )
{ return; }

int  homing_init(int id,
                 double servo_period,
                 int n_joints,
                 int n_extrajoints,
                 emcmot_joint_t* pjoints) {
     joints = pjoints;
     return makepins(id,n_joints);
}
bool do_homing(void)                                 {return 1;}
bool get_allhomed()                                  {return 1;}
bool get_homed(int jno)                              {return 1;}
bool get_home_is_idle(int jno)                       {return 1;}
bool get_home_is_synchronized(int jno)               {return 0;}
bool get_home_needs_unlock_first(int jno)            {return 0;}
int  get_home_sequence(int jno)                      {return 0;}
bool get_homing(int jno)                             {return 0;}
bool get_homing_at_index_search_wait(int jno)        {return 0;}
bool get_homing_is_active()                          {return 0;}
bool get_index_enable(int jno)                       {return 0;}
void read_homing_in_pins(int njoints)                {return;}
void do_home_joint(int jno)                          {return;}
void set_unhomed(int jno,motion_state_t motstate)    {return;}
void do_cancel_homing(int jno)                       {return;}
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
                             )                       {return;}
void update_joint_homing_params (int    jno,
                                 double offset,
                                 double home,
                                 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);

EXPORT_SYMBOL(homing_init);
EXPORT_SYMBOL(do_homing);
EXPORT_SYMBOL(get_allhomed);
EXPORT_SYMBOL(get_homed);
EXPORT_SYMBOL(get_home_is_idle);
EXPORT_SYMBOL(get_home_is_synchronized);
EXPORT_SYMBOL(get_home_needs_unlock_first);
EXPORT_SYMBOL(get_home_sequence);
EXPORT_SYMBOL(get_homing);
EXPORT_SYMBOL(get_homing_at_index_search_wait);
EXPORT_SYMBOL(get_homing_is_active);
EXPORT_SYMBOL(get_index_enable);
EXPORT_SYMBOL(read_homing_in_pins);
EXPORT_SYMBOL(do_home_joint);
EXPORT_SYMBOL(do_cancel_homing);
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