[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