-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathFB_VD_SEW_AMA_Bin.SCL
233 lines (207 loc) · 8.08 KB
/
FB_VD_SEW_AMA_Bin.SCL
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
(*****************************************************************************************
** **
** Copyright (c) 2019 Himmelt **
** All Rights Reserved Himmelt@SoraWorld **
** E-mail master@void-3.cn **
** **
******************************************************************************************
** Date Version Author Decsription **
------------------------------------------------------------------------------------------
** 2019.10.14 0.6 Himmelt Add FQR output signal **
** 2019.08.15 0.5 Himmelt Beta ; fix mode signals **
change speed/position to real number **
** 2019.08.13 0.4 Himmelt Beta ; add all signals **
** 2019.08.13 0.3 Himmelt Alpha; add K100 & K100K **
** 2019.08.13 0.2 Himmelt Alpha; add AMA address test **
** 2019.08.10 0.1 Himmelt Alpha; **
*****************************************************************************************)
FUNCTION_BLOCK FB_VD_SEW_AMA_Bin
TITLE = 'VD_SEW_AMA_Bin'
VERSION: '0.5'
AUTHOR: Himmelt
NAME: VD_AMA
FAMILY: VDEBUG
VAR_INPUT
Addr : INT := 0;
K100 : BOOL := false;
Pos_1 : DINT := 0;
Pos_2 : DINT := 0;
Pos_3 : DINT := 0;
Pos_4 : DINT := 0;
speed : INT := 0;
offset : DINT := 0;
Limit_L : DINT := 0;
Limit_R : DINT := 0;
END_VAR
VAR_TEMP
// AMA_A
inhibit : BOOL;// A1.0 controller inhibit/enable
notEStop : BOOL;// A1.1 enable/rapid stop
notStop : BOOL;// A1.2 enable/stop
reserve1 : BOOL;// A1.3 reserved
rampScaling: BOOL;// A1.4 ramp scaling
reserve2 : BOOL;// A1.5 reserved
reset : BOOL;// A1.6 reset
fastMode : BOOL;// A1.7 fast fast/slow fast
start : BOOL;// A0.0 start
jogCW : BOOL;// A0.1 jog cw
jogCCW : BOOL;// A0.2 jog ccw
mode_0 : BOOL;// A0.3 mode 2^0
mode_1 : BOOL;// A0.4 mode 2^1
mode_2 : BOOL;// A0.5 mode 2^2
synchronize: BOOL;// A0.6 synchronize offset
softLimit : BOOL;// A0.7 software limit switch off
targetPos1 : BOOL;// A3.0 single bit position 1
targetPos2 : BOOL;// A3.1 single bit position 2
targetPos3 : BOOL;// A3.2 single bit position 3
targetPos4 : BOOL;// A3.3 single bit position 4
// AMA_E
axisSync : BOOL;// E1.0 axis synchronous/axis turns
invReady : BOOL;// E1.1 inverter ready
IPOS_Ref : BOOL;// E1.2 IPOS referenced
inPosition : BOOL;// E1.3 in position/axis standstill
notBreak : BOOL;// E1.4 break released
fault : BOOL;// E1.5 inverter fault/warning
limitCW : BOOL;// E1.6 limit switch cw
limitCCW : BOOL;// E1.7 limit switch ccw
invStatus : BYTE;// EB0 inverter status/fault(bit 0-8)
signalPos1 : BOOL;// E7.0 single bit position 1
signalPos2 : BOOL;// E7.1 single bit position 2
signalPos3 : BOOL;// E7.2 single bit position 3
signalPos4 : BOOL;// E7.3 single bit position 4
signalCam1 : BOOL;// E9.0 single bit cam 1
signalCam2 : BOOL;// E9.1 single bit cam 2
signalCam3 : BOOL;// E9.2 single bit cam 3
signalCam4 : BOOL;// E9.3 single bit cam 4
posMode : BOOL;
jogMode : BOOL;
techMode : BOOL;
refMode : BOOL;
targetPos : REAL;
_speed : REAL;
END_VAR
VAR
actualPos : REAL := 0;
END_VAR
VAR_OUTPUT
K100K : BOOL := false;
nK100K : BOOL := false;
BE_Ref : BOOL := false;
QKRxK : BOOL := false;
FQM : BOOL := false;
FQR : BOOL := false;
BBM : BOOL := false;
BBR : BOOL := false;
BG_1 : BOOL := false;
BG_2 : BOOL := false;
BG_3 : BOOL := false;
BG_4 : BOOL := false;
value : DINT := 0;
_actualPos AT value : DWORD;
END_VAR
(* --------------------------- BEGIN --------------------------- *)
inhibit := A[Addr + 1, 0];// A1.0
notEStop := A[Addr + 1, 1];// A1.1
notStop := A[Addr + 1, 2];// A1.2
_speed := speed / 50.0;
IF K100 & NOT inhibit & notEStop & notStop THEN
fastMode := A[Addr + 1, 7];// A1.7
start := A[Addr + 0, 0];// A0.0
jogCW := A[Addr + 0, 1];// A0.1
jogCCW := A[Addr + 0, 2];// A0.2
mode_0 := A[Addr + 0, 3];// A0.3
mode_1 := A[Addr + 0, 4];// A0.4
mode_2 := A[Addr + 0, 5];// A0.5
targetPos1 := A[Addr + 3, 0];// A3.0
targetPos2 := A[Addr + 3, 1];// A3.1
targetPos3 := A[Addr + 3, 2];// A3.2
targetPos4 := A[Addr + 3, 3];// A3.3
posMode := NOT mode_0 & mode_1 & NOT mode_2;
jogMode := NOT mode_0 & NOT mode_1 & NOT mode_2;
techMode:= mode_0 & NOT mode_1 & mode_2;
refMode := mode_0 & NOT mode_1 & NOT mode_2;
IF start & posMode THEN
IF NOT fastMode THEN
_speed := _speed * 0.1;
END_IF;
IF targetPos1 THEN
targetPos := Pos_1;
ELSIF targetPos2 THEN
targetPos := Pos_2;
ELSIF targetPos3 THEN
targetPos := Pos_3;
ELSIF targetPos4 THEN
targetPos := Pos_4;
ELSE
targetPos := 5 * Limit_L;
END_IF;
IF actualPos > Limit_L & actualPos < Limit_R & actualPos <> targetPos THEN
IF actualPos - targetPos > _speed THEN
actualPos := actualPos - _speed;
ELSIF targetPos - actualPos > _speed THEN
actualPos := actualPos + _speed;
ELSE
actualPos := targetPos;
END_IF;
axisSync := true;
END_IF;
ELSIF jogMode THEN
_speed := _speed * 0.01;
IF jogCW THEN
actualPos := actualPos + _speed;
axisSync := true;
ELSIF jogCCW THEN
actualPos := actualPos - _speed;
axisSync := true;
END_IF;
END_IF;
END_IF;
(* --------------------------- FINISH --------------------------- *)
K100K := K100;
nK100K := NOT K100;
BE_Ref := actualPos > -offset & actualPos < offset;
QKRxK := false;
FQM := true;
FQR := true;
BBM := true;
BBR := true;
BG_1 := actualPos >= Pos_1 - offset & actualPos <= Pos_1 + offset;
BG_2 := actualPos >= Pos_2 - offset & actualPos <= Pos_2 + offset;
BG_3 := actualPos >= Pos_3 - offset & actualPos <= Pos_3 + offset;
BG_4 := actualPos >= Pos_4 - offset & actualPos <= Pos_4 + offset;
value := REAL_TO_DINT(actualPos);
invReady := K100;
IPOS_Ref := true;
notBreak := start;
fault := false;
limitCW := value >= Limit_R;
limitCCW := value <= Limit_L;
signalPos1 := value = Pos_1 & (start & posMode & targetPos1 OR NOT start);
signalPos2 := value = Pos_2 & (start & posMode & targetPos2 OR NOT start);
signalPos3 := value = Pos_3 & (start & posMode & targetPos3 OR NOT start);
signalPos4 := value = Pos_4 & (start & posMode & targetPos4 OR NOT start);
signalCam1 := false;
signalCam2 := false;
signalCam3 := false;
signalCam4 := false;
inPosition := signalPos1 OR signalPos2 OR signalPos3 OR signalPos4;
(* --------------------------- Write AMA_E --------------------------- *)
EB[Addr] := 16#0 ;// EB 0
E[Addr + 1, 0] := axisSync ;// E1.0
E[Addr + 1, 1] := invReady ;// E1.1
E[Addr + 1, 2] := IPOS_Ref ;// E1.2
E[Addr + 1, 3] := inPosition;// E1.3
E[Addr + 1, 4] := notBreak ;// E1.4
E[Addr + 1, 5] := fault ;// E1.5
E[Addr + 1, 6] := limitCW ;// E1.6
E[Addr + 1, 7] := limitCCW ;// E1.7
ED[Addr + 2] := _actualPos;// ED 2
E[Addr + 7, 0] := signalPos1;// E7.0
E[Addr + 7, 1] := signalPos2;// E7.1
E[Addr + 7, 2] := signalPos3;// E7.2
E[Addr + 7, 3] := signalPos4;// E7.3
E[Addr + 9, 0] := signalCam1;// E9.0
E[Addr + 9, 1] := signalCam2;// E9.1
E[Addr + 9, 2] := signalCam3;// E9.2
E[Addr + 9, 3] := signalCam4;// E9.3
END_FUNCTION_BLOCK