跳到主要内容

CiA 402 -- 伺服驱动器

CiA 402 (IEC 61800-7-204) 是基于 CoE 的伺服驱动器设备协议。

备注

CiA 402 运行在 CoE (SDO) 之上，仅当从站支持 CoE 时可用。

状态枚举​

typedef enum {
    CIA402_NOT_READY          = 0,   /* 未就绪 */
    CIA402_SWITCH_ON_DISABLED = 1,   /* 开关禁用 */
    CIA402_READY_TO_SWITCH_ON = 2,   /* 准备开关 */
    CIA402_SWITCHED_ON        = 3,   /* 已开关 */
    CIA402_OPERATION_ENABLED  = 4,   /* 运行使能 */
    CIA402_QUICK_STOP_ACTIVE  = 5,   /* 快速停止 */
    CIA402_FAULT_REACTION     = 6,   /* 故障响应 */
    CIA402_FAULT              = 7,   /* 故障 */
    CIA402_UNKNOWN            = 99   /* 未知 */
} cia402_state_t;

操作模式枚举​

typedef enum {
    CIA402_PP  = 1,    /* 轮廓位置模式 */
    CIA402_VL  = 2,    /* 速度模式 */
    CIA402_PV  = 3,    /* 轮廓速度模式 */
    CIA402_PT  = 4,    /* 轮廓转矩模式 */
    CIA402_HM  = 6,    /* 回零模式 */
    CIA402_IP  = 7,    /* 插补位置模式 */
    CIA402_CSP = 8,    /* 周期同步位置模式 */
    CIA402_CSV = 9,    /* 周期同步速度模式 */
    CIA402_CST = 10    /* 周期同步转矩模式 */
} cia402_mode_t;

PDO 初始化​

CiA402_InitializePdoOffsets()​

int CiA402_InitializePdoOffsets(uint16_t master, uint16_t slave);

预扫描 CiA 402 标准对象（ControlWord/StatusWord/PositionActual 等）在 IOmap 中的字节偏移并缓存。OP 状态后调用一次，后续所有 CiA402_* 函数走零拷贝路径，无需每次重新扫描。等价于 cia402_scan_pdo() 的封装。

状态控制​

CiA402_Enable()​

int CiA402_Enable(uint16_t master, uint16_t slave, int max_retries);

一步使能。DLL 内部自动执行完整的状态链转换（故障清除 -> Shutdown -> Switch On -> Enable Operation）。

参数:

  • master (uint16_t) — 主站索引
  • slave (uint16_t) — 从站索引
  • max_retries (int) — 最大重试次数

返回值:

  • int — 0 成功，非 0 失败

CiA402_Disable()​

int CiA402_Disable(uint16_t master, uint16_t slave);
int CiA402_DisableOperation(uint16_t master, uint16_t slave);
int CiA402_QuickStop(uint16_t master, uint16_t slave);
  • CiA402_Disable() 完全失能（写 ControlWord = 0x0000，禁用电压）
  • CiA402_DisableOperation() 仅退出 Operation Enabled 回到 Switched On（保电不输出力矩）
  • CiA402_QuickStop() 触发 Quick Stop（Controlword Bit2=0），按 0x605A 选项码减速停止

CiA402_FaultReset()​

int CiA402_FaultReset(uint16_t master, uint16_t slave);

故障复位。自动产生 ControlWord Bit 7 上升沿，确保故障清除可靠完成。

参数:

  • master (uint16_t) — 主站索引
  • slave (uint16_t) — 从站索引

返回值:

  • int — 0 成功，非 0 失败

CiA402_WriteControlWord()​

int CiA402_WriteControlWord(uint16_t master, uint16_t slave, uint16_t cw);

写入控制字 (0x6040)。

参数:

  • cw (uint16_t) — 控制字值

返回值:

  • int — 0 成功，非 0 失败

状态读取​

StateDrive​

由 CiA402_ParseState() 返回的当前驱动器状态枚举值（cia402_state_t），等价于 C# 的 slave.CoE.CiA402.StateDrive。典型流程: 读 StatusWord → ParseState → 判断当前是否处于 OPERATION_ENABLED / FAULT 等。

Statusword​

通过 CiA402_ReadStatusWord() 读 0x6041 实时状态字。位含义见 状态字位掩码常量。Bit10 (TargetReached)、Bit12 (Set-point Ack / HomingAttained)、Bit13 (Following Error / HomingError) 是 PP / HM 握手关键。

Controlword​

通过 CiA402_WriteControlWord() 写 0x6040 控制字。Bit4 (NewSetpoint / StartHoming)、Bit6 (PP 相对/绝对)、Bit8 (Halt) 是常用应用层位。

