TwinCAT3 - 实现CiA402
1，起缘
在TwinCAT3项目中涉及到轴运动时，通常做法都是在PLC中安装TC1250或者TF5000，搭配支持EtherCAT over CANOpen的驱动器，就可以按照TwinCAT教材中的标准做法实现轴运动控制。
但最近在做一个项目时，起初以为并不需要轴运动，所以PLC安装的是TC1200，也没有购买TF5000，这意味着项目不能添加NC轴。结果见到实际设备后，是有一根实轴的，不过运动控制很简单，只需要距离运动和速度运动就可以了。就想着不买TF5000了，看看有没有别的办法。
2，想办法
翻阅文档后发现，只要从驱动器厂家获取到驱动器的xml设备描述文件，并且在PLC的EtherCAT网络拓扑中扫描出驱动器来，就意味着TwinCAT3已经帮助我们完成了EtherCAT over CANOpen的底层通信，我们要做的就是自己实现NC部分的功能。
TwinCAT的NC部分涉及到两块，一块是CiA402（或曰DS402），定义了运动控制相关的PDO，SDO，驱动器状态机和运行模式等。另一块是PLCOpen，定义了MC_Power，MC_MoveAbsolute，MC_Stop等函数接口以及轴状态的封装。
其中CiA402是必须要实现的，PLCOpen实不实现都无所谓。因此只需要实现CiA402能控制驱动器就行了，并且我也只需要MC_Power，MC_MoveAbsolute，MC_MoveRelative几个基本功能（复杂的我也玩不来）。至于MC函数封装表面上符合PLCOpen，差不多就行了。
3，开搞
手头的驱动器型号是台达ASDA-A2-E，要到网上找对应的CANopen通信手册。其实绝大部分支持CANopen的驱动器通信协议都遵循着CiA402，只是在脉冲换算，支持的操作模式等细节上有差异。最终搜寻到两本靠谱的手册：《SV660C系列伺服应用手册-CANopen通讯篇》和《台達伺服ASDA-A2 CANopen通訊應用手冊》。
3.1，CANOpen通信
3.1.1 对象字典
由于EtherCAT over CANOpen的底层通信已经实现好了，我们只需要知道CANOpen是以对象字典的方式进行通信就够了。简单讲就是每一个16进制数字都代表一个变量。如果你你熟悉C#，那么通信协议可以理解为一个Dictionary<ushort, object>。如果你熟悉MODBUS，那么可以理解16进制数字就是寄存器地址，变量就是寄存器的内容，比如16#6040这个寄存器的内容就是控制字，将控制字填到16#6040这个寄存器时就已经生效了。
3.1.2 通信建立
驱动器上电，按照说明书调好驱动器的参数，用网线连接驱动器和PLC，找驱动器厂家拿到驱动器对应的xml设备描述文件，新建一个TwinCAT3项目，连接到PLC，切换PLC至Config模式，扫描设备，不出意外的话，就会得到驱动器啦。
接下来编辑驱动器的PDO页面，将我们可能用到的对象字典全部添加进来。添加过程略过（可参考TwinCAT3的教程），下面是添加后的结果。
状态字Statusword，操作模式Modes of operation，控制字Controlword，目标位置Target position等对象都是经常用到的，下文都会一一讲到。
3.2，CiA402伺服状态机
手册说必须按照CiA402协议规定的流程引导伺服驱动器，伺服驱动器才可运行于指定的状态。与伺服状态机相关的对象是状态字Statusword和控制字Controlword。状态机转移的详情如下：
| 状态机转移 | 控制字（16#6040） | 状态字（16#6041） |
|---|---|---|
| 0 上电→初始化 | 自然过渡，无需控制指令 | 0x0000 |
| 1 初始化→伺服无故障 | 自然过渡，无需控制指令 | 0x0250（0010-0101-0000 伺服无故障） |
| 2 伺服无故障→伺服准备好 | 0x06（0000-0000-0110 伺服准备） | 0x0231（0010-0011-0001 伺服准备好） |
| 3 伺服准备好→等待打开伺服使能 | 0x07（0000-0000-0111 等待使能） | 0x0233（0010-0011-0011 等待伺服使能） |
| 4 等待打开伺服使能→伺服运行 | 0x0F（0000-0000-1111 伺服运行） | 0x0237（0010-0011-0111 伺服运行） |
| 5 伺服运行→等待打开伺服使能 | 0x07 | 0x0233 |
| 6 等待打开伺服使能→伺服准备好 | 0x06 | 0x0231 |
| 7 伺服准备好→伺服无故障 | 0x00（0000-0000-0000 伺服无故障） | 0x0250（0010-0101-0000 伺服无故障） |
| 8 伺服运行→伺服准备好 | 0x06 | 0x0231 |
| 9 伺服运行→伺服无故障 | 0x00 | 0x0250 |
| 10 等待打开伺服使能→伺服无故障 | 0x00 | 0x0250 |
| 11 伺服运行→快速停机 | 0x02（0000-0000-0010 快速停机） | 0x0217（0010-0001-0111 快速停机） |
| 12 快速停机→伺服无故障 | 自然过渡，无需控制指令 | 0x0250 |
| 13 Any→故障停机 | 发生故障，自动切换 | 0x021F（0010-0001-1111 故障停机） |
| 14 故障停机→故障 | 自然过渡，无需控制指令 | 0x0218（0010-0001-1000 故障） |
| 15 故障→伺服无故障 | 0x80（0000-1000-0000 故障复位） | 0x0250（0010-0101-0000） |
| 16 快速停机→伺服运行 | 0x0F | 0x0237 |
控制字Controlword和状态字Statusword的详情如下：
| 控制字的bit | 0 | 1 | 2 | 3 | 4-6 | 7 | 8 | 9-15 |
|---|---|---|---|---|---|---|---|---|
| 名称 | 伺服开启 | 主回路上电 | 快速停机 | 伺服运行 | 与操作模式相关 | 故障复位 | 暂停 | NA |
| 描述 | 0-否 1-是 | 0-否 1-是 | 0-是 1-否 | 0-否 1-是 | 上升沿有效 | 0-否 1-是 |
| 操作模式 | 轮廓位置模式 | 回零模式 | 轮廓速度模式 | 轮廓转矩模式 |
|---|---|---|---|---|
| 控制字bit4 | 使能新位置指令（上升沿触发） | 开始回零（上升沿触发，须保持） | NA | NA |
| 控制字bit5 | 位置指令更新模式（0-非立刻更新 1-立刻更新） | NA | NA | NA |
| 控制字bit6 | 位置指令类型（0-绝对位置指令 1-相对位置指令） | NA | NA | NA |
| 状态字的bit | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12-15 |
|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 名称 | 伺服准备好 | 伺服可以运行 | 伺服运行 | 故障 | 主回路上电 | 快速停机 | 伺服不可运行 | 警告 | 厂家自定义 | 远程控制 | 目标到达 | 内部限制 | 运行模式相关 |
| 运行模式 | 轮廓位置模式 | 回零模式 | 轮廓速度模式 | 轮廓转矩模式 |
|---|---|---|---|---|
| 状态字bit12 | 能够接收位置指令(0-能 1-不能) | 原点回零完成（0-否 1-是） | 零速信号(0-否 1-是) | NA |
| 状态字bit13 | 位置偏差状态(0-阈值内 1-超出阈值) | 原点回零错误（0-无 1-有） | NA | NA |
| 状态字bit14 | NA | NA | NA | NA |
| 状态字bit15 | NA | NA | NA | NA |
当前的状态机通过状态字Statusword判断（如果进入伺服运行之前出现了表中没有的状态字，很可能是驱动器上电异常或者报错了），而状态机的转移完全由控制字Controlword来操作。一般来讲，驱动器上电之后，我们要根据当前的状态来给出控制字，最终目的是把驱动器转移到伺服运行这个状态机。简单起见，无论当前处于何种状态，我们都往伺服运行这个方向走。
为了实现目的，写如下代码：
TYPE E_DriveStatus :
(
Init := 0, //初始化 //not ready to switch on
NoFault := 1, //伺服无故障 //switch on disabled
Ready := 2, //伺服准备好 //ready to switch on
WaitOn := 3, //等待打开伺服使能 //switched on
Operation := 4, //伺服运行 //operation enabled
QuickStop := 5, //快速停机 //quick stop active
FaultStop := 6, //故障停机 //fault reaction active
Fault := 7 //故障 //fault
);
END_TYPE
TYPE AXIS_REF :
STRUCT
//0x6040 在OP状态才生效 控制字的每一个bit位单独赋值无意义，必须与其他位共同构成某一控制指令
ControlWord AT%Q*: UINT;
//0x6041 在OP或Safe-OP下才更新 状态字的每一个bit位单独读取无意义，必须与其他位共同组成当前状态
StatusWord AT%I*: UINT;
END_STRUCT
END_TYPE
FUNCTION_BLOCK MC_Run
VAR_IN_OUT
Axis: AXIS_REF;
END_VAR
VAR
//----------
StatusWordBit9: UINT; //用于判断状态机，状态机只和bit0-bit9有关
//----------
DriveStatus: E_DriveStatus; //驱动器状态
END_VAR
//状态机转移----------
StatusWordBit9:= Axis.StatusWord AND 2#1111111111; //用于判断状态机，状态机只和bit0-bit9有关
IF StatusWordBit9 = 16#0 THEN //0
DriveStatus:= E_DriveStatus.Init;
END_IF
IF StatusWordBit9 = 16#0250 THEN //1 //10 //7 //9 //12 //15
DriveStatus:= E_DriveStatus.NoFault;
END_IF
IF StatusWordBit9 = 16#0231 THEN //2 //6 //8
DriveStatus:= E_DriveStatus.Ready;
END_IF
IF StatusWordBit9 = 16#0233 THEN //3 //5
DriveStatus:= E_DriveStatus.WaitOn;
END_IF
IF StatusWordBit9 = 16#0237 THEN //4 //16
DriveStatus:= E_DriveStatus.Operation;
END_IF
IF StatusWordBit9 = 16#0217 THEN //11
DriveStatus:= E_DriveStatus.QuickStop;
END_IF
IF StatusWordBit9 = 16#021F THEN //13
DriveStatus:= E_DriveStatus.FaultStop;
END_IF
IF StatusWordBit9 = 16#0218 THEN //14
DriveStatus:= E_DriveStatus.Fault;
END_IF
//用户指令运行----------
CASE DriveStatus OF
E_DriveStatus.NoFault:
Axis.ControlWord:= 16#06; //前往E_DriveStatus.Ready，终点是E_DriveStatus.Operation
E_DriveStatus.Ready:
Axis.ControlWord:= 16#07; //前往E_DriveStatus.WaitOn，终点是E_DriveStatus.Operation
E_DriveStatus.WaitOn:
Axis.ControlWord:= 16#0F; //前往E_DriveStatus.Operation
E_DriveStatus.Operation:
//驱动器的运动，先按下不表
E_DriveStatus.QuickStop:
Axis.ControlWord:= 16#0F; //停机完成时会前往E_DriveStatus.Operation（不能让ControlWord保持0x02，会卡在E_DriveStatus.QuickStop）
E_DriveStatus.Fault:
Axis.ControlWord:= 16#80; //切换到E_DriveStatus.NoFault
END_CASE
设备上电，程序跑起来，按照程序的逻辑，只要驱动器上电正常不报错，就会进入伺服运行状态机。
3.3，伺服运行
在伺服运行状态，就可以控制驱动器开始运动了。
3.3.1 操作模式
大部分驱动器支持8种操作模式：
- 周期同步位置模式Cyclic Synchronous Position Mode，即MC_MoveAbsolute和MC_MoveRelative
- 周期同步速度模式Cyclic Synchronous Velocity Mode，即MC_MoveVelocity
- 周期同步转矩模式Cyclic Synchronous Torque Mode，即MC_TorqueControl
- 轮廓位置模式Profile Position Mode，类似MC_MoveAbsolute和MC_MoveRelative
- 轮廓速度模式Profile Velocity Mode，类似MC_MoveVelocity
- 轮廓转矩模式Profile Torque Mode，类似MC_TorqueControl
- 回零模式Homing Mode，类似MC_Home
- 插补模式Interpolation Position Mode，类似MC_ExtSetPointGenFeed
8种模式可以分成3类：
- 周期同步模式：有3个模式，此模式下PLC内部来做路径规划，在每个与驱动器的通信周期，PLC都会计算出目标位置（速度/扭矩）然后发送给驱动器。这部分功能由PLC供应商来提供，至于倍福，这部分功能就封装在开头提到的TC1250或者TF5000中。
- 轮廓模式：也有3个模式，使用起来效果与周期同步模式是类似的，区别在于此模式下PLC仅在指令下达时给予驱动器目标位置、目标速度与加减速度等设定，然后由驱动器自身来做路径规划，CANOpen通信控制的话，主要就是用轮廓模式。
- 回零和插补：回零就是找原点嘛，可以用也可自己写。插补就是手动做路径规划，基本用不着。
与操作模式相关的对象是操作模式Modes of operation：（吐槽一下SV660C手册好多地方都写错了，比如下表是操作模式的值，SV660C手册却说是bit）
| 操作模式的值 | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 |
|---|---|---|---|---|---|---|---|---|
| 描述 | NA | 轮廓位置模式 | NA | 轮廓速度模式 | 轮廓转矩模式 | NA | 回零模式 | 插补模式 |
3.3.2 轮廓位置模式
与轮廓位置模式相关的对象有操作模式Modes of operation，控制字Controlword，目标位置Target position，轮廓速度Profile velocity，轮廓加速度Profile acceleration，轮廓减速度Profile deceleration。不同的驱动器，目标位置和轮廓速度的换算会不同，下面以台达ASDA-A2为例说明控制流程。
控制流程如下：
- 令操作模式Modes of operation=16#1
- 给定目标位置Target position，单位是脉冲数
- 给定轮廓速度Profile velocity，单位是脉冲数/秒
- 给定轮廓加速度Profile acceleration和轮廓减速度Profile deceleration，单位是从0rpm加速到3000rpm所需的毫秒数
- 控制字Controlword的bit4上升沿，使能新位置指令。bit5置1，使用立刻更新模式。bit6置0，使用绝对位置模式
- 判断状态字StatusWord已经接收到了指令，然后复位控制字Controlword的bit4，方便下次给上升沿
- 判断状态字StatusWord到达目标位置，结束流程
其中，脉冲数到实际距离的换算还涉及到驱动器的ScalingFactorDenominator（即电机转一圈多少个脉冲）和减速机的减速比。
综上，写如下代码：
TYPE AXIS_REF :
STRUCT
//0x6060
ModeOperation AT%Q*: SINT;
//0x6040 在OP状态才生效 控制字的每一个bit位单独赋值无意义，必须与其他位共同构成某一控制指令
ControlWord AT%Q*: UINT;
//0x607A
TargetPosition AT%Q*: DINT;
//0x6081
ProfileVelocity AT%Q*: DINT;
//0x6083
ProfileAcc AT%Q*: DINT;
//0x6084
ProfileDec AT%Q*: DINT;
//0x6041 在OP或Safe-OP下才更新 状态字的每一个bit位单独读取无意义，必须与其他位共同组成当前状态
StatusWord AT%I*: UINT;
END_STRUCT
END_TYPE
TYPE AXIS_INFO :
STRUCT
TargetPosition: REAL; //目标位置 mm
TargetVelocity: REAL; //目标速度 mm/s
ProfileVelocity: REAL; //轮廓速度 mm/s
ProfileAcc: REAL; //轮廓加速度 mm/s
ProfileDec: REAL; //轮廓减速度 mm/s
END_STRUCT
END_TYPE
TYPE AXIS_PARA :
STRUCT
ScalingFactorNumerator: REAL; //分子，填电机转一圈走多少mm
ScalingFactorDenominator: REAL; //分母，填电机转一圈多少个脉冲，比如ASDA驱动器就是1280000，SV660C-22bit驱动器就是4194304
END_STRUCT
END_TYPE
FUNCTION_BLOCK MC_Run
VAR_IN_OUT
Axis: AXIS_REF;
Info: AXIS_INFO;
Para: AXIS_PARA;
END_VAR
VAR
//----------
DriveStatus: E_DriveStatus; //驱动器状态
//----------
DriveMoveCommand: INT; //0:无 1:轮廓位置控制移动 3:轮廓速度控制移动 6:原点模式 20:快速停机 21:故障复位
AbsOrRel: INT; //轮廓位置控制移动方式 0:绝对移动 1:相对移动
DriveCommandState: INT; //指令状态机
ScalingFactor: REAL; //分母/分子 = 脉冲数/mm
END_VAR
//Out-----
ScalingFactor:= Para.ScalingFactorDenominator / MAX(Para.ScalingFactorNumerator, 0.001);
//用户指令运行----------
CASE DriveStatus OF
E_DriveStatus.NoFault:
Axis.ControlWord:= 16#06; //前往E_DriveStatus.Ready，终点是E_DriveStatus.Operation
E_DriveStatus.Ready:
Axis.ControlWord:= 16#07; //前往E_DriveStatus.WaitOn，终点是E_DriveStatus.Operation
E_DriveStatus.WaitOn:
Axis.ControlWord:= 16#0F; //前往E_DriveStatus.Operation
E_DriveStatus.Operation:
IF DriveMoveCommand = 1 THEN //轮廓位置控制移动
CASE DriveCommandState OF
0:
Axis.ModeOperation:= 1; //轮廓位置控制模式 //0x6060
IF AbsOrRel = 0 THEN //绝对
Axis.TargetPosition:= REAL_TO_DINT(Info.TargetPosition * ScalingFactor); //0x607A //单位:脉冲数
ELSIF AbsOrRel = 1 THEN //相对
Axis.TargetPosition:= Axis.PositionActualValue + REAL_TO_DINT(Info.TargetPosition * ScalingFactor);
END_IF
Axis.ProfileVelocity:= REAL_TO_DINT(Info.ProfileVelocity * ScalingFactor); //0x6081 //单位:脉冲数/秒
Axis.ProfileAcc:= LIMIT(1, REAL_TO_DINT(50000.0 * Para.ScalingFactorNumerator / MAX(Info.ProfileAcc, 0.001)), 65500); //0x6083 //从0rpm到3000rpm的毫秒数 //量程1-65500
Axis.ProfileDec:= LIMIT(1, REAL_TO_DINT(50000.0 * Para.ScalingFactorNumerator / MAX(Info.ProfileDec, 0.001)), 65500); //0x6084
Axis.ControlWord.4:= 0;
DriveCommandState:= 10;
10:
IF Axis.ModeOperationDisplay = 1 THEN
//控制字0x0F->0x3F
Axis.ControlWord.4:= 1; //上升沿:使能位移指令
Axis.ControlWord.5:= 1; //0:非立刻更新 1:立刻更新
Axis.ControlWord.6:= 0; //位置指令类型 0:表示607A是绝对位置 1:表示607A是相对位置(设1不行啊)
DriveCommandState:= 20;
END_IF
20:
IF Axis.StatusWord.12 THEN //不可接收新位移指令，说明伺服收到了位移指令
Axis.ControlWord.4:= 0; //复位位移指令 0x3F->0x2F
DriveCommandState:= 30;
END_IF
30:
IF Axis.StatusWord.10 THEN //目标到达
DriveMoveCommand:= 0; //完事
DriveCommandState:= 0;
END_IF
END_CASE
END_IF
E_DriveStatus.QuickStop:
Axis.ControlWord:= 16#0F; //停机完成时会前往E_DriveStatus.Operation（不能让ControlWord保持0x02，会卡在E_DriveStatus.QuickStop）
E_DriveStatus.Fault:
Axis.ControlWord:= 16#80; //切换到E_DriveStatus.NoFault
END_CASE
按照流程把相应的参数填好，不出意外电机就可以转起来啦。
3.3.3 轮廓速度模式
控制流程和轮廓位置模式类似，具体如下：
- 控制字Controlword保持在0x0F
- 令操作模式Modes of operation=16#3
- 给定目标速度Target velocity，单位是0.1rpm
- 给定轮廓加速度Profile acceleration和轮廓减速度Profile deceleration，单位是从0rpm加速到3000rpm所需的毫秒数
- 判断状态字StatusWord到达目标速度，结束流程
//用户指令运行----------
CASE DriveStatus OF
E_DriveStatus.Operation:
IF DriveMoveCommand = 3 THEN //轮廓速度控制模式
CASE DriveCommandState OF
0:
Axis.ControlWord:= 16#0F; //控制字保持在0x0F
DriveCommandState:= 10;
10:
Axis.ModeOperation:= 3; //轮廓速度控制模式 //0x6060
//单位:0.1rpm //X(mm/s) = 60*X(mm/min) = 60*X/分子(rpm) = 600*X/分子(0.1rpm)
Axis.TargetVelocity:= REAL_TO_DINT(600 * Info.TargetVelocity / Para.ScalingFactorNumerator); //0x60FF
Axis.ProfileAcc:= LIMIT(1, REAL_TO_DINT(50000.0 * Para.ScalingFactorNumerator / MAX(Info.ProfileAcc, 0.001)), 65500); //0x6083 //从0rpm到3000rpm的毫秒数 ms/3000rpm //量程1-65500
Axis.ProfileDec:= LIMIT(1, REAL_TO_DINT(50000.0 * Para.ScalingFactorNumerator / MAX(Info.ProfileDec, 0.001)), 65500); //0x6084 //从0rpm到3000rpm的毫秒数
DriveCommandState:= 20;
20:
IF Axis.ModeOperationDisplay = 3 THEN
DriveCommandState:= 30;
END_IF
30:
IF Axis.StatusWord.10 THEN //目标到达
DriveMoveCommand:= 0; //完事
DriveCommandState:= 0;
END_IF
END_CASE
END_IF
END_CASE
此处只贴出了关键代码，同样把相应的参数填好，不出意外电机就可以转起来啦。
3.3.4 其他
快速停机和故障复位都比较简单，只需要写控制字Controlword和操作模式Modes of operation就可以实现。
至于MC_SetPosition，暂时没确定倍福是怎么做的，不过用操作模式Modes of operation的回零模式，并结合回零方式Homing Method可以实现相同的效果。
4，用起来
后面忘了截图了，贴一张前期刚刚用轮廓速度模式把电机转起来的图。
