Orders
EtherCAT gives you the bus. SOEM gives you the master stack. What ties everything together into a working machine is the C++ layer that sits above both — the code that plans trajectories, issues setpoints every millisecond, detects faults, and keeps the axes moving without hesitation.
This post walks through building that layer from scratch: wrapping SOEM in a clean C++ class, setting up a real-time cyclic thread, implementing trapezoidal and S-curve profiles, and wiring the whole thing into ROS 2 if you need it.
Why C++ for Motion Control
Python is fine for configuration, visualization, and offline trajectory math. It is not fine for a 1 kHz control loop. The GIL, garbage collection pauses, and interpreter overhead introduce latency spikes that are unpredictable and unacceptable in a real-time context.
C++ gives you three things that matter here:
- Deterministic execution. No garbage collector, no interpreter. Memory is managed explicitly, and cycle time jitter stays in the microsecond range on a properly tuned system.
- Object-oriented drive abstraction. Each axis becomes a
MotionAxisobject. The cyclic loop iterates over a vector of axes and callsupdate()on each. Adding a fourth axis does not change the loop logic. - Direct SOEM integration. SOEM is a C library. Calling it from C++ is zero-overhead and gives you full control over the IOmap layout, slave addressing, and process data casting.
Wrapping SOEM in a C++ Class
SOEM's API is flat C. Wrapping it in a class gives you RAII cleanup, encapsulated state, and a surface area you can mock in unit tests.
class EtherCATMaster {
public:
explicit EtherCATMaster(const std::string& ifname);
~EtherCATMaster();
bool init();
bool startCyclicThread(void* (*threadFunc)(void*), void* arg);
void shutdown();
uint8_t* iomap() { return iomap_; }
int slaveCount() const { return ec_slavecount; }
private:
std::string ifname_;
uint8_t iomap_[4096];
pthread_t cyclic_thread_;
};
bool EtherCATMaster::init() {
if (!ec_init(ifname_.c_str())) return false;
if (ec_config_init(FALSE) <= 0) return false;
ec_config_map(iomap_);
ec_configdc();
ec_writestate(EC_STATE_OPERATIONAL);
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE * 4);
return ec_slave[0].state == EC_STATE_OPERATIONAL;
}
bool EtherCATMaster::startCyclicThread(void* (*fn)(void*), void* arg) {
pthread_attr_t attr;
struct sched_param param{};
pthread_attr_init(&attr);
pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
param.sched_priority = 80;
pthread_attr_setschedparam(&attr, ¶m);
pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
int rc = pthread_create(&cyclic_thread_, &attr, fn, arg);
pthread_attr_destroy(&attr);
return rc == 0;
}
void EtherCATMaster::shutdown() {
ec_slave[0].state = EC_STATE_INIT;
ec_writestate(0);
ec_close();
}
Keep ec_send_processdata() and ec_receive_processdata() out of this class. They belong in the cyclic thread function, not in the master itself.
Real-Time Thread Architecture
A production motion controller runs two threads with clearly separated responsibilities.
Main thread — non-real-time. Parses operator commands, runs the trajectory planner, writes target setpoints into a shared data structure, and handles logging. This thread can block, allocate memory, and do I/O freely.
Cyclic thread — real-time, SCHED_FIFO, priority 80. Runs at exactly 1 kHz (1 ms period). Sends process data to slaves, waits for the response, reads encoder feedback, applies the latest setpoints, checks for faults, and writes new commands. This thread must never block on a mutex, allocate heap memory, or call any function that might sleep.
void* cyclicThread(void* arg) {
auto* master = static_cast<EtherCATMaster*>(arg);
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
const long period_ns = 1'000'000; // 1 ms
while (running.load(std::memory_order_relaxed)) {
ts.tv_nsec += period_ns;
if (ts.tv_nsec >= 1'000'000'000L) {
ts.tv_nsec -= 1'000'000'000L;
ts.tv_sec++;
}
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, nullptr);
ec_send_processdata();
int wkc = ec_receive_processdata(EC_TIMEOUTRET);
processAxes(master->iomap(), wkc);
}
return nullptr;
}
Setting SCHED_FIFO at priority 80 places the cyclic thread above almost every other process on the system. On a PREEMPT-RT kernel, worst-case wakeup latency with this setup is typically under 50 µs.
Trapezoidal Velocity Profile
A trapezoidal profile divides a point-to-point move into three phases: acceleration, constant velocity, and deceleration. It is the standard choice for industrial axes because it is computationally trivial and easy to tune.
Given a move of distance d, maximum velocity v_max, and maximum acceleration a_max:
t_acc = v_max / a_max
d_acc = 0.5 * a_max * t_acc² (distance covered while accelerating)
d_flat = d - 2 * d_acc (distance at constant velocity)
t_flat = d_flat / v_max
t_total = 2 * t_acc + t_flat
If d < 2 * d_acc, the axis never reaches v_max — it must be a triangular profile with a reduced peak velocity: v_peak = sqrt(a_max * d).
struct TrapProfile {
double v_max, a_max;
double t_acc, t_flat, t_total;
double d_acc, d_start;
void plan(double distance) {
d_start = 0.0;
t_acc = v_max / a_max;
d_acc = 0.5 * a_max * t_acc * t_acc;
if (distance < 2.0 * d_acc) { // triangular
double v_peak = std::sqrt(a_max * distance);
t_acc = v_peak / a_max;
d_acc = 0.5 * a_max * t_acc * t_acc;
t_flat = 0.0;
} else {
t_flat = (distance - 2.0 * d_acc) / v_max;
}
t_total = 2.0 * t_acc + t_flat;
}
double positionAt(double t) const {
if (t < t_acc)
return 0.5 * a_max * t * t;
if (t < t_acc + t_flat)
return d_acc + v_max * (t - t_acc);
double t_dec = t - t_acc - t_flat;
return d_acc + v_max * t_flat + v_max * t_dec - 0.5 * a_max * t_dec * t_dec;
}
};
Call positionAt(elapsed) each millisecond in the cyclic thread to get the commanded position. The cyclic thread only does arithmetic — no dynamic memory, no branches on mode.
S-Curve Profile
A trapezoidal profile has instantaneous changes in acceleration at the phase boundaries. That sudden jerk excites mechanical resonances, especially on light-frame machines and long ballscrews.
An S-curve profile adds a seventh segment that limits jerk — the rate of change of acceleration — to a finite value j_max. Instead of snapping to a_max, the acceleration ramps up smoothly over a time t_j = a_max / j_max.
The planning math is more involved (seven time intervals instead of three), but the payoff is significant: smoother motion, lower vibration at the end of a move, and less wear on mechanical components. For axes where settling time matters — pick-and-place, dispensing, scanning — S-curves are worth the added complexity.
The MotionAxis Class
Each drive on the bus becomes an instance of MotionAxis. The class owns the drive's byte offsets into the IOmap, the current trajectory, and the control mode.
class MotionAxis {
public:
MotionAxis(int slave_idx, uint16_t* controlword_ptr,
int32_t* target_pos_ptr, int32_t* actual_pos_ptr,
uint16_t* statusword_ptr);
void moveTo(double target_counts);
void setVelocity(double vel_counts_per_sec);
void stop();
double getActualPosition() const;
void update(double dt_sec); // called from cyclic thread
private:
int slave_idx_;
uint16_t* controlword_;
int32_t* target_pos_;
int32_t* actual_pos_;
uint16_t* statusword_;
TrapProfile profile_;
double move_origin_ = 0.0;
double move_target_ = 0.0;
double elapsed_sec_ = 0.0;
bool moving_ = false;
};
moveTo() calls profile_.plan(distance) and sets moving_ = true. Each call to update() increments elapsed_sec_ by dt_sec, evaluates positionAt(), and writes the result into target_pos_. The cyclic thread calls update(0.001) on every axis in sequence before sending process data.
Reading and Writing Process Data
SOEM maps all slave I/O into a flat byte array — iomap_. Each slave's outputs and inputs sit at fixed offsets determined during ec_config_map(). You cast pointers into that array to get typed access:
// Outputs (master -> drive)
auto* ctrl = reinterpret_cast<uint16_t*>(ec_slave[idx].outputs + 0);
auto* modes = reinterpret_cast<int8_t*> (ec_slave[idx].outputs + 2);
auto* tgt = reinterpret_cast<int32_t*> (ec_slave[idx].outputs + 4);
// Inputs (drive -> master)
auto* status = reinterpret_cast<uint16_t*>(ec_slave[idx].inputs + 0);
auto* actual = reinterpret_cast<int32_t*> (ec_slave[idx].inputs + 4);
Byte offsets depend on your drive's PDO mapping. Read them from the EtherCAT slave information (ESI) XML or confirm them with ethercat slaves -v from the IgH master tools.
Error Handling in Real-Time
The cyclic thread must handle errors without blocking or logging. Two failure modes matter most.
WKC mismatch. The working counter returned by ec_receive_processdata() must equal the expected value (ec_group[0].outputsWKC * 3 + ec_group[0].inputsWKC). A mismatch means a slave did not respond. Increment a counter; if it exceeds a threshold, command all axes to a controlled stop and set an error flag for the main thread to read.
Drive fault. Bit 3 of the statusword is the Fault bit per CiA 402. Check it every cycle:
void checkFault(uint16_t statusword, uint16_t* controlword) {
if (statusword & (1 << 3)) { // Fault bit set
// Fault reset: write 0x0080 then clear it
*controlword = 0x0080;
fault_reset_pending_ = true;
}
if (fault_reset_pending_) {
*controlword &= ~0x0080; // clear reset bit
fault_reset_pending_ = false;
}
}
Do not attempt to resume motion after a fault reset without first checking that the drive returned to the Switch On Disabled state. Set a flag; let the main thread decide whether to re-home and continue or halt the machine.
Logging from the Non-RT Thread
The cyclic thread must never call printf, std::cout, or any logging framework that acquires a lock. Use a lock-free single-producer single-consumer ring buffer: the cyclic thread writes diagnostic samples (timestamp, position, velocity, statusword) into the ring; the main thread drains it and writes to disk or publishes to a monitoring topic.
A fixed-size ring buffer of 4096 entries at 1 kHz gives the main thread four seconds of breathing room before data is overwritten. This is enough for any realistic main-thread scheduling hiccup.
Integrating with ROS 2
If your application uses ROS 2, the ros2_control hardware interface is the right integration point. Implement hardware_interface::SystemInterface and delegate to your EtherCATMaster and MotionAxis objects:
hardware_interface::return_type
LichiHardware::read(const rclcpp::Time&, const rclcpp::Duration&) {
for (size_t i = 0; i < axes_.size(); ++i)
hw_positions_[i] = axes_[i].getActualPosition() / counts_per_rad_[i];
return hardware_interface::return_type::OK;
}
hardware_interface::return_type
LichiHardware::write(const rclcpp::Time&, const rclcpp::Duration&) {
for (size_t i = 0; i < axes_.size(); ++i)
axes_[i].moveTo(hw_commands_[i] * counts_per_rad_[i]);
return hardware_interface::return_type::OK;
}
The read() and write() calls happen in the controller_manager update loop. Keep them non-blocking — just copy values in and out of the shared state that your cyclic thread owns. The EtherCAT cyclic thread runs independently at 1 kHz regardless of the ROS 2 update rate.
Production-Ready EtherCAT Motion Control for Your Machine
Lichi Robotics develops complete C++ EtherCAT motion control systems — from trajectory planning to servo commissioning — for industrial robots, CNC machines, and automated systems across India.
WhatsApp Us — free demo with complete technical setup support included.