Set the correct MapIO and SYNC DC into erob servomotor

[paul218]

Hello, dear community,
I have a problem working with these motors using EtherCAT (erob_servoMotors)
The ESI file ZeroErr Driver3_1_4.txt
The manual reference eRob CANopen and EtherCAT User Manual v1.1_Latest (1)_compressed.pdf

The behaviour is a little strange, I am a little confused about where is my mistake. If it is in mapping configuration or SYNC DC.
When I wrote the register directly with the ec_SDOwrite the motor moved and I can read the PDO TX_PDD. However, I can not write the RX_PDO when the state machine is in operational mode. I attached this version of the code . SDO_ctr.txt

First I explain my mapping process:
The manufacturer recommends this:

Then the code used to map is the next:

/* request EC_STATE_PRE_OP state for all slaves */
...

//Map RXPOD
    uint16 map_1c12[2] = {0x0001, 0x1605};
    ec_SDOwrite(SLAVE_ID, 0x1c12, 0x00, TRUE, sizeof(map_1c12), &map_1c12, EC_TIMEOUTSAFE);

//Map TXPOD
    uint16 map_1c13[2] = {0x0001, 0x1A06};
    ec_SDOwrite(SLAVE_ID, 0x1c13, 0x00, TRUE, sizeof(map_1c13), &map_1c13, EC_TIMEOUTSAFE);

The slave info after IO mapping is the next:

To get the PDO mapping I create the next structure and assigned it to the inputs, outputs ec_slave[]
However, to compensate for the aligned empty bytes mention before I assigned an int16_t variable instead of int8_ to Modes_Operation and Modes_Operation_Dsiplay.

//create a struct according to the motor datasheet
typedef struct PACKED
{
    int32_t         Target_Position;
    int32_t         Target_Velocity;
    int16_t         Target_Torque;
    int16_t         Max_Torque;
    uint16_t        Control_Word;
    int16_t         Modes_Operation;

} RX_POD_0x1605;



typedef struct PACKED
{
    uint16_t       Error_Code;
    uint16_t       Status_Word;
    int32_t        Position_Actual_Value;
    int32_t        Velocity_Actual_Value;
    int16_t        Torque_Actual_Value;
    int16_t       Modes_Operation_Dsiplay;

} TX_POD_0x1A06;

//asig the structer to a pointer
RX_POD_0x1605 * RX_POD_IO;
TX_POD_0x1A06 * TX_POD_IO;
.......
  /* connect struct pointers to slave I/O pointers */

  RX_POD_IO = (RX_POD_0x1605*) ec_slave[SLAVE_ID].outputs;
  TX_POD_IO = (TX_POD_0x1A06*) ec_slave[SLAVE_ID].inputs;

......

At this point. Is my mapping configuration correct to align the empty bytes?

Second, I did not control the motor writing the RX_PDO. I tried to check the SYNC DC
I try to check if is a problem with the synchronization of the distributed clocks. Therefore, I rewrite the code using the suggestion of #487, #520, and #482. I attached the second version of my code. PDO_control.txt

I did the next steps according to #520 , but I can't control the motor using PDO control_word mapping.
What is my mistake ?

//1.- Call ec_config_init() to move from INIT to PRE-OP state.
ec_init("enp3s0")
ec_config_init(FALSE)
_______________________________________________________________
//2.- Do slave-specific configurations via SDO communication
//I used the same code mentioned before

/* request EC_STATE_PRE_OP state for all slaves */
...
//Map RXPOD
    uint16 map_1c12[2] = {0x0001, 0x1605};
    ec_SDOwrite(SLAVE_ID, 0x1c12, 0x00, TRUE, sizeof(map_1c12), &map_1c12, EC_TIMEOUTSAFE);
//Map TXPOD
    uint16 map_1c13[2] = {0x0001, 0x1A06};
    ec_SDOwrite(SLAVE_ID, 0x1c13, 0x00, TRUE, sizeof(map_1c13), &map_1c13, EC_TIMEOUTSAFE);

//set profiles values velocity, acceleration
uint32_t val_PV= 145630; //cnt/s
ec_SDOwrite(SLAVE_ID, 0x6081, 0x00, FALSE, sizeof(val_PV), &val_PV, EC_TIMEOUTSAFE); // Profile velocity
....
_______________________________________________________________
//3.- Set ecx_context.manualstatechange = 1. Map PDOs for all slaves by calling ec_config_map(). (State EC_STATE_PRE_OP)
ecx_context.manualstatechange = 1;
char IOmap[2048];
ec_config_map(&IOmap)
_______________________________________________________________
//4.- Manually call the transition to SAFE-OP (some slaves require starting sync0 here, but this is a violation of the EtherCAT protocol spec so it is not the default)

        /* wait for all slaves to reach EC_STATE_SAFE_OP state */
        ec_slave[SLAVE_ID].state = EC_STATE_SAFE_OP;
        ec_writestate(0);
        ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE);

