EtherCAT Servo Programming in C: From Zero to Moving Axis

EtherCAT is the industrial Ethernet protocol of choice for real-time motion control. Sub-microsecond jitter, deterministic cycle times, and a standard device profile — CiA 402 — that every major servo drive vendor implements. This guide walks through a complete working example: open the network, bring the drive through the CiA 402 state machine, enable Cyclic Synchronous Position (CSP) mode, run a real-time loop, and shut down cleanly.

All code targets Linux with SOEM (Simple Open EtherCAT Master) and a PREEMPT-RT kernel.

---

Prerequisites

Before writing a single line of C, three things must be in place.

Hardware: A servo drive with an EtherCAT port (Beckhoff AX5000, Delta ASDA-A3, Panasonic MINAS A6, or similar), connected directly to a dedicated NIC on your Linux box. Do not run EtherCAT through a network switch.

Kernel: A PREEMPT-RT patched kernel. Check with uname -r — the string should contain PREEMPT_RT. Without it your cyclic thread will suffer millisecond-scale jitter that causes the drive to trip on synchronisation errors.

SOEM: Clone, build, and install:

git clone https://github.com/OpenEtherCATsociety/SOEM.git
cd SOEM
mkdir build && cd build
cmake ..
make -j$(nproc)
sudo make install

You will also need root (or CAP_NET_RAW) to open the raw socket. Running as root during development is the simplest approach.

---

Program Structure

A minimal EtherCAT servo program has three parts:

• main() — initialise SOEM, bring the network to Operational state, start the cyclic thread, then block until a signal arrives.
• cyclic_task() — a POSIX thread locked to a fixed period via clock_nanosleep. This is where process data is exchanged and position targets are written.
• signal_handler() — catches SIGINT / SIGTERM, sets a flag so the cyclic thread can exit cleanly and the drive can be disabled before the process ends.

---

Initialisation Sequence

After ec_init("eth0") opens the NIC, the following sequence scans the bus, allocates the process-data image, configures distributed clocks, and advances the network to SAFE-OP before requesting OPERATIONAL:

#include "ethercat.h"
#include <stdio.h>

char IOmap[4096];

int init_network(const char *ifname)
{
    if (!ec_init(ifname)) {
        fprintf(stderr, "ec_init failed on %s\n", ifname);
        return -1;
    }

    if (ec_config_init(FALSE) <= 0) {
        fprintf(stderr, "No slaves found\n");
        return -1;
    }
    printf("Found %d slave(s)\n", ec_slavecount);

    ec_config_map(&IOmap);
    ec_configdc();                                         /* distributed clocks */

    ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);

    ec_slave[0].state = EC_STATE_OPERATIONAL;
    ec_send_processdata();
    ec_receive_processdata(EC_TIMEOUTRET);
    ec_writestate(0);

    ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
    if (ec_slave[0].state != EC_STATE_OPERATIONAL) {
        fprintf(stderr, "Failed to reach OPERATIONAL\n");
        return -1;
    }
    return 0;
}

ec_config_map populates IOmap with the concatenated process-data image of all slaves. The drive's input and output PDOs map into this buffer at offsets stored in ec_slave[1].inputs and ec_slave[1].outputs.

---

CiA 402 State Machine Bring-Up

A freshly powered drive sits in Switch On Disabled. The CiA 402 state machine must be walked forward via SDO writes to controlword object 0x6040 before the drive will accept position commands.

#include "ethercat.h"
#include <stdint.h>

int cia402_enable(uint16_t slave)
{
    uint16_t cw;
    int size = sizeof(cw);

    /* 1. Shutdown — transitions to Ready To Switch On */
    cw = 0x0006;
    ec_SDOwrite(slave, 0x6040, 0x00, FALSE, size, &cw, EC_TIMEOUTRXM);
    osal_usleep(20000);

    /* 2. Switch On — transitions to Switched On */
    cw = 0x0007;
    ec_SDOwrite(slave, 0x6040, 0x00, FALSE, size, &cw, EC_TIMEOUTRXM);
    osal_usleep(20000);

    /* 3. Enable Operation — transitions to Operation Enabled */
    cw = 0x000F;
    ec_SDOwrite(slave, 0x6040, 0x00, FALSE, size, &cw, EC_TIMEOUTRXM);
    osal_usleep(20000);

    return 0;
}

The 20 ms delays give the drive time to settle at each state transition. In production code, poll statusword 0x6041 and check the relevant bits instead of sleeping blind.

---

Selecting Cyclic Synchronous Position Mode

Before enabling operation, write 8 to the modes-of-operation object 0x6060. This tells the drive to accept a fresh target position every network cycle rather than executing an internal motion profile.

uint8_t mode = 8;   /* CSP = 8 per CiA 402 */
ec_SDOwrite(1, 0x6060, 0x00, FALSE, sizeof(mode), &mode, EC_TIMEOUTRXM);

Verify acceptance by reading 0x6061 (modes-of-operation display) after the drive reaches Operation Enabled. It must echo 8.

