[etherlab-users] Problem With Maxon EPOS3

Laurent B. laurent.barbe at unistra.fr
Tue Oct 8 17:09:52 CEST 2013


Hi,
i would like to test an EPOS3 Drive from maxon with EtherCAT Master.
I have a ubuntu 12.04 with 3.2 Kernel patches with RT_PREEMPT.
I conenct the EPOS3 on ethernet port, and I got the following PDO structure 


/* Master 0, Slave 0, "EPOS3"
 * Vendor ID:       0x000000fb
 * Product code:    0x64400000
 * Revision number: 0x22100000
 */

ec_pdo_entry_info_t slave_0_pdo_entries[] = {
    {0x6040, 0x00, 16}, /* 0x6040:00 */
    {0x607a, 0x00, 32}, /* 0x607A:00 */
    {0x60b0, 0x00, 32}, /* 0x60B0:00 */
    {0x60b1, 0x00, 32}, /* 0x60B1:00 */
    {0x60b2, 0x00, 16}, /* 0x60B2:00 */
    {0x6060, 0x00, 8}, /* 0x6060:00 */
    {0x2078, 0x01, 16}, /* 0x2078:01 */
    {0x60b8, 0x00, 16}, /* 0x60B8:00 */
    {0x6041, 0x00, 16}, /* 0x6041:00 */
    {0x6064, 0x00, 32}, /* 0x6064:00 */
    {0x606c, 0x00, 32}, /* 0x606C:00 */
    {0x6077, 0x00, 16}, /* 0x6077:00 */
    {0x6061, 0x00, 8}, /* 0x6061:00 */
    {0x2071, 0x01, 16}, /* 0x2071:01 */
    {0x60b9, 0x00, 16}, /* 0x60B9:00 */
    {0x60ba, 0x00, 32}, /* 0x60BA:00 */
    {0x60bb, 0x00, 32}, /* 0x60BB:00 */
};

ec_pdo_info_t slave_0_pdos[] = {
    {0x1600, 8, slave_0_pdo_entries + 0}, /* 1st receive PDO Mapping */
    {0x1a00, 9, slave_0_pdo_entries + 8}, /* 1st transmit PDO Mapping */
};

ec_sync_info_t slave_0_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, slave_0_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, slave_0_pdos + 1, EC_WD_DISABLE},
    {0xff}
};


after i wrote a simple program in C (user space) only to Enable the drive,
but I can read PDO but I can not write to PDO.

Here after my program
#include <sys/io.h>
#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h>
#include <sys/mman.h>
#include <malloc.h>

/****************************************************************************/

#include "ecrt.h"

/****************************************************************************/

// Application parameters
#define FREQUENCY 1000
#define CLOCK_TO_USE CLOCK_REALTIME
#define MEASURE_TIMING 0

/****************************************************************************/

#define NSEC_PER_SEC (1000000000L)
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)

#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \
    (B).tv_nsec - (A).tv_nsec)

#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

/****************************************************************************/

// EtherCAT
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t *domain1 = NULL;
static ec_domain_state_t domain1_state = {};

/****************************************************************************/

// process data
static uint8_t *domain1_pd = NULL;

#define EPOS3_SlavePos  0, 0
#define Maxon_EPOS3     0x000000fb, 0x64400000

// offsets for PDO entries
static int EPOS3_0_Control_Word;
static int EPOS3_0_Target_Position;
static int EPOS3_0_Position_Offset;

static int EPOS3_0_Velocity_Offset;
static int EPOS3_0_Torque_Offset;
static int EPOS3_0_Operation_Mode;
static int EPOS3_0_Digital_Output_funct;
static int EPOS3_0_Touch_Probe_funct;


static int EPOS3_0_Status_Word;
static int EPOS3_0_Position_Actual;
static int EPOS3_0_Velocity_Actual;
static int EPOS3_0_Torque_Actual;
static int EPOS3_0_Operation_Display_Mode;
static int EPOS3_0_Digital_Input_funct;
static int EPOS3_0_Touch_Probe_Status;
static int EPOS3_0_Touch_Probe_Pos_pos;
static int EPOS3_0_Touch_Probe_Pos_neg;
const static ec_pdo_entry_reg_t domain1_regs[] =
{
    //RX_PDO
    {EPOS3_SlavePos,Maxon_EPOS3,0x6040,0,&EPOS3_0_Control_Word},
//Control_Word RW U16bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x607a,0,&EPOS3_0_Target_Position}, //Target
Position RW I32bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x60b0,0,&EPOS3_0_Position_Offset}, //Torque
Offset RW I32bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x60b1,0,&EPOS3_0_Velocity_Offset}, //Torque
Offset RW I32bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x60b2,0,&EPOS3_0_Torque_Offset}, //Torque
Offset RW I16bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x6060,0,&EPOS3_0_Operation_Mode}, //Modes
of operation RW I8bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x2078,1,&EPOS3_0_Digital_Output_funct},
//Digital output functionalities RW U16bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x60B8,0,&EPOS3_0_Touch_Probe_funct},
    //TX_PDO
    {EPOS3_SlavePos,Maxon_EPOS3,0x6041,0,&EPOS3_0_Status_Word}, //Status