CiA402_ReadStatusWord()​

uint16_t CiA402_ReadStatusWord(uint16_t master, uint16_t slave);

读取状态字 (0x6041)。

返回值:

  • uint16_t — StatusWord 值

CiA402_ParseState()​

cia402_state_t CiA402_ParseState(uint16_t statusword);

从 StatusWord 解析当前驱动器状态。

参数:

  • statusword (uint16_t) — StatusWord 值

返回值:

  • cia402_state_t — 驱动器状态

CiA402_GetEnableCommand()​

uint16_t CiA402_GetEnableCommand(uint16_t statusword);

根据当前 StatusWord 计算下一步使能所需的 ControlWord。

参数:

  • statusword (uint16_t) — StatusWord 值

返回值:

  • uint16_t — 应写入的 ControlWord 值

操作模式​

CiA402_SetMode()​

int CiA402_SetMode(uint16_t master, uint16_t slave, int8_t mode);

设置操作模式 (0x6060)。

参数:

  • mode (int8_t) — 操作模式 (cia402_mode_t)

返回值:

  • int — 0 成功，非 0 失败

CiA402_GetMode()​

int8_t CiA402_GetMode(uint16_t master, uint16_t slave);

读取当前操作模式 (0x6061)。

返回值:

  • int8_t — 操作模式 (cia402_mode_t)

PDO 映射扫描 (ethercat_advanced.h)​

cia402_pdo_map_t​

typedef struct {
    int controlword;        /* 0x6040 RxPDO 字节偏移 (-1=未映射) */
    int target_position;    /* 0x607A */
    int target_velocity;    /* 0x60FF */
    int target_torque;      /* 0x6071 */
    int modes_of_operation; /* 0x6060 */
    int statusword;         /* 0x6041 TxPDO 字节偏移 (-1=未映射) */
    int position_actual;    /* 0x6064 */
    int velocity_actual;    /* 0x606C */
    int torque_actual;      /* 0x6077 */
    int modes_display;      /* 0x6061 */
} cia402_pdo_map_t;

cia402_scan_pdo()​

int cia402_scan_pdo(dll_t* dll, uint16_t master, uint16_t slave,
                           cia402_pdo_map_t* map);

扫描 CiA 402 PDO 映射，自动定位各对象在 IOmap 中的字节偏移。

参数:

  • dll (dll_t*) — DLL 实例
  • master (uint16_t) — 主站索引
  • slave (uint16_t) — 从站索引
  • map (cia402_pdo_map_t*) — 输出映射信息

返回值:

  • int — 0 成功，非 0 失败

PDO 映射便捷读写​

int cia402_read_statusword(dll_t* dll, uint16_t master, uint16_t slave,
                                  const cia402_pdo_map_t* map, uint16_t* out);
int cia402_write_controlword(dll_t* dll, uint16_t master, uint16_t slave,
                                    const cia402_pdo_map_t* map, uint16_t val);
int cia402_read_position(dll_t* dll, uint16_t master, uint16_t slave,
                                const cia402_pdo_map_t* map, int32_t* out);
int cia402_write_target_position(dll_t* dll, uint16_t master, uint16_t slave,
                                        const cia402_pdo_map_t* map, int32_t val);
int cia402_read_velocity(dll_t* dll, uint16_t master, uint16_t slave,
                                const cia402_pdo_map_t* map, int32_t* out);
int cia402_write_target_velocity(dll_t* dll, uint16_t master, uint16_t slave,
                                        const cia402_pdo_map_t* map, int32_t val);
int cia402_read_torque(dll_t* dll, uint16_t master, uint16_t slave,
                              const cia402_pdo_map_t* map, int16_t* out);
int cia402_write_target_torque(dll_t* dll, uint16_t master, uint16_t slave,
                                      const cia402_pdo_map_t* map, int16_t val);

所有函数返回 0 成功，非 0 失败。

示例:

#include "ethercat_advanced.h"

/* 扫描 PDO 映射 */
cia402_pdo_map_t map;
if (cia402_scan_pdo(&dll, master, 1, &map) == 0) {
    printf("StatusWord 偏移: %d\n", map.statusword);
    printf("ControlWord 偏移: %d\n", map.controlword);
    printf("实际位置偏移: %d\n", map.position_actual);
    printf("目标位置偏移: %d\n", map.target_position);

    /* 通过映射读取状态字 */
    uint16_t sw;
    cia402_read_statusword(&dll, master, 1, &map, &sw);
    printf("StatusWord: 0x%04X\n", sw);

    /* 写入目标位置 */
    cia402_write_target_position(&dll, master, 1, &map, 100000);
}

