机器人动起来[2]:从Simulink模型到倍福PLC项目

前言

​ 通过TE 1400 （TwinCAT 3 Target For Simulink）的封装，使用Simulink Coder自动转换为C/C++代码，Simulink中的控制模型可以在TwinCAT 3中使用，从而方便地将仿真中的控制算法迁移进实机控制中需求的C代码，同时嵌入在TwinCAT 3中的模型框图也可用作参数调整、调试、信号和状态监测的控件。
​ Simulink模型通过TE 1400组件导出过程详见笔者此前文章[^1]，本篇教程将从TwinCAT 3软件已在TcCOM Objects顺利导入模型开始，即开始状态如图所示，最终实现一个完整的TwinCAT PLC项目的建立。

image-20241029110335605

先导知识

TwinCAT 3的软件结构

Modules

TwinCAT 3 中的 Module 类似于高级语言中的类（Class），每个 Module 是一个包含特定功能的模块化软件单元。

  • 内存定义：在 Module 中，开发者可以定义一块内存区域，并在这块内存中指定接口变量和内部变量。
  • 变量关系：通过代码设定各变量之间的逻辑和关系。
    • 接口变量 用于 Module 之间的数据交互。
    • 内部变量 用于 Module 内部的逻辑处理。
  • 封装数据和行为：Module 的作用之一是封装数据和行为，提供一定程度的功能复用，支持灵活的模块化开发。
  • 复用性：每个 Module 可以独立开发、测试和调试，并且能够在其他系统或项目中复用。

经TE 1400导出后的Module存储路径为：C:\ProgramData\Beckhoff\TwinCAT\3.1\CustomConfig\Modules；通过模块化的设计，TwinCAT 3 系统能够灵活、快速地适应复杂的工业控制应用。

Object

TwinCAT 3 中的 Object 用于抽象和管理控制系统中的各类组件，通过模块化设计和面向对象的方式提升系统的灵活性和可扩展性，对于TE 1400导出的Object需要手动指定Task。

  • 模块化单元：每个 Object 是一个独立的模块单元，包含变量和逻辑，用于特定功能的实现。
  • 层次结构：支持嵌套的层次结构，便于系统中各模块的组织和管理。
  • 变量管理：包括接口变量（用于模块间通信）和内部变量（用于模块内部逻辑）。
  • 功能方法：定义对象的行为和控制逻辑，例如启动、停止等功能。
  • 唯一标识：每个 Object 有唯一标识符或地址，便于访问和系统通信。

Task

在 TwinCAT 3 中，Task 是所有Object执行的唯一入口，每个 Task 可以配置不同的执行周期和优先级，以确保控制逻辑的实时性和确定性。以下是 TwinCAT 3 中 Task 的主要功能和作用：

  • 实时任务调度：Task 负责调度系统中的控制程序，使得各模块的逻辑在设定的周期内运行，从而实现高效和实时控制。

  • 多任务并行执行：TwinCAT 3 支持多核 CPU 系统上运行多个 Task，可以配置不同的 Task 并行执行不同的逻辑（如 PLC 逻辑、运动控制、I/O 处理等），多核系统要手动指定Task运行的CPU。

  • 优先级管理：每个 Task 可以设置优先级，高优先级的 Task 会优先执行，确保关键任务的实时性和响应性。

  • 周期性配置：Task 的执行周期（Cycle ticks）可以自由配置（如 1 ms、10 ms 等），Task的周期必须是Base Time的整数倍，注意周期的数值要与封装Simulink模型时求解器固定步长时的设定相同。

  • 资源管理：Task 管理系统资源的分配和使用，包括 CPU 资源的占用率监控，避免系统资源争用带来的延迟。

  • 任务监控与诊断：TwinCAT 3 提供任务监控和诊断功能，便于用户实时查看 Task 的运行情况（如周期时间、执行状态等），及时发现并优化性能瓶颈。

Core

无论是单核还是多核CPU，都需要为TwinCAT用到的每个核指定Base Time和CPU Limit，多核CPU默认只有第一个核用于TwinCAT。

  • 首先要设置TwinCAT可以使用哪些核，分别可以用到的最大占用率CPU Limit是多少，以及每隔多长时间Base Time去检查一次是否要执行Task所调用的Object。
  • Base Time的默认值为1 ms，允许最小值为50 $\mu s$。

