TwinCAT3 - 实现CiA402

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。状态机转移的详情如下：

控制字Controlword和状态字Statusword的详情如下：

当前的状态机通过状态字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种操作模式：

1. 周期同步位置模式Cyclic Synchronous Position Mode，即MC_MoveAbsolute和MC_MoveRelative
2. 周期同步速度模式Cyclic Synchronous Velocity Mode，即MC_MoveVelocity
3. 周期同步转矩模式Cyclic Synchronous Torque Mode，即MC_TorqueControl
4. 轮廓位置模式Profile Position Mode，类似MC_MoveAbsolute和MC_MoveRelative
5. 轮廓速度模式Profile Velocity Mode，类似MC_MoveVelocity
6. 轮廓转矩模式Profile Torque Mode，类似MC_TorqueControl
7. 回零模式Homing Mode，类似MC_Home
8. 插补模式Interpolation Position Mode，类似MC_ExtSetPointGenFeed

8种模式可以分成3类：

1. 周期同步模式：有3个模式，此模式下PLC内部来做路径规划，在每个与驱动器的通信周期，PLC都会计算出目标位置（速度/扭矩）然后发送给驱动器。这部分功能由PLC供应商来提供，至于倍福，这部分功能就封装在开头提到的TC1250或者TF5000中。
2. 轮廓模式：也有3个模式，使用起来效果与周期同步模式是类似的，区别在于此模式下PLC仅在指令下达时给予驱动器目标位置、目标速度与加减速度等设定，然后由驱动器自身来做路径规划，CANOpen通信控制的话，主要就是用轮廓模式。
3. 回零和插补：回零就是找原点嘛，可以用也可自己写。插补就是手动做路径规划，基本用不着。

与操作模式相关的对象是操作模式Modes of operation：（吐槽一下SV660C手册好多地方都写错了，比如下表是操作模式的值，SV660C手册却说是bit）

3.3.2 轮廓位置模式

与轮廓位置模式相关的对象有操作模式Modes of operation，控制字Controlword，目标位置Target position，轮廓速度Profile velocity，轮廓加速度Profile acceleration，轮廓减速度Profile deceleration。不同的驱动器，目标位置和轮廓速度的换算会不同，下面以台达ASDA-A2为例说明控制流程。

控制流程如下：

1. 令操作模式Modes of operation=16#1
2. 给定目标位置Target position，单位是脉冲数
3. 给定轮廓速度Profile velocity，单位是脉冲数/秒
4. 给定轮廓加速度Profile acceleration和轮廓减速度Profile deceleration，单位是从0rpm加速到3000rpm所需的毫秒数
5. 控制字Controlword的bit4上升沿，使能新位置指令。bit5置1，使用立刻更新模式。bit6置0，使用绝对位置模式
6. 判断状态字StatusWord已经接收到了指令，然后复位控制字Controlword的bit4，方便下次给上升沿
7. 判断状态字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 轮廓速度模式

控制流程和轮廓位置模式类似，具体如下：

1. 控制字Controlword保持在0x0F
2. 令操作模式Modes of operation=16#3
3. 给定目标速度Target velocity，单位是0.1rpm
4. 给定轮廓加速度Profile acceleration和轮廓减速度Profile deceleration，单位是从0rpm加速到3000rpm所需的毫秒数
5. 判断状态字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，用起来

后面忘了截图了，贴一张前期刚刚用轮廓速度模式把电机转起来的图。