CODESYS开发教程4-总线伺服控制

今天继续我们的小白教程，老鸟就不要在这浪费时间了😊。

前面一期我们讲了如何实现IO模块的添加和控制。这一期我们以合信的控制器和禾川的总线伺服驱动为例，主要给大家演示一下如何给控制器添加EtherCAT总线接口的伺服从站设备，以及如何使能和控制伺服电机运动。

一、总线伺服设备安装及添加

流程跟前面的教程一样，首先要找到设备描述文件，然后安装并添加到工程中。

1.设备文件下载及安装

由于手头只有禾川的X3E总线伺服，下面就以它为例来进行讲解。禾川的资料下载地址为：https://www.hcfa.cn/service/other.html

X系列伺服的设备描述文件下载地址：

https://www.hcfa.cn/ajax/download.aspx?id=4212&name=59714E0AFAFDE0D61A576AFD16F8A497

该文件解压后可以得到一个XML文件，需要在CODESYS编程环境的菜单栏“工具à设备存储库”中安装该描述文件。

安装完成后在“设备存储库”下方的“现场总线àEthercatà从站àHCFA Co.,Ltd-X3E-DRIVER”下面能够看对应的设备“HCFA X3E Servo Driver”。

注意，在使用大部分的第三方总线伺服驱动时，通常都需要按照以上步骤下载对应的设备文件并安装对应设备，所以需要确认设备文件跟控制器在型号是否是对应的，否则即使安装了后面也有无法正常连接和控制伺服。

2.伺服设备添加

我们仍然使用前面创建的“testCTH0”工程，添加总线伺服设备首先需要添加EtherCAT主站，在设备里面的工程名“testCTH0”上点右键，为该工程添加“EtherCAT Master SoftMotion”。

然后在设备树中的“EtherCAT Master SoftMotion”上点右键添加设备，在打开的窗口中选择刚才安装的设备“现场总线àEthercatà从站àHCFA X3E Servo Driver”。

伺服添加完成后如下图所示：

按照以上方式，可以依次添加多个伺服。请注意这里伺服在设备树下的顺序，即代表伺服的实际连接顺序，因此伺服的排列顺序和型号需要实际伺服的连线一一对应。另外，设备树下伺服的个数和实际连接的个数也必须要一样。d

二、伺服控制编程

下面我们用这个控制器来做一个简单的伺服电机正反转程序，给大家演示一下如何使能伺服并控制电机运动。

与第一期类似，首先需要通过在“Application”上点右键选择“添加对象”添加必要的模块，即任务配置和POU，POU的名字仍然用main，类型还是选择程序。然后在“任务配置àEtherCAT_Task”中添加对main程序的调用，这里间隔需要根据伺服支持的周期修改为4ms，添加后如下图所示：

这里需要注意的是，在添加总线设备“EtherCAT_Master_SoftMotion”时，会在任务配置中自动添加“EtherCAT_Task”任务，该任务用于EtherCAT主站与伺服从站通信。任务优先级通常为1，类型为循环，定时周期4ms。这里需要注意以下几点：

（1）如果还有其它任务，建议任务优先级要低于总线任务优先级，即优先级>1；

（2）“EtherCAT_Task”任务中不要执行耗时较长的程序，如文件读写，以免总线通信超时导致报错。

（3）“EtherCAT_Task”任务的定时周期必须是总线伺服支持的。大部分总线伺服支持的通信周期为：1ms、2ms、4ms、8ms。使用不支持的周期总线通信会报错。

1.伺服通信结构定义

首先需要定义一个用于和伺服通信的数据结构。在“Application”上点右键，选择“添加对象àDUT”。

在打开的“添加DUT”页面中，将名称改为“MotorAxis”，类型选“结构“。

结构中定义的数据其实是来自总线伺服的接口定义，双击打开前面添加的总线伺服设备“HCFA_X3E_Servo_Driver”，选择“过程数据”即可看到。

其中控制伺服需要用到的变量如下表所示。

输入输出	名称	变量类型	变量名	说明
输入	Control Word	UINT	Controlword	控制字
	Modes of operation	SINT	Mode	伺服模式
	Targetposition	DINT	targetPosition	目标位置
输出	Error Code	UINT	Errorcode	错误码
	Status Word	UINT	Statusword	状态字
	Position actual value	DINT	ActPosition	实际位置

MotorAxis结构定义如下：

TYPE MotorAxis :

STRUCT

       Controlword         : UINT;           //控制字

       Mode                   : SINT;           //模式

       targetPosition       : DINT;           //目标位置 pulse

       Errorcode             : UINT;           //错误码

       Statusword           : UINT;           //状态字

       ActPosition           : DINT;           //实际位置

       Digitalinputs         : UDINT;        //数字输入

       //内部使用变量

       iState                   : INT;             //内部状态切换

