OpenSML运动控制基础应用介绍

OpenSML运动控制基础应用介绍

CiA402是迄今最流行的运动控制架构，基于OpenSML的基础运动控制可以很方便地控制轴运动。

1 简介:

  CiA（CAN in Automation）是一个组织，CiA402是该组织开发的通讯子协议。我们常用的EtherCAT控制驱动器实际用的多为CoE模式，即CANopen Over EtherCAT。
  在cia402标准子协议主导下，绝大多数ethercat驱动器都支持cia402，通过PDO（过程数据对象）进行数据交互，进而控制轴运动。
  对于主流主站PLC来说，例如BECKHOFF、CoDeSys，它们可以通过手动写程序来控制驱动器，也可以通过购买SoftMotion授权实现更简单、更可靠的控制。
  本文对开源的OpenSML进行简单分析并实际应用。GitHub地址：https://github.com/feecat/OpenSML

2 架构:

  分析原始代码可得知，作者实现了基础的Softmotion Light功能，按照标准手册对控制字、状态字进行操作，并写为功能块方便在不同PLC平台使用。
  运动控制分为插补和非插补，前者对实时性和程序要求较高。OpenSML实现的是后者，非插补运动，也是做非标机械最常用的多轴运动控制。当您需要做CNC机床时，仍需要购买SoftMotion CNC/Robotics 授权。

3 开始使用

  1、在CoDeSys中导入下载的OpenSML.library库文件（其它平台可以手动拷贝结构体和程序），并在Library Mamager里添加对应的库。
  
2、创建变量和程序，我直接在PLC_PRG内定义了一组，如图。为了简化起见，我假设设备为绝对值编码器，不需要回参。
  
3、添加EtherCAT Master并添加驱动器，我这里以安川SGD7S举例。之后在驱动器的process data中选择1st pdo mapping，并链接上变量。
  
4、上传并链接驱动器测试
  

4 程序分析

我接触过最出彩的CiA402用户文档是KEBA旗下的ServoOne用户手册，详细描述了控制字状态字、控制机制、模式等，链接为SO_UserM_EtherCAT-CANopen_2020-04_EN.pdf，感兴趣的童鞋重点看一看第10和第11章。

4.1 OpenSML_Power

  原作者注释的很详细了，主要分为如下几步：
  0：初始化，将关键的几个变量都复位掉。
  10：对驱动器做复位，如果有报警这一步就会复位掉，没报警也没有影响。
  20~40：都是按步骤操作，参考上面KEBA手册的63页。部分驱动器对信号顺序有要求，程序是按照时序编写的。
  50：成功上使能后给xDone信号，上位机执行下一步。

  需要注意的是，控制字.2是QuickStop，.8是Halt，根据实际情况不同这里可以做些修改，Halt比QuickStop造成的影响小，作者的OpenSML_Stop即使用的是Halt位。

(*
MC_Power
Can Also be used MC_Reset, just trigger xEnable.
It is Highly Recommend to use OpenSML_Stop to force stop movements.
*)
Axis.ControlWord.2:=xEmergencyStop;//QuickStop
CASE iState OF
    0://start
        xDone:=FALSE;
        xError:=FALSE;
        IF xEnable THEN
            Axis.ControlWord.0:=FALSE;//SwitchOn
            Axis.ControlWord.1:=FALSE;//EnableVoltage
            Axis.ControlWord.3:=FALSE;//EnableOperation
            Axis.ControlWord.7:=FALSE;//FaultReset
            IF Axis.StatusWord.3 THEN iState:=10;//If there have error, Reset first
            ELSE iState:=20;//No error, go go
            END_IF
        END_IF
    10://reset
        Axis.ControlWord.7:=TRUE;//bit7 faultreset
        tonTimeout(IN:=TRUE,PT:=tTimeOut);
        IF tonTimeout.Q AND NOT Axis.StatusWord.3 THEN//not fault
            Axis.ControlWord.7:=FALSE;//bit7 faultreset
            tonTimeout(IN:=FALSE);
            iState:=20;
        ELSIF tonTimeout.Q THEN iState:=999;//error
        END_IF
    20://Enable Voltage
        Axis.ControlWord.1:=TRUE;
        tonTimeout(IN:=TRUE,PT:=tTimeOut);
        IF Axis.StatusWord.0 THEN//ReadyToSwitchOn
            tonTimeout(IN:=FALSE);
            iState:=30;
        ELSIF tonTimeout.Q THEN iState:=999;//error
        END_IF
    30://Switch ON
        Axis.ControlWord.0:=TRUE;
        tonTimeout(IN:=TRUE,pt:=tTimeOut);
        IF Axis.StatusWord.1 THEN
            tonTimeout(IN:=FALSE);
            iState:=40;
        ELSIF tonTimeout.Q THEN iState:=999;//error
        END_IF
    40://EnableOperation
        Axis.ControlWord.3:=TRUE;
        tonTimeout(IN:=TRUE,pt:=tTimeOut);
        IF Axis.StatusWord.2 THEN
            tonTimeout(IN:=FALSE);
            iState:=50;
        ELSIF tonTimeout.Q THEN iState:=999;//error
        END_IF
    50://Success
        xDone:=TRUE;
        IF NOT xEnable THEN
            iState:=0;
        END_IF
        IF Axis.StatusWord.3 THEN
            iState:=999;//Error when running
        END_IF
    999://Error wait
        tonTimeout(IN:=FALSE);
        xError:=TRUE;
        IF NOT xEnable THEN
            iState:=0;
        END_IF
