Advanced Search

component cia402_homecomp "CIA402 homing module with safety watchdog + estop";

pin out bit is_module = 1;
pin out bit cia_estop_out;   // GLOBAL EMERGENCY STOP OUTPUT

license "GPL";
option homemod;

#include "motion.h"
#include "homing.h"

/* ---------------- SAFETY PARAMETERS ---------------- */

param rw float start_timeout_s = 2.0;
param rw float run_timeout_s   = 20.0;

/* ---------------- LOCAL DATA ---------------- */

typedef struct
{
    bool request;
    bool start;
    bool homed;
    bool error;
    bool active;
} cia_local_t;

static cia_local_t ciaH[EMCMOT_MAX_JOINTS];

static double servo_dt;

/* ---------------- HAL PINS ---------------- */

typedef struct
{
    hal_bit_t *request;
    hal_bit_t *is_active;
    hal_bit_t *start;

    hal_bit_t *homed;
    hal_bit_t *error;

} pins_t;

static pins_t *pins;

/* ---------------- PIN CREATION ---------------- */

static int cia_makepins(int id, int njoints)
{
    int r = 0;

    pins = hal_malloc(sizeof(pins_t) * njoints);
    if (!pins) return -1;

    for(int j=0;j<njoints;j++)
    {
        r += hal_pin_bit_newf(HAL_IN , &pins[j].request, id,
                             "joint.%d.request-cia-homing", j);

        r += hal_pin_bit_newf(HAL_OUT, &pins[j].is_active, id,
                             "joint.%d.is-cia-homing", j);

        r += hal_pin_bit_newf(HAL_OUT, &pins[j].start, id,
                             "joint.%d.start-cia-homing", j);

        r += hal_pin_bit_newf(HAL_IN , &pins[j].homed, id,
                             "joint.%d.cia-homed", j);

        r += hal_pin_bit_newf(HAL_IN , &pins[j].error, id,
                             "joint.%d.cia-error", j);
    }

    return r;
}

/* ---------------- IO ---------------- */

static void read_pins(int njoints)
{
    for(int j=0;j<njoints;j++)
    {
        ciaH[j].request = *pins[j].request;
        ciaH[j].homed   = *pins[j].homed;
        ciaH[j].error   = *pins[j].error;
    }
}

static void write_pins(int njoints)
{
    for(int j=0;j<njoints;j++)
    {
        *pins[j].is_active = ciaH[j].active;
        *pins[j].start     = ciaH[j].start;
    }
}

/* ---------------- GLOBAL ESTOP ---------------- */

static void global_estop(int njoints)
{
    cia_estop_out = 1;

    for(int j=0;j<njoints;j++)
    {
        ciaH[j].start  = 0;
        ciaH[j].active = 0;

        H[j].homing = 0;
        H[j].home_state = HOME_IDLE;
    }
}

/* ---------------- SAFETY STATE MACHINE ---------------- */

static int cia_1joint_sm(int j)
{
    typedef enum { IDLE, WAIT_START, RUN } st_t;

    static st_t st[EMCMOT_MAX_JOINTS];
    static double start_t[EMCMOT_MAX_JOINTS];
    static double run_t[EMCMOT_MAX_JOINTS];

    switch(st[j])
    {
        case IDLE:

            if (H[j].home_state == HOME_START && ciaH[j].request)
            {
                ciaH[j].start  = 1;
                ciaH[j].active = 1;

                H[j].homing = 1;
                H[j].homed  = 0;

                start_t[j]=0;
                run_t[j]=0;

                st[j]=WAIT_START;
            }
            break;

        case WAIT_START:

            start_t[j]+=servo_dt;

            if (ciaH[j].error)
            {
                rtapi_print_msg(RTAPI_MSG_ERR,"CIA start error joint %d\n",j);
                return -1;
            }

            if (start_t[j] > start_timeout_s)
            {
                rtapi_print_msg(RTAPI_MSG_ERR,"CIA start timeout joint %d\n",j);
                return -1;
            }

            if (!ciaH[j].request)
                st[j]=RUN;

            break;

        case RUN:

            run_t[j]+=servo_dt;

            if (ciaH[j].error)
                return -1;

            if (ciaH[j].homed)
            {
                ciaH[j].start=0;
                ciaH[j].active=0;

                H[j].homing=0;
                H[j].homed=1;
                H[j].home_state=HOME_IDLE;

                st[j]=IDLE;
                return 0;
            }

            if (run_t[j] > run_timeout_s)
            {
                rtapi_print_msg(RTAPI_MSG_ERR,"CIA run timeout joint %d\n",j);
                return -1;
            }
            break;
    }

    return 1;
}

/* ---------------- API ---------------- */

int homing_init(int id,
                double servo_period,
                int n_joints,
                int n_extrajoints,
                emcmot_joint_t *pjoints)
{
    servo_dt = servo_period;

    int r = base_homing_init(id, servo_period, n_joints, n_extrajoints, pjoints);
    r += cia_makepins(id, n_joints);

    return r;
}

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

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

bool do_homing(void)
{
    int busy=0;
    int fault=0;

    do_homing_sequence();

    for(int j=0;j<all_joints;j++)
    {
        if(!ciaH[j].request)
            busy+=base_1joint_state_machine(j);
        else
        {
            int r=cia_1joint_sm(j);

            if(r<0) fault=1;
            else busy+=r;
        }
    }

    if(fault)
    {
        global_estop(all_joints);
        return 0;
    }

    homing_active=(busy>0);
    return 0;
}