CODESYS学习笔记

记录一下学习CODESYS遇到的一些知识点

1.运动控制的两种方式

在CODESYS中，与运动控制有关的，主要有两个库：SM3_CNC、SM3_Robotics

假设我们对轴已经配置（组态）完毕，那么SM3_CNC、SM3_Robotics对轴的操作逻辑上会有所不同。
简单来说，SM3_CNC是逐个轴控制、SM3_Robotics是整个轴组一起控制。

1.1.SM3_CNC

SM3_CNC需要单独对每一个轴进行Power、move等操作。同时，还需要引入一个运动学模型，比如：SMC_TRAFO_Gantry2。
运动学模型分为前向动力学模型、方向动力学模型。其中前向动力学是指输入各个电机的当前位置然后输出末端位姿的数学模型。反之，反向动力学是指输入末端的位姿，求解各个电机的位置的数学模型。

在CODESYS中，正向动力学模型一般命名为：SMC_TRAFOF_XXX，而反向动力学一般命名为：SMC_TRAFO_XXX。正向动力学求解肯定有且仅有唯一解；但是反向动力学求解，大多数情况下都存在多组解。

1.2.SM3_Robotics

对于SM3_Robotics则一般需要把各个轴绑定成一个轴组（AxisGroup），然后调用函数对轴组进行统一操作，比如：SMC_GroupPower、MC_GroupEnable等等。
SM3_Robotics在进行运动控制时，也是需要进行反向动力学（可能也需要正向动力学）的配置，并且也是使用前面提到的SMC_TRAFOF_XXX，但是这个配置不用我们再单独使用SMC_TRAFOF/SMC_TRAFO之类的功能块，而是在轴组的配置界面中配置即可。也就是，当我们设置好轴组后，轴组已经把自己封装、抽象成了一个运动控制对象，后续我们需要操作就直接对这个对象进行操作即可，而不用单独对其每一条轴进行操作。

从他们的函数使用关系来看，SM3_Robotics可能是在SM3_CNC的基础上拓展而来？

2.两种运动控制方式的速度、加速度等参数的控制

2.1.SM3_CNC

在CNC中，通过解析G代码，取得各个插值点以及其具体的速度、加速度等值，存放在一个SMC_OUTQUEUE中。解析得到的代码可以这样查看到：在你的CNC对象上右键、选择CNC信息。

然后在弹出的窗口中点击【显示生成的代码】。从下面可以看到，的确是实例化了一个SMC_OUTQUEUE类型的变量。

通过把这个queue传递给插补器，让插补器根据queue里面的离散点位进行更多点位的插补。通过查看SMC_OUTQUEUE的元素类型，可以知道其包含了速度、位置等信息。

插补器功能块（SMC_Interpolator）通过插补后，输出的piSetPosition包含当前需要执行的位置的相关参数。

2.2.SM3_Robotics

在SM3_Robotics中，基本是手动设置目标位置的参数。但是不同的移动函数关于速度、加速度的使用是不同的。

假如使用的是MC_MoveCircular、MC_MoveLinear、SMC_GroupJog2，那么是可以设置速度、加速度等参数的。
但是假如使用的是MC_MoveDirectAbsolute，是无法直接设置具体数值，只能设置比例系数xxxFactor。
具体情况请参考帮助手册。

3.CNC的M指令的使用（实现）逻辑。

参考：【CODESYS运动控制之如何使用M功能.docx】

办法是监听SMC_Interpolator的wM输出（可能还需要配合iStatus），在检测到wM输出有效值时，利用wM值进行相应的操作，操作完后设置一下bAcknM,告知SMC_Interpolator，让其继续执行后面的代码。
也就是说，此时SMC_Interpolator只是当了个传话筒的作用，具体的功能还是得自己根据SMC_Interpolator给过来的wM值来实现具体的功能。

4.SM3_Robotics中的坐标系

在机械手控制中，有若干个坐标系类型：

ACS： axis coordinate system
轴坐标系，当使用此坐标系时，每个参数都表示对应的轴的当前角度/位置

