-
Notifications
You must be signed in to change notification settings - Fork 2
/
EncoderET200S1SSI.scl
238 lines (229 loc) · 8.23 KB
/
EncoderET200S1SSI.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
234
235
236
237
238
FUNCTION_BLOCK FB123
TITLE ='Input driver for ET200S 1SSI'
AUTHOR : EMC
FAMILY : EMC2
NAME : Enc1SSI
VERSION : '2.0'
VAR_INPUT
EncErr : BOOL ; //1=Encoder error
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
Status : STRUCT
Istwert2424 : BOOL ; //internal use
STS_UP : BOOL ; //UP status
STS_DN : BOOL ; //DN status
STS_DI : BOOL ; //DI status
EXTF : BOOL ; //1=Group error "absolute value encoder" or "short circuit of the encoder supply"
ERR_PARA : BOOL ; //1=Parameter assignment error
RDY : BOOL ; //1=Ready for operation (feedback is valid)
bit31 : BOOL ;
Istwert1623 : BYTE ; //internal use
Istwert0815 : BYTE ; //internal use
Istwert0007 : BYTE ; //internal use
END_STRUCT ;
Status_dw AT Status : DWORD;
GeberAlt_DI : DINT ; //internal use
Addr_ActPos_DI : INT ; //internal use
v_Faktor : REAL ; //internal use
MaxZahl : DINT ; //internal use
MaxZahlHalb : DINT ; //internal use
Richtungsanpassung : INT := 1; //internal use
PositionOffset : DINT ; //internal use
EncErr_alt : BOOL ; //internal use
Taktsynchron : BOOL ; //internal use
END_VAR
VAR_TEMP
Geber_DI : DINT ; //internal use
LageDiff_DI : DINT ; //internal use
Addr_InChannel : INT ; //internal use
TruncKorr : REAL ; //internal use
TruncKorr_DI : DINT ; //internal use
Err_DW : DWORD ; //internal use
Err_DW_str AT Err_DW : STRUCT
SWLimitMinExceeded : BOOL ; //1=Min. SW limit switch exceeded
SWLimitMaxExceeded : BOOL ; //1=Max. SW limit switch exceeded
TargetErr : BOOL ; //1=Target beyond travel range
NoSync : BOOL ; //1=Axis not synchronized
DirectionErr : BOOL ; //1=Motion not allowed in direction entered
DataErr : BOOL ; //1=Invalid motion FB parameter
StartErr : BOOL ; //1=Start not possible in current axis state
DistanceErr : BOOL ; //1=Motion is too far
MasterErr : BOOL ; //1=Master axis error with hard stop or wrong axis status
bit09 : BOOL ;
bit10 : BOOL ;
bit11 : BOOL ;
bit12 : BOOL ;
bit13 : BOOL ;
bit14 : BOOL ;
bit15 : BOOL ;
StoppedMotion : BOOL ; //1=Axis is in stop and must be acknowledged
EnableDriveErr : BOOL ; //1=Drive enable missing
FollowingDistErr : BOOL ; //1=Max. following distance exceeded
StandstillErr : BOOL ; //1=Outside standstill range
TargetApproachErr : BOOL ; //1=Target approach error
EncoderErr : BOOL ; //1=Encoder error
OutputErr : BOOL ; //1=Output driver error
ConfigErr : BOOL ; //1=Axis data incorrectly configured
DriveErr : BOOL ; //1=Drive error
bit25 : BOOL ;
bit26 : BOOL ;
bit27 : BOOL ;
bit28 : BOOL ;
bit29 : BOOL ;
bit30 : BOOL ;
bit31 : BOOL ;
END_STRUCT ;
GeberAmAchsende : DINT ; //internal use
GeberAmAchsanfang : DINT ; //internal use
Hilf_DI : DINT ; //internal use
SFCErr : INT ; //Rьckgabewert des SFC6 (RD_SINFO) lokal, da keine relevante Info
TOP_SI : STRUCT
EV_CLASS : BYTE ;
EV_NUM : BYTE ;
PRIORITY : BYTE ;
NUM : BYTE ;
TYP2_3 : BYTE ;
TYP1 : BYTE ;
ZI1 : WORD ;
ZI2_3 : DWORD ;
END_STRUCT ;
START_UP_SI : STRUCT
EV_CLASS : BYTE ;
EV_NUM : BYTE ;
PRIORITY : BYTE ;
NUM : BYTE ;
TYP2_3 : BYTE ;
TYP1 : BYTE ;
ZI1 : WORD ;
ZI2_3 : DWORD ;
END_STRUCT ;
END_VAR
BEGIN
IF NOT Init AND NOT EncErr AND EncErr_alt THEN
Init:=true;
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
END_IF;
IF Init THEN
Addr_InChannel:=Axis.InputModuleInAddr;
Addr_ActPos_DI:=Addr_InChannel;
v_Faktor:=Axis.aufl/Axis.Sample_T;
Axis.ExecRef:=0;
Status_dw:=DW#16#0;
Axis.EintreiberFahrbereit:=false;
EncErr_alt:=false;
Axis.DoneRef:=false;
Axis.ExecRef:=0;
Axis.EncoderType:=true;
MaxZahl:=Axis.AbsGeberLaenge_DI;
MaxZahlHalb:=MaxZahl/2;
Richtungsanpassung:=Axis.PolarityEncoder;
SFCErr:=RD_SINFO(TOP_SI := TOP_SI
,START_UP_SI := START_UP_SI
);
SFCErr:=BYTE_TO_INT(TOP_SI.NUM);
Taktsynchron:=SFCErr>=61 AND SFCErr<=64;
END_IF;
IF Axis.Sim OR EncErr THEN
Status_dw:=DW#16#40000000;
ELSE
IF Taktsynchron THEN
Status_dw:=ID[Addr_ActPos_DI];
ELSE
Status_dw:=PID[Addr_ActPos_DI];
END_IF;
Axis.EncoderValue:=DWORD_TO_DINT(Status_dw AND DW#16#1FFFFFF);
END_IF;
Geber_DI:=Axis.EncoderValue;
IF EncErr THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
EncErr_alt:=true;
ELSE
IF Init THEN
PositionOffset:=Axis.PositionOffset;
GeberAmAchsanfang:=ROUND((Axis.AxisLimitMin-Axis.RefPoint)/
Axis.aufl*INT_TO_REAL(Richtungsanpassung)+
DINT_TO_REAL(PositionOffset)+DINT_TO_REAL(MaxZahl))
MOD MaxZahl;
GeberAmAchsende:=ROUND((Axis.AxisLimitMax-Axis.RefPoint)/
Axis.aufl*INT_TO_REAL(Richtungsanpassung)+
DINT_TO_REAL(PositionOffset)+DINT_TO_REAL(MaxZahl))
MOD MaxZahl;
IF Richtungsanpassung=-1 THEN
Hilf_DI:=GeberAmAchsanfang;
GeberAmAchsanfang:=GeberAmAchsende;
GeberAmAchsende:=Hilf_DI;
END_IF;
Hilf_DI:=0;
IF GeberAmAchsanfang>GeberAmAchsende THEN
IF PositionOffset>=GeberAmAchsanfang THEN
IF Geber_DI<=GeberAmAchsende THEN
Hilf_DI:=MaxZahl;
END_IF;
END_IF;
IF PositionOffset<=GeberAmAchsende THEN
IF Geber_DI>=GeberAmAchsanfang THEN
Hilf_DI:=-MaxZahl;
END_IF;
END_IF;
END_IF;
Axis.LageIstwert_DI:=(Geber_DI+Hilf_DI-PositionOffset)*
INT_TO_DINT(Richtungsanpassung)+PositionOffset;
GeberAlt_DI:=Geber_DI;
END_IF;
IF Status.EXTF OR Status.ERR_PARA OR NOT Status.RDY THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
ELSE
IF Axis.ExecRef=1 THEN
Axis.InternalNomPosition:=0.0;
Axis.LastNomPosition:=0.0;
Axis.LageIstwert_DI:=Geber_DI;
Axis.PositionOffset:=Geber_DI;
Axis.InternalPosOffset:=Axis.RefPoint;
Axis.DoneRef:=true;
ELSE
Axis.DoneRef:=false;
END_IF;
LageDiff_DI:=(Geber_DI-GeberAlt_DI)*INT_TO_DINT(Richtungsanpassung);
IF LageDiff_DI<-MaxZahlHalb THEN
LageDiff_DI:=LageDiff_DI+MaxZahl;
END_IF;
IF LageDiff_DI>MaxZahlHalb THEN
LageDiff_DI:=LageDiff_DI-MaxZahl;
END_IF;
Axis.LageIstwert_DI:=Axis.LageIstwert_DI+LageDiff_DI;
Axis.ActPosition:=DINT_TO_REAL(Axis.LageIstwert_DI-Axis.PositionOffset)*
Axis.aufl+Axis.RefPoint;
Axis.InternalActPos:=Axis.ActPosition-Axis.InternalPosOffset;
GeberAlt_DI:=Geber_DI;
Axis.ActVelocity:=DINT_TO_REAL(LageDiff_DI)*v_Faktor;
IF Axis.AxisType THEN
TruncKorr:=(Axis.ActPosition-Axis.AxisLimitMin)/Axis.AchsLaenge;
TruncKorr_DI:=TRUNC(TruncKorr);
IF TruncKorr<0.0 AND DINT_TO_REAL(TruncKorr_DI)<>TruncKorr THEN
TruncKorr_DI:=TruncKorr_DI-1;
END_IF;
Axis.ActPosition:=Axis.ActPosition-DINT_TO_REAL(TruncKorr_DI)*Axis.AchsLaenge;
ELSE
Axis.Err.SWLimitMinExceeded:=Axis.SWLimitEnable and Axis.Sync AND
Axis.ActPosition<=Axis.AxisLimitMin;
Axis.Err.SWLimitMaxExceeded:=Axis.SWLimitEnable and Axis.Sync AND
Axis.ActPosition>=Axis.AxisLimitMax;
Err_DW_str:=Axis.Err;
Axis.Error:=Err_DW<>0;
END_IF;
END_IF;
END_IF;
IF Init THEN
Init:=false;
Axis.InternalNomPosition:=Axis.InternalActPos;
Axis.LastNomPosition:=Axis.InternalActPos;
Axis.VLastNomPosition:=Axis.InternalActPos;
END_IF;
END_FUNCTION_BLOCK