运动参数 (SDO 访问)​

以下参数通过 SDO 读写，适合参数配置。高频控制请使用 PDO 映射。

对象函数模式类型说明
0x6064SDOread(master, slave, 0x6064, 0)int32实际位置
0x606CSDOread(master, slave, 0x606C, 0)int32实际速度
0x6077SDOread(master, slave, 0x6077, 0)int16实际转矩（千分之额定）
0x607ASDOwrite(master, slave, 0x607A, 0)int32目标位置
0x60FFSDOwrite(master, slave, 0x60FF, 0)int32目标速度
0x6071SDOwrite(master, slave, 0x6071, 0)int16目标转矩
0x6081SDOwriteuint32轮廓速度
0x6083SDOwriteuint32轮廓加速度
0x6084SDOwriteuint32轮廓减速度
0x6085SDOwriteuint32快速停止减速度
0x607ESDOwriteuint8极性（Bit6=速度反转, Bit7=位置反转）
0x6086SDOwriteint16运动轮廓类型（0=梯形, 1=S形）
0x605ASDOwriteint16快速停止选项码
0x6072SDOwriteuint16最大转矩（千分之额定）
0x6076SDOwriteuint32电机额定转矩（mNm）
0x60B0SDOwriteint32位置偏移（CSP 模式叠加）
0x60B1SDOwriteint32速度偏移（CSV 模式叠加）
0x60C2:01SDOwriteuint8插补时间周期值
0x60C2:02SDOwriteint8插补时间周期指数（周期 = 值 × 10^指数 秒）
0x60E0SDOwriteuint16正方向力矩限制（千分之额定）
0x60E1SDOwriteuint16负方向力矩限制（千分之额定）

回零参数​

对象类型说明
0x6098int8回零方法
0x607Cint32回零偏移
0x6099:01uint32回零搜索速度
0x6099:02uint32回零爬行速度
0x609Auint32回零加速度

触探功能 (Touch Probe)​

ConfigureTouchProbe()​

/* 通过 SDO 写 0x60B8 配置 Touch Probe 功能字 */
uint16_t fn = 0x0001;  /* Bit0=启用探针1, Bit1=边沿选择 */
SDOwrite(master, slave, 0x60B8, 0, FALSE, sizeof(fn), &fn, EC_TIMEOUTRXM);
对象类型说明
0x60B8uint16Touch Probe 功能控制（Bit0=启用, Bit1=边沿选择）
0x60B9uint16Touch Probe 状态（只读）
0x60BAint32正边沿捕获位置（只读）
0x60BBint32负边沿捕获位置（只读）

数字 IO​

对象类型说明
0x60FDuint32数字输入（只读，32 位位图）
0x60FE:01uint32数字输出（读写，32 位位图）

软件位置限制​

对象类型说明
0x607D:01int32最小位置限制
0x607D:02int32最大位置限制

快速运动控制​

NewSetpoint()​

PP 模式发送新定位命令: 写 0x607A 目标位置 + 触发 Controlword Bit4 (NewSetpoint)。relative=TRUE 时同时设置 Bit6 (Relative)。

ClearNewSetpoint()​

PP 模式清除 NewSetpoint 标志（Controlword Bit4=0）。在 TargetReached (Statusword Bit10=1) 后调用，完成 Set-point Ack 握手。

StartHoming()​

HM 模式启动回零: 通过 SDO 配置 0x6098 (HomingMethod) / 0x607C (HomeOffset) 后, Controlword Bit4=1 启动。HomingAttained (Statusword Bit12) 表示完成, HomingError (Bit13) 表示错误。

PP / HM 握手由应用通过 PDO/SDO 操作 Controlword 完成,典型流程:

/* PP 模式发送新设定点 */
uint16_t cw = 0x000F;            /* Operation Enable */
cw |= 0x0010;                    /* Bit4: New Setpoint */
if (relative) cw |= 0x0040;      /* Bit6: Relative */
dll.CiA402_WriteControlWord(master, slave, cw);
/* TargetReached 后清 Bit4: */
dll.CiA402_WriteControlWord(master, slave, cw & ~0x0010);

/* HM 启动: 通过 SDO 配置 0x6098/0x607C 后, Controlword Bit4=1 启动 */

回零相关参数:

  • 0x6098 (int8) — 回零方法 (HomingMethod)
  • 0x607C (int32) — 回零偏移 (HomeOffset)

回零状态位 (Statusword):

  • Bit 12 — HomingAttained
  • Bit 13 — HomingError

驱动器信息​

SupportedDriveModes​

