跳到主要内容

CiA 402 - 伺服驱动器协议

CiA 402 是 CANopen 驱动器和运动控制设备协议，是伺服驱动器最广泛使用的标准，掌握该协议是实现伺服控制的关键。

协议简介​

什么是 CiA 402？​

CiA 402 是一个基于 CoE (CANopen over EtherCAT) 的设备协议，专门为驱动器和运动控制设备定义标准接口。

协议层次关系：

EtherCAT 应用层
    └── CoE (CANopen over EtherCAT)
            ├── CiA 401 (通用 I/O 模块)
            ├── CiA 402 (驱动器和运动控制) ← 当前协议
            └── 其他 CiA 设备协议

CiA 402 定义了什么？​

CiA 402 在 CoE 对象字典的 0x6000-0x67FF 范围内定义了：

  • 标准状态机：统一的驱动器状态转换逻辑
  • 操作模式：多种运动控制模式（位置、速度、转矩等）
  • 标准对象：统一的控制和反馈参数
  • PDO 映射：实时运动控制数据交换
  • 互操作性：不同厂商驱动器可互换

为什么需要 CiA 402？​

  • 标准化接口 — 所有驱动器使用相同的对象地址和功能
  • 即插即用 — 主站代码无需为不同品牌驱动器定制
  • 降低成本 — 减少集成工作量和学习成本
  • 行业认可 — 几乎所有伺服厂商都支持 CiA 402

状态机​

CiA 402 定义了完整的驱动器状态机

主要状态​

状态说明控制字状态字
Not ready to switch on初始化状态，驱动未就绪-xxxx xxxx x0xx 0000
Switch on disabled驱动禁用，可接收命令-xxxx xxxx x1xx 0000
Ready to switch on准备开启，电压未使能0x06xxxx xxxx x01x 0001
Switched on已开启，电压使能但未运行0x07xxxx xxxx x01x 0011
Operation enabled运行使能，可正常操作0x0Fxxxx xxxx x01x 0111
Quick stop active快速停止激活0x02xxxx xxxx x00x 0111
Fault reaction active故障反应中-xxxx xxxx x0xx 1111
Fault故障状态0x80xxxx xxxx x0xx 1000

状态转换命令​

命令控制字说明
Shutdown0x06关闭，进入 Ready to switch on
Switch on0x07开启，进入 Switched on
Enable operation0x0F使能操作，进入 Operation enabled
Disable voltage0x00禁用电压，返回 Switch on disabled
Quick stop0x02快速停止
Fault reset0x80故障复位

控制字与状态字​

控制字 (Controlword, 0x6040)​

Bit名称说明
0Switch on开启命令
1Enable voltage使能电压
2Quick stop快速停止（0=激活）
3Enable operation使能操作
4Operation mode specific新设定点
5Operation mode specific立即改变
6Operation mode specific相对
7Fault reset故障复位（上升沿）
8Halt暂停
9-10Operation mode specific操作模式特定
11-15Manufacturer specific厂商特定

常用控制字：

const ushort SHUTDOWN        = 0x06;  // 0000 0110
const ushort SWITCH_ON       = 0x07;  // 0000 0111
const ushort ENABLE_OP       = 0x0F;  // 0000 1111
const ushort DISABLE_VOLTAGE = 0x00;  // 0000 0000
const ushort QUICK_STOP      = 0x02;  // 0000 0010
const ushort FAULT_RESET     = 0x80;  // 1000 0000

状态字 (Statusword, 0x6041)​

Bit名称说明
0Ready to switch on准备就绪
1Switched on已开启
2Operation enabled操作使能
3Fault故障
4Voltage enabled电压使能
5Quick stop快速停止（0=激活）
6Switch on disabled开启禁用
7Warning警告
8Manufacturer specific厂商特定
9Remote远程模式
10Target reached目标到达
11Internal limit active内部限位激活
12Operation mode specific设定点确认
13Operation mode specificFollowing error
14-15Manufacturer specific厂商特定

操作模式​

模式定义 (Modes of operation, 0x6060)​

值缩写模式说明
1PPProfile Position Mode轮廓位置模式，点到点运动
3PVProfile Velocity Mode轮廓速度模式，速度控制
4PTProfile Torque Mode轮廓转矩模式，转矩控制
6HMHoming Mode回零模式，建立位置参考
7IPInterpolated Position Mode插补位置模式，轨迹跟踪
8CSPCyclic Synchronous Position Mode周期同步位置模式
9CSVCyclic Synchronous Velocity Mode周期同步速度模式
10CSTCyclic Synchronous Torque Mode周期同步转矩模式