END_CASE

4.2 OpenSML_ProfilePosition

  Power初始化完成后，进入ProfilePosition模式，需要切换Modes_of_operation。在程序中先做了模式切换后，再上Enable Operation，即可通过控制字.4控制运动。

(*
Profile Position Mode (=1)
Manually Write To SDO or Write To PDO If Needed:
6083: Profile Acceleration
6084: Profile Deceleration
6085: Quick Stop Deceleration
*)
//Settings
Axis.ControlWord.5:=xContinueMove;//It means if 1st positon not arrived and there got 2st position, It Will No Deceleration And continues the movements.
Axis.ControlWord.6:=xRelativeMode;//0 is Absolute, 1 is Relative
Axis.ControlWord.4:=xExecute AND iState = 20;
xBusy:=iState=20 AND Axis.StatusWord.12 AND NOT Axis.StatusWord.10;//Acknowledge and not Reached
xDone:=iState=20 AND Axis.StatusWord.10 AND NOT Axis.StatusWord.13;//Target Reached and not Following Error
xMoving:=NOT Axis.StatusWord.14;//Not Standstill

CASE iState OF
    0://start
        xDone:=FALSE;
        xBusy:=FALSE;
        xError:=FALSE;
        IF xEnable AND Axis.StatusWord.1 AND NOT Axis.StatusWord.2 AND NOT Axis.StatusWord.3 THEN
            iState:=10;//go go
        ELSIF xEnable AND Axis.StatusWord.2 THEN
            Axis.ControlWord.3:=FALSE;//drop down operation enabled
        ELSIF xEnable THEN
            iState:=999;//error
        END_IF
    10://Set Mode
        Axis.Modes_of_operation:=1;//Profile Position Mode
        Axis.ControlWord.3:=Axis.Modes_of_operation_display=1;//Success change mode, enable operation
        IF Axis.StatusWord.2 THEN//Operation Enabled
            iState:=20;
        END_IF
    20://Movement
        Axis.Profile_Velocity:=udiProfileVelocity;
        Axis.Target_Position:=diTargetPosition;
        IF Axis.StatusWord.13 OR Axis.StatusWord.3 THEN//Axis Error
            iState:=999;
        ELSIF NOT xEnable THEN
            Axis.Profile_Velocity:=0;
            Axis.Target_Position:=0;
            iState:=0;
        END_IF
    999://Error wait
        Axis.Profile_Velocity:=0;
        Axis.Target_Position:=0;
        tonTimeout(IN:=FALSE);
        xError:=TRUE;
        IF NOT xEnable THEN
            iState:=0;
        END_IF
END_CASE

  其它程序参考用户手册和注释还是比较易懂的，最常用的非插补运动就是Profile Position和Profile Velocity了。

5 总结

  运动控制对于初学plc的人来说是个神秘地带，其实它只是复杂一点的IO模块而已。不过，牵涉到插补运动就会复杂很多了，目前只有购买授权才能实现。
  OpenSML实现了SoftMotionLight（SML_Basic）的主要功能，不需要购买授权，且省去了查手册写程序的劳作。