Advanced Search

    <slave idx="0" type="generic" vid="0000ffff" pid="00000001" configPdos="true">
      <!-- 
        Distributed Clock Configuration
        assignActivate: Activation time for DC in ms
        sync0Cycle: Sync0 cycle multiplier (*1 = 1 ms)
        sync0Shift: Phase shift for Sync0 event
      -->
      <dcConf assignActivate="300" sync0Cycle="*1" sync0Shift="0"/>
      <sdoConfig idx="6098" subIdx="00"><sdoDataRaw data ="13 00"/></sdoConfig>  <!-- Home mode 19 (use HOME-switch input)-->
      <!-- 
        Sync Manager for Output (Commands sent to drive)
        idx: Sync Manager index (2 is standard for outputs)
        dir: Direction (out = from master to slave)
      -->
      <syncManager idx="2" dir="out">
        <!-- 
          Process Data Object (PDO) for Output
          idx: PDO index (1600h is standard for TxPDO)
        -->
        <pdo idx="1600">
          <pdoEntry idx="6040" subIdx="00" bitLen="16" halPin="controlword" halType="u32"/>
          <pdoEntry idx="6060" subIdx="00" bitLen="8" halPin="opmode" halType="s32"/>
          <pdoEntry idx="607A" subIdx="00" bitLen="32" halPin="target-position" halType="s32"/>
          <pdoEntry idx="60FF" subIdx="00" bitLen="32" halPin="target-speed" halType="s32"/>
        </pdo>
      </syncManager>

      <!-- 
        Sync Manager for Input (Status received from drive)
        idx: Sync Manager index (3 is standard for inputs)
        dir: Direction (in = from slave to master)
      -->
      <syncManager idx="3" dir="in">
        <!-- 
          Process Data Object (PDO) for Input
          idx: PDO index (1A00h is standard for RxPDO)
        -->
        <pdo idx="1a00">
          <pdoEntry idx="6041" subIdx="00" bitLen="16" halPin="statusword" halType="u32"/>
          <pdoEntry idx="6061" subIdx="00" bitLen="8" halPin="opmode-display" halType="s32"/>
          <pdoEntry idx="6064" subIdx="00" bitLen="32" halPin="actual-position" halType="s32"/>
          <pdoEntry idx="606C" subIdx="00" bitLen="32" halPin="actual-velocity" halType="s32"/>
          <pdoEntry idx="6077" subIdx="00" bitLen="16" halPin="actual-torque" halType="s32"/>
          <!-- Digtial_inputs (cia402 compatible) -->
          <pdoEntry idx="60FD" subIdx="0" bitLen="32" halType="complex">
            <complexEntry bitLen="1" halPin="CW-limit" halType="bit"/>
            <complexEntry bitLen="1" halPin="CCW-limit" halType="bit"/>
            <complexEntry bitLen="1" halPin="HOME-switch" halType="bit"/>
            <complexEntry bitLen="23"/>
            <complexEntry bitLen="1" halPin="IN-4" halType="bit"/>
          </pdoEntry>

        </pdo>
      </syncManager>
    </slave>

#!/bin/bash
halcmd setp lcec.0.0.opmode 6
halcmd setp lcec.0.0.controlword 6
halcmd setp lcec.0.0.controlword 7
halcmd setp lcec.0.0.controlword 15
halcmd setp lcec.0.0.controlword 31
sleep 2
halcmd setp lcec.0.0.opmode 8
halcmd setp lcec.0.0.controlword 6
halcmd setp lcec.0.0.controlword 7
halcmd setp lcec.0.0.controlword 15
halcmd setp lcec.0.0.target-speed 1
halcmd setp lcec.0.0.target-position 4000
exit 0

loadrt [KINS]KINEMATICS
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS

loadusr -W lcec_conf ethercat-conf.xml

loadrt lcec

addf lcec.read-all            servo-thread
addf motion-command-handler   servo-thread
addf motion-controller        servo-thread
addf lcec.write-all           servo-thread

setp iocontrol.0.emc-enable-in 1

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;
}