通过 SDO 0x6502 (uint32) 读取支持的驱动模式位掩码: Bit 0=PP, Bit 1=VL, Bit 2=PV, Bit 3=PT, Bit 5=HM, Bit 6=IP, Bit 7=CSP, Bit 8=CSV, Bit 9=CST。

IsModeSupported​

(SupportedDriveModes >> bit) & 1 即可判断指定模式是否可用。

SupportedHomingMethods​

通过 SDO 0x60E3 (int8 数组) 读取驱动器支持的回零方法编号列表。subindex 0 为列表长度, 1..N 为各方法编号。

TxPDO 数据有效性​

通过 SDO 读取 0x603E 判断 TxPDO 数据是否有效。非零表示驱动器 TxPDO 数据不可信，例如编码器未就绪时位置值无意义。

uint8_t invalid = 0;
int sz = sizeof(invalid);
SDOread(master, slave, 0x603E, 0, FALSE, &sz, &invalid, EC_TIMEOUTRXM);
if (invalid == 0) {
    /* TxPDO 数据有效，可安全使用 */
    int32_t pos;
    cia402_read_position(&dll, master, slave, &map, &pos);
}

驱动器信息表​

  • 0x6502 (uint32) — 支持的驱动模式位掩码（Bit0=PP, Bit7=CSP, Bit8=CSV, Bit9=CST）
  • 0x60E3 (int8[]) — 支持的回零方法列表
  • 0x603E (uint8) — TxPDO 数据有效性（非零=数据无效）

同步功能​

对象类型访问说明
0x60D9:01uint16读写同步设置 (SynchronizationSettings)，同步使能位掩码
0x60DAuint16只读驱动同步状态 (DriveSyncStatus)，指示驱动器是否已同步到主站时钟
/* 读取驱动器同步状态 */
uint16_t sync_status = 0;
int sz = sizeof(sync_status);
SDOread(master, slave, 0x60DA, 0, FALSE, &sz, &sync_status, EC_TIMEOUTRXM);
printf("同步状态: 0x%04X\n", sync_status);

控制字命令常量​

常量值说明
0x0006Shutdown — 关机
0x0007Switch On — 开启
0x000FEnable Operation — 使能运行
0x0000Disable Voltage — 禁用电压
0x0002Quick Stop — 快速停止
0x0080Fault Reset — 故障复位

状态字位掩码常量​

Bit掩码说明
Bit 00x0001Ready to Switch On
Bit 10x0002Switched On
Bit 20x0004Operation Enabled
Bit 30x0008Fault
Bit 40x0010Voltage Enabled
Bit 50x0020Quick Stop
Bit 70x0080Warning
Bit 90x0200Remote
Bit 100x0400Target Reached
Bit 120x1000Homing Attained / Set-point Ack
Bit 130x2000Homing Error / Following Error

完整示例​

#define DYNAMIC_LOAD
#include "ethercat.h"
#include "ethercat_advanced.h"
#include <stdio.h>

int main(int argc, char* argv[])
{
    dll_t dll;
    LOAD_DLL(&dll, "Darra.Core.dll");

    uint16_t master = dll.Initialize();
    dll.SetNetwork(master, argv[1], "");
    dll.SetStateSequence(master, EC_STATE_OPERATIONAL, 15000);
    dll.Start(master);

    uint16_t slave = 1;

    /* 检查初始状态 */
    uint16_t sw = dll.CiA402_ReadStatusWord(master, slave);
    cia402_state_t state = dll.CiA402_ParseState(sw);
    printf("当前状态: %d (SW=0x%04X)\n", state, sw);

    /* 故障复位 */
    if (state == CIA402_FAULT) {
        dll.CiA402_FaultReset(master, slave);
    }

    /* 设置 CSP 模式 */
    dll.CiA402_SetMode(master, slave, CIA402_CSP);
    printf("操作模式: %d\n", dll.CiA402_GetMode(master, slave));

    /* 一步使能 */
    if (dll.CiA402_Enable(master, slave, 50) == 0) {
        printf("驱动使能成功\n");

        /* 扫描 PDO 映射 */
        cia402_pdo_map_t map;
        if (cia402_scan_pdo(&dll, master, slave, &map) == 0) {
            /* 读取实际位置 */
            int32_t actual;
            cia402_read_position(&dll, master, slave, &map, &actual);
            printf("实际位置: %d\n", actual);

            /* 写入目标位置 */
            cia402_write_target_position(&dll, master, slave, &map, actual + 1000);
        }
    }

    getchar();
    dll.Stop(master);
    dll.Dispose(master);
    UNLOAD_DLL(&dll);
    return 0;
}