模式详解​

PP - Profile Position Mode​

特点：

  • 点到点运动
  • 内部轨迹规划
  • 梯形/S形速度曲线

主要对象：

  • 0x607A: Target position（目标位置）
  • 0x6081: Profile velocity（轮廓速度）
  • 0x6083: Profile acceleration（轮廓加速度）
  • 0x6084: Profile deceleration（轮廓减速度）

CSP - Cyclic Synchronous Position Mode​

特点：

  • 周期性位置指令
  • 主站轨迹规划
  • 最高实时性

主要对象：

  • 0x607A: Target position（目标位置，每周期更新）
  • 0x60B2: Torque offset（转矩偏移）

CSV - Cyclic Synchronous Velocity Mode​

特点：

  • 周期性速度指令
  • 适合速度跟随

主要对象：

  • 0x60FF: Target velocity（目标速度）
  • 0x60B2: Torque offset（转矩偏移）

CST - Cyclic Synchronous Torque Mode​

特点：

  • 周期性转矩指令
  • 力控应用

主要对象：

  • 0x6071: Target torque（目标转矩）

常用对象​

控制与状态​

索引名称类型访问说明
0x6040ControlwordUNSIGNED16RW/RXPDO控制字
0x6041StatuswordUNSIGNED16RO/TXPDO状态字
0x6060Modes of operationINTEGER8RW操作模式
0x6061Modes of operation displayINTEGER8RO实际操作模式

位置对象​

索引名称类型单位说明
0x6062Position demand valueINTEGER32内部单位位置需求值
0x6063Position actual internal valueINTEGER32内部单位实际位置（内部）
0x6064Position actual valueINTEGER32用户单位实际位置
0x607ATarget positionINTEGER32用户单位目标位置
0x607CHome offsetINTEGER32用户单位回零偏移
0x607DSoftware position limitARRAY用户单位软件位置限制

速度对象​

索引名称类型单位说明
0x606BVelocity demand valueINTEGER32内部单位速度需求值
0x606CVelocity actual valueINTEGER32用户单位实际速度
0x60FFTarget velocityINTEGER32用户单位目标速度
0x6081Profile velocityUNSIGNED32用户单位轮廓速度
0x6083Profile accelerationUNSIGNED32用户单位/s²轮廓加速度
0x6084Profile decelerationUNSIGNED32用户单位/s²轮廓减速度

转矩对象​

索引名称类型单位说明
0x6071Target torqueINTEGER16千分之额定目标转矩
0x6077Torque actual valueINTEGER16千分之额定实际转矩
0x60B2Torque offsetINTEGER16千分之额定转矩偏移

代码示例​

伺服使能流程​

void EnableServo(Slave servo)
{
    ushort statusword;

    // 1. 检查并清除故障
    byte[] sw = servo.CoE.SDORead(0x6041, 0);
    statusword = BitConverter.ToUInt16(sw, 0);

    if ((statusword & 0x08) != 0) // Bit 3 = Fault
    {
        servo.CoE.SDOWrite(0x6040, 0, BitConverter.GetBytes((ushort)0x80));
        Thread.Sleep(100);
    }

    // 2. Shutdown
    servo.CoE.SDOWrite(0x6040, 0, BitConverter.GetBytes((ushort)0x06));
    Thread.Sleep(10);

    // 3. Switch on
    servo.CoE.SDOWrite(0x6040, 0, BitConverter.GetBytes((ushort)0x07));
    Thread.Sleep(10);

    // 4. Enable operation
    servo.CoE.SDOWrite(0x6040, 0, BitConverter.GetBytes((ushort)0x0F));
    Thread.Sleep(10);

    // 验证状态
    sw = servo.CoE.SDORead(0x6041, 0);
    statusword = BitConverter.ToUInt16(sw, 0);

    if ((statusword & 0x27) == 0x27) // Bits 0,1,2,5 = Operation enabled
    {
        Console.WriteLine("伺服使能成功");
    }
}

位置模式（PP）​

