色标补偿功能块实现详解

色标补偿功能块实现详解

功能概述

本功能块用于实现色标信号的自动补偿，主要应用于印刷、包装等需要精确套色的自动化设备。通过实时采集色标信号，计算补偿值并输出，实现高精度的套色控制。

程序结构

功能块采用结构化编程，主要包含以下部分：

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
FUNCTION_BLOCK DFB_CompensateOneFlag
VAR_IN_OUT
    // 色标抓取轴
    inAxis1: AXIS_REF_SM3;
END_VAR
VAR_INPUT
    iTrig: INT;
    // 功能块启动
    bEnable: BOOL;
    // 色标信号
    bInput1: BOOL;
    // 主轴长度设置
    lrLength: LREAL;
    // 排版数
    lrQueue: LREAL;
    // 牵引速度偏移
    lrAdjust: LREAL;
    // 最大修正值
    lrMaxCorrectionValue: LREAL;
    // 修标目标长度
    lrTargetValue: LREAL;
END_VAR
VAR_OUTPUT
    bDone: BOOL;
    bBusy: BOOL;
    // 抓取误差值输出
    lrValueOut: LREAL;
    bError: BOOL;
    dwErrorID: DWORD;
    lrFirstValueOut: LREAL;
END_VAR

主要功能实现

1. 初始化处理

1
2
3
4
5
6
7
8
IF bDone THEN 
    bDone := FALSE;
END_IF

// 位置归零功能块复位
IF FB_SetPosition1.Done OR FB_SetPosition1.Busy OR FB_SetPosition1.Error THEN 
    FB_SetPosition1.Execute := FALSE;
END_IF

2. 功能块启动

1
2
3
4
5
6
7
dR_TRIG[1](CLK := bEnable);
IF dR_TRIG[1].Q THEN
    bError := FALSE;
    lrFirstData[3] := 0;
    lrFirstData[2] := 0;
    lrFirstData[1] := 0;    
END_IF

3. 高速抓取处理