PP 模式 — 轮廓位置控制​

#define DYNAMIC_LOAD
#include "ethercat.h"
#include "ethercat_advanced.h"

int main(int argc, char* argv[])
{
    dll_t dll;
    LOAD_DLL(&dll, "Darra.Core.dll");

    uint16_t master = dll.Initialize();
    dll.SetNetwork(master, argv[1], "");
    dll.SetStateSequence(master, EC_STATE_OPERATIONAL, 15000);
    dll.Start(master);

    uint16_t slave = 1;

    /* 设置 PP 模式 */
    dll.CiA402_SetMode(master, slave, CIA402_PP);

    /* 配置轮廓参数 (SDO) */
    uint32_t vel = 10000, acc = 50000, dec = 50000;
    SDOwrite(master, slave, 0x6081, 0, FALSE, sizeof(vel), &vel, EC_TIMEOUTRXM);
    SDOwrite(master, slave, 0x6083, 0, FALSE, sizeof(acc), &acc, EC_TIMEOUTRXM);
    SDOwrite(master, slave, 0x6084, 0, FALSE, sizeof(dec), &dec, EC_TIMEOUTRXM);

    /* 使能 */
    if (dll.CiA402_Enable(master, slave, 50) == 0) {
        printf("驱动使能成功\n");

        /* 发送定位命令: 写目标位置 0x607A, 然后 Controlword Bit4=NewSetpoint */
        int32_t target = 100000;
        SDOwrite(master, slave, 0x607A, 0, FALSE, sizeof(target), &target, EC_TIMEOUTRXM);
        uint16_t cw = 0x001F;  /* Operation Enable + NewSetpoint */
        dll.CiA402_WriteControlWord(master, slave, cw);

        /* 等待到达 */
        uint16_t sw;
        do {
            sw = dll.CiA402_ReadStatusWord(master, slave);
        } while ((sw & 0x0400) == 0);  /* 等待 TargetReached (Bit 10) */

        printf("目标已到达\n");
        dll.CiA402_WriteControlWord(master, slave, cw & ~0x0010);  /* 清 NewSetpoint */
    }

    getchar();
    dll.Stop(master);
    dll.Dispose(master);
    UNLOAD_DLL(&dll);
    return 0;
}

HM 模式 — 回零​

#define DYNAMIC_LOAD
#include "ethercat.h"

int main(int argc, char* argv[])
{
    dll_t dll;
    LOAD_DLL(&dll, "Darra.Core.dll");

    uint16_t master = dll.Initialize();
    dll.SetNetwork(master, argv[1], "");
    dll.SetStateSequence(master, EC_STATE_OPERATIONAL, 15000);
    dll.Start(master);

    uint16_t slave = 1;

    /* 设置 HM 模式 */
    dll.CiA402_SetMode(master, slave, CIA402_HM);

    /* 设置回零参数 */
    int8_t method = 35;  /* 当前位置回零 */
    int32_t offset = 0;
    uint32_t search_speed = 5000, zero_speed = 500, homing_acc = 10000;

    SDOwrite(master, slave, 0x6098, 0, FALSE, sizeof(method), &method, EC_TIMEOUTRXM);
    SDOwrite(master, slave, 0x607C, 0, FALSE, sizeof(offset), &offset, EC_TIMEOUTRXM);
    SDOwrite(master, slave, 0x6099, 1, FALSE, sizeof(search_speed), &search_speed, EC_TIMEOUTRXM);
    SDOwrite(master, slave, 0x6099, 2, FALSE, sizeof(zero_speed), &zero_speed, EC_TIMEOUTRXM);
    SDOwrite(master, slave, 0x609A, 0, FALSE, sizeof(homing_acc), &homing_acc, EC_TIMEOUTRXM);

    /* 使能 */
    if (dll.CiA402_Enable(master, slave, 50) == 0) {
        printf("驱动使能成功，开始回零...\n");

        /* 启动回零: Controlword Bit4=Start Homing */
        dll.CiA402_WriteControlWord(master, slave, 0x001F);

        /* 等待回零完成 */
        uint16_t sw;
        do {
            sw = dll.CiA402_ReadStatusWord(master, slave);
            if (sw & 0x2000) {  /* Bit 13: HomingError */
                printf("回零错误!\n");
                break;
            }
        } while ((sw & 0x1000) == 0);  /* Bit 12: HomingAttained */

        if (sw & 0x1000) {
            printf("回零完成\n");
        }
    }

    getchar();
    dll.Stop(master);
    dll.Dispose(master);
    UNLOAD_DLL(&dll);
    return 0;
}