word RO U16bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x6064,0,&EPOS3_0_Position_Actual},
//Position actual value RO I32bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x606c,0,&EPOS3_0_Velocity_Actual},
//Velocity actual value RO I32bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x6077,0,&EPOS3_0_Torque_Actual}, //Torque
actual value RO I16bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x6061,0,&EPOS3_0_Operation_Display_Mode},
//Modes of operation display RO I8bit
    {EPOS3_SlavePos,Maxon_EPOS3,0x2071,1,&EPOS3_0_Digital_Input_funct},
//Digital Input functionalities RO record
    {EPOS3_SlavePos,Maxon_EPOS3,0x60B9,0,&EPOS3_0_Touch_Probe_Status},
    {EPOS3_SlavePos,Maxon_EPOS3,0x60BA,0,&EPOS3_0_Touch_Probe_Pos_pos},
    {EPOS3_SlavePos,Maxon_EPOS3,0x60BB,0,&EPOS3_0_Touch_Probe_Pos_neg},
    {}
};
/* Master 0, Slave 0, "EPOS3"
 * Vendor ID:       0x000000fb
 * Product code:    0x64400000
 * Revision number: 0x22100000
 */

ec_pdo_entry_info_t EPOS3_0_pdo_entries[] = {
    {0x6040, 0x00, 16}, /* 0x6040:00 */ // control word 16
    {0x607a, 0x00, 32}, /* 0x607A:00 */ // Target position 32
    {0x60b0, 0x00, 32}, /* 0x60B0:00 */ //position offset 32
    {0x60b1, 0x00, 32}, /* 0x60B1:00 */ //velocity offset 32
    {0x60b2, 0x00, 16}, /* 0x60B2:00 */ // torque offset 16
    {0x6060, 0x00, 8}, /* 0x6060:00 */ // modes of operations 8
    {0x2078, 0x01, 16}, /* 0x2078:01 */ //digital output functionnalities
state 16
    {0x60b8, 0x00, 16}, /* 0x60B8:00 */ //16
    {0x6041, 0x00, 16}, /* 0x6041:00 */ // Status word 16
    {0x6064, 0x00, 32}, /* 0x6064:00 */ // position actual value 32
    {0x606c, 0x00, 32}, /* 0x606C:00 */ // velocity actual value 32
    {0x6077, 0x00, 16}, /* 0x6077:00 */ // torque actual value 16
    {0x6061, 0x00, 8}, /* 0x6061:00 */  // 8
    {0x2071, 0x01, 16}, /* 0x2071:01 */ // digital input functionalities
state 16
    {0x60b9, 0x00, 16}, /* 0x60B9:00 */ // 16
    {0x60ba, 0x00, 32}, /* 0x60BA:00 */ //
    {0x60bb, 0x00, 32}, /* 0x60BB:00 */
};

ec_pdo_info_t EPOS3_0_pdos[] = {
    {0x1600, 8, EPOS3_0_pdo_entries + 0}, /* 1st receive PDO Mapping */
    {0x1a00, 9, EPOS3_0_pdo_entries + 8}, /* 1st transmit PDO Mapping */
};

ec_sync_info_t EPOS3_0_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, EPOS3_0_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, EPOS3_0_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

static unsigned int counter = 0;
static unsigned int blink = 0;
static unsigned int sync_ref_counter = 0;
const struct timespec cycletime = {0, PERIOD_NS};

/*****************************************************************************/

struct timespec timespec_add(struct timespec time1, struct timespec time2)
{
    struct timespec result;

    if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
        result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
    } else {
        result.tv_sec = time1.tv_sec + time2.tv_sec;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
    }

    return result;
}

/*****************************************************************************/