void ProfilePositionMove(Slave servo, int targetPos)
{
    // 1. 设置操作模式为 PP
    servo.CoE.SDOWrite(0x6060, 0, new byte[] { 1 });

    // 2. 设置运动参数
    servo.CoE.SDOWrite(0x6081, 0, BitConverter.GetBytes(1000u));  // 速度 1000
    servo.CoE.SDOWrite(0x6083, 0, BitConverter.GetBytes(5000u));  // 加速度
    servo.CoE.SDOWrite(0x6084, 0, BitConverter.GetBytes(5000u));  // 减速度

    // 3. 写入目标位置
    servo.CoE.SDOWrite(0x607A, 0, BitConverter.GetBytes(targetPos));

    // 4. 触发运动（bit 4 = new set-point）
    servo.CoE.SDOWrite(0x6040, 0, BitConverter.GetBytes((ushort)0x1F));

    // 5. 等待到达
    while (true)
    {
        byte[] sw = servo.CoE.SDORead(0x6041, 0);
        ushort statusword = BitConverter.ToUInt16(sw, 0);

        if ((statusword & 0x0400) != 0) // Bit 10 = Target reached
        {
            Console.WriteLine("到达目标位置");
            break;
        }

        Thread.Sleep(10);
    }
}

周期同步位置模式（CSP）​

void CyclicSynchronousPosition(Slave servo)
{
    // 1. 设置操作模式为 CSP
    servo.CoE.SDOWrite(0x6060, 0, new byte[] { 8 });

    // 2. 使能伺服
    EnableServo(servo);

    // 3. 周期性发送位置
    int currentPos = 0;
    while (running)
    {
        // 生成轨迹（示例：正弦波）
        currentPos = (int)(10000 * Math.Sin(angle));
        angle += 0.01;

        // 通过 PDO 发送位置（需提前配置 PDO 映射）
        byte[] posData = BitConverter.GetBytes(currentPos);
        Array.Copy(posData, 0, outputBuffer, posOffset, 4);

        // 或通过 SDO（非实时）
        servo.CoE.SDOWrite(0x607A, 0, posData);

        master.Exchange(); // 1ms 周期
        Thread.Sleep(1);
    }
}

单位与缩放​

位置单位​

通过以下对象配置：

  • 0x6093: Position factor (分子/分母)
  • 0x6094: Velocity encoder factor

示例：将用户单位转换为编码器脉冲

用户单位 = 编码器脉冲 × (0x6093 分子 / 0x6093 分母)

速度单位​

默认单位：用户单位/秒

转矩单位​

千分之额定转矩（1000 = 100%）

回零模式（Homing）​

回零方法 (0x6098)​

方法编号参考信号说明
1-14限位开关 + 零位信号不同方向和序列组合
17-18原点开关基于原点开关
19-22原点开关 + 零位信号组合方式
23-27原点开关（两侧）双侧原点开关
33-34零位信号仅使用零位信号
35当前位置当前位置设为原点

回零流程​

void HomingSequence(Slave servo)
{
    // 1. 设置回零模式
    servo.CoE.SDOWrite(0x6060, 0, new byte[] { 6 });

    // 2. 设置回零方法（例如：方法 1）
    servo.CoE.SDOWrite(0x6098, 0, new byte[] { 1 });

    // 3. 设置回零速度
    servo.CoE.SDOWrite(0x6099, 1, BitConverter.GetBytes(500u));  // 搜索速度
    servo.CoE.SDOWrite(0x6099, 2, BitConverter.GetBytes(100u));  // 零位速度

    // 4. 启动回零（bit 4 = start）
    servo.CoE.SDOWrite(0x6040, 0, BitConverter.GetBytes((ushort)0x1F));

    // 5. 等待完成
    while (true)
    {
        byte[] sw = servo.CoE.SDORead(0x6041, 0);
        ushort statusword = BitConverter.ToUInt16(sw, 0);

        if ((statusword & 0x1000) != 0) // Bit 12 = Homing attained
        {
            Console.WriteLine("回零完成");
            break;
        }

        if ((statusword & 0x2000) != 0) // Bit 13 = Homing error
        {
            Console.WriteLine("回零错误");
            break;
        }

        Thread.Sleep(10);
    }
}

状态字解析​

string GetDriveState(ushort statusword)
{
    if ((statusword & 0x4F) == 0x00) return "Not ready to switch on";
    if ((statusword & 0x4F) == 0x40) return "Switch on disabled";
    if ((statusword & 0x6F) == 0x21) return "Ready to switch on";
    if ((statusword & 0x6F) == 0x23) return "Switched on";
    if ((statusword & 0x6F) == 0x27) return "Operation enabled";
    if ((statusword & 0x6F) == 0x07) return "Quick stop active";
    if ((statusword & 0x4F) == 0x0F) return "Fault reaction active";
    if ((statusword & 0x4F) == 0x08) return "Fault";

    return "Unknown state";
}

// 使用
byte[] sw = servo.CoE.SDORead(0x6041, 0);
ushort statusword = BitConverter.ToUInt16(sw, 0);
Console.WriteLine($"驱动状态: {GetDriveState(statusword)}");