总结

TcCOM Module实例化为Object，Object由Task调用，Task设置Cycle ticks并指定到Core，Core需指定Base Time和CPU Limit。

  1. 经ADS路由连接至目标控制器，在Devices处右键选择Scan开始扫描设备和IO；
image-20241029193914765

编程PC操作

  1. 目标是希望Simulink模型不自动执行，而是被PLC条件调用，在Simulink模型中打开Model Configuration，将CallBy设置为Module；找到PLC Function Block，设置成Module specific FB with properties for all parameters，这样就会在模块生成的时候自动创建PLC调用此模块的功能块，点击apply；
image-20241030142601049
  1. 在Code Generation选项卡，修改target file为TwinCAT.tlc；
image-20241030143313294

​ 在Tc Build选项中可修改导出的模块名字；

image-20241030142916388
  1. 注意确认求解器为固定步长求解器，且已设置步长，插好加密狗后，确认dongle授权valid，即可点击编译模型，点击Simulink下方View Diagnostics可查看导出结果；
image-20241030143127395 image-20241030143041903 image-20241030143615745
  1. 打开TwinCAT 3 XAE shell，新建项目，在TcCOM Objects处点击鼠标右键，选择添加新项；
image-20241030143917400
  1. 点选刚才生成的Module，并点击ok；
image-20241030144114725
  1. 在PLC处点击鼠标右键，选择添加新项；
image-20241030144222643

​ 选择Standard PLC Project，修改好名称后，点击添加；

image-20241030144408144
  1. 在POUs处点击右键，选择Import PLCopenXML；
image-20241030144613626

​ 找到刚才导出的Module文件存放路径（注意此处可能和笔者路径不一样），选择PlcOpenPOUs.xml文件，并且点击open加入到PLC项目中；

image-20241030145914915

​ 点击ok确认添加；

image-20241030150001243
  1. 接下来开始写程序，首先对模块接口功能块进行变量声明，在oid中输入TcCOM中模块的Object ID，Object ID可以双击Object后查看；
image-20241030150227930

​ 双击MAIN(PRG)编写程序；

image-20241030150549689

​ 随后可以在程序中调用相应的method进行模块调用，也可以直接访问到输入输出等参数进行赋值，详见[^2]，具体代码如下，注意由于md语言不支持PLC的Structure Text代码高亮，为尽可能多地代码高亮，采用pascal语言展示：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
PROGRAM MAIN

VAR CONSTANT
	PI : REAL:=3.141592653;     //声明常量数值

END_VAR