1
2
3
4
5
TON1(IN := NOT MC_TouchProbe.Done, PT := T#2MS, Q => , ET => );
bBusy := TRUE;
IF TON1.Q THEN 
    MC_TouchProbe.Execute := TRUE;
END_IF

4. 误差计算与补偿

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
IF bEnable AND bBusy THEN
    R1(CLK := ABS(Axis1^.fActPosition*lrVirtualTractionSpeedRatio)-lrFirstData[1] > ((lrQueue*lrLength)*1.2), Q => );
    R3(CLK := MC_TouchProbe.Done, Q => );
    
    IF R3.Q THEN
        lrFirstData[5] := lrFirstData[4];
        lrFirstData[4] := lrFirstData[3];
        lrFirstData[3] := lrFirstData[2];
        lrFirstData[2] := lrFirstData[1];
        lrFirstData[1] := lrRecorPOS;
        
        IF lrFirstData[2] > 0 THEN
            // 误差计算逻辑
            lrD_value := (MATH.fmod(lrX := lrFirstData[1], lrM := (lrQueue*lrLength)) - 
                         (MATH.fmod(lrX := lrFirstData[2], lrM := (lrQueue*lrLength));
            lrValueOut := MIN(MAX(lrD_value, -lrMaxCorrectionValue), lrMaxCorrectionValue);
            bDone := TRUE;
        END_IF
    END_IF
END_IF

4. 完整程序

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
FUNCTION_BLOCK DFB_CompensateOneFlag

VAR_IN_OUT
	// 色标抓取轴
	inAxis1: AXIS_REF_SM3;
END_VAR

VAR_INPUT
	iTrig: INT;
	// 功能块启动
	bEnable: BOOL;
	// 色标信号
	bInput1: BOOL;
	// 主轴长度设置
	lrLength: LREAL;
	// 排版数
	lrQueue: LREAL;
	// 牵引速度偏移
	lrAdjust: LREAL;
	// 最大修正值
	lrMaxCorrectionValue: LREAL;
	// 修标目标长度
	lrTargetValue: LREAL;
END_VAR
VAR_OUTPUT
	bDone: BOOL;
	bBusy: BOOL;
	// 抓取误差值输出
	lrValueOut: LREAL;
	bError: BOOL;
	dwErrorID: DWORD;
	lrFirstValueOut: LREAL;
END_VAR
VAR
	Axis1: POINTER TO AXIS_REF_MAPPING_SM3;
	DMC_TouchProbeCyclically1: DMC_TouchProbeCyclically;
	dR_TRIG: ARRAY[1..10] OF R_TRIG;
	lrFirstData: ARRAY[1..5] OF LREAL;
	dataTrigger: DL_MotionControl.DMC_TRIGGER_REF;
	// 差值
	lrD_value: LREAL;
	// 抓取值
	lrGrabvalue: LREAL;
	// 高速抓取输出数值
	lrRecorPOS: LREAL;
	// 检测长度
	lrMeasurePos: LREAL;
	FB_SetPosition1: MC_SetPosition;
	lrD_value2: LREAL;
	R3: R_TRIG;
	R1: R_TRIG;
	// 丢标位置
	lrLossPos: LREAL;
	// 主轴牵引轴速比
	lrVirtualTractionSpeedRatio: LREAL;
	AA: LREAL;
	BB: LREAL;
	lrRecordedPosition1: LREAL;
	MC_TouchProbe: MC_TouchProbe;
	Trigger: SM3_Basic.TRIGGER_REF;
END_VAR
VAR_OUTPUT
	RecordedPosition: LREAL;
END_VAR
VAR
	TON2: TON;
	TON1: TON;
END_VAR


//无编码器色标功能块（单个色标信号）

Axis1:=ADR(inAxis1);

IF	bDone THEN 
	bDone:=FALSE;
END_IF	


//位置归零功能块复位
IF 	FB_SetPosition1.Done OR  FB_SetPosition1.Busy  OR 	FB_SetPosition1.Error THEN 
	FB_SetPosition1.Execute	:=FALSE;
END_IF


//功能块启动
dR_TRIG[1](CLK:=bEnable);
IF 	dR_TRIG[1].Q
	THEN
//MC_TouchProbe.Execute		:=TRUE;//高速循环抓取功能块启动
//	FB_SetPosition1.Execute					:=TRUE;//位置归零功能块启动
	bError									:=FALSE;
	lrFirstData[3]							:=0;
	lrFirstData[2]							:=0;
	lrFirstData[1]							:=0;	
END_IF	

(********************功能块运行中显示***********************)


IF 	bEnable
	THEN 
	TON1(IN:=NOT MC_TouchProbe.Done , PT:=T#2MS , Q=> , ET=> );
	bBusy		:=TRUE;
	IF  TON1.Q THEN MC_TouchProbe.Execute:=TRUE;END_IF
END_IF


(*******************循环高速抓取功能块************************)	
TON2(IN:= , PT:=T#2MS , Q=> , ET=> );		
IF MC_TouchProbe.Done THEN TON2.IN:=TRUE;END_IF
IF TON2.Q THEN MC_TouchProbe.Execute:=FALSE; TON2.IN:=FALSE; END_IF

Trigger.iTriggerNumber		:=iTrig;		//抓取点选择
Trigger.bFastLatching:=1;
//Trigger.bInput:=
//Trigger.bActive:=
MC_TouchProbe(Axis:= Axis1^, TriggerInput:= Trigger, WindowOnly:= FALSE, RecordedPosition=>RecordedPosition );



IF	bEnable  THEN 
	//数值输出
	lrVirtualTractionSpeedRatio:=(lrLength)/((lrLength+lrAdjust));
	lrRecorPOS:=MC_TouchProbe.RecordedPosition*lrVirtualTractionSpeedRatio;
	FB_SetPosition1(Axis:= inAxis1 , Position:= 0, );
	//报警输出
	IF 	MC_TouchProbe.Error OR 	FB_SetPosition1.Error OR  (lrMaxCorrectionValue <= 0) OR  (Axis1^.fActPosition*lrVirtualTractionSpeedRatio > (lrLossPos+(lrQueue*lrLength)*3)) THEN 
		bError :=TRUE;
	END_IF
	IF 	MC_TouchProbe.Error OR 	FB_SetPosition1.Error THEN 
		dwErrorID:=1;
	END_IF
	IF (lrMaxCorrectionValue <= 0)THEN 
		dwErrorID:=2;
	END_IF
	AA:=Axis1^.fActPosition*lrVirtualTractionSpeedRatio;
	BB:=lrLossPos+(lrQueue*lrLength)*3;
	IF bBusy AND(Axis1^.fActPosition*lrVirtualTractionSpeedRatio > (lrLossPos+(lrQueue*lrLength)*3)) THEN 
		dwErrorID:=3;
	END_IF
	
	ELSE	//******信号复位，数据归零
		bError									:=FALSE;
		FB_SetPosition1.Execute					:=FALSE;
		MC_TouchProbe.Execute		:=FALSE;
		lrFirstValueOut							:=0;
		lrLossPos								:=0;
		lrFirstData[5]							:=0;
		lrFirstData[4]							:=0;
		lrFirstData[3]							:=0;
		lrFirstData[2]							:=0;
		lrFirstData[1]							:=0;
		lrValueOut								:=0;
		lrD_value								:=0;
		dwErrorID								:=0;
END_IF

(*********************色标误差滤波、计算************************)		
IF 	bEnable AND bBusy THEN 
	R1(CLK:= ABS(Axis1^.fActPosition*lrVirtualTractionSpeedRatio)-lrFirstData[1]> ((lrQueue*lrLength)*1.2), Q=> );
	R3(CLK:=MC_TouchProbe.Done, Q=> );
	IF  R3.Q THEN 
		lrFirstData[5]				:=lrFirstData[4];
		lrFirstData[4]				:=lrFirstData[3];
		lrFirstData[3]				:=lrFirstData[2];
		lrFirstData[2]				:=lrFirstData[1];
		lrFirstData[1]				:=lrRecorPOS;
		IF lrFirstData[2]>0 THEN
			IF ((MATH.fmod(lrX:=(lrRecorPOS-lrFirstData[2]) , lrM:=(lrQueue*lrLength))>= (lrQueue*lrLength)*0.9))	//前80%误检测排查
			OR ((MATH.fmod(lrX:=(lrRecorPOS-lrFirstData[2]) , lrM:=(lrQueue*lrLength))<= (lrQueue*lrLength)*0.1))//超过袋长20%误检测排查 
				THEN 
				lrLossPos	:=lrRecorPOS;
				IF ABS(MATH.fmod(lrX:=lrFirstData[1],lrM:=(lrQueue*lrLength))-(MATH.fmod(lrX:=lrFirstData[2],lrM:=(lrQueue*lrLength))))<(lrQueue*lrLength)*0.05
					THEN 
					lrD_value	:= (MATH.fmod(lrX:=lrFirstData[1] , lrM:=(lrQueue*lrLength) )-(MATH.fmod(lrX:=lrFirstData[2] , lrM:=(lrQueue*lrLength))));
				END_IF
				IF (MATH.fmod(lrX:=lrFirstData[1],lrM:=(lrQueue*lrLength))-(MATH.fmod(lrX:=lrFirstData[2],lrM:=(lrQueue*lrLength))))>(lrQueue*lrLength)*0.05
					THEN
					lrD_value:=((MATH.fmod(lrX:=lrFirstData[1],lrM:=(lrQueue*lrLength))-(lrQueue*lrLength))-(MATH.fmod(lrX:=lrFirstData[2],lrM:=(lrQueue*lrLength))));
				ELSIF (MATH.fmod(lrX:=lrFirstData[1],lrM:=(lrQueue*lrLength))-(MATH.fmod(lrX:=lrFirstData[2],lrM:=(lrQueue*lrLength))))<-((lrQueue*lrLength)*0.05)
					THEN 
					lrD_value:=((MATH.fmod(lrX:=lrFirstData[1],lrM:=(lrQueue*lrLength))+(lrQueue*lrLength))-(MATH.fmod(lrX:=lrFirstData[2],lrM:=(lrQueue*lrLength))));
				END_IF	
				//	lrD_value:=(lrFirstData[1]-lrFirstData[2])-lrTargetValue;
				lrValueOut := MIN(MAX(lrD_value, -lrMaxCorrectionValue), lrMaxCorrectionValue);
// 				IF 	lrD_value	>= lrMaxCorrectionValue
// 					THEN 
// 					lrValueOut		:= lrMaxCorrectionValue;
// 				END_IF
// 				IF  lrD_value	<= (lrMaxCorrectionValue * -1)
// 					THEN 
// 					lrValueOut		:= (lrMaxCorrectionValue * -1);
// 				END_IF
// 				IF 	ABS(lrD_value) < lrMaxCorrectionValue
// 					THEN 
// 					lrValueOut		:= lrD_value;
// 				END_IF
				bDone				:=TRUE;
				ELSE
				lrFirstData[1]				:=lrFirstData[2];
				lrFirstData[2]				:=lrFirstData[3];
				lrFirstData[3]				:=lrFirstData[4];
				lrFirstData[4]				:=lrFirstData[5];
			END_IF
			ELSE
			lrFirstValueOut:=lrRecorPOS;	
		END_IF	
	END_IF
	IF R1.Q
		THEN
// 		lrFirstData[4]				:=lrFirstData[3];
// 		lrFirstData[3]				:=lrFirstData[2];
// 		lrFirstData[2]				:=lrFirstData[1];
// 		lrFirstData[1]				:=lrFirstData[1]+(lrQueue*lrLength);
		lrValueOut					:=0.0;
		bDone						:=TRUE;
	END_IF
END_IF

使用说明

输入参数

参数名 类型 说明
bEnable BOOL 功能块使能信号
bInput1 BOOL 色标输入信号
lrLength LREAL 主轴长度
lrQueue LREAL 排版数量
lrAdjust LREAL 牵引速度偏移量
lrMaxCorrectionValue LREAL 最大修正值
lrTargetValue LREAL 目标修正值

输出参数

参数名 类型 说明
bDone BOOL 完成标志
bBusy BOOL 忙标志
lrValueOut LREAL 补偿值输出
bError BOOL 错误标志
dwErrorID DWORD 错误代码

应用场景

  1. 印刷机械的套色控制
  2. 包装机械的色标定位
  3. 标签印刷机的色标跟踪
  4. 软包装设备的色标补偿

注意事项

  1. 确保输入参数在合理范围内
  2. 色标信号需要稳定可靠
  3. 主轴速度变化不宜过大
  4. 建议在使用前进行参数校准