_______________________________________________________________
//5.- Call ec_configdc() in PRE-OP state

        /* wait for all slaves to reach EC_STATE_PRE_OP state */
        ec_slave[SLAVE_ID].state = EC_STATE_PRE_OP;
        ec_writestate(0);
        ec_statecheck(0, EC_STATE_PRE_OP,  EC_TIMEOUTSTATE);

        /* configure DC options for every DC capable slave found in the list */
        ec_configdc();
_______________________________________________________________
//6.- Do 10,000 process data cycles in SAFE-OP state

        ec_slave[SLAVE_ID].state = EC_STATE_SAFE_OP;
        ec_writestate(0);
        ec_statecheck(0, EC_STATE_SAFE_OP,  EC_TIMEOUTSTATE);

        for (int i =1; i<10000;i++)
        {
            ec_send_processdata();
            ec_receive_processdata(EC_TIMEOUTSTATE);
        }

______________________________________________________________
//7.-  start the regular PDO transfer at the proper interval

//from the ethercat_read thread
dorun = 1;
______________________________________________________________
//8.-Synchronise the slave and master clock by setting the master clock = slave reference clock

ec_configdc();
______________________________________________________________
//9.- Call ec_dcsync0() for the reference clock slave

 ec_dcsync01(1, TRUE, 5000,0,0);
_______________________________________________________________
//10.- Wait for a few seconds

usleep(2*1e6);
//set the automatic SM swtiching
ecx_context.manualstatechange = 0;
_______________________________________________________________
//11.- Transition to OP state

    ec_send_processdata();
    ec_receive_processdata(EC_TIMEOUTRET);

    /* request OP state for all slaves */
    ec_slave[SLAVE_ID].state = EC_STATE_OPERATIONAL;
    ec_writestate(0);
    ec_statecheck(0, EC_STATE_OPERATIONAL,  EC_TIMEOUTSTATE);


 if (ec_slave[SLAVE_ID].state == EC_STATE_OPERATIONAL )
{
  //        inOP = TRUE; //init check error
  //        needlf = TRUE;
}
_______________________________________________________________
//12.- //set the profile motion of motor and read variables

....

[ArthurKetels]

First thing I noticed is your cycletime, you set it to 5000. This is 5000ns or 5us. I think you intended 5ms -> 5000000ns.

Did you also implement a real time task where the PDO transfer is done? I see you set dorun=1, but is the other end also implemented? Did you check the actual transfers with Wireshark? Are there any error returns? What is the returned workcounter of the PDO transfer?

[paul218]

Hi dear @ArthurKetels thanks for your reply.
I am so sorry I check the code and the cycle time is #define SYNC0TIME 2000000 // ns
(this value is taken from the dataset reference max up to 250 us)

Therefore, the cycle time in the real-time task is: int ctime = 5*SYNC0TIME;// 1ms cycle time

int main(int argc, char **argv)
{

    needlf = FALSE;
    inOP = FALSE;
    dorun =0 ;


    struct sched_param    param;
    int                   policy = SCHED_OTHER;
    int ctime = 5*SYNC0TIME;// 1ms cycle time

    memset(&schedp, 0, sizeof(schedp));
    /* do not set priority above 49, otherwise sockets are starved */
    schedp.sched_priority = 30;
    sched_setscheduler(0, SCHED_FIFO, &schedp);
    osal_usleep(1e6);

    osal_thread_create_rt(&thread1, stack64k * 2, (void *)&ecatthread_, (void *)&ctime);
    memset(&param, 0, sizeof(param));
    /* give it higher priority */
    param.sched_priority = 40;
    pthread_setschedparam(*thread1, policy, &param);


    osal_thread_create(&thread2, stack64k * 4, (void*)&ecatcheck, NULL);

    erob_test();

    schedp.sched_priority = 0;
    sched_setscheduler(0, SCHED_OTHER, &schedp);

    printf("End program\n");

    return 0;

}

Also, I set the same cycle time in real-time task ec_sync(ec_DCtime, cycletime, &toff);