VAR
	fb_test	:	FB_SingleLegTorqueCalcu(OID:=16#01010010);
	input:BOOL:=TRUE;           //条件调用PLC，input为真使能模型
	(****** System command ******)
	EnableJointFlag	 : ARRAY[1..6] OF BOOL:=[1,1,1,1,1,1];  //输入标志，指示是否使能电机
	System_Enable : BOOL;  //6个电机作为1个整体的使能标志
	enable_succeed: ARRAY[1..6] OF BOOL;   //输出标志，指示电机是否成功进入操作使能状态
	systemReadyCount : INT;    //电机使能成功计数变量

	Label : INT:=0;
	i: INT;
	j: INT;
	MOTORReductionRatio : ARRAY[1..6] OF INT:=[101,101,101,101,101,101];   //减速器减速比
	//Position_Initial1 : ARRAY[1..6] OF DINT:=[-15684647,-8089690,7254205,-1028930,7387277,-12992303]; 
	Position_Initial : ARRAY[1..6] OF DINT:=[0,0,0,0,0,0]; 

	Key1:DINT;//Y
	Key2:DINT;//B
	Key3:DINT;//A
	Key4:DINT;//X
	Key5:DINT;//LB
	Key6:DINT;//RB
	
	L1:DINT;//LT
	L2:DINT;//BACK
	R1:DINT;//RT
	R2:DINT;//START
	S1:DINT;//PRESS RIGHT
	S2:DINT;//PRESS LEFT
	
	Axis_x:LREAL;//LATERAL RIGHT
	Axis_y:LREAL;//LONGITUTAL RIGHT
	Axis_z:LREAL;//LATERAL LEFT
	Axis_r:LREAL;//LONGITUTAL LEFT

	parameter1_2:LREAL;
	parameter1_3:LREAL;
	
	modechange:INT:=0;
END_VAR


VAR_INPUT
	//// Inputs, get actual value from driver ////
	Statusword			AT %I*: ARRAY[1..6] OF UINT;
	ActualVelocity		AT %I*: ARRAY[1..6] OF DINT; // unit: 0.1 counts
	ActualPosition		AT %I*: ARRAY[1..6] OF DINT; // unit: counts
	TorqueActualValue	AT %I*: ARRAY[1..6] OF INT;	 // unit: 额定转矩/1000
	
END_VAR

VAR_OUTPUT
	//// Outputs, set target value to driver ////
	Controlword			AT %Q*: ARRAY[1..6] OF UINT;
	TargetVelocity		    AT %Q*: ARRAY[1..6] OF DINT; // unit: 1000rpm
	ModeOfOperation		AT %Q*: ARRAY[1..6] OF SINT; // 9:速度环,10:电流环
	TargetTorque		    AT %Q*: ARRAY[1..6] OF INT:=[0,0,0,0,0,0]; // unit: 额定转矩/1000
	TargetOffset1		    AT %Q*: ARRAY[1..6] OF INT:=[0,-400,0,0,0,0]; // unit: 额定转矩/1000
END_VAR



IF input THEN
	fb_test.Execute();	//input为真，则调用之前定义的功能块实例（Simulink模型）
END_IF

//读状态字，根据状态字发控制字，使能电机
FOR i:=1 TO 6 BY 1 DO 
	IF System_Enable=1 THEN 	//检验整个系统是否使能
		IF (EnableJointFlag[i]) THEN //检验系统中每个电机是否使能
		//状态字和2#1001111按位与操作，校验状态字的位7、位3、位2、位1、位0，根据当前状态和状态转换条件发送控制字
        	//检验状态字是否为Fault状态(2#0xx1000)
			IF (Statusword[i] AND (2#1001111))=2#0001000 THEN 
				Controlword[i]:=2#10000000;			//若为Fault状态，则Reset Fault，控制字2#1xxxxxx
			END_IF
			//检验状态字是否为Switch On Disabled状态(2#1xx0000)
			IF (Statusword[i] AND (2#1001111))=2#1000000 THEN
				Controlword[i]:=2#0110;			//若为Switch On Disabled状态，则Shutdown，控制字2#0xxxx110
				enable_succeed[i]:=FALSE;  		//使能标志为FALSE
			END_IF

		//状态字和2#1101111按位与操作，校验状态字的位7、位6、位3、位2、位1、位0，根据当前状态和状态转换条件发送控制字
            //检验状态字是否为Ready to Switch On状态（2#01x0001）
			IF (Statusword[i] AND (2#1101111))=2#0100001 THEN 
				Controlword[i]:=2#0111;			//若为Ready to Switch On状态，则Switch On，控制字（2#0xxx0111）
			END_IF
			//检验状态字是否为Switched On状态（2#01x0011）
			IF (Statusword[i] AND (2#1101111))=2#0100011 THEN 
				Controlword[i]:=2#1111;			//若为Switched On状态，则Enable Operation，控制字（2#0xxx1111）
			END_IF
			//检验状态字是否为Operation Enable状态（2#01x0111）
			IF (Statusword[i] AND (2#1101111))=2#0100111  THEN 
				enable_succeed[i]:=TRUE;  		//使能成功，标志为TRUE
			END_IF		 	
		ELSE
			Controlword[i]:=2#0110;
		END_IF
	ELSE
		Controlword[i]:=2#0110;  //电机使能标志为0，即EnableJointFlag[i]=0，则Shutdown，控制字2#0xxxx110
	END_IF
END_FOR
//	以下为手柄控制接口，调节单个电机，Key1对应Y键，Key2对应B键，Key3对应A键
IF (L1=1) THEN			//对应手柄LT键按下，系统使能
	System_Enable:=TRUE;
	ELSE
		System_Enable:=FALSE;
END_IF
IF (R2=1) THEN			//对应手柄Start键按下，各电机正向转动
	IF (Key1=1) THEN	//Y键按下，电机1正转
		TargetVelocity[1]:=20000;
		ELSE
			TargetVelocity[1]:=0;
	END_IF
	IF (Key2=1) THEN	//B键按下，电机2正转
		TargetVelocity[2]:=20000;
		ELSE
			TargetVelocity[2]:=0;
	END_IF
	IF (Key3=1) THEN	//A键按下，电机3正转
		TargetVelocity[3]:=20000;
		ELSE
			TargetVelocity[3]:=0;
	END_IF
	IF (Key4=1) THEN	//X键按下，电机4正转
		TargetVelocity[4]:=20000;
		ELSE
			TargetVelocity[4]:=0;
	END_IF
	IF (Key5=1) THEN	//LB键按下，电机5正转
		TargetVelocity[5]:=20000;
		ELSE
			TargetVelocity[5]:=0;
	END_IF
	IF (Key6=1) THEN	//RB键按下，电机6正转
		TargetVelocity[6]:=20000;
		ELSE
			TargetVelocity[6]:=0;
	END_IF
	ELSE					//对应手柄Start键未按下，各电机反向转动
		IF (Key1=1) THEN	//Y键按下，电机1反转
			TargetVelocity[1]:=-20000;
			ELSE
				TargetVelocity[1]:=0;
		END_IF
		IF (Key2=1) THEN	//B键按下，电机2反转
			TargetVelocity[2]:=-20000;
			ELSE
				TargetVelocity[2]:=0;
		END_IF
		IF (Key3=1) THEN	//A键按下，电机3反转
			TargetVelocity[3]:=-20000;
			ELSE
				TargetVelocity[3]:=0;
		END_IF
		IF (Key4=1) THEN	//X键按下，电机4反转
			TargetVelocity[4]:=-20000;
			ELSE
				TargetVelocity[4]:=0;
		END_IF
		IF (Key5=1) THEN	//LB键按下，电机5反转
			TargetVelocity[5]:=-20000;
			ELSE
				TargetVelocity[5]:=0;
		END_IF
		IF (Key6=1) THEN	//RB键按下，电机6反转
			TargetVelocity[6]:=-20000;
			ELSE
				TargetVelocity[6]:=0;
		END_IF
END_IF


//使能成功计数器
systemReadyCount := 0;
FOR i:=1 TO 6 BY 1 DO
	IF enable_succeed[i]=TRUE THEN
		systemReadyCount := systemReadyCount+1;
	END_IF
END_FOR

// 1为电流环，0位速度环
IF modechange=1 THEN
	ModeOfOperation[1]:=10;
	ModeOfOperation[2]:=10;
	ModeOfOperation[3]:=10;
	ModeOfOperation[4]:=10;
	ModeOfOperation[5]:=10;
	ModeOfOperation[6]:=10;
	ELSE
	ModeOfOperation[1]:=9;
	ModeOfOperation[2]:=9;
	ModeOfOperation[3]:=9;
	ModeOfOperation[4]:=9;
	ModeOfOperation[5]:=9;
	ModeOfOperation[6]:=9;
END_IF

//	反馈量传递
//	速度单位转换：每秒脉冲数 --> rad/s，单圈19位编码器，每转2^19=524288个脉冲，转数乘2*PI转换为rad，除以减速比得到输出端速度
//	角度单位转换：脉冲数	--> rad，单圈19位编码器，每转2^19=524288个脉冲，转数乘2*PI转换为rad，减速比只影响角速度，角度无须除以减速比
//	角速度（rad/s）赋值给Simulink模型的输入，在TwinCAT3中手动设置映射
fb_test.stInput.velocity1_1:=DINT_TO_LREAL(ActualVelocity[1])/10/21368*2*PI/MOTORReductionRatio[1];
fb_test.stInput.velocity1_2:=DINT_TO_LREAL(ActualVelocity[2])/10/21368*2*PI/MOTORReductionRatio[2];
fb_test.stInput.velocity1_3:=DINT_TO_LREAL(ActualVelocity[3])/10/21368*2*PI/MOTORReductionRatio[3];
fb_test.stInput.velocity1_4:=DINT_TO_LREAL(ActualVelocity[4])/10/17944*2*PI/MOTORReductionRatio[4];
fb_test.stInput.velocity1_5:=DINT_TO_LREAL(ActualVelocity[5])/10/17944*2*PI/MOTORReductionRatio[5];
fb_test.stInput.velocity1_6:=DINT_TO_LREAL(ActualVelocity[6])/10/17944*2*PI/MOTORReductionRatio[6]; 

//	角度（rad）赋值给Simulink模型的输入，在TwinCAT3中手动设置映射
fb_test.stInput.position1_1:=DINT_TO_LREAL(ActualPosition[1]-Position_Initial[1])/524288*2*PI;
fb_test.stInput.position1_2:=DINT_TO_LREAL(ActualPosition[2]-Position_Initial[2])/524288*2*PI;
fb_test.stInput.position1_3:=DINT_TO_LREAL(ActualPosition[3]-Position_Initial[3])/524288*2*PI;
fb_test.stInput.position1_4:=DINT_TO_LREAL(ActualPosition[4]-Position_Initial[4])/524288*2*PI;
fb_test.stInput.position1_5:=DINT_TO_LREAL(ActualPosition[5]-Position_Initial[5])/524288*2*PI;
fb_test.stInput.position1_6:=DINT_TO_LREAL(ActualPosition[6]-Position_Initial[6])/524288*2*PI;
parameter1_2:=DINT_TO_LREAL(ActualPosition[2]-Position_Initial[2])/524288*2*PI+PI/6;
parameter1_3:=DINT_TO_LREAL(ActualPosition[3]-Position_Initial[3])/524288*2*PI+PI/6;


//Count
fb_test.stInput.label:=DINT_TO_INT(L2);	//Back键控制llabel
fb_test.stInput.reset:=DINT_TO_INT(R1);	//RT键控制reset

//转矩单位转换，将Simulink模型中输出的转矩（N*m）转换为电机转矩（千分之一额定转矩）
//	RGM25 输出端额定转矩118N*m，RGM20 额定转矩61N*m

TargetTorque[1]:=LREAL_TO_INT(fb_test.stOutput.torque1_1/118*1000);
TargetTorque[2]:=LREAL_TO_INT(fb_test.stOutput.torque1_2/118*1000);
TargetTorque[3]:=LREAL_TO_INT(fb_test.stOutput.torque1_3/118*1000);
TargetTorque[4]:=LREAL_TO_INT(fb_test.stOutput.torque1_4/61*1000);
TargetTorque[5]:=LREAL_TO_INT(fb_test.stOutput.torque1_5/61*1000);
TargetTorque[6]:=LREAL_TO_INT(fb_test.stOutput.torque1_6/61*1000);

​ “IF input THEN”之前的语句为变量声明部分，之后的语句为调用执行函数部分，分别粘贴进MAIN函数对应区域；

image-20241030161545612
  1. 双击Object，在Context选项卡中分配模块Task为PLCTask（必须和所调用PLC的Task一致）；
image-20241030162328580
  1. 在TwinCAT 3 XAE的生成选项卡中点击生成解决方案；
image-20241030163016090

​ TwinCAT选项卡中，点击Active Configuration；

image-20241030162826988

​ 此时可能报错如下>>AdsError: 1828(0x724,ADSERROR: no license found)<<failed；

image-20241030162745440

​ 可能的一种解决方案为，远程工控机的倍福实时组件许可证过期，只需在SYSTEM-License中点击7 Days Trial License，并按提示输入5位验证码续期7天即可；

image-20241030163712483

参考链接

[^1]: TE 1400组件使用指南：https://blog.csdn.net/weixin_43807835/article/details/140323746；
[^2]: Simulink模型和Maxon驱动器接口文件编写：https://blog.csdn.net/weixin_43807835/article/details/140664483