---

The Real-Time Cyclic Loop

With the drive in Operation Enabled and CSP mode active, the cyclic thread takes over. Every cycle it sends the process-data frame, receives the echo, writes a new target position, and reads back the actual position.

#include "ethercat.h"
#include <time.h>
#include <stdint.h>
#include <stdatomic.h>

#define CYCLE_NS  1000000L   /* 1 ms cycle */

volatile atomic_int keep_running = 1;

void *cyclic_task(void *arg)
{
    struct timespec ts;
    int32_t target_pos = 0;
    int32_t step       = 500;   /* counts per cycle during move */
    int     wkc;

    clock_gettime(CLOCK_MONOTONIC, &ts);

    while (atomic_load(&keep_running)) {
        /* Advance timer */
        ts.tv_nsec += CYCLE_NS;
        if (ts.tv_nsec >= 1000000000L) {
            ts.tv_nsec -= 1000000000L;
            ts.tv_sec++;
        }
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, NULL);

        /* Exchange process data */
        ec_send_processdata();
        wkc = ec_receive_processdata(EC_TIMEOUTRET);

        if (wkc < ec_group[0].outputsWKC) {
            /* Working counter mismatch — log but do not abort immediately */
            continue;
        }

        /* Write target position (object 0x607A, mapped to output PDO) */
        *((int32_t *)(ec_slave[1].outputs + 4)) = target_pos;

        /* Read actual position (object 0x6064, mapped to input PDO) */
        int32_t actual = *((int32_t *)(ec_slave[1].inputs + 4));
        (void)actual;   /* use for following-error monitoring in real code */

        /* Simple open-loop trapezoidal increment */
        target_pos += step;
    }

    return NULL;
}

The byte offsets into ec_slave[1].outputs and ec_slave[1].inputs depend on the PDO mapping configured in the drive's object dictionary. Use ethercat slaves -p 1 -v or SOEM's slaveinfo example to dump the actual offsets before hardcoding them.

---

Position Units, Gear Ratios, and Encoder Resolution

EtherCAT drives report and accept positions in encoder counts. A typical 23-bit single-turn encoder gives 8,388,608 counts per revolution. With a 10:1 gearbox on the output shaft, one output revolution equals 83,886,080 counts.

Convert from user units (degrees, millimetres) before writing to the PDO:

/* Degrees to counts for a 23-bit encoder, no gearbox */
int32_t deg_to_counts(float deg)
{
    return (int32_t)(deg / 360.0f * 8388608.0f);
}

Always clamp the computed value to the drive's software position limits (objects 0x607D sub 1 and 2) to avoid hardware limit trips during commissioning.

---

Safe Shutdown

When the signal handler fires, the cyclic thread must disable operation before setting the slave back to INIT. Cutting power to the drive while it is in Operation Enabled leaves the drive in a fault state that requires a manual reset.

void shutdown_drive(uint16_t slave)
{
    uint16_t cw = 0x0007;   /* Disable Operation → Switched On */
    ec_SDOwrite(slave, 0x6040, 0x00, FALSE, sizeof(cw), &cw, EC_TIMEOUTRXM);
    osal_usleep(30000);

    cw = 0x0006;            /* Shutdown → Ready To Switch On */
    ec_SDOwrite(slave, 0x6040, 0x00, FALSE, sizeof(cw), &cw, EC_TIMEOUTRXM);
    osal_usleep(30000);

    ec_slave[0].state = EC_STATE_INIT;
    ec_writestate(0);
    ec_close();
}

---

Troubleshooting

WKC errors in every cycle. The working counter returned by ec_receive_processdata is lower than ec_group[0].outputsWKC. Most common cause: the NIC is shared with a normal network. Use a dedicated NIC and disable EEE (Energy Efficient Ethernet) with ethtool -K eth0 eee off.

Drive never reaches OPERATIONAL. Check that the drive's EtherCAT node address is 1 (or matches your ec_slave index). Some drives require a non-zero sync manager configuration before they accept OP. Read the drive's ESI file and compare the SM parameters with what SOEM negotiated (ec_slave[1].SM[2] and SM[3]).

Position following error fault. The target position is moving faster than the drive can follow. Reduce step in the cyclic loop, or increase the drive's following-error window (object 0x6065). At 1 ms cycle time and 8,388,608 counts/rev, a step of 8389 counts/cycle corresponds to exactly 1 rev/s — a safe starting point.

Drive faults on power-up. Read fault code from 0x603F via SDO and cross-reference the drive's manual. A value of 0x7500 means EtherCAT synchronisation fault — almost always a missing PREEMPT-RT kernel or cycle-time mismatch.

---

Get Your Servo Moving — Without Writing the Code Yourself

Lichi Robotics delivers complete EtherCAT servo programming solutions — SOEM integration, CiA 402 commissioning, and real-time cyclic control — for industrial machines and robotics projects across India.

👉 See EtherCAT Solutions & Pricing →

WhatsApp Us for a Free Demo — we provide complete setup support at no cost.