Skip to content

Commit 25d70b9

Browse files
hyujunhyujun
committed
feat: Introduce CiA402Device base class for motor drives and enhance Ecat_Slave with SDO and Distributed Clock configuration.
1 parent da6de53 commit 25d70b9

File tree

8 files changed

+241
-458
lines changed

8 files changed

+241
-458
lines changed

‎include/ecat/core/CiA402Device.h‎

Lines changed: 178 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,178 @@
1+
/**
2+
* @file CiA402Device.h
3+
* @brief Common Base Class for CiA 402 EtherCAT Motor Drives
4+
* @date 2026-03-27
5+
* @version 2.1.0
6+
*/
7+
8+
#pragma once
9+
10+
#include <cstdint>
11+
#include "ecat/core/Ecat_Slave.h"
12+
13+
namespace hyuEcat {
14+
15+
/**
16+
* @brief Generalized CiA 402 Device Base Class
17+
* Inherits from Slave and implements the standard CiA 402 state machine.
18+
*/
19+
class CiA402Device : public Slave
20+
{
21+
public:
22+
CiA402Device(uint32_t vendor_id, uint32_t product_id)
23+
: Slave(vendor_id, product_id) {}
24+
virtual ~CiA402Device() = default;
25+
26+
bool initialized() const noexcept { return initialized_; }
27+
bool isHoming() const noexcept { return homing_; }
28+
29+
enum ModeOfOperation : int8_t
30+
{
31+
MODE_NO_MODE = 0,
32+
MODE_PROFILED_POSITION = 1,
33+
MODE_PROFILED_VELOCITY = 3,
34+
MODE_PROFILED_TORQUE = 4,
35+
MODE_HOMING = 6,
36+
MODE_INTERPOLATED_POSITION = 7,
37+
MODE_CYCLIC_SYNC_POSITION = 8,
38+
MODE_CYCLIC_SYNC_VELEOCITY = 9,
39+
MODE_CYCLIC_SYNC_TORQUE = 10
40+
};
41+
42+
void writeTorque (int16_t torque) noexcept { target_torque_ = torque; }
43+
void writeVelocity(int32_t velocity) noexcept { target_velocity_ = velocity; }
44+
void writePosition(int32_t position) noexcept { target_position_ = position; }
45+
46+
void setModeOfOperation(ModeOfOperation mode) noexcept { mode_of_operation_ = mode; }
47+
48+
int32_t position() const noexcept { return position_; }
49+
int32_t velocity() const noexcept { return velocity_; }
50+
int16_t torque() const noexcept { return torque_; }
51+
52+
int getDeviceStateAsInt() const noexcept { return static_cast<int>(state_); }
53+
54+
const char* GetDevState() const noexcept
55+
{
56+
const int s = static_cast<int>(state_);
57+
return (s >= 0 && s < kNumStates) ? kStateStr[s] : "Unknown";
58+
}
59+
60+
protected:
61+
// ---- CiA 402 Device State Machine ----
62+
enum DeviceState : int
63+
{
64+
STATE_UNDEFINED = 0,
65+
STATE_START = 1,
66+
// 2 unused
67+
STATE_NOT_READY_TO_SWITCH_ON = 3,
68+
STATE_SWITCH_ON_DISABLED = 4,
69+
STATE_READY_TO_SWITCH_ON = 5,
70+
STATE_SWITCH_ON = 6,
71+
STATE_OPERATION_ENABLED = 7,
72+
STATE_QUICK_STOP_ACTIVE = 8,
73+
STATE_FAULT_REACTION_ACTIVE = 9,
74+
STATE_FAULT = 10,
75+
STATE_HOMING_PROGRESS = 11,
76+
STATE_HOMING_NOT_START = 12,
77+
STATE_HOMING_ATTAINED_NOT_REACHED = 13,
78+
STATE_HOMING_COMPLITE = 14,
79+
STATE_HOMING_ERROR = 15,
80+
STATE_HOMING_UNDIFINED = 16
81+
};
82+
83+
static constexpr int kNumStates = 17;
84+
static constexpr const char* kStateStr[kNumStates] = {
85+
"Undefined", // 0
86+
"Start", // 1
87+
"Undefined", // 2
88+
"Not Ready to Switch On", // 3
89+
"Switch On Disabled", // 4
90+
"Ready to Switch On", // 5
91+
"Switch On", // 6
92+
"Operation Enabled", // 7
93+
"Quick Stop Active", // 8
94+
"Fault Reaction Active", // 9
95+
"Fault", // 10
96+
"Homing Progress", // 11
97+
"Homing Not Start", // 12
98+
"Homing Attained Not Reached", // 13
99+
"Homing Finished", // 14
100+
"Homing Error", // 15
101+
"Homing Undefined" // 16
102+
};
103+
104+
DeviceState deviceState(uint16_t sw) noexcept
105+
{
106+
if ((sw & 0x4F) == 0x00) return STATE_NOT_READY_TO_SWITCH_ON;
107+
else if ((sw & 0x4F) == 0x40) return STATE_SWITCH_ON_DISABLED;
108+
else if ((sw & 0x6F) == 0x21) return STATE_READY_TO_SWITCH_ON;
109+
else if ((sw & 0x6F) == 0x23) return STATE_SWITCH_ON;
110+
else if ((sw & 0x6F) == 0x27) {
111+
if (mode_of_operation_display_ == MODE_HOMING) {
112+
const uint8_t h = static_cast<uint8_t>(sw >> 8) & 0x34u;
113+
if (h == 0x00) return STATE_HOMING_PROGRESS;
114+
else if (h == 0x04) return STATE_HOMING_NOT_START;
115+
else if (h == 0x10) return STATE_HOMING_ATTAINED_NOT_REACHED;
116+
else if (h == 0x14) { homing_ = true; return STATE_HOMING_COMPLITE; }
117+
else if (h == 0x20 || h == 0x24) return STATE_HOMING_ERROR;
118+
return STATE_HOMING_UNDIFINED;
119+
}
120+
return STATE_OPERATION_ENABLED;
121+
}
122+
else if ((sw & 0x6F) == 0x07) return STATE_QUICK_STOP_ACTIVE;
123+
else if ((sw & 0x4F) == 0x0F) return STATE_FAULT_REACTION_ACTIVE;
124+
else if ((sw & 0x4F) == 0x08) return STATE_FAULT;
125+
return STATE_UNDEFINED;
126+
}
127+
128+
uint16_t transition(DeviceState state, uint16_t cw) const noexcept
129+
{
130+
switch (state)
131+
{
132+
case STATE_START:
133+
case STATE_NOT_READY_TO_SWITCH_ON: return cw;
134+
case STATE_SWITCH_ON_DISABLED: return (cw & 0x7Eu) | 0x06u;
135+
case STATE_READY_TO_SWITCH_ON: return (cw & 0x77u) | 0x07u;
136+
case STATE_SWITCH_ON: return (cw & 0x70u) | 0x0Fu;
137+
case STATE_OPERATION_ENABLED: return 0x0Fu;
138+
case STATE_QUICK_STOP_ACTIVE: return (cw & 0x7Fu) | 0x0Fu;
139+
case STATE_FAULT_REACTION_ACTIVE: return cw;
140+
case STATE_FAULT: return cw | 0x80u;
141+
case STATE_HOMING_PROGRESS:
142+
case STATE_HOMING_ATTAINED_NOT_REACHED:
143+
case STATE_HOMING_UNDIFINED: return (cw & 0x1Fu) | 0x1Fu;
144+
case STATE_HOMING_NOT_START: return homing_ready_ ? ((cw & 0x1Fu) | 0x1Fu) : cw;
145+
case STATE_HOMING_COMPLITE:
146+
case STATE_HOMING_ERROR: return (cw & 0x0Fu) | 0x0Fu;
147+
default: return cw;
148+
}
149+
}
150+
151+
// ---- Status Tracking ----
152+
int last_status_word_ = -1;
153+
DeviceState last_state_ = STATE_START;
154+
DeviceState state_ = STATE_START;
155+
156+
bool initialized_ = false;
157+
bool homing_ = false;
158+
bool homing_ready_ = false;
159+
160+
// ---- PDO data (common for CiA 402) ----
161+
int32_t target_position_ = 0;
162+
int32_t target_velocity_ = 0;
163+
int16_t target_torque_ = 0;
164+
uint16_t max_torque_ = 1000;
165+
uint16_t control_word_ = 0;
166+
int8_t mode_of_operation_ = MODE_CYCLIC_SYNC_TORQUE;
167+
168+
int32_t position_ = 0;
169+
int32_t velocity_ = 0;
170+
int16_t torque_ = 0;
171+
uint16_t status_word_ = 0;
172+
int8_t mode_of_operation_display_ = 0;
173+
};
174+
175+
// Out-of-class definition required in C++14 for static constexpr arrays
176+
constexpr const char* CiA402Device::kStateStr[CiA402Device::kNumStates];
177+
178+
} /* namespace hyuEcat */