MCS：The machine coordinate system (MCS) is defined by the applied kinematics which determine its position and orientation.
机器坐标系。参数一般是：x, y, z, pitch, roll, yaw。机器坐标一般是以机械臂的底座为原点。

WCS： world coordinate system
世界坐标系。一般来说，机器坐标系和机器坐标系是平移关系。
引入世界坐标系的目的，可能是为了多个机器/机械手/传感器等设备的配合。

TCS：tool coordinate system
工具坐标系，也就是机械臂末端的坐标系。在进行一些特殊移动时，比较方便。

5.SM3_Robotics如何实现插补并连续执行？

不知道在机械手控制中能否使用插补？假如可以的话，那我就可以把ROS规划的路径trajectory发送给PLC执行了。
从理论上应该是可以的，但是有个问题。插补器插补出来的数据，虽然比原来的更加稠密，但本质上还是位置点，CNC那边是怎么解决点与点之间的连接、过渡的？总不能走一个点停一下，然后再走另外一个点吧？
从例程看，CNC使用的是SMC_ControlAxisByPos。这个功能块看起来是起到动态控制位置的，也就是说它一直都在动态地检测传递过来的位置信息，一旦与当前的位置不符，就执行移动。这样，哪怕不断地传递新的点位过来，也可以保证运动是连续的。

而且从其使能参数bEnable来看，它是在true之后，一直在执行，相当于一个状态。

但是对于轴组，假如我调用MC_MoveLinearAbsolute 来执行，那肯定不可以吧。因为这个功能块在走到了目标位置就会停下来。关于这点，可以从它的执行（是能）参数Execute的介绍可以看到，它是在此参数的上升沿才执行一次的。难道需要到一个点位后，再继续传输下一个点位，然后在同一个扫描（执行）周期内把Execute参数先设置为false然后再设置为true？
假如调用了MC_MoveLinearAbsolute，应该就不用插补器了。因为这个功能块应该内部已经插补了。

大概研究了一下SMC_GroupUpdateContinueData 这个功能块，它可能是和其他功能块配合实现运动时暂停然后再继续原来的路径的。不是用来执行包含了若干个点的路径的。
所以，只能先把需要经过的路径点保存起来，然后用MC_MoveLinearAbsolute逐一把各个点位走完？

6.Jerk的意思

在使用运动控制功能块时，经常可以看到Jerk参数：

之前还以为是什么急停参数。后来看了资料才知道，是加加速度的意思。【运动控制中的Jerk】

7.关于SMC_GroupJog2中的ABC_as_ACS参数的意义

在轴组点动控制功能块SMC_GroupJog2中，有一个有点奇怪的函数ABC_as_ACS
我们结合例程（C:\Program Files (x86)\CODESYS 3.5.19.40\CODESYS\CODESYS SoftMotion\Examples\Tutorial\Robotics_Jogging.project）来理解一下。
将其设置成false的时候，一切都很好理解。选择好对应的坐标系（ACS、MCS、WCS、TCS）后，点击并按住上方六个参数（X、Y、Z、A、B、C）的12个按钮中的任意一个，便可以对机械臂进行点动操作。在不选择ACS作为操作坐标系时，其中XYZ参数为空间坐标，ABC为对应的绕XYZ轴旋转的角度。
但是一旦勾选了ABC_as_ACS，有时就变得诡异了。
首先，不诡异的是ABC的jog，方向是与选择了ACS控制时一样，也就是ABC这三个是直接控制了最后三个轴的转动角度，这时候ABC_as_ACS（ABC的意义与选择ACS运动时一样）是字面意思、名副其实的。
但是，试图进行XYZ的运动时，选择不同的坐标系会有不同的效果。当选择MCS、WCS时，移动XYZ都正常，都是沿着对应的XYZ轴进行移动。但是选择了TCS时，就不行了，XYZ移动的时候看不出是什么规律。
结论：排除选择了TCS的情况下，一旦勾选了ABC_as_ACS，那么XYZ移动时是在笛卡尔空间中对末端进行点动，而ABC则直接控制了最后三个轴的转动，同时其他轴会进行相应的补偿，从而使末端的位置不发生改变（即仅改变末端的姿态）。