Giters

orocos / soem

ROS package wrapping the Simple Open EtherCAT Master (SOEM) from https://github.com/OpenEtherCATsociety/SOEM

Home Page:http://wiki.ros.org/soem

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

Send and Receive processdata Twice.

human2154 opened this issue · comments

commented

Question

Hello.
I'm a beginner in E-cat. And thank you for developing SOEM.

I use 'simpletest' for initializing slaves and a 1000Hz thread for calculating and sending-receiving E-cat data.
In a normal situation, the data is only sent at 1000us timing. But in my graph, as the picture attached, it sends at 0ms and 1000us twice.

I'll attach my code and timing graph. Please help me. Thank you.

Platinum

#include <ros/ros.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <iostream>
#include <list>
#include <signal.h>
#include <stdlib.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <sys/resource.h>
#include <sys/sem.h>
#include <sys/shm.h>
#include <sys/ipc.h>
#include <cobalt/time.h>
#include <cobalt/unistd.h>
#include <cobalt/pthread.h>
#include <fcntl.h>   // File control definitions
#include <errno.h>   // Error number definitions
#include <termios.h> // POSIX terminal control definitions
#include "utility.h"
#include "ethercat.h"
using namespace std;
/*--------------------------Variables-----------------------------*/
#pragma region globalVars
// thread EtherCAT
#define EC_TIMEOUTMON 500
#define stack64k (64 * 1024)
#define NSEC_PER_SEC 1000000000
#define NUM_AXIS_TMP 1
pthread_t thread1; // ethercat
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
char IOmap[4096] = {0}; // for XMC E-Cat
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
//----------------------------------------------------
#pragma pack(1)
typedef struct DSTRUCT_OUT     // pdo
{                              // 1c12
    int32_t target_position;   // 0x607a
    int32_t target_velocity;   // 0x60ff
    int16_t max_torque;        // 0x6072
    uint16_t control_word;     // 0x6040 pdo domain 0x1604
    int16_t target_torque;     // 0x6071
    uint8_t mode_of_operation; // 0x6060
} out_ELMOt;
typedef struct DSTRUCT_IN           // pdo
{                                   // 1c13
    int32_t position_actual;        // 0x6064
    int32_t position_follow_err;    // 0x60f4
    int16_t torque_actual;          // 0x6077
    uint16_t status_word;           // 0x6041
    uint8_t mode_of_operation_disp; //
    int32_t velocity_actual;        // 0x606C
} in_ELMOt;
#pragma pack(4)
static out_ELMOt *out_ELMO[NUM_AXIS_TMP] = {NULL};
static in_ELMOt *in_ELMO[NUM_AXIS_TMP] = {NULL};
/*----------------------------------------------------------------*/
// EtherCAT
int ELMOsetupGOLD(uint16 slave);
int ELMOsetupPlatinum(uint16 slave);
bool simpletest(char *ifname);
void add_timespec(struct timespec *ts, int64 addtime);
void ec_sync(int reftime, int cycletime, int *offsettime);
//------------------------------------------------------------
void *ecatthread(void *ptr)
{
    struct timespec ts, tleft;
    int ht;
    int cycletime;
    int toff = 0;
    struct sched_param p;
    p.sched_priority = sched_get_priority_max(SCHED_FIFO);
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &p);
    clock_gettime(CLOCK_MONOTONIC, &ts);
    ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */
    ts.tv_nsec = ht * 1000000;
    cycletime = *(int *)ptr * 1000; /* cycletime in ns */
    printf("create ecatthread! \n");
    printf("pthread priority = %d\n", p.sched_priority);
    ec_send_processdata();
    //-----------------------------------------------------------------------------------------
    while (1)
    {
        /* calculate next cycle start */
        add_timespec(&ts, cycletime + toff);
        std::cout << cycletime << " cycletime | toff " << toff << std::endl;
        /* wait to cycle start */
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft);
        ec_receive_processdata(EC_TIMEOUTRET);
        if (ec_slave[0].hasdc)
        {
            ec_sync(ec_DCtime, cycletime, &toff);
        }
        ec_send_processdata();
    }
}
// Main Thread
int main(int argc, char **argv)
{
    ros::init(argc, argv, "soem_node");
    ros::NodeHandle nh;
    char IOmap[1028] = {0};
    char ch = 0;
    int cnt = 0;
    int ctime = 1000; // us()
    char *eth_name = "rteth0";
    pthread_attr_t thread_attr;
    struct sched_param param;
    try
    {
        if (!simpletest(eth_name)) //
            throw "Failed initialization for master \n";
        if (ec_slavecount < NUM_AXIS_TMP)
            throw "Number of axis exceeded to ecat slaves \n";
        for (int i = 0; i < NUM_AXIS_TMP; i++)
        {
            out_ELMO[i] = (out_ELMOt *)ec_slave[i + 1].outputs;
            in_ELMO[i] = (in_ELMOt *)ec_slave[i + 1].inputs;
            out_ELMO[i]->target_position = in_ELMO[i]->position_actual;
        }
        pthread_attr_init(&thread_attr);
        pthread_create(&thread1, NULL, ecatthread, (void *)&ctime);
    }
    catch (const char *str)
    {
        perror(str);
        abort();
    }
    mlockall(MCL_CURRENT | MCL_FUTURE);
    while (ros::ok())
    {
        cnt = (cnt + 1) % 10;
        if (cnt == 0) //
        {
        }
        //========================================================
        usleep(50000); //
    }
    return 0;
}
/*--------------------------------------------------------------*/