void check_domain1_state(void)
{
    ec_domain_state_t ds;

    ecrt_domain_state(domain1, &ds);

    if (ds.working_counter != domain1_state.working_counter)
        printf("Domain1: WC %u.\n", ds.working_counter);
    if (ds.wc_state != domain1_state.wc_state)
        printf("Domain1: State %u.\n", ds.wc_state);

    domain1_state = ds;
}

/*****************************************************************************/

void check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master,&ms);

    if (ms.slaves_responding != master_state.slaves_responding)
        printf("master state=> %u slave(s). \n", ms.slaves_responding);

    if(ms.al_states != master_state.al_states)
    {
        switch(ms.al_states)
        {
        case EC_AL_STATE_INIT:
            printf("master state is INIT.\n");
            break;
        case EC_AL_STATE_PREOP:
            printf("master state is PREOP.\n");
            break;
        case EC_AL_STATE_SAFEOP:
            printf("master state is SAFEOP.\n");
            break;
        case EC_AL_STATE_OP:
            printf("master state is OP now.\n");
            break;
        default:
            printf("master state unknown.\n");
            break;
        }
    }
    if(ms.link_up != master_state.link_up)
        printf("master state=>Link is %s .\n", ms.link_up ? "up" : "down");

    master_state = ms;
}

/****************************************************************************/

void cyclic_task()
{
    struct timespec wakeupTime, time;
    uint16_t status, mode =0;
    // get current time
    clock_gettime(CLOCK_TO_USE, &wakeupTime);

    while(1) {
        wakeupTime = timespec_add(wakeupTime, cycletime);
        clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);

        ecrt_master_receive(master);
        // receive process data
        ecrt_domain_process(domain1);

        // check process data state (optional)
        check_domain1_state();

        if (counter)
        {
            counter--;
        }
        else
        { // do this at 1 Hz
            counter = FREQUENCY;

            // check for master state (optional)
            check_master_state();


            // calculate new process data
            blink = !blink;
        }


        // write process data
        switch(master_state.al_states)
        {
        case EC_AL_STATE_INIT:

            break;
        case EC_AL_STATE_PREOP:

            break;
        case EC_AL_STATE_SAFEOP:

            break;
        case EC_AL_STATE_OP:
            status = EC_READ_U16(domain1_pd + EPOS3_0_Status_Word);
            switch(mode)
            {
            case 0:
                if (status == 0x0140 )
                {
                    printf("EPOS3_0 Status Word = %d \n",status);
                    EC_WRITE_U16(domain1_pd+EPOS3_0_Control_Word, 0x0006);
                    mode = 1;
                }
                break;
            case 1:
                if ( status == 0x0121)
                {
                    printf("EPOS3_0 Status Word = %d \n",status);
                    EC_WRITE_U16(domain1_pd+EPOS3_0_Control_Word, 0x000F);
                    mode = 2;
                }

                break;
            default:
                break;
            }
            break;
        }

        // write application time to master
        clock_gettime(CLOCK_TO_USE, &time);
        ecrt_master_application_time(master, TIMESPEC2NS(time));

        if (sync_ref_counter)
        {
            sync_ref_counter--;
        }
        else
        {
            sync_ref_counter = 1; // sync every cycle
            ecrt_master_sync_reference_clock(master);
        }
        ecrt_master_sync_slave_clocks(master);
        // send process data
        ecrt_domain_queue(domain1);
        ecrt_master_send(master);


    }
}

/****************************************************************************/

int main(void)
{
    ec_slave_config_t *sc;

    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
    {
        perror("mlockall failed");
        return -1;
    }
    iopl(3);
    master = ecrt_request_master(0);
    if (!master)
        return -1;

    domain1 = ecrt_master_create_domain(master);
    if (!domain1)
        return -1;

    // Create configuration for bus coupler

    if (!(sc = ecrt_master_slave_config(master,
                                        EPOS3_SlavePos,
                                        Maxon_EPOS3)))
    {
        fprintf(stderr, "Failed to get slave configuration.\n");
        return -1;
    }
    sc = ecrt_master_slave_config(master, EPOS3_SlavePos, Maxon_EPOS3);
    if(!sc)
    {
        printf("Failed to get EPOS3_0 slave configuration... \n");
        return -1;
    }

    if(ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs))
    {
        printf("Failed to register PDO entries.\n");
        return -1;
    }

    printf("Activating master...\n");
    if (ecrt_master_activate(master))
        return -1;

    if (!(domain1_pd = ecrt_domain_data(domain1)))
    {
        return -1;
    }

    pid_t pid = getpid();
    if (setpriority(PRIO_PROCESS, pid, -19))
        fprintf(stderr, "Warning: Failed to set priority: %s\n",
                strerror(errno));
    printf("Starting cyclic function.\n");
    cyclic_task();

    return 0;
}