END_STRUCT

END_TYPE

2.伺服使能流程

这块涉及到EtherCAT总线伺服的启动流程，整个流程主要分为两部分，第一部分是检测所有连接的伺服是否进入可操作状态（OP状态），第二部分是对进行伺服使能处理以保证后续程序能够正常控制电机运动。

从站状态检测过程可以简单粗暴的直接用延时来替代，即程序启动后延时一段时间，等所有从站初始化完成进入OP状态后再开始进入使能流程。这里需要注意的问题是，连接的从站越多，等待从站完成初始化的延时时间越长，请朋友们根据自己连接的从站数量调整延时时间。

对于新建工程，要使用定时器TON需要为工程添加相应的CODESYS库，该功能位于库Standard中。在设备树中双击“库管理器”，在打开的页面中点击左上角的“添加库”，找到下面红框中的Standard库并加入。当你不知道所用函数具体在哪个库中的时候，可以在“添加库”打开的页面中直接输入函数或功能块名，如果名字没错的话包含该函数的库大概率是排在第一位的，直接选择加入即可。如果这样找不到，还可以到CODESYS的在线帮助中去搜索。

由于整个使能过程对新手来说比较复杂，这里先提供一个使能代码给大家使用，具体的流程在以后的文章中会详细的介绍。使能流程的代码如下：

