Advanced Search

component cia402_safety_homecomp "cia402 homing module";

description """
A homing module for cia402 devices buildable with halcompile.
This module should be used with the provided cia402.comp

\\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-cia-homing that enables an
alternate joint homing state machine for requested joints. A hal output
pin joint.N.is_cia-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:
Sor by inifile setting: \\fB[EMCMOT]HOMEMOD=user_homecomp\\fR

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

pin out bit is_module = 1; // required by halcompile
pin out bit homing_error; // new pin to signal homing error
pin in float cia_homing_timer1; // Timer for homing start timeout
pin in float cia_homing_timer2; // Timer for movement check timeout

license "GPL";
author "me and chatgpt";
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.
*/

#define HOMING_BASE /home/ich/linuxcnc/Sicherung/cia402/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 executed before rtapi_app_main()
EXTRA_SETUP()
{
    rtapi_print_msg(RTAPI_MSG_ERR, "HomeComp Started\n");
    return 0;
}

//=====================================================================
#ifdef HOMING_BASE
#define USE_HOMING_BASE XSTR(HOMING_BASE)

// Disable duplicate symbols from homing.c
#define CUSTOM_HOMEMODULE
#include USE_HOMING_BASE

// CIA Status Word
typedef union
{
    struct
    {
        unsigned char ReadyToSwitchOn : 1;
        unsigned char SwitchOn : 1;
        unsigned char OperationEnabled : 1;
        unsigned char Fault : 1;
        unsigned char VoltageEnabled : 1;
        unsigned char QuickStop : 1;
        unsigned char SwitchOnDisabled : 1;
        unsigned char Warning : 1;
        unsigned char keep1 : 1;
        unsigned char Remote : 1;
        unsigned char TargetReached : 1;
        unsigned char bit11 : 1;
        unsigned char HomingCompletedOutput : 1;
        unsigned char keep2 : 1;
        unsigned char keep3 : 1;
        unsigned char HomingComplete : 1;
    } b;
    hal_u32_t Word;
} Status_t;

typedef struct
{
    bool request_cia_homing;
    bool is_cia_homing;
    bool start_cia_homing;
    // CIA402 pins
    Status_t Status;
    float timer1; // Timer for start timeout
    float timer2; // Timer for movement check
    int step;     // safety state machine step
} cia_home_local_data;

static cia_home_local_data ciaH[EMCMOT_MAX_JOINTS];

// Data for per-joint CIA-Homing HAL pins
typedef struct
{
    hal_bit_t *request_cia_homing;
    hal_bit_t *is_cia_homing;
    hal_bit_t *start_cia_homing;
    hal_bit_t *homing_error;
    hal_u32_t *statusword;
    hal_float_t *timer1;
    hal_float_t *timer2;
} cia_one_joint_home_data_t;

typedef struct
{
    cia_one_joint_home_data_t cia_jhd[EMCMOT_MAX_JOINTS];
} cia_all_joints_home_data_t;

static cia_all_joints_home_data_t *cia_joint_home_data = 0;

//-------------------------------------------------------------
static int cia_makepins(int id, int njoints)
{
    int jno, retval;
    cia_one_joint_home_data_t *addr;

    cia_joint_home_data = hal_malloc(sizeof(cia_all_joints_home_data_t));
    if (!cia_joint_home_data)
    {
        rtapi_print_msg(RTAPI_MSG_ERR, "HOMING: malloc failed\n");
        return -1;
    }

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

        retval += hal_pin_bit_newf(HAL_IN, &(addr->request_cia_homing), id,
                                   "joint.%d.request-cia-homing", jno);
        retval += hal_pin_bit_newf(HAL_OUT, &(addr->is_cia_homing), id,
                                   "joint.%d.is-cia-homing", jno);
        retval += hal_pin_bit_newf(HAL_IO, &(addr->start_cia_homing), id,
                                   "joint.%d.start-cia-homing", jno);
        retval += hal_pin_bit_newf(HAL_OUT, &(addr->homing_error), id,
                                   "joint.%d.homing-error", jno);
        retval += hal_pin_u32_newf(HAL_IN, &(addr->statusword), id,
                                   "joint.%d.cia-statusword", jno);
        retval += hal_pin_float_newf(HAL_IN, &(addr->timer1), id,
                                     "joint.%d.homing-timer1", jno);
        retval += hal_pin_float_newf(HAL_IN, &(addr->timer2), id,
                                     "joint.%d.homing-timer2", jno);
    }
    return retval;
}

//-------------------------------------------------------------
static void cia_read_homing_in_pins(int njoints)
{
    int jno;
    cia_one_joint_home_data_t *addr;
    for (jno = 0; jno < njoints; jno++)
    {
        addr = &(cia_joint_home_data->cia_jhd[jno]);
        ciaH[jno].request_cia_homing = *(addr->request_cia_homing);
        ciaH[jno].Status.Word = *(addr->statusword);
        ciaH[jno].timer1 = *(addr->timer1);
        ciaH[jno].timer2 = *(addr->timer2);
    }
}

//-------------------------------------------------------------
static void cia_write_homing_out_pins(int njoints)
{
    int jno;
    cia_one_joint_home_data_t *addr;
    for (jno = 0; jno < njoints; jno++)
    {
        addr = &(cia_joint_home_data->cia_jhd[jno]);
        *(addr->is_cia_homing) = ciaH[jno].is_cia_homing;
        *(addr->start_cia_homing) = ciaH[jno].start_cia_homing;
        *(addr->homing_error) = homing_error; // global error flag
    }
}

//-------------------------------------------------------------
static int cia_1joint_state_machine(int joint_num)
{
    typedef enum
    {
        CIA_IDLE = 0,
        CIA_BEGIN = 1,
        CIA_START_WAIT = 2,
        CIA_RUNNING = 3,
        CIA_FINISHED = 4,
    } cia_home_state_t;

    static cia_home_state_t chomestate[EMCMOT_MAX_JOINTS] = {0};
    static float t_start[EMCMOT_MAX_JOINTS] = {0};
    static float t_run[EMCMOT_MAX_JOINTS] = {0};
    cia_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] == CIA_IDLE))
    {
        H[joint_num].homing = 1;
        H[joint_num].homed = 0;
        chomestate[joint_num] = CIA_BEGIN;
        ciaH[joint_num].step = 0;
        t_start[joint_num] = 0;
        t_run[joint_num] = 0;
        homing_error = 0;
    }

    switch (chomestate[joint_num])
    {
    case CIA_BEGIN:
        // initialize parameters
        rtapi_print("CIA homing: init parameters\n");
        t_start[joint_num] = 0;
        t_run[joint_num] = 0;
        chomestate[joint_num] = CIA_START_WAIT;
        break;

    case CIA_START_WAIT:
        t_start[joint_num] += H[joint_num].servo_period;
        // check if CIA402 bit9 (Remote) set to start homing
        if (ciaH[joint_num].Status.b.Remote)
        {
            chomestate[joint_num] = CIA_RUNNING;
        }
        else if (t_start[joint_num] > ciaH[joint_num].timer1)
        {
            rtapi_print("CIA homing: start timeout\n");
            homing_error = 1;
            H[joint_num].homing = 0;
            H[joint_num].home_state = HOME_IDLE;
            chomestate[joint_num] = CIA_IDLE;
            return 0;
        }
        break;

    case CIA_RUNNING:
        t_run[joint_num] += H[joint_num].servo_period;
        // safety check: axis movement
        if (!H[joint_num].joint_moving) // Example: joint_moving needs to be replaced by real check
        {
            rtapi_print("CIA homing: axis not moving, error\n");
            homing_error = 1;
            H[joint_num].homing = 0;
            H[joint_num].home_state = HOME_IDLE;
            chomestate[joint_num] = CIA_IDLE;
            return 0;
        }
        // check for CIA homing completion
        if (ciaH[joint_num].Status.b.HomingCompletedOutput &&
            ciaH[joint_num].Status.b.HomingComplete)
        {
            chomestate[joint_num] = CIA_FINISHED;
        }
        else if (t_run[joint_num] > ciaH[joint_num].timer2)
        {
            rtapi_print("CIA homing: movement timeout\n");
            homing_error = 1;
            H[joint_num].homing = 0;
            H[joint_num].home_state = HOME_IDLE;
            chomestate[joint_num] = CIA_IDLE;
            return 0;
        }
        break;

    case CIA_FINISHED:
        rtapi_print("CIA homing complete\n");
        ciaH[joint_num].start_cia_homing = 0;
        H[joint_num].homing = 0;
        H[joint_num].homed = 1;
        H[joint_num].home_state = HOME_IDLE;
        chomestate[joint_num] = CIA_IDLE;
        return 0;

    case CIA_IDLE:
    default:
        break;
    }
    ciaH[joint_num].is_cia_homing = (chomestate[joint_num] != CIA_IDLE);
    return 1;
}

//-------------------------------------------------------------
// Augment base functions
int homing_init(int id, double servo_period, int n_joints, int n_extrajoints, emcmot_joint_t *pjoints)
{
    int retval = base_homing_init(id, servo_period, n_joints, n_extrajoints, pjoints);
    retval += cia_makepins(id, n_joints);
    return retval;
}

void read_homing_in_pins(int njoints)
{
    base_read_homing_in_pins(njoints);
    cia_read_homing_in_pins(njoints);
}

void write_homing_out_pins(int njoints)
{
    base_write_homing_out_pins(njoints);
    cia_write_homing_out_pins(njoints);
}

// do_homing with CIA safety
bool do_homing(void)
{
    int joint_num;
    int homing_flag = 0;
    bool beginning_allhomed = get_allhomed();

    do_homing_sequence();
    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 (ciaH[joint_num].is_cia_homing)
            homing_flag += cia_1joint_state_machine(joint_num);
        else
            homing_flag += base_1joint_state_machine(joint_num);
    }

    homing_active = (homing_flag > 0) ? 1 : 0;

    if (!beginning_allhomed && get_allhomed())
    {
        homing_active = 0;
        return 1;
    }

    return 0;
}

//=====================================================================
// EXPORT_SYMBOLS
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);

// Clean up optional
#undef XSTR
#undef STR
#undef HOMING_BASE
#undef USE_HOMING_BASE
#undef CUSTOM_HOMEMODULE

#endif // HOMING_BASE