‎include/ecat/core/Ecat_Master.h‎

Lines changed: 0 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -73,11 +73,6 @@ class Master {
7373
*/
7474
void addSlave(uint16_t alias, uint16_t position, Slave* slave);
7575

76-
/**
77-
* @brief Add EtherCAT Elmo slaves with homing to EtherCAT master
78-
*/
79-
void addSlaveWithHoming(uint16_t alias, uint16_t position, EcatElmo* slave);
80-
8176
/**
8277
* @brief Activate the master (no Distributed Clock).
8378
* After ecrt_domain_data() is obtained, calls slave->syncDomainMap()

‎include/ecat/core/Ecat_Slave.h‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,14 @@ class Slave
8888
*/
8989
virtual void updateTx() {}
9090

91+
/**
92+
* @brief Setup initial SDO configuration.
93+
*
94+
* Called once by Master::addSlave() during pre-operational state.
95+
* Derived classes can override this to call ecrt_slave_config_sdo* functions.
96+
*/
97+
virtual void setupSDOs(ec_slave_config_t* config) {}
98+
9199
// ================================================================
92100
// Advanced stable-1.6 features (non-RT)
93101
// ================================================================
@@ -198,6 +206,14 @@ class Slave
198206
int getSlaveAlias () const noexcept { return slave_alias; }
199207
int getSlavePosition() const noexcept { return slave_position; }
200208

209+
// ================================================================
210+
// Distributed Clock (DC) configuration
211+
// ================================================================
212+
213+
bool dc_enabled_ = false;
214+
uint16_t dc_assign_activate_ = 0x0000;
215+
uint32_t dc_sync0_shift_ = 0;
216+
201217
// ================================================================
202218
// Public identifiers
203219
// ================================================================

0 commit comments

Comments
 (0)