/* RT EtherCAT thread */
void ecatthread_( void *ptr )
{
   struct timespec   ts;
   struct timeval    tp;
   int ht;
   int i;
   int pcounter = 0;
   int64 cycletime;

   pthread_mutex_lock(&mutex);
   gettimeofday(&tp, NULL);

    /* Convert from timeval to timespec */
   ts.tv_sec  = tp.tv_sec;
   ht = (tp.tv_usec / 1000) + 1; /* round to nearest ms */
   ts.tv_nsec = ht * 1000000;
   //cycletime = *(int*)ptr * 1000; /* cycletime in ns */
   cycletime = *(int*)ptr * 5*SYNC0TIME; /* cycletime in ns */
   toff = 0;
   dorun = 0;
   while(1)
   {
      /* calculate next cycle start */
      add_timespec(&ts, cycletime + toff);
      /* wait to cycle start */
      pthread_cond_timedwait(&cond, &mutex, &ts);
      if (dorun>0)
      {
         gettimeofday(&tp, NULL);

         ec_send_processdata();

         ec_receive_processdata(EC_TIMEOUTRET);

         /* calulate toff to get linux time and DC synced */
         ec_sync(ec_DCtime, cycletime, &toff);
      }
   }
}

And the delay after the SYNC

/* PI calculation to get linux time synced to DC time */
void ec_sync(int64 reftime, int64 cycletime , int64 *offsettime)
{
    static int64 integral = 0;
    int64 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);
    gl_delta = delta;
}


/* add ns to timespec */
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;
    }
}

But. I can not synchronize yet. Another suggestion?
Additionally, I will check with Wireshark

[ArthurKetels]

I think you have to concept of sync0time and cycletime wrong. For 99% of the servo controllers both need to be the same. And my suggestion is to use a cycle time of 1ms and a sync0time of 1ms.

Btw, 2000000ns is 2ms. So 5*2ms is 10ms. And most likely the drive will not accept such slow cycle time.

[URALI002]

@paul218 any update on this?

[paul218]

First thing I noticed is your cycletime, you set it to 5000. This is 5000ns or 5us. I think you intended 5ms -> 5000000ns.

Did you also implement a real time task where the PDO transfer is done? I see you set dorun=1, but is the other end also implemented? Did you check the actual transfers with Wireshark? Are there any error returns? What is the returned workcounter of the PDO transfer?

dear @ArthurKetels
I set the cycletime to 1ms correctly, so now I can read the PDO values.
My workcounter is = 3
However, I can not write values on the motor. I am using the following real time task.

/* RT EtherCAT thread */
OSAL_THREAD_FUNC_RT ecatthread(void *ptr)
{
    struct timespec   ts, tleft;
    int ht;
    int64 cycletime;
    struct timeval    tp;


    clock_gettime(CLOCK_MONOTONIC, &ts);
    ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */
    ts.tv_nsec = ht * 1000000;
    if (ts.tv_nsec >= NSEC_PER_SEC) {
        ts.tv_sec++;
        ts.tv_nsec -= NSEC_PER_SEC;
    }
    cycletime = *(int*)ptr * 1000; /* cycletime in ns */

    toff = 0;
    dorun = 0;
    ec_send_processdata();
    while(1)
    {

        dorun++;
        /* calculate next cycle start */
        add_timespec(&ts, cycletime + toff);
        /* wait to cycle start */
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft);
        //pthread_cond_timedwait(&cond, &mutex, &ts);

        if(start_ecatthread_thread)
        {


            wkc = ec_receive_processdata(EC_TIMEOUTRET);

            if (ec_slave[0].hasdc)
            {
                /* calulate toff to get linux time and DC synced */
                ec_sync(ec_DCtime, cycletime, &toff);
            }

            ec_send_processdata();
        }


    }
}

Also I checked if all PDO maping are correct with "slaveinfo etho -map"

But, I can not find where is my mistake !

Could you help me ?
I atached the Wireshark file when I run the program
ethercat_.zip
and my program file
main (copy).txt
Thanks for your hepl!!!

[ArthurKetels]

In your code

       set_motor = (Motor_input*) ec_slave[0].inputs;
       read_motor = (Motor_output*) ec_slave[0].outputs;

You link the structures to the pointers of ec_slave[0], but the first slave starts at ec_slave[1]. See also the output of slaveinfo, it mentions slave 1. Always check the vendor and product code in ec_slave to know you have the correct slave. And when it does not work print out the actual pointer address to see if it points to the correct memory location in IOmap. Not rocket science.

[ZeroErrControl]

Streamline your eRob integration with our latest open-source software, drivers, and examples now available on GitHub: github.com/ZeroErrControl
Find resources for:
• ROS2 & MoveIt2
• TwinCAT3 & Python
• CAN, CANopen, EtherCAT, SOEM
• Linux & Windows
• USD Files & LLM Control (Isaac Sim)
Explore the repositories and get started today!

[ZeroErrControl]

You can refer to this link: https://github.com/ZeroErrControl/eRob_SOEM_linux/blob/main/test/linux/simple_test/eRob_test.cpp. I hope it will be helpful to you.