I check dmesg :
[609034.495106] EtherCAT 0: Link state of ecm0 changed to UP.
[609034.499351] EtherCAT 0: 1 slave(s) responding on main device.
[609034.499355] EtherCAT 0: Slave states on main device: INIT.
[609034.499462] EtherCAT 0: Scanning bus.
[609034.535463] EtherCAT 0: Bus scanning completed in 36 ms.
[609034.535468] EtherCAT 0: Using slave 0 as DC reference clock.
[609034.539485] EtherCAT 0: Slave states on main device: PREOP.
[609034.615408] EtherCAT: Requesting master 0...
[609034.615413] EtherCAT: Successfully requested master 0.
[609034.615498] EtherCAT 0: Domain0: Logical address 0x00000000, 46 byte,
expected working counter 3.
[609034.615502] EtherCAT 0:   Datagram domain0-0-main: Logical offset
0x00000000, 46 byte, type LRW.
[609034.619179] EtherCAT 0: Master thread exited.
[609034.619186] EtherCAT 0: Starting EtherCAT-OP thread.
[609034.619332] EtherCAT WARNING 0: 122 datagrams TIMED OUT!
[609034.724335] EtherCAT ERROR 0-0: SDO download 0x1600:03 (4 bytes) aborted.
[609034.724342] EtherCAT ERROR 0-0: SDO abort message 0x08000021: "Data
cannot be transferred or stored to the application because of local control".
[609034.724347] EtherCAT WARNING 0-0: Failed to map PDO entry 0x60B0:00 (32
bit) to position 3.
[609034.724349] EtherCAT WARNING 0-0: Currently mapped PDO entries:
0x6040:00/16 0x607A:00/32 0x60B0:00/32 0x60B1:00/32 0x60B2:00/16 0x6060:00/8
0x2078:01/16 0x60B8:00/16. Entries to map: 0x6040:00/16 0x607A:00/32
0x60B0:00/32 0x60B1:00/32 0x60B2:00/16 0x6060:00/8 0x2078:01/16 0x60B8:00/16
[609034.724381] EtherCAT WARNING 0-0: Failed to configure mapping of PDO 0x1600.
[609034.833512] EtherCAT ERROR 0-0: SDO download 0x1A00:05 (4 bytes) aborted.
[609034.833518] EtherCAT ERROR 0-0: SDO abort message 0x08000021: "Data
cannot be transferred or stored to the application because of local control".
[609034.833523] EtherCAT WARNING 0-0: Failed to map PDO entry 0x6061:00 (8
bit) to position 5.
[609034.833525] EtherCAT WARNING 0-0: Currently mapped PDO entries:
0x6041:00/16 0x6064:00/32 0x606C:00/32 0x6077:00/16 0x6061:00/8 0x2071:01/16
0x60B9:00/16 0x60BA:00/32 0x60BB:00/32. Entries to map: 0x6041:00/16
0x6064:00/32 0x606C:00/32 0x6077:00/16 0x6061:00/8 0x2071:01/16 0x60B9:00/16
0x60BA:00/32 0x60BB:00/32
[609034.833560] EtherCAT WARNING 0-0: Failed to configure mapping of PDO 0x1A00.
[609034.883167] EtherCAT 0: Domain 0: Working counter changed to 3/3.
[609034.898343] EtherCAT 0: Slave states on main device: OP.
[609036.844657] EtherCAT 0: Releasing master...
[609036.847192] EtherCAT 0: Master thread exited.
[609036.847210] EtherCAT 0: Starting EtherCAT-IDLE thread.
[609036.847337] EtherCAT 0: Released.
[609036.847373] EtherCAT 0: 0 slave(s) responding on main device.
[609036.851441] EtherCAT 0: 1 slave(s) responding on main device.
[609036.851445] EtherCAT 0: Slave states on main device: OP.
[609036.851552] EtherCAT 0: Scanning bus.
[609036.886690] EtherCAT 0: Bus scanning completed in 36 ms.
[609036.886695] EtherCAT 0: Using slave 0 as DC reference clock.
[609036.905371] EtherCAT 0: Slave states on main device: PREOP.
[609037.285909] EtherCAT WARNING: Datagram ffff8800a253c678 (master-fsm) was
SKIPPED 1 time.

Probably i do something wrong during configuration, but I don't known ?
Anybody could help me ?




More information about the etherlab-users mailing list