FOR i := 0 TO AxisNum-1 DO

       IF bSetServOn THEN //使能

              IF (UINT_TO_DWORD(g_Axis[i].Statusword) AND 16#000F) = 16#0008 THEN

                     g_Axis[i].targetPosition := g_Axis[i].ActPosition;

              ELSIF ((UINT_TO_DWORD(g_Axis[i].Statusword) AND 16#004F) = 16#0040) THEN

                     g_Axis[i].targetPosition := g_Axis[i].ActPosition;

                     curPos[i] := g_Axis[i].ActPosition; //记录初始位置

                     g_Axis[i].iState := 1; //Switch on disabled

              ELSIF (UINT_TO_DWORD(g_Axis[i].Statusword) AND 16#002F) = 16#0021 THEN

                     g_Axis[i].iState := 2; //Ready to Switch on

              ELSIF (UINT_TO_DWORD(g_Axis[i].Statusword) AND 16#002F) = 16#0023 THEN

                     g_Axis[i].iState := 3; //Switch on

              ELSIF (UINT_TO_DWORD(g_Axis[i].Statusword) AND 16#003F) = 16#0037 THEN

                     g_Axis[i].iState := 4; //Servo on

              END_IF

              CASE g_Axis[i].iState OF

                     0: g_Axis[i].Controlword := 128; //reset

                     1: g_Axis[i].Controlword := 6;      //上电

                     2: g_Axis[i].Controlword := 7;

                     3: g_Axis[i].Controlword := 15;

                     4: bRegulatorState[i] := TRUE;

              END_CASE

       ELSE //掉使能

              CASE g_Axis[i].iState OF

                     3,4:

                            g_Axis[i].Controlword := 7;

                            g_Axis[i].iState := 5;

                     5:

                            g_Axis[i].Controlword := 6;

                            g_Axis[i].iState := 6;

                     6:

                            IF (UINT_TO_DWORD(g_Axis[i].Statusword) AND 16#0023) = 16#0023 THEN

                                   g_Axis[i].Controlword := 4;

                                   g_Axis[i].iState := 7;

                            END_IF

                     7:

                            bRegulatorState[i] := FALSE;

                            g_Axis[i].iState := 0;

                            //bSetServOn:=FALSE;

                            bServOndone:=FALSE;

              END_CASE

       END_IF

END_FOR

3.伺服运动控制

当伺服进入使能状态后，对伺服的控制主要是给每个伺服周期性发送目标位置，并检测伺服是否存在错误。由于采用的是绝对值伺服，因此不需要执行回零。下面的程序实现了伺服正反转的过程

程序如下：

PROGRAM main

VAR

       i                    : INT;

       delay             : TON;

       bSetServOn   : BOOL   :=FALSE; //开始使能标志

       bServOndone : BOOL;                       //所有伺服使能完成标志

       AxisNum       : INT:=2;

       g_Axis           : ARRAY [0..1] OF MotorAxis; //实轴定义

       HMI_bServon : BOOL:=FALSE;    //使能

       HMI_uiErrID   : INT:=0;               //报错标志

       bRegulatorState :ARRAY[0..1] OF BOOL:=[2(FALSE)];      //使能成功

       curPos           : ARRAY[0..1] OF DINT; //起点位置

       cmdPos         : INT:=150; //命令位置

       cnt                : DINT:=0;

END_VAR

//以下为程序部分

//正反转测试

IF bServOndone THEN

       FOR i:=0 TO AxisNum-1 DO

              IF bRegulatorState[i] THEN

                     IF    g_Axis[i].targetPosition-curPos[i]>500000 THEN

                            cmdPos:=-100;

                     ELSIF g_Axis[i].targetPosition-curPos[i]<-500000 THEN

                            cmdPos:=100;

                     END_IF

                     g_Axis[i].targetPosition:=g_Axis[i].targetPosition+cmdPos;

              END_IF

       END_FOR

       cnt:=cnt+1;

       IF (cnt MOD 49999)=0 THEN

              HMI_bServon:=NOT HMI_bServon;

       END_IF

END_IF

//伺服错误检测

IF g_Axis[0].Errorcode<>0 OR g_Axis[0].Errorcode<>0 THEN

       HMI_uiErrID := 1;

END_IF

二、总线伺服配置

伺服控制程序实现后，需要将控制程序中的变量与伺服的控制信号对应起来，这个过程通常称为映射，永固保证每个周期发送给伺服的目标位置能够传递给伺服并从伺服读取状态和数据。映射的方式和前面IO模块的映射类似。

双击设备树中的伺服模块“HCFA_X3E_Servo_Driver”，在打开的窗口中选择“EtherCATI/O映射”，然后在右边窗口中选择“Cotrol Word”对应的变量，双击下图椭圆红框中的空白处，在其右边会出现“…”。

点击这个省略号会弹出下图窗口。在该窗口中选择“Applicationàmainàg_Axis”，即前面在main程序中定义的代表伺服接口的变量。

完成后还需要在g_Axis后面增加“[0]”，表示数组的第0个结构对应设备树中的第一个伺服。其余的变量按照同样的方式一一对应，完成后如下图所示：

同样的方式完成第二个伺服的接口变量的映射，第二个伺服对应的接口变量是g_Axis[1]。

这样就完成了main程序中的伺服接口变量g_Axis与伺服驱动的对应。后续在程序中对g_Axis的操作会由总线任务“EtherCAT_Task”周期性的将更新到伺服并从伺服读取相应的状态，目前主站与伺服的通信周期为4ms。

三、运行测试

今天写到这里突然发现手头居然没有合信的控制器了，只有一个禾川的Q0，这是个什么情况？难道要把前面的图和文章都重新来一遍（作者一副满脑袋问号的傻相）？想想还是算了吧。说实话，运行测试其实也没有什么东西可看的，就是2个电机来回的转~~~~

为了证明这个程序是能用的，下面用禾川的Q0控制器来跑一下，顺便填一下前面没有具体写怎么连接控制器的坑。

1.连接控制器

禾川的Q0控制器的IP地址为192.168.88.100，因此连接控制器的上位机IP必须在同一网段内，这里设置为192.168.88.101。然后在工程是设备树中双击“HCQ0_1200_D”，在打开的控制器设备页面中，选择“通讯设置à扫描网络”，在之后弹出的窗口中，点右边的“扫描网络”按钮，只要设置和连接正确，就会在Gateway下面显示当前连接的控制器。

选择该控制器后，回到设备页面能够看到设备图片上会出现绿色圆点，表明控制器已经连接成功。

在连接禾川的EtherCAT总线伺服过程中，还有一个需要配置的内容，这就是指定“EtherCAT_Master_SoftMotion”使用哪个网络，即指定MAC地址，如下图所示。需要选择其中名为“ecat1”的这个MAC地址。

也可以在该配置页直接选择“按名称选择网络”，网络名称填“ecat1”。这部分每个公司的控制器可能有所区别，需要根据品牌来做相应的设置。

2.运行程序

首先按F11编译程序，在确认没有错误的情况下登录控制器（菜单栏“在线à登录到”），并下载程序。编程环境会提示是否下载最新代码，当然是选择是，不然我们要干啊？程序下载完成后，控制器默认是停止运行的状态。然后按F5启动程序。由于没有外部输入信号，这里可以直接使用强制方式将HMI_bServon置为TRUE，让程序能够开始运行。

运行测试感觉好像没什么可写的，因为程序的功能开发和测试实际上就是一个反复的迭代过程，需要大家自己通过反复实践来仔细体会~~~。

当你把这个过程重复2^10次以后，就能深刻体会为什么程序员被称为码农……^o^。

四、结论

总线伺服的使用相对于IO来说，不管是配置还是控制都要更加复杂一些。不过一步一步学到现在，有没有发现很多步骤其实都是类似的。只要对需要经常使用的功能有了基本的概念，用起来就感觉没那么难了。实际上，CODESYS开发环境的大部分功能对于我们来说都是干扰项，刚开始的时候不要管它就好了^-^。恭喜各位又离成为码农近了一步，继续加油~~~

------------------

原创不易，感兴趣的多支持！