int ELMOsetupGOLD(uint16 slave)
{
    int wkc = 0;
    uint32_t sdoObj = 0x00000000;
    uint8_t diable_bits = 0x00;
    uint8_t enable_bits = 0x01;
    uint16_t objAddr = 0x0000;
    uint16_t TxAddr = 0x1607;
    uint16_t RxAddr = 0x1A07;

    wkc += ec_SDOwrite(slave, TxAddr, 0x00, FALSE, sizeof(diable_bits), &(diable_bits), EC_TIMEOUTSAFE); // 0 disable
    sdoObj = 0x607A0020;                                                                                 // Target position
    wkc += ec_SDOwrite(slave, TxAddr, 0x01, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60FF0020; // Target velocity
    wkc += ec_SDOwrite(slave, TxAddr, 0x02, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60720010; // Max torque
    wkc += ec_SDOwrite(slave, TxAddr, 0x03, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60400010; // Controlword
    wkc += ec_SDOwrite(slave, TxAddr, 0x04, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60710010; // Target torque
    wkc += ec_SDOwrite(slave, TxAddr, 0x05, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60600008; // Modes of operation
    wkc += ec_SDOwrite(slave, TxAddr, 0x06, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    enable_bits = 0x06;
    wkc += ec_SDOwrite(slave, TxAddr, 0x00, FALSE, sizeof(enable_bits), &(enable_bits), EC_TIMEOUTSAFE);

    wkc += ec_SDOwrite(slave, RxAddr, 0x00, FALSE, sizeof(diable_bits), &(diable_bits), EC_TIMEOUTSAFE); // 0 disable
    sdoObj = 0x60640020;                                                                                 // Position actual value
    wkc += ec_SDOwrite(slave, RxAddr, 0x01, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60F40020; // Following error actual value
    wkc += ec_SDOwrite(slave, RxAddr, 0x02, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60770010; // Torque actual value
    wkc += ec_SDOwrite(slave, RxAddr, 0x03, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60410010; // Statusword
    wkc += ec_SDOwrite(slave, RxAddr, 0x04, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60610008; // Modes of operation display
    wkc += ec_SDOwrite(slave, RxAddr, 0x05, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x606C0020; // Additional position actual value
    wkc += ec_SDOwrite(slave, RxAddr, 0x06, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    enable_bits = 0x06;
    wkc += ec_SDOwrite(slave, RxAddr, 0x00, FALSE, sizeof(enable_bits), &(enable_bits), EC_TIMEOUTSAFE);

    // Tx PDO disable
    wkc += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(diable_bits), &(diable_bits), EC_TIMEOUTSAFE);
    // Tx PDO dictionary mapping
    objAddr = TxAddr;
    wkc += ec_SDOwrite(slave, 0x1c12, 0x01, FALSE, sizeof(objAddr), &(objAddr), EC_TIMEOUTSAFE);

    // Rx PDO disable
    wkc += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(diable_bits), &(diable_bits), EC_TIMEOUTSAFE);
    // Rx PDO dictionary mapping
    objAddr = RxAddr;
    wkc += ec_SDOwrite(slave, 0x1c13, 0x01, FALSE, sizeof(objAddr), &(objAddr), EC_TIMEOUTSAFE);

    enable_bits = 0x01;
    wkc += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(enable_bits), &(enable_bits), EC_TIMEOUTSAFE);
    wkc += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(enable_bits), &(enable_bits), EC_TIMEOUTSAFE);
    printf("wkc : %d\n", wkc);

    uint8 u8val = 8;                                                               // 8:position 9:velocity 10:torque
    ec_SDOwrite(slave, 0x6060, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); //    cyclic sychronous position mode

    printf("supported drive modes: %d\n", u8val);
}

int ELMOsetupPlatinum(uint16 slave)
{
    int wkc = 0;
    uint8_t diable_bits = 0x00;
    uint8_t enable_bits = 0x01;
    uint16_t objAddr = 0x0000;
    uint32_t sdoObj = 0x00000000;

    wkc += ec_SDOwrite(slave, 0x1601, 0x00, FALSE, sizeof(diable_bits), &(diable_bits), EC_TIMEOUTSAFE); // 0 disable
    sdoObj = 0x607A0020;                                                                                 // Target position
    wkc += ec_SDOwrite(slave, 0x1601, 0x01, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60FF0020; // Target velocity
    wkc += ec_SDOwrite(slave, 0x1601, 0x02, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60720010; // Max torque
    wkc += ec_SDOwrite(slave, 0x1601, 0x03, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60400010; // Controlword
    wkc += ec_SDOwrite(slave, 0x1601, 0x04, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60710010; // Target torque
    wkc += ec_SDOwrite(slave, 0x1601, 0x05, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60600008; // Modes of operation
    wkc += ec_SDOwrite(slave, 0x1601, 0x06, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    enable_bits = 0x06; // nums of sdo // enable
    wkc += ec_SDOwrite(slave, 0x1601, 0x00, FALSE, sizeof(enable_bits), &(enable_bits), EC_TIMEOUTSAFE);

    wkc += ec_SDOwrite(slave, 0x1A01, 0x00, FALSE, sizeof(diable_bits), &(diable_bits), EC_TIMEOUTSAFE); // 0 disable
    sdoObj = 0x60640020;                                                                                 // Position actual value
    wkc += ec_SDOwrite(slave, 0x1A01, 0x01, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60F40020; // Following error actual value
    wkc += ec_SDOwrite(slave, 0x1A01, 0x02, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60770010; // Torque actual value
    wkc += ec_SDOwrite(slave, 0x1A01, 0x03, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60410010; // Statusword
    wkc += ec_SDOwrite(slave, 0x1A01, 0x04, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x60610008; // Modes of operation display
    wkc += ec_SDOwrite(slave, 0x1A01, 0x05, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    // sdoObj = 0x30CD02;//0x36E40220 ; // Additional position actual value 0x66110020; 0x30CD0220;
    // wkc += ec_SDOwrite(slave, 0x1A01, 0x06, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    sdoObj = 0x606C0020; // Velocity actual value
    wkc += ec_SDOwrite(slave, 0x1A01, 0x06, FALSE, sizeof(sdoObj), &(sdoObj), EC_TIMEOUTSAFE);
    enable_bits = 0x06; // nums of sdo // enable
    wkc += ec_SDOwrite(slave, 0x1A01, 0x00, FALSE, sizeof(enable_bits), &(enable_bits), EC_TIMEOUTSAFE);

    // Tx PDO disable
    wkc += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(diable_bits), &(diable_bits), EC_TIMEOUTSAFE);
    // Tx PDO dictionary mapping
    objAddr = 0x1601;
    wkc += ec_SDOwrite(slave, 0x1c12, 0x01, FALSE, sizeof(objAddr), &(objAddr), EC_TIMEOUTSAFE);
    // Rx PDO disable
    wkc += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(diable_bits), &(diable_bits), EC_TIMEOUTSAFE);
    // Rx PDO dictionary mapping
    objAddr = 0x1A01;
    wkc += ec_SDOwrite(slave, 0x1c13, 0x01, FALSE, sizeof(objAddr), &(objAddr), EC_TIMEOUTSAFE);

    enable_bits = 0x01;
    wkc += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof(enable_bits), &(enable_bits), EC_TIMEOUTSAFE);
    enable_bits = 0x01;
    wkc += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof(enable_bits), &(enable_bits), EC_TIMEOUTSAFE);
    printf("wkc : %d\n", wkc);

    uint8 u8val = 8;                                                               // 8:position 9:velocity 10:torque
    ec_SDOwrite(slave, 0x6060, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM); //    cyclic sychronous position mode

    printf("supported drive modes: %d\n", u8val);
}
bool simpletest(char *ifname)
{
    int i, j, oloop, iloop, chk, slc;
    u_int mmResult;
    needlf = FALSE;
    inOP = FALSE;
    printf("Starting simple test\n");
    /* initialise SOEM, bind socket to ifname */
    if (ec_init(ifname)) // Network Interface Card NIC
    {
        printf("ec_init on %s succeeded.\n", ifname);
        /* find and auto-config slaves */
        if (ec_config_init(FALSE) > 0) // Mailbox Setup, Slave -> PRE_OP Mode, All data read and configured are stored in a global array
        {
            printf("%d slaves found and configured.\n", ec_slavecount);
            if ((ec_slavecount >= 1)) // ec_slabecount : number of slaves found on the network
            {
                for (slc = 1; slc <= ec_slavecount; slc++) // ec_slave[0] : reserved for the master
                {
                    printf("Name: %s EEpMan: %d eep_id: %d configadr: %d aliasadr: %d State %d\n\r ",
                           ec_slave[slc].name, ec_slave[slc].eep_man, ec_slave[slc].eep_id, ec_slave[slc].configadr, ec_slave[slc].aliasadr, ec_slave[slc].state);
                    if (ec_slave[slc].eep_man == 154 && ec_slave[slc].eep_id == 198948) // ELMO GOLD's man & id
                    {
                        printf("slave%d -> ELMO GOLD detected\n", slc - 1);
                        ec_slave[slc].PO2SOconfig = &ELMOsetupGOLD;
                    }
                    else if (ec_slave[slc].eep_man == 154 && ec_slave[slc].eep_id == 17825794) // ELMO PLATINUM's man & id
                    {
                        printf("slave%d -> ELMO PLATINUM detected\n", slc - 1);
                        ec_slave[slc].PO2SOconfig = &ELMOsetupPlatinum;
                        ec_slave[slc].blockLRW = 1; // Platinum does not support LRW function
                        ec_slave[0].blockLRW++;     // Platinum does not support LRW function
                    }
                    else if (ec_slave[slc].eep_man == 3138 && ec_slave[slc].eep_id == 0) // XMC's man & id
                    {
                        printf("slave%d -> XMC detected\n", slc - 1);
                    }
                }
            }
            ec_config_map(&IOmap); // 32 Map all PDOs from slaves to IOmap with Outputs / Inputs in sequential order(legacy SOEM way).
            ec_configdc();

            printf("Slaves mapped, state to SAFE_OP.\n");
            /* wait for all slaves to reach SAFE_OP state , When the mapping is done
            SOEM requests slaves to enter SAFE_OP.*/
            ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
            // Operation Mode
            oloop = ec_slave[0].Obytes;
            if ((oloop == 0) && (ec_slave[0].Obits > 0))
                oloop = 1;
            if (oloop > 8)
                oloop = 8;
            iloop = ec_slave[0].Ibytes;
            if ((iloop == 0) && (ec_slave[0].Ibits > 0))
                iloop = 1;
            if (iloop > 8)
                iloop = 8;
            printf("segments : %d : %d %d %d %d\n", ec_group[0].nsegments,
                   ec_group[0].IOsegment[0], ec_group[0].IOsegment[1], ec_group[0].IOsegment[2],
                   ec_group[0].IOsegment[3]);
            printf("Request operational state for all slaves\n");
            expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
            printf("Calculated workcounter %d\n", expectedWKC);
            ec_slave[0].state = EC_STATE_OPERATIONAL;
            ///////////////////////////////// 0 or 1
            /* send one valid process data to make outputs in slaves happy*/
            ec_send_processdata();
            ec_receive_processdata(EC_TIMEOUTRET);
            /* request OP state for all slaves */
            ec_writestate(0); ///////////////////////////////// 0 or 1
            chk = 200;
            /* wait for all slaves to reach OP state */
            do
            {
                ec_send_processdata();
                ec_receive_processdata(EC_TIMEOUTRET);
                ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
            } while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
            //////////////////////////////////////////////////
            //////////////////////////////////////////////////
            if (ec_slave[0].state == EC_STATE_OPERATIONAL)
            {
                printf("Operational state reached for all slaves.\n");
                inOP = TRUE;
                /* cyclic loop, reads data from RT thread */
                int PCL = 30; // Processdata Cycle Loop
                for (i = 1; i <= PCL; i++)
                {
                    ec_send_processdata();
                    wkc = ec_receive_processdata(EC_TIMEOUTRET);
                    if (wkc >= expectedWKC)
                    {
                        printf("Processdata cycle %4d, WKC %d , O:", i, wkc);
                        for (j = 0; j < oloop; j++)
                        {
                            printf(" %2.2x", *(ec_slave[0].outputs + j));
                        }
                        printf(" I:");
                        for (j = 0; j < iloop; j++)
                        {
                            printf(" %2.2x", *(ec_slave[0].inputs + j));
                        }
                        printf("\nOutput Byte: %d / input Byte: %d\n", oloop, iloop);
                        // printf(" T:%lld\r", ec_DCtime);
                        needlf = TRUE;
                    }
                    osal_usleep(50000);
                }
                inOP = FALSE;
            }
            else
            {
                printf("Not all slaves reached operational state.\n");
                ec_readstate();
                for (i = 1; i <= ec_slavecount; i++)
                {
                    if (ec_slave[i].state != EC_STATE_OPERATIONAL)
                    {
                        printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
                               i, ec_slave[i].state, ec_slave[i].ALstatuscode,
                               ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
                    }
                }
                return false;
            }
            printf("\nRequest init state for all slaves\n");
            ec_slave[0].state = EC_STATE_INIT;
            /* request INIT state for all slaves */
            ec_writestate(0);
        }
        else
        {
            printf("No slaves found!\n");
            return false;
        }
        // printf("End simple test\n");
    }
    else
    {
        printf("No socket connection on %s\nExcecute as root\n", ifname);
        return false;
    }
    return true;
}
void add_timespec(struct timespec *ts, int64 addtime)
{
    int64 sec, nsec;
    nsec = addtime % NSEC_PER_SEC;
    sec = (addtime - nsec) / NSEC_PER_SEC;
    ts->tv_sec += sec;
    ts->tv_nsec += nsec;
    if (ts->tv_nsec > NSEC_PER_SEC)
    {
        nsec = ts->tv_nsec % NSEC_PER_SEC;
        ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
        ts->tv_nsec = nsec;
    }
}
/* PI calculation to get linux time synced to DC time */
void ec_sync(int reftime, int cycletime, int *offsettime)
{
    static int integral = 0;
    static int delta;
    /* set linux sync point 50us later than DC sync, just as example */
    delta = (reftime - 50000) % cycletime;
    if (delta > (cycletime / 2))
    {
        delta = delta - cycletime;
    }
    if (delta > 0)
    {
        integral++;
    }
    if (delta < 0)
    {
        integral--;
    }
    *offsettime = -(delta / 100) - (integral / 20); // 100 20
    // gl_delta = delta;
}

soem_ros_Twice_error_code.pdf

ROS Distro (please check as appropriate):

  • [O] Kinetic
  • Melodic
  • Noetic
  • Other: ...

SOEM ROS Wrapper version (please check and complete as appropriate):

  • [O ] from apt: 1.4.1
  • [] from source: 1.?.?
  • Commit SHA:

Hardware setup
Describe exactly which type of hardware setup you have (i.e. which ethercat slaves)

  • Numbers of ELMO twitter Platinum Motor Controller
commented

SOEM (Simple Open EtherCAT Master)
Slaveinfo
Starting slaveinfo
ec_init on rteth0 succeeded.
1 slaves found and configured.
Calculated workcounter 3

Slave:1
Name:ModuleSlotsDrive
Output size: 48bits
Input size: 48bits
State: 4
Delay: 0[ns]
Has DC: 1
DCParentport:0
Activeports:1.0.0.0
Configured address: 1001
Man: 0000009a ID: 01100002 Rev: 00120025
SM0 A:1000 L: 256 F:00010026 Type:1
SM1 A:1400 L: 256 F:00010022 Type:2
SM2 A:1800 L: 6 F:00010064 Type:3
SM3 A:1c00 L: 6 F:00010020 Type:4
FMMU0 Ls:00000000 Ll: 6 Lsb:0 Leb:7 Ps:1800 Psb:0 Ty:02 Act:01
FMMU1 Ls:00000006 Ll: 6 Lsb:0 Leb:7 Ps:1c00 Psb:0 Ty:01 Act:01
FMMUfunc 0:1 1:2 2:3 3:0
MBX length wr: 256 rd: 256 MBX protocols : 0e
CoE details: 2f FoE details: 01 EoE details: 01 SoE details: 00
Ebus current: 0[mA]
only LRD/LWR:0
CoE Object Description found, 492 entries.
Index: 1000 Datatype: 0007 Objectcode: 07 Name: Device type
Sub: 00 Datatype: 0007 Bitlength: 0020 Obj.access: 0007 Name: Device type
Value :0x00020192 131474

This is my slave info.

  • First. when I do
    ec_slave[slc].blockLRW = 1;
    It can read and write PDOs but send twice

  • And is there any data about CoEdetails 8bit information???
    ec_slave[slc].CoEdetails
    when I change this data 0x2F to 0x3F and turn off LRW, It communicates once but I cannot read and write PDOs.

commented

Hi @human2154 ,

Having a quick glance at your code, it seems you are calling ec_send_processdata twice in your ecatthread method. Once directly before the infinite while loop, and once at the end after the clock_nanosleep call.
So I would say that is expected behavior?

If not, please state more clearly what you are actually trying to do and what is called where. I'll not go over 14 pages of code without more information ;-)

About the general SOEM questions, I suggest to ask those at the upstream repo where SOEM actually is developed. They don't seem to be ROS related. Maybe try to create a non-ROS minimal example. This should help with getting those resolved...

commented

First. Thank you for your reply.

  • In ethercatthread there are two ec_send_processdata();. The first one runs just once when initializing the thread. And in while loop, there's a second ec_send_processdata(); function that runs repeatedly at 1kHz. When I delete the first one, there was no difference.

  • What I really want to do is my Ethercat slave device read and write just once in the loop. The below picture is what I really want Timing graph. It just has single lines near the 1ms. When I check WKC in while loop. The WKC is 3 (I use just 1 slave).
    Gold No DC

  • I thought I have a mistake in initializing my slave. So I tried to below things. And I checked the WKC too.

  1. I did not run PDO mapping ec_slave[slc].PO2SOconfig = &ELMOsetupPlatinum; but the result is same. And WKC was 3.
  2. I did not run LRW options ec_slave[slc].blockLRW = 1; So I cant config IO map.
  3. I changed ec_slave[slc].CoEdetails 0x2F to 0x3F and did not run LRW options ec_slave[slc].blockLRW = 1;. It communicate once. But, the input output data was empty. And WKC was 2.

I really weird because I delete most of the codes, but the results are always the same....
Thank you for all your kindness.

commented

Well with that bunch of code it is really hard to debug without having access to the hardware. Can you provide a minimal example (best, a single main function) that still shows the issue? With what program are you creating the graphs? Are you sure you are measuring correct?

I cannot help much with the issues your having and, again, IMO they are not ROS-related. So I suggest to use the original SOEM issue tracker and provide them with a non-ROS minimal example. They are so much more familiar with this than me. So they might be able to pinpoint the issue right away....

commented

Thank you.

In this program the ethercat thread below is the only read and write code.
The other code is just for initialize.
And I check this EtherCAT timing with the 'Hilsher net Analyzer tool' and it works well.

void *ecatthread(void *ptr)
{
    struct timespec ts, tleft;
    int ht;
    int cycletime;
    int toff = 0;
    struct sched_param p;
    p.sched_priority = sched_get_priority_max(SCHED_FIFO);
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &p);
    clock_gettime(CLOCK_MONOTONIC, &ts);
    ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */
    ts.tv_nsec = ht * 1000000;
    cycletime = *(int *)ptr * 1000; /* cycletime in ns */
    printf("create ecatthread! \n");
    printf("pthread priority = %d\n", p.sched_priority);
    ec_send_processdata();
    //-----------------------------------------------------------------------------------------
    while (1)
    {
        /* calculate next cycle start */
        add_timespec(&ts, cycletime + toff);
        std::cout << cycletime << " cycletime | toff " << toff << std::endl;
        /* wait to cycle start */
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft);
        ec_receive_processdata(EC_TIMEOUTRET);
        if (ec_slave[0].hasdc)
        {
            ec_sync(ec_DCtime, cycletime, &toff);
        }
        ec_send_processdata();
    }
}

Thanks to recommend me to move this issue at the original SOEM git.
So, removing this issue is better for you? Or, just leave it?

commented

I suggest you create the issue upstream, and we can close this one here and link against the upstream issue.

commented

Thank you.

With your help, I solved some problems and write a new issue at original SOEM
Here is the new link. Thank you.

OpenEtherCATsociety/SOEM#597