plc4good.org.ua

: EMC - Easy Motion Control (sources 26 from 26)




emc


, .
Easy Motion Control .
"Technical Specification V1.0" PLCopen.



emc



(Easy Motion Control, 02/2003) (pdf, 2Mb)

(Getting Started, 02/2003) (pdf, 0.7Mb)

EMC library v2.0 (zip, 150Kb)

EMC library v2.1 (zip, 60Kb)

Example of use (zip, 500Kb)




:



UDT1 AXIS_REF - Data type for axis data

FB1 MC_MoveAbsolute - Move to absolute position

FB2 MC_MoveRelative - Move a relative distance

FB3 MC_MoveJog - Move the axis using directional inputs (jogging)

FB4 MC_Home - Reference search / setting

FB5 MC_StopMotion - Stop any motion

FB11 MC_Control - Position control

FB12 MC_Simulation - Simulate axis

FB21 EncoderIM178 - Input driver for IM178-4

FB22 EncoderFM450 - Input driver for FM450

FB23 EncoderET200S1SSI - Input driver for ET200S 1SSI

FB24 EncoderAbsSensorDP - Input driver for DP-sensor

FB25 EncoderSM338 - Input driver for SM338

FB26 EncoderET200S1Count - Input driver for ET200S 1Count

FB27 EncoderFM350 - Input driver for FM350

FB28 EncoderCPU314C - Input driver for CPU 314C counter

FB29 EncoderUniversal - Universal input driver

FB31 OutputIM178 - Output driver for IM178-4

FB32 OutputSM432 - Output driver for SM432

FB33 OutputET200S2AO - Output driver for ET200S 2AO

FB34 OutputCPU314C - Output driver for CPU 314C

FB35 OutputSM332 - Output driver for SM332

FB36 OutputUniversal - Universal output driver

FB37 OutputMM4_DP - Output driver for Micromaster 4 DP

FB41 MC_GearIn - Electronic gear (2 axes)

FC0 MC_Init - Initialize axis DB




:



emc



UDT1 AXIS_REF - Data type for axis data



Name: UDT1
Symbolic Name: AXIS_REF
Symbol Comment: Data type for axis data
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use:
Size in work memory:
Object name: emc2
Signature:



TYPE UDT1 // AXIS_REF - Data type for axis data
STRUCT
Sample_T : REAL := 1.000000e-002; //Axis processing sample time [s]
AxisType : BOOL ; //0=Linear; 1=Rotary
EncoderType : BOOL ; //0=Incremental; 1=Absolute
Sim : BOOL ; //1=Simulation on
AxisLimitMax : REAL := 1.000000e+006; //Maximum axis coordinate [u]
AxisLimitMin : REAL := -1.000000e+006; //Minimum axis coordinate [u]
MaxVelocity : REAL := 1.000000e-001; //Maximum velocity [u/s]
MaxAcceleration : REAL := 1.000000e-001; //Maximum acceleration [u/s]
MaxDeceleration : REAL := 1.000000e-001; //Maximum deceleration [u/s]
MonTimeTargetAppr : REAL := 1.000000e+000; //Monitoring time for target approach [s]
TargetRange : REAL := 1.000000e+000; //Target range [u]
StandstillRange : REAL := 1.500000e+000; //Standstill range [u]
MaxFollowingDist : REAL := 5.000000e+000; //Max. following distance [u]
MonitorTargetAppr : BOOL := TRUE; //1=Monitor target approach
SWLimitEnable : BOOL := TRUE; //1=Enable software limit switches
FactorP : REAL := 1.000000e+000; //P-Factor of position loop [1/s]
ManVelocity : REAL ; //Manual velocity [u/s]
ManEnable : BOOL ; //1=Enable manual mode
EmergencyDec : REAL := 1.000000e+003; //Deceleration for emergency stop [u/s]
InputModuleInAddr : INT ; //Input address for input module (encoder connection)
InputModuleOutAddr : INT ; //Output address for input module (encoder connection)
InputChannelNo : INT ; //Channel no. for input module (0..n)
StepsPerRev : DINT := L#1; //Steps per encoder revolution [steps/rev]
DisplacementPerRev : REAL := 1.000000e+000; //Displacement of the axis per encoder revolution [u/rev]
NumberRevs : INT := 1; //Number of revolutions (1:Singleturn, >1:Multiturn)
PolarityEncoder : INT := 1; //Polarity of encoder (-1 or 1)
PositionOffset : DINT ; //Position offset / Absolute encoder adjustment [step]
OutputModuleOutAddr : INT ; //Output address for output module (drive connection)
OutputModuleInAddr : INT ; //Input address for output module (drive connection)
OutputChannelNo : INT ; //Channel no. for output module (0..n)
PolarityDrive : INT := 1; //Polarity of drive (-1 or 1)
DriveInputAtMaxVel : REAL := 9.000000e+000; //Reference value for max. axis velocity (>0)
OffsetCompensation : REAL ; //Offset compensation [V]
DriveInputAt100 : REAL := 1.000000e+001; //Reference value for 100% rpm
Override : REAL := 1.000000e+002; //Velocity override (0..100%)
ActPosition : REAL ; //Actual position [u]
FollowingDistance : REAL ; //Following distance [u]
RemainingDistance : REAL ; //Remaining distance [u]
NomVelocity : REAL ; //Nominal velocity of axis [u/s]
ActVelocity : REAL ; //Actual velocity of axis [u/s]
Sync : BOOL ; //1=Axis synchronized
Error : BOOL := TRUE; //1=Group error
ErrorAck : BOOL ; //1=Group acknowledgment for errors
AxisState : INT := 1; //internal use
Err : 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 := TRUE; //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 ;
Config : STRUCT
Err_AxisLimit : BOOL ; //1=Incorrect axis limits
Err_MaxVelocity : BOOL ; //1=Incorrect max. velocity
Err_MaxAcceleration : BOOL ; //1=Incorrect max. acceleration
Err_MaxDeceleration : BOOL ; //1=Incorrect max. deceleration
Err_MonTimeTargetAppr : BOOL ; //1=Incorrect monitoring time for target approach
Err_TargetRange : BOOL ; //1=Incorrect target range
Err_StandstillRange : BOOL ; //1=Incorrect standstill range
Err_MaxFollowingDist : BOOL ; //1=Incorrect max. follwing distance
Err_EmergencyDec : BOOL ; //1=Incorrect emergency deceleration
Err_StepsPerRev : BOOL ; //1=Incorrect steps per encoder revolution
Err_DisplacementPerRev : BOOL ; //1=Incorrect displacement per encoder revolution
Err_NumberRevs : BOOL ; //1=Incorrect number of encoder revolutions
Err_PolarityEncoder : BOOL ; //1=Incorrect encoder polarity
Err_PolarityDrive : BOOL ; //1=Incorrect drive polarity
Err_DriveInputAtMaxVel : BOOL ; //1=Incorrect reference value for max. velocity
Err_AxisLength : BOOL ; //1=Incorrect axis length
Err_EncoderRange : BOOL ; //1=Encoder range is not correct for axis length
Err_MaxVelRotaryAxis : BOOL ; //1=Max. velocity and sample time are not correct for rotary axis length
Err_DriveInputAt100 : BOOL ; //1=Incorrect reference value for 100% rpm
END_STRUCT ;
EncoderValue : DINT ; //Current encoder value [step]
RefPoint : REAL ; //internal use
OutVelocity : REAL ; //internal use
LastNomPosition : REAL ; //internal use
LastNomVelocity : REAL ; //internal use
LastNomDeceleration : REAL ; //internal use
InternalNomPosition : REAL ; //internal use
InternalActPos : REAL ; //internal use
InternalPosOffset : REAL ; //internal use
aufl : REAL ; //internal use
AchsLaenge : REAL ; //internal use
AchsLaenge_DI : DINT ; //internal use
AbsGeberLaenge_DI : DINT ; //internal use
LageIstwert_DI : DINT ; //internal use
Coord : DINT ; //internal use
DoneRef : BOOL ; //internal use
EinInitLaeuft : BOOL ; //internal use
AusInitLaeuft : BOOL ; //internal use
EinQuittungLaeuft : BOOL ; //internal use
AusQuittungLaeuft : BOOL ; //internal use
EintreiberFahrbereit : BOOL ; //internal use
ExecRef : INT ; //internal use
VLastNomPosition : REAL ; //internal use
VLastNomVelocity : REAL ; //internal use
LastInternalPosOffset : REAL ; //internal use
RundKorrektur : REAL ; //internal use
LastRundKorrektur : REAL ; //internal use
LengthUnit : STRING [4 ] := 'mm'; //internal use
InputModuleType : STRING [24 ] := 'IM 178-4'; //internal use
OutputModuleType : STRING [24 ] := 'IM 178-4'; //internal use
Init : STRUCT //Init bits for 32 FBs
I0 : BOOL := TRUE; //Init bit 0
I1 : BOOL := TRUE; //Init bit 1
I2 : BOOL := TRUE; //Init bit 2
I3 : BOOL := TRUE; //Init bit 3
I4 : BOOL := TRUE; //Init bit 4
I5 : BOOL := TRUE; //Init bit 5
I6 : BOOL := TRUE; //Init bit 6
I7 : BOOL := TRUE; //Init bit 7
I8 : BOOL := TRUE; //Init bit 8
I9 : BOOL := TRUE; //Init bit 9
I10 : BOOL := TRUE; //Init bit 10
I11 : BOOL := TRUE; //Init bit 11
I12 : BOOL := TRUE; //Init bit 12
I13 : BOOL := TRUE; //Init bit 13
I14 : BOOL := TRUE; //Init bit 14
I15 : BOOL := TRUE; //Init bit 15
I16 : BOOL := TRUE; //Init bit 16
I17 : BOOL := TRUE; //Init bit 17
I18 : BOOL := TRUE; //Init bit 18
I19 : BOOL := TRUE; //Init bit 19
I20 : BOOL := TRUE; //Init bit 20
I21 : BOOL := TRUE; //Init bit 21
I22 : BOOL := TRUE; //Init bit 22
I23 : BOOL := TRUE; //Init bit 23
I24 : BOOL := TRUE; //Init bit 24
I25 : BOOL := TRUE; //Init bit 25
I26 : BOOL := TRUE; //Init bit 26
I27 : BOOL := TRUE; //Init bit 27
I28 : BOOL := TRUE; //Init bit 28
I29 : BOOL := TRUE; //Init bit 29
I30 : BOOL := TRUE; //Init bit 30
I31 : BOOL := TRUE; //Init bit 31
END_STRUCT ;
END_STRUCT ;
END_TYPE
 





FB1 MC_MoveAbsolute - Move to absolute position



Name: MC_Abs
Symbolic Name: MC_MoveAbsolute
Symbol Comment: Move to absolute position
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 3924
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB1
TITLE ='Move to absolute position'
AUTHOR : EMC
FAMILY : EMC2
NAME : MC_Abs
VERSION : '2.0'
 
 
VAR_INPUT
Execute : BOOL ; //Start the motion at rising edge
Position : REAL ; //Target position for the motion [u]
Velocity : REAL := 1.000000e-001; //Maximum velocity [u/s]
Acceleration : REAL := 1.000000e-001; //Acceleration [u/s]
Deceleration : REAL := 1.000000e-001; //Deceleration [u/s]
Direction : INT ; //Direction (rotary axis only: -1=negative, 0=shortest, 1=positive)
END_VAR
VAR_OUTPUT
Busy : BOOL ; //1=Moving
Done : BOOL ; //1=Position reached
CommandAborted : BOOL ; //1=Command was aborted by another command
Error : BOOL ; //1=Error has occurred within FB
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
ziel : REAL ; //internal use
v_max : REAL ; //internal use
v_max_B AT v_max : ARRAY[0..31] OF BOOL;
a_1 : REAL ; //internal use
a_1_B AT a_1 : ARRAY[0..31] OF BOOL;
b : REAL ; //internal use
b_B AT b : ARRAY[0..31] OF BOOL;
b0 : REAL ; //internal use
bk1 : REAL ; //internal use
Zustand : INT ; //internal use
Bereich : INT ; //internal use
Coord : DINT ; //internal use
ta : REAL ; //internal use
taHalbe : REAL ; //internal use
aufl : REAL ; //internal use
Error_alt : BOOL ; //internal use
END_VAR
VAR_TEMP
over : REAL ; //internal use
vk : REAL ; //internal use
vk_B AT vk : ARRAY[0..31] OF BOOL ;
sk : REAL ; //internal use
vk1 : REAL ; //internal use
sk1 : REAL ; //internal use
Bremsweg : REAL ; //internal use
Restweg : REAL ; //internal use
RB : REAL ; //internal use
vv : REAL ; //internal use
delta_s : REAL ; //internal use
v_over : REAL ; //internal use
Hilf_R : REAL ; //internal use
Hilf_DI : DINT ; //internal use
ZielStrich : REAL ; //internal use
LageIstwert_DI : DINT ; //internal use
InternalActPos : REAL ; //internal use
RundKorrektur : REAL ; //internal use
BremswegRunden : DINT ; //internal use
BremswegRest : REAL ; //internal use
AchsLaenge : REAL ; //internal use
AchsLaengeHalb : REAL ; //internal use
Err_DW : DWORD ; //internal use
Err_w AT Err_DW : STRUCT
w0 : WORD;
w1 : WORD;
END_STRUCT ;
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 ;
END_VAR
BEGIN
 
IF Init
THEN
Init:=false;
Done:=false;
Busy:=false;
CommandAborted:=false;
Error:=false;
Error_alt:=false;
Zustand:=1;
Bereich:=5;
ta:=Axis.Sample_T;
taHalbe:=ta*5.000000e-001;
aufl:=Axis.aufl;
END_IF;
 
IF Zustand=1
THEN
IF NOT Busy AND Execute
THEN
Busy:=true;
Done:=false;
CommandAborted:=false;
Error:=false;
Error_alt:=false;
Coord:=Axis.Coord+1;
AchsLaenge:=Axis.AchsLaenge;
AchsLaengeHalb:=AchsLaenge*5.000000e-001;
a_1:=Axis.MaxAcceleration;
b:=Axis.MaxDeceleration;
v_max:=Axis.MaxVelocity;
IF v_max>Velocity THEN v_max:=Velocity; END_IF;
IF a_1>Acceleration THEN a_1:=Acceleration; END_IF;
IF b>Deceleration THEN b:=Deceleration; END_IF;
ziel:=Position;
sk:=Axis.LastNomPosition;
vk:=Axis.LastNomVelocity;
InternalActPos:=Axis.InternalActPos;
LageIstwert_DI:=Axis.LageIstwert_DI;
RundKorrektur:=Axis.RundKorrektur;
Restweg:=ziel-Axis.InternalPosOffset-sk;
IF Axis.AxisType
THEN
Hilf_DI:=LageIstwert_DI;
LageIstwert_DI:=(Hilf_DI-Axis.PositionOffset) MOD Axis.AchsLaenge_DI + Axis.PositionOffset;
Hilf_DI:=Hilf_DI-LageIstwert_DI;
Hilf_R:=DINT_TO_REAL(Hilf_DI)*aufl;
sk:=sk-Hilf_R;
RundKorrektur:=RundKorrektur+Hilf_R;
InternalActPos:=InternalActPos-Hilf_R;
Restweg:=ziel-Axis.InternalPosOffset-sk;
IF Restweg>=AchsLaenge THEN Restweg:=Restweg-AchsLaenge;END_IF;
IF Restweg<=-AchsLaenge THEN Restweg:=Restweg+AchsLaenge;END_IF;
IF ABS(Restweg)<aufl THEN Restweg:=0.0; END_IF;
IF b>0.0
THEN
Bremsweg:=ABS(vk)*vk*0.5/b;
BremswegRunden:=TRUNC(ABS(Bremsweg)/AchsLaenge);
IF Bremsweg<0.0
THEN
BremswegRunden:=-BremswegRunden;
END_IF;
BremswegRest:=Bremsweg-DINT_TO_REAL(BremswegRunden)*AchsLaenge;
IF Direction>=1
THEN
IF Restweg<0.0 THEN Restweg:=Restweg+AchsLaenge;END_IF;
IF BremswegRest<0.0
THEN
IF BremswegRest<Restweg-AchsLaenge
THEN
Restweg:=Restweg-AchsLaenge;
END_IF;
ELSIF BremswegRest=0.0
THEN
IF Bremsweg<0.0 AND Restweg=0.0
THEN
Restweg:=Restweg+AchsLaenge;
END_IF;
ELSE
IF BremswegRest>Restweg
THEN
Restweg:=Restweg+AchsLaenge;
END_IF;
END_IF;
ELSIF Direction<=-1
THEN
IF Restweg>0.0 THEN Restweg:=Restweg-AchsLaenge;END_IF;
IF BremswegRest>0.0
THEN
IF BremswegRest>Restweg+AchsLaenge
THEN
Restweg:=Restweg+AchsLaenge;
END_IF;
ELSIF BremswegRest=0.0
THEN
IF Bremsweg>0.0 AND Restweg=0.0
THEN
Restweg:=Restweg-AchsLaenge;
END_IF;
ELSE
IF BremswegRest<Restweg
THEN
Restweg:=Restweg-AchsLaenge;
END_IF;
END_IF;
ELSIF Direction=0
THEN
IF Restweg>AchsLaengeHalb
THEN
Restweg:=Restweg-AchsLaenge;
ELSIF Restweg<-AchsLaengeHalb
THEN
Restweg:=Restweg+AchsLaenge;
END_IF;
IF BremswegRest>0.0
THEN
IF BremswegRest>=Restweg+AchsLaengeHalb
THEN
Restweg:=Restweg+AchsLaenge;
END_IF;
ELSIF BremswegRest<0.0
THEN
IF BremswegRest<=Restweg-AchsLaengeHalb
THEN
Restweg:=Restweg-AchsLaenge;
END_IF;
END_IF;
END_IF;
Restweg:=DINT_TO_REAL(BremswegRunden)*AchsLaenge+Restweg;
END_IF;
END_IF;
ziel:=Restweg;
ZielStrich:=ziel/aufl;
Err_DW_str:=Axis.Err;
Err_DW_str.SWLimitMinExceeded:=false;
Err_DW_str.SWLimitMaxExceeded:=false;
Axis.Err.StartErr:=Err_DW<>DW#16#0 OR Axis.AxisState=6 OR Axis.AxisState=8 OR Axis.ManEnable;
IF NOT Axis.Err.StartErr
THEN
Axis.Err.NoSync:=NOT Axis.Sync;
Axis.Err.DataErr:= (v_max<=0.0 OR
a_1<=0.0 OR
b<=0.0) OR
(Axis.LastNomDeceleration<>b AND
vk<>0.0) OR
ABS(Position)>1.6777216e+007;
Axis.Err.TargetErr:=ABS(ZielStrich)>1.6777216e+007 OR
(NOT Axis.AxisType AND Axis.SWLimitEnable) AND
(Position>=Axis.AxisLimitMax OR Position<=Axis.AxisLimitMin) OR
(vk=0.0 AND Axis.AxisType) AND (Position>=Axis.AxisLimitMax OR Position<Axis.AxisLimitMin);

END_IF;
Err_DW_str:=Axis.Err;
Err_DW_str.SWLimitMinExceeded:=false;
Err_DW_str.SWLimitMaxExceeded:=false;
IF Err_DW<>DW#16#0
THEN
Axis.Error:=true;
Error:=true;
Zustand:=4;
ELSE
Axis.InternalPosOffset:=Axis.InternalPosOffset+sk;
Axis.LastInternalPosOffset:=Axis.LastInternalPosOffset+sk;
InternalActPos:=InternalActPos-sk;
Axis.VLastNomPosition:=Axis.VLastNomPosition-Axis.LastNomPosition;
Axis.LastNomPosition:=0.0;
Axis.LastNomDeceleration:=b;
IF vk=0.0
THEN
IF Restweg<0.0
THEN
a_1:=-a_1;
b:=-b;
v_max:=-v_max;
END_IF;
b0:=0.0;
bk1:=b;
ELSE
a_1_B[7]:=vk_B[7];
b_B[7]:=vk_B[7];
v_max_B[7]:=vk_B[7];
b0:=b;
bk1:=b;
END_IF;
Axis.LageIstwert_DI:=LageIstwert_DI;
Axis.InternalActPos:=InternalActPos;
Axis.RundKorrektur:=RundKorrektur;
Zustand:=2;
Axis.Coord:=Coord;
Axis.AxisState:=3;
END_IF;
END_IF;
END_IF;
 
IF Zustand=2 OR Zustand=3
THEN
vk:=Axis.LastNomVelocity;
sk:=Axis.LastNomPosition;
Bremsweg:=vk*vk*0.5/bk1;
Restweg:=ziel-sk;
IF Axis.Error
THEN
Err_DW_str:=Axis.Err;
IF Restweg>0.0 AND Err_DW_str.SWLimitMinExceeded
THEN
Err_DW_str.SWLimitMinExceeded:=false;
END_IF;
IF Restweg<0.0 AND Err_DW_str.SWLimitMaxExceeded
THEN
Err_DW_str.SWLimitMaxExceeded:=false;
END_IF;
IF Err_DW=DW#16#0 THEN Error_alt:=true; END_IF;
IF NOT Error_alt
THEN
Error:=true;
Error_alt:=true;
IF Err_w.w1<>W#16#0
THEN
Zustand:=4;
ELSE
IF Zustand=2
THEN
ziel:=sk+Bremsweg;
Restweg:=Bremsweg;
END_IF;
END_IF;
END_IF;
END_IF;
IF Axis.Coord<>Coord OR Axis.ManEnable
THEN
CommandAborted:=true;
Zustand:=4;
END_IF;
END_IF;
 
IF Zustand=2
THEN
IF Restweg=0.0 AND vk<>0.0
THEN
Restweg:=-Bremsweg;
END_IF;
over:=ABS(Axis.Override)/100.0;
IF over>1.0 THEN over:=1.0; END_IF;
v_over:=v_max*over;
IF v_over<>0.0
THEN
vv:=vk/v_over;
ELSE
vv:=2.0;
END_IF;
IF Bremsweg<>0.0
THEN
RB:=Restweg/Bremsweg;
ELSE
RB:=2.0;
END_IF;
IF Restweg<>0.0
THEN
IF RB>1.0
THEN
b0:=0.0;
IF vv<1.0
THEN
Bereich:=1;
vk1:=a_1*ta+vk;
ELSIF vv=1.0
THEN
Bereich:=2;
vk1:=vk;
ELSE
Bereich:=6;
vk1:=vk-(bk1*ta);
IF vk1*vk<=0.0 OR
vk=0.0 AND
v_over=0.0
THEN
vk1:=0.0;
END_IF;
END_IF;
ELSE
IF RB<1.0 AND b0<>0.0
THEN
Bereich:=4;
bk1:=b0;
ELSE
Bereich:=3;
b0:=0.0;
END_IF;
END_IF;
ELSE
Bereich:=5;
END_IF;
IF Bereich=1 OR Bereich=2 OR Bereich=6
THEN
IF (vk1-v_over)*
(vk-v_over)<=0.0 AND
vk<>v_over
THEN
vk1:=v_over;
END_IF;
delta_s:=(vk+vk1)*taHalbe;
Bremsweg:=vk1*vk1*0.5/bk1;
IF Bremsweg<>0.0 AND
vk<>0.0
THEN
IF ABS((Restweg-delta_s)/Bremsweg)<1.0
THEN
bk1:=vk*vk*0.5/Restweg;
Bereich:=3;
ELSE
bk1:=b;
END_IF;
ELSE
bk1:=b;
END_IF;
sk1:=sk+delta_s;
END_IF;
IF Bereich=3
THEN
vk1:=vk-bk1*ta;
IF vk1*vk<=0.0 OR
(vk=0.0 AND
v_over=0.0)
THEN
vk1:=0.0;
END_IF;
sk1:=(vk+vk1)*taHalbe+sk;
ELSIF Bereich=4
THEN
vk1:=vk-bk1*ta;
IF vk1*vk<=0.0
THEN
vk1:=0.0;
b0:=0.0;
a_1:=-a_1;
b:=-b;
bk1:=b;
v_max:=-v_max;
END_IF;
sk1:=(vk+vk1)*taHalbe+sk;
ELSIF Bereich=5
THEN
vk1:=0.0;
sk1:=ziel;
Zustand:=3;
END_IF;
IF (sk1-ziel)*(sk-ziel)<=0.0 AND
Bereich<>4
THEN
sk1:=ziel;
vk1:=0.0;
END_IF;
Axis.InternalNomPosition:=sk1;
Axis.NomVelocity:=vk1;
Axis.RemainingDistance:=ziel-sk1;
ELSIF Zustand=3
THEN
IF Axis.AxisState=2
THEN
IF NOT Axis.Error
THEN
Done:=true;
END_IF;
Busy:=false;
Zustand:=5;
ELSIF Error_alt
THEN
Axis.AxisState:=8;
ELSE
Axis.AxisState:=5;
END_IF;
ELSIF Zustand=4
THEN
Busy:=false;
Zustand:=5;
ELSIF Zustand=5
THEN
IF NOT Execute
THEN
Done:=false;
CommandAborted:=false;
Error:=false;
Busy:=false;
Zustand:=1;
END_IF;
END_IF;

END_FUNCTION_BLOCK



emc

Block checksum .





FB2 MC_MoveRelative - Move a relative distance



Name: MC_Rel
Symbolic Name: MC_MoveRelative
Symbol Comment: Move a relative distance
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 2982
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB102
TITLE ='Move a relative distance'
AUTHOR : EMC
FAMILY : EMC2
NAME : MC_Rel
VERSION : '2.0'
 
 
VAR_INPUT
Execute : BOOL ; //Start the motion at rising edge
Distance : REAL ; //Relative distance for the motion [u]
Velocity : REAL := 1.000000e-001; //Maximum velocity [u/s]
Acceleration : REAL := 1.000000e-001; //Acceleration [u/s]
Deceleration : REAL := 1.000000e-001; //Deceleration [u/s]
END_VAR
VAR_OUTPUT
Busy : BOOL ; //1=Moving
Done : BOOL ; //1=Distance reached
CommandAborted : BOOL ; //1=Command was aborted by another command
Error : BOOL ; //1=Error has occurred within FB
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
ziel : REAL ; //internal use
v_max : REAL ; //internal use
v_max_B AT v_max : ARRAY[0..31] OF BOOL;
a_1 : REAL ; //internal use
a_1_B AT a_1 : ARRAY[0..31] OF BOOL;
b : REAL ; //internal use
b_B AT b : ARRAY[0..31] OF BOOL;
ta : REAL ; //internal use
taHalbe : REAL ; //internal use
b0 : REAL ; //internal use
bk1 : REAL ; //internal use
Zustand : INT ; //internal use
Bereich : INT ; //internal use
Coord : DINT ; //internal use
aufl : REAL ; //internal use
Error_alt : BOOL ; //internal use
END_VAR
VAR_TEMP
over : REAL ; //internal use
vk : REAL ; //internal use
vk_B AT vk : ARRAY[0..31] OF BOOL ;

sk : REAL ; //internal use
vk1 : REAL ; //internal use
sk1 : REAL ; //internal use
Bremsweg : REAL ; //internal use
Restweg : REAL ; //internal use
RB : REAL ; //internal use
vv : REAL ; //internal use
delta_s : REAL ; //internal use
v_over : REAL ; //internal use
Hilf_R : REAL ; //internal use
Hilf_DI : DINT ; //internal use
ZielStrich : REAL ; //internal use
ziel_AWK : REAL ; //internal use
Err_DW : DWORD ; //internal use
Err_w AT Err_DW : STRUCT
w0 : WORD;
w1 : WORD;
END_STRUCT ;
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 ;

END_VAR
BEGIN
 
IF Init
THEN
Init:=false;
Done:=false;
Busy:=false;
CommandAborted:=false;
Error:=false;
Error_alt:=false;
Zustand:=1;
Bereich:=5;
ta:=Axis.Sample_T;
taHalbe:=ta*5.000000e-001;
aufl:=Axis.aufl;
END_IF;

IF Zustand=1
THEN
IF NOT Busy AND Execute
THEN
Busy:=true;
Done:=false;
CommandAborted:=false;
Error:=false;
Error_alt:=false;
Coord:=Axis.Coord+1;
a_1:=Axis.MaxAcceleration;
b:=Axis.MaxDeceleration;
v_max:=Axis.MaxVelocity;
IF v_max>Velocity THEN v_max:=Velocity; END_IF;
IF a_1>Acceleration THEN a_1:=Acceleration; END_IF;
IF b>Deceleration THEN b:=Deceleration; END_IF;
ziel:=Axis.LastNomPosition+Axis.InternalPosOffset+Distance;
ziel_AWK:=ziel;
sk:=Axis.LastNomPosition;
vk:=Axis.LastNomVelocity;
Restweg:=Distance;
ziel:=Restweg;
ZielStrich:=ziel/aufl;
Err_DW_str:=Axis.Err;
Err_DW_str.SWLimitMinExceeded:=false;
Err_DW_str.SWLimitMaxExceeded:=false;
Axis.Err.StartErr:=Err_DW<>DW#16#0 OR Axis.AxisState=6 OR Axis.AxisState=8 OR Axis.ManEnable;
IF NOT Axis.Err.StartErr
THEN
Axis.Err.DataErr:= (v_max<=0.0 OR
a_1<=0.0 OR
b<=0.0) OR
(Axis.LastNomDeceleration<>b AND
vk<>0.0) OR
ABS(ziel_AWK)>1.6777216e+007;
Axis.Err.TargetErr:=ABS(ZielStrich)>1.6777216e+007 OR
(NOT Axis.AxisType AND Axis.SWLimitEnable AND Axis.Sync) AND
(ziel_AWK>=Axis.AxisLimitMax OR ziel_AWK<=Axis.AxisLimitMin);

END_IF;
Err_DW_str:=Axis.Err;
Err_DW_str.SWLimitMinExceeded:=false;
Err_DW_str.SWLimitMaxExceeded:=false;
IF Err_DW<>DW#16#0
THEN
Axis.Error:=true;
Error:=true;
Zustand:=4;
ELSE
IF Axis.AxisType
THEN
Hilf_DI:=Axis.LageIstwert_DI;
Axis.LageIstwert_DI:=(Axis.LageIstwert_DI-Axis.PositionOffset) MOD
Axis.AchsLaenge_DI + Axis.PositionOffset;
Hilf_DI:=Hilf_DI-Axis.LageIstwert_DI;
Hilf_R:=DINT_TO_REAL(Hilf_DI)*aufl;
sk:=sk-Hilf_R;
Axis.RundKorrektur:=Axis.RundKorrektur+Hilf_R;
Axis.InternalActPos:=Axis.InternalActPos-Hilf_R;
END_IF;
Axis.InternalPosOffset:=Axis.InternalPosOffset+sk;
Axis.LastInternalPosOffset:=Axis.LastInternalPosOffset+sk;
Axis.InternalActPos:=Axis.InternalActPos-sk;
Axis.VLastNomPosition:=Axis.VLastNomPosition-Axis.LastNomPosition;
Axis.LastNomPosition:=0.0;
Axis.LastNomDeceleration:=b;

IF vk=0.0
THEN
IF Restweg<0.0
THEN
a_1:=-a_1;
b:=-b;
v_max:=-v_max;
END_IF;
b0:=0.0;
bk1:=b;
ELSE
a_1_B[7]:=vk_B[7];
b_B[7]:=vk_B[7];
v_max_B[7]:=vk_B[7];
b0:=b;
bk1:=b;
END_IF;
Zustand:=2;
Axis.Coord:=Coord;
Axis.AxisState:=3;
END_IF;
END_IF;
END_IF;
IF Zustand=2 OR Zustand=3
THEN
vk:=Axis.LastNomVelocity;
sk:=Axis.LastNomPosition;
Bremsweg:=vk*vk*0.5/bk1;
Restweg:=ziel-sk;
IF Axis.Error
THEN
Err_DW_str:=Axis.Err;
IF Restweg>0.0 AND Err_DW_str.SWLimitMinExceeded
THEN
Err_DW_str.SWLimitMinExceeded:=false;
END_IF;
IF Restweg<0.0 AND Err_DW_str.SWLimitMaxExceeded
THEN
Err_DW_str.SWLimitMaxExceeded:=false;
END_IF;
IF Err_DW=DW#16#0 THEN Error_alt:=true; END_IF;
IF NOT Error_alt
THEN
Error:=true;
Error_alt:=true;
IF Err_w.w1<>W#16#0
THEN
Zustand:=4;
ELSE
IF Zustand=2
THEN
ziel:=sk+Bremsweg;
Restweg:=Bremsweg;
END_IF;
END_IF;
END_IF;
END_IF;
IF Axis.Coord<>Coord OR Axis.ManEnable
THEN
CommandAborted:=true;
Zustand:=4;
END_IF;
END_IF;
 
IF Zustand=2
THEN
IF Restweg=0.0 AND vk<>0.0
THEN
Restweg:=-Bremsweg;
END_IF;
over:=ABS(Axis.Override)/100.0;
IF over>1.0 THEN over:=1.0; END_IF;
v_over:=v_max*over;
IF v_over<>0.0
THEN
vv:=vk/v_over;
ELSE
vv:=2.0;
END_IF;
IF Bremsweg<>0.0
THEN
RB:=Restweg/Bremsweg;
ELSE
RB:=2.0;
END_IF;
IF Restweg<>0.0
THEN
IF RB>1.0
THEN
b0:=0.0;
IF vv<1.0
THEN
Bereich:=1;
vk1:=a_1*ta+vk;
ELSIF vv=1.0
THEN
Bereich:=2;
vk1:=vk;
ELSE
Bereich:=6;
vk1:=vk-(bk1*ta);
IF vk1*vk<=0.0 OR
vk=0.0 AND
v_over=0.0
THEN
vk1:=0.0;
END_IF;
END_IF;
ELSE
IF RB<1.0 AND b0<>0.0
THEN
Bereich:=4;
bk1:=b0;
ELSE
Bereich:=3;
b0:=0.0;
END_IF;
END_IF;
ELSE
Bereich:=5;
END_IF;
IF Bereich=1 OR Bereich=2 OR Bereich=6
THEN
IF (vk1-v_over)*
(vk-v_over)<=0.0 AND
vk<>v_over
THEN
vk1:=v_over;
END_IF;
delta_s:=(vk+vk1)*taHalbe;
Bremsweg:=vk1*vk1*0.5/bk1;
IF Bremsweg<>0.0 AND
vk<>0.0
THEN
IF ABS((Restweg-delta_s)/Bremsweg)<1.0
THEN
bk1:=vk*vk*0.5/Restweg;
Bereich:=3;
ELSE
bk1:=b;
END_IF;
ELSE
bk1:=b;
END_IF;
sk1:=sk+delta_s;
END_IF;
IF Bereich=3
THEN
vk1:=vk-bk1*ta;
IF vk1*vk<=0.0 OR
(vk=0.0 AND
v_over=0.0)
THEN
vk1:=0.0;
END_IF;
sk1:=(vk+vk1)*taHalbe+sk;
ELSIF Bereich=4
THEN
vk1:=vk-bk1*ta;
IF vk1*vk<=0.0
THEN
vk1:=0.0;
b0:=0.0;
a_1:=-a_1;
b:=-b;
bk1:=b;
v_max:=-v_max;
END_IF;
sk1:=(vk+vk1)*taHalbe+sk;
ELSIF Bereich=5
THEN
vk1:=0.0;
sk1:=ziel;
Zustand:=3;
END_IF;
IF (sk1-ziel)*(sk-ziel)<=0.0 AND
Bereich<>4
THEN
sk1:=ziel;
vk1:=0.0;
END_IF;
Axis.InternalNomPosition:=sk1;
Axis.NomVelocity:=vk1;
Axis.RemainingDistance:=ziel-sk1;
ELSIF Zustand=3
THEN
IF Axis.AxisState=2
THEN
IF NOT Axis.Error
THEN
Done:=true;
END_IF;
Busy:=false;
Zustand:=5;
ELSIF Error_alt
THEN
Axis.AxisState:=8;
ELSE
Axis.AxisState:=5;
END_IF;
ELSIF Zustand=4
THEN
Busy:=false;
Zustand:=5;
ELSIF Zustand=5
THEN
IF NOT Execute
THEN
Done:=false;
CommandAborted:=false;
Error:=false;
Busy:=false;
Zustand:=1;
END_IF;
END_IF;
END_FUNCTION_BLOCK



emc

Block checksum .





FB3 MC_MoveJog - Move the axis using directional inputs (jogging)



Name: MC_Jog
Symbolic Name: MC_MoveJog
Symbol Comment: Move the axis using directional inputs (jogging)
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 3110
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB103
TITLE ='Move the axis using directional inputs (jogging)'
AUTHOR : EMC
FAMILY : EMC2
NAME : MC_Jog
VERSION : '2.0'
 
 
VAR_INPUT
JogPos : BOOL ; //1=Jog positive
JogNeg : BOOL ; //1=Jog negative
Velocity : REAL := 1.000000e-001; //Maximum velocity [u/s]
Acceleration : REAL := 1.000000e-001; //Acceleration [u/s]
Deceleration : REAL := 1.000000e-001; //Deceleration [u/s]
END_VAR
VAR_OUTPUT
Busy : BOOL ; //1=Moving
Done : BOOL ; //1=Motion complete
CommandAborted : BOOL ; //1=Command was aborted by another command
Error : BOOL ; //1=Error has occurred within FB
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
v_max : REAL ; //internal use
v_max_B AT v_max : ARRAY[0..31] OF BOOL;
a_1 : REAL ; //internal use
a_1_B AT a_1 : ARRAY[0..31] OF BOOL;
b : REAL ; //internal use
b_B AT b : ARRAY[0..31] OF BOOL;
ta : REAL ; //internal use
taHalbe : REAL ; //internal use
Richtung : INT ; //internal use
b0 : REAL ; //internal use
bk1 : REAL ; //internal use
Grenze : REAL ; //internal use
Zustand : INT ; //internal use
Bereich : INT ; //internal use
Coord : DINT ; //internal use
aufl : REAL ; //internal use
AchsLaenge : REAL ; //internal use
END_VAR
VAR_TEMP
over : REAL ; //internal use
vk : REAL ; //internal use
vk_B AT vk : ARRAY[0..31] OF BOOL ;
sk : REAL ; //internal use
vk1 : REAL ; //internal use
sk1 : REAL ; //internal use
Bremspunkt : REAL ; //internal use
vv : REAL ; //internal use
v_over : REAL ; //internal use
Hilf_R : REAL ; //internal use
Hilf_DI : DINT ; //internal use
RundKorrektur_DI : DINT ; //internal use
Err_DW : DWORD ; //internal use
Err_w AT Err_DW : STRUCT
w0 : WORD;
w1 : WORD;
END_STRUCT ;
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 ;
END_VAR
BEGIN
IF Init
THEN
Init:=false;
Done:=false;
Busy:=false;
CommandAborted:=false;
Error:=false;
Zustand:=1;
Bereich:=5;
Richtung:=0;
ta:=Axis.Sample_T;
taHalbe:=ta*5.000000e-001;
aufl:=Axis.aufl;
Grenze:=aufl*1.6777216e+007;
AchsLaenge:=Axis.AchsLaenge;
END_IF;
 
IF Zustand=1
THEN
IF NOT Busy AND (JogPos OR JogNeg)
THEN
Busy:=true;
Done:=false;
CommandAborted:=false;
Error:=false;
Richtung:=0;
Coord:=Axis.Coord+1;
a_1:=Axis.MaxAcceleration;
b:=Axis.MaxDeceleration;
v_max:=Axis.MaxVelocity;
IF v_max>Velocity THEN v_max:=Velocity; END_IF;
IF a_1>Acceleration THEN a_1:=Acceleration; END_IF;
IF b>Deceleration THEN b:=Deceleration; END_IF;
Err_DW_str:=Axis.Err;
Err_DW_str.SWLimitMinExceeded:=false;
Err_DW_str.SWLimitMaxExceeded:=false;
 
Axis.Err.StartErr:=Err_DW<>DW#16#0 OR
Axis.AxisState=6 OR
Axis.AxisState=8 OR
(JogPos AND JogNeg)OR
Axis.ManEnable;
IF NOT Axis.Err.StartErr
THEN
Err_DW_str:=Axis.Err;
IF Err_DW_str.SWLimitMinExceeded AND JogPos
THEN
Err_DW_str.SWLimitMinExceeded:=false;
END_IF;
IF Err_DW_str.SWLimitMaxExceeded AND JogNeg
THEN
Err_DW_str.SWLimitMaxExceeded:=false;
END_IF;

Axis.Err.DataErr:= (v_max<=0.0 OR
a_1<=0.0 OR
b<=0.0) OR
(Axis.LastNomDeceleration<>b AND
Axis.LastNomVelocity<>0.0);
Axis.Err.DirectionErr:=Err_DW_str.SWLimitMinExceeded OR
Err_DW_str.SWLimitMaxExceeded;

END_IF;

Err_DW_str:=Axis.Err;
Err_DW_str.SWLimitMinExceeded:=false;
Err_DW_str.SWLimitMaxExceeded:=false;
IF Err_DW<>DW#16#0
THEN
Axis.Error:=true;
Error:=true;
Zustand:=4;
ELSE
sk:=Axis.LastNomPosition;
vk:=Axis.LastNomVelocity;

IF Axis.AxisType
THEN
Hilf_DI:=Axis.LageIstwert_DI;
Axis.LageIstwert_DI:=(Axis.LageIstwert_DI-Axis.PositionOffset) MOD
Axis.AchsLaenge_DI + Axis.PositionOffset;
Hilf_DI:=Hilf_DI-Axis.LageIstwert_DI;
Hilf_R:=DINT_TO_REAL(Hilf_DI)*aufl;
sk:=sk-Hilf_R;
Axis.RundKorrektur:=Axis.RundKorrektur+Hilf_R;
Axis.InternalActPos:=Axis.InternalActPos-Hilf_R;
END_IF;

Axis.InternalPosOffset:=Axis.InternalPosOffset+sk;
Axis.LastInternalPosOffset:=Axis.LastInternalPosOffset+sk;
Axis.InternalActPos:=Axis.InternalActPos-sk;
Axis.VLastNomPosition:=Axis.VLastNomPosition-Axis.LastNomPosition;
Axis.LastNomPosition:=0.0;
IF JogPos
THEN
Richtung:=1;
END_IF;
IF JogNeg
THEN
Richtung:=-1;
END_IF;
Axis.LastNomDeceleration:=b;
IF vk=0.0
THEN
IF Richtung=-1
THEN
a_1:=-a_1;
b:=-b;
v_max:=-v_max;
END_IF;
b0:=0.0;
bk1:=b;
ELSE
a_1_B[7]:=vk_B[7];
b_B[7]:=vk_B[7];
v_max_B[7]:=vk_B[7];
b0:=b;
bk1:=b;
Richtung:=1;
IF vk<0.0
THEN
Richtung:=-1;
END_IF;
END_IF;
Zustand:=2;
Axis.Coord:=Coord;
Axis.AxisState:=4;
Axis.RemainingDistance:=0.0;
END_IF;
END_IF;
END_IF;
 
IF Zustand=2 OR Zustand=3
THEN
vk:=Axis.LastNomVelocity;
sk:=Axis.LastNomPosition;
IF Richtung<>0
THEN
Bremspunkt:=vk*vk*0.5/b+sk;
IF ABS(Bremspunkt)>=Grenze
THEN
Axis.Error:=true;
Axis.Err.DistanceErr:=true;
END_IF;
IF NOT Axis.AxisType AND Axis.Sync AND Axis.SWLimitEnable
THEN
Bremspunkt:=Bremspunkt+Axis.InternalPosOffset;
IF (Bremspunkt>=Axis.AxisLimitMax AND Richtung=1) OR
(Richtung=-1 AND Bremspunkt<=Axis.AxisLimitMin)
THEN
Axis.Error:=true;
Axis.Err.DistanceErr:=true;
END_IF;
END_IF;
END_IF;
IF Axis.Error
THEN
Err_DW_str:=Axis.Err;
IF Richtung=1 AND Err_DW_str.SWLimitMinExceeded
THEN
Err_DW_str.SWLimitMinExceeded:=false;
END_IF;
IF Richtung=-1 AND Err_DW_str.SWLimitMaxExceeded
THEN
Err_DW_str.SWLimitMaxExceeded:=false;
END_IF;
IF Err_DW<>DW#16#0
THEN
Error:=true;
IF Err_w.w1<>W#16#0
THEN
Zustand:=4;
ELSE
IF Zustand=2
THEN
Richtung:=0;
b0:=0.0;
END_IF;
END_IF;
END_IF;
END_IF;
IF Axis.Coord<>Coord OR Axis.ManEnable
THEN
CommandAborted:=true;
Zustand:=4;
END_IF;
END_IF;
IF Zustand=2
THEN
over:=ABS(Axis.Override)/100.0;
IF over>1.0
THEN
over:=1.0;
END_IF;
v_over:=v_max*over;
IF v_over<>0.0
THEN
vv:=vk/v_over;
ELSE
vv:=2.0;
END_IF;

 
 
IF (Richtung=1 AND JogPos) OR (Richtung=-1 AND JogNeg)
THEN
b0:=0.0;
IF vv<1.0
THEN
Bereich:=1;
vk1:=a_1*ta+vk;

ELSIF vv=1.0
THEN
Bereich:=2;
vk1:=vk;
ELSE
Bereich:=6;
vk1:=vk-bk1*ta;
IF vk1*vk<=0.0 OR
(vk=0.0 AND v_over=0.0)
THEN
vk1:=0.0;
END_IF;
END_IF;
ELSE
IF b0<>0.0
THEN
Bereich:=4;
bk1:=b0;
ELSE
Bereich:=3;
END_IF;
END_IF;

IF Bereich=1 OR Bereich=2 OR Bereich=6
THEN
IF (vk1-v_over)*
(vk-v_over)<=0.0 AND
vk<>v_over
THEN
vk1:=v_over;
END_IF;
sk1:=(vk+vk1)*taHalbe+sk;
END_IF;
IF Bereich=3
THEN
vk1:=vk-bk1*ta;
IF vk1*vk<=0.0 OR
(vk=0.0 AND v_over=0.0)
THEN
vk1:=0.0;
END_IF;
sk1:=(vk+vk1)*taHalbe+sk;
IF vk1=0.0 AND v_over<>0.0
THEN
Zustand:=3;
Bereich:=5;
END_IF;

ELSIF Bereich=4
THEN
vk1:=vk-bk1*ta;
IF vk1*vk<=0.0
THEN
vk1:=0.0;
b0:=vk1;
a_1:=-a_1;
b:=-b;
bk1:=b;
v_max:=-v_max;
Richtung:=-Richtung;
END_IF;
sk1:=(vk+vk1)*taHalbe+sk;
END_IF;

IF Axis.AxisType
THEN
IF ABS(sk1)>=AchsLaenge
THEN
RundKorrektur_DI:=TRUNC(sk1/AchsLaenge);
Axis.LageIstwert_DI:=Axis.LageIstwert_DI-RundKorrektur_DI*Axis.AchsLaenge_DI;
Axis.RundKorrektur:=DINT_TO_REAL(RundKorrektur_DI)*AchsLaenge;
sk1:=sk1-Axis.RundKorrektur;
Axis.LastNomPosition:=Axis.LastNomPosition-Axis.RundKorrektur;
Axis.VLastNomPosition:=Axis.VLastNomPosition-Axis.RundKorrektur;
Axis.InternalActPos:=Axis.InternalActPos-Axis.RundKorrektur;
END_IF;
END_IF;
Axis.InternalNomPosition:=sk1;
Axis.NomVelocity:=vk1;

ELSIF Zustand=3
THEN
IF Axis.AxisState=2
THEN
IF NOT Axis.Error
THEN
Done:=true;
END_IF;
Busy:=false;
Zustand:=5;
ELSIF Error
THEN
Axis.AxisState:=8;
ELSE
Axis.AxisState:=5;
END_IF;
ELSIF Zustand=4
THEN
Busy:=false;
Richtung:=0;
Zustand:=5;
ELSIF Zustand=5
THEN
IF (NOT JogPos AND Richtung=1) OR
(NOT JogNeg AND Richtung=-1) OR
(NOT JogPos AND NOT JogNeg)
THEN
Done:=false;
CommandAborted:=false;
Error:=false;
Busy:=false;
Zustand:=1;
END_IF;
END_IF;
 
END_FUNCTION_BLOCK
 



emc

Block checksum .





FB4 MC_Home - Reference search / setting



Name: MC_Home
Symbolic Name: MC_Home
Symbol Comment: Reference search / setting
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 2886
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB104
TITLE ='Reference search / setting'
AUTHOR : EMC
FAMILY : EMC2
NAME : MC_Home
VERSION : '2.0'
 
 
VAR_INPUT
Execute : BOOL ; //Start the motion / set reference point at rising edge
Position : REAL ; //Reference point coordinate [u]
Velocity : REAL ; //Maximum velocity [u/s]
Acceleration : REAL := 1.000000e-001; //Acceleration [u/s]
Deceleration : REAL := 1.000000e-001; //Deceleration [u/s]
Direction : INT := 1; //Initial direction: 1=positive, -1=negative
END_VAR
VAR_OUTPUT
Busy : BOOL ; //1=Referencing
Done : BOOL ; //1=Standstill has been reached / Function completed
CommandAborted : BOOL ; //1=Command was aborted by another command
Error : BOOL ; //1=Error has occurred within FB
END_VAR
VAR_IN_OUT
Axis : UDT1; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
v_max : REAL ; //internal use
a_1 : REAL ; //internal use
b : REAL ; //internal use
ta : REAL ; //internal use
taHalbe : REAL ; //internal use
Zustand : INT ; //internal use
Bereich : INT ; //internal use
Coord : DINT ; //internal use
Grenze : REAL ; //internal use
aufl : REAL ; //internal use
Error_alt : BOOL ; //internal use
DoneRef_alt : BOOL ; //internal use
END_VAR
VAR_TEMP
over : REAL ; //internal use
vk : REAL ; //internal use
sk : REAL ; //internal use
vk1 : REAL ; //internal use
sk1 : REAL ; //internal use
vv : REAL ; //internal use
v_over : REAL ; //internal use
Bremspunkt : REAL ; //internal use
Hilf_R : REAL ; //internal use
Hilf_DI : DINT ; //internal use
StrichZahl : REAL ; //internal use
Err_DW : DWORD ; //internal use
Err_w AT Err_DW : STRUCT
w0 : WORD;
w1 : WORD;
END_STRUCT ;
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 ;

END_VAR
BEGIN
 
IF Init
THEN
Init:=false;
Done:=false;
Busy:=false;
CommandAborted:=false;
Error:=false;
Error_alt:=false;
Zustand:=1;
Bereich:=5;
ta:=Axis.Sample_T;
taHalbe:=ta*5.000000e-001;
aufl:=Axis.aufl;
Grenze:=aufl*1.6777216e+007;
END_IF;
 
IF Zustand=7 OR Zustand=2 OR Zustand=3 OR Zustand=6
THEN
vk:=Axis.LastNomVelocity;
sk:=Axis.LastNomPosition;
IF Axis.ExecRef<>0
THEN
Bremspunkt:=vk*vk*0.5/b+sk;
IF ABS(Bremspunkt)>=Grenze
THEN
Axis.Error:=true;
Axis.Err.DistanceErr:=true;
END_IF;
END_IF;
IF Axis.Error
THEN
Err_DW_str:=Axis.Err;
IF NOT Error_alt
THEN
Error:=true;
Error_alt:=true;
IF Err_w.w1<>W#16#0
THEN
Zustand:=4;
ELSE
IF Zustand=2
THEN
Axis.ExecRef:=0;
END_IF;
END_IF;
END_IF;
END_IF;
IF Axis.Coord<>Coord OR Axis.ManEnable
THEN
CommandAborted:=true;
Zustand:=4;
END_IF;
END_IF;
 
IF Zustand=1
THEN
IF NOT Busy AND Execute
THEN
Busy:=true;
Done:=false;
CommandAborted:=false;
Error:=false;
Error_alt:=false;
DoneRef_alt:=false;
Coord:=Axis.Coord+1;
a_1:=Axis.MaxAcceleration;
b:=Axis.MaxDeceleration;
v_max:=Axis.MaxVelocity;
IF v_max>Velocity THEN v_max:=Velocity; END_IF;
IF a_1>Acceleration THEN a_1:=Acceleration; END_IF;
IF b>Deceleration THEN b:=Deceleration; END_IF;
Err_DW_str:=Axis.Err;
Err_DW_str.SWLimitMinExceeded:=false;
Err_DW_str.SWLimitMaxExceeded:=false;
Axis.Err.StartErr:=Err_DW<>DW#16#0 OR Axis.AxisState<>2 OR Axis.ManEnable;

IF NOT Axis.Err.StartErr
THEN
StrichZahl:=Position/aufl;
IF ABS(StrichZahl)>2.147483648e+009
THEN
Axis.Err.DataErr:=true;
ELSE
StrichZahl:=DINT_TO_REAL(ROUND(StrichZahl))*aufl;
Axis.Err.DataErr:=v_max<0.0 OR
v_max>0.0 AND (a_1<=0.0 OR b<=0.0 OR Axis.EncoderType) OR
ABS(StrichZahl)>1.6777216e+007;
Axis.Err.TargetErr:=(NOT Axis.AxisType AND Axis.SWLimitEnable) AND
(StrichZahl>=Axis.AxisLimitMax OR StrichZahl<=Axis.AxisLimitMin) OR
(StrichZahl>=Axis.AxisLimitMax OR StrichZahl<Axis.AxisLimitMin) AND
Axis.AxisType;
END_IF;
END_IF;

Err_DW_str:=Axis.Err;
Err_DW_str.SWLimitMinExceeded:=false;
Err_DW_str.SWLimitMaxExceeded:=false;
IF Err_DW<>DW#16#0
THEN
Axis.Error:=true;
Error:=true;
Zustand:=4;
ELSE
Axis.Sync:=false;
Axis.RemainingDistance:=0.0;
Axis.RefPoint:=StrichZahl;
Axis.AxisState:=9;
Axis.Coord:=Coord;
IF v_max=0.0
THEN
Zustand:=6;
Axis.ExecRef:=1;
ELSE
Zustand:=7;
Axis.ExecRef:=2;
sk:=Axis.LastNomPosition;
IF Axis.AxisType
THEN
Hilf_DI:=Axis.LageIstwert_DI;
Axis.LageIstwert_DI:=(Axis.LageIstwert_DI-Axis.PositionOffset) MOD
Axis.AchsLaenge_DI+Axis.PositionOffset;
Hilf_DI:=Hilf_DI-Axis.LageIstwert_DI;
Hilf_R:=DINT_TO_REAL(Hilf_DI)*aufl;
sk:=sk-Hilf_R;
Axis.InternalActPos:=Axis.InternalActPos-Hilf_R;
END_IF;
Axis.InternalPosOffset:=Axis.InternalPosOffset+sk;
Axis.InternalActPos:=Axis.InternalActPos-sk;
Axis.InternalNomPosition:=0.0;
Axis.LastNomPosition:=0.0;
Axis.LastNomDeceleration:=b;
IF Direction<=-1
THEN
a_1:=-a_1;
b:=-b;
v_max:=-v_max;
END_IF;
END_IF;
END_IF;
END_IF;
ELSIF Zustand=6
THEN
IF Axis.DoneRef
THEN
Axis.ExecRef:=0;
Zustand:=8;
END_IF;
ELSIF Zustand=8
THEN
Axis.Sync:=true;
Done:=true;
Busy:=false;
Zustand:=5;
Axis.AxisState:=2;
ELSIF Zustand=7
THEN
IF Axis.EintreiberFahrbereit
THEN
Zustand:=2;
END_IF;
IF Axis.DoneRef
THEN
Axis.ExecRef:=0;
Zustand:=8;
END_IF;
ELSIF Zustand=2
THEN
IF Axis.DoneRef
THEN
Axis.ExecRef:=0;
END_IF;
over:=ABS(Axis.Override)/100.0;
IF over>1.0
THEN
over:=1.0;
END_IF;
v_over:=v_max*over;
IF v_over<>0.0
THEN
vv:=vk/v_over;
ELSE
vv:=2.0;
END_IF;
IF Axis.ExecRef<>0
THEN
IF vv<1.0
THEN
Bereich:=1;
vk1:=a_1*ta+vk;
ELSIF vv=1.0
THEN
Bereich:=2;
vk1:=vk;
ELSE
Bereich:=6;
vk1:=vk-b*ta;
IF vk1*vk<=0.0 OR
(vk=0.0 AND v_over=0.0)
THEN
vk1:=0.0;
END_IF;
END_IF;
ELSE
IF NOT DoneRef_alt AND Axis.DoneRef
THEN
Bereich:=7;
DoneRef_alt:=true;
ELSE
Bereich:=3;
END_IF;
END_IF;
IF Bereich=1 OR Bereich=2 OR Bereich=6
THEN
IF (vk1-v_over)*(vk-v_over)<=0.0 AND vk<>v_over
THEN
vk1:=v_over;
END_IF;
sk1:=(vk+vk1)*taHalbe+sk;
END_IF;

IF Bereich=3
THEN
vk1:=vk-b*ta;
IF vk1*vk<=0.0 OR (vk=0.0 AND v_over=0.0)
THEN
vk1:=0.0;
END_IF;
sk1:=(vk+vk1)*taHalbe+sk;
IF vk1=0.0 AND v_over<>0.0
THEN
Zustand:=3;
Bereich:=5;
END_IF;

ELSIF Bereich=7
THEN
vk1:=vk;
sk1:=DINT_TO_REAL(Axis.LageIstwert_DI-Axis.PositionOffset)*aufl+sk;
END_IF;
Axis.InternalNomPosition:=sk1;
Axis.NomVelocity:=vk1;

ELSIF Zustand=3
THEN
IF Axis.AxisState=2
THEN
IF NOT Axis.Error
THEN
Done:=true;
Axis.Sync:=true;
END_IF;
Busy:=false;
Zustand:=5;
ELSIF Error_alt
THEN
Axis.AxisState:=8;
ELSE
Axis.AxisState:=5;
END_IF;
 
ELSIF Zustand=4
THEN
Busy:=false;
Axis.ExecRef:=0;
Zustand:=5;
 
ELSIF Zustand=5
THEN
IF NOT Execute
THEN
Done:=false;
CommandAborted:=false;
Error:=false;
Busy:=false;
Zustand:=1;
END_IF;
END_IF;
 
END_FUNCTION_BLOCK



emc

Block checksum .





FB5 MC_StopMotion - Stop any motion

Name: MC_Stop
Symbolic Name: MC_StopMotion
Symbol Comment: Stop any motion
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 1114
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB105
TITLE ='Stop any motion'
AUTHOR : EMC
FAMILY : EMC2
NAME : MC_Stop
VERSION : '2.0'
 
 
VAR_INPUT
Execute : BOOL ; //Start the action at rising edge
Deceleration : REAL ; //Deceleration [u/s]
END_VAR
VAR_OUTPUT
Busy : BOOL ; //1=Moving
Done : BOOL ; //1=Zero velocity has been reached
CommandAborted : BOOL ; //1=Command was aborted by manual mode
Error : BOOL ; //1=Error has occurred within FB
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
ziel : REAL ; //internal use
b : REAL ; //internal use
taHalbe : REAL ; //internal use
Zustand : INT ; //internal use
Coord : DINT ; //internal use
END_VAR
VAR_TEMP
Err_DW : DWORD ; //internal use
Err_w AT Err_DW : STRUCT
w0 : WORD;
w1 : WORD;
END_STRUCT ;
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 ;

END_VAR
BEGIN
 
IF Init
THEN
Init:=false;
Done:=false;
Busy:=false;
CommandAborted:=false;
Error:=false;
Zustand:=1;
END_IF;
 
IF Zustand=1 THEN
IF NOT Busy AND Execute THEN
Busy:=true;
Done:=false;
CommandAborted:=false;
Error:=false;
taHalbe:=Axis.Sample_T*0.5;
Coord:=Axis.Coord+1;
b:=Axis.MaxDeceleration;
IF Deceleration>0.0 THEN
b:=Deceleration;
IF Axis.LastNomDeceleration>b AND Axis.LastNomVelocity<>0.0 THEN
b:=Axis.LastNomDeceleration;
END_IF;
END_IF;
IF Axis.AxisState=1 OR
(Axis.AxisState>=6 AND Axis.AxisState<=8) OR
Axis.ManEnable
THEN
Axis.Err.StartErr:=true;
Axis.Error:=true;
Error:=true;
Zustand:=4;
ELSE
Zustand:=2;
Axis.Coord:=Coord;
Axis.AxisState:=6;
Axis.LastNomDeceleration:=b;
IF Axis.LastNomVelocity<0.0 THEN
b:=-b;
END_IF;
ziel:=Axis.LastNomVelocity*Axis.LastNomVelocity*0.5/b+Axis.LastNomPosition;
IF Axis.RemainingDistance<>0.0 THEN
Axis.RemainingDistance:=Axis.InternalNomPosition-Axis.LastNomPosition+Axis.RemainingDistance;
END_IF;
END_IF;
END_IF;
END_IF;
 
IF Zustand=2 OR Zustand=3 THEN
IF Axis.Error THEN
Err_DW_str:=Axis.Err;
IF Err_w.w1<>w#16#0 THEN
Zustand:=4;
Error:=true;
END_IF;
END_IF;
IF Axis.ManEnable THEN
Zustand:=4;
CommandAborted:=true;
END_IF;
END_IF;
 
IF Zustand=2 THEN
Axis.NomVelocity:=Axis.LastNomVelocity-b*Axis.Sample_T;
IF Axis.NomVelocity*Axis.LastNomVelocity<=0.0 THEN
Axis.NomVelocity:=0.0;
END_IF;
Axis.InternalNomPosition:=(Axis.LastNomVelocity+Axis.NomVelocity)*taHalbe+Axis.LastNomPosition;
IF Axis.NomVelocity=0.0 THEN
Axis.InternalNomPosition:=ziel;
Zustand:=3;
END_IF;
IF Axis.RemainingDistance<>0.0 THEN
Axis.RemainingDistance:=Axis.RemainingDistance-(Axis.InternalNomPosition-Axis.LastNomPosition);
END_IF;
 
ELSIF Zustand=3 THEN
IF Axis.AxisState=2 THEN
IF NOT Axis.Err.TargetApproachErr THEN
Done:=true;
END_IF;
Busy:=false;
Zustand:=5;
ELSE
Axis.AxisState:=8;
END_IF;
 
ELSIF Zustand=4 THEN
Busy:=false;
Zustand:=5;
 
ELSIF Zustand=5 THEN
IF NOT Execute THEN
Done:=false;
CommandAborted:=false;
Error:=false;
Busy:=false;
Zustand:=1;
END_IF;
END_IF;
 
END_FUNCTION_BLOCK



emc

Block checksum .





FB11 MC_Control - Position control

Make by Thomas Wiens



Name: MC_Ctrl
Symbolic Name: MC_Control
Symbol Comment: Position control
Family: EMC2
Version: 2.1
Author: EMC
Last modified: 12/03/2009
Use: UDT1
Size in work memory: 1772
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB111
 
TITLE = 'Position control'
AUTHOR : EMC
FAMILY : EMC2
NAME : MC_Ctrl
VERSION : '2.1'
 
VAR_INPUT
EnableDrive : BOOL; //1=Drive enable request
END_VAR
VAR_OUTPUT
DriveEnabled : BOOL; //1=Drive enable set
END_VAR
VAR_IN_OUT
Axis : UDT1; //Global axis data
Init : BOOL := true; //1=Initialize block
END_VAR
VAR
t : REAL; //internal use
ta : REAL; //internal use
ErrorAck_alt : BOOL; //internal use
Sim_alt : BOOL; //internal use
END_VAR
VAR_TEMP
Err_DW : DWORD; //internal use
Err_w AT Err_DW : STRUCT
w0 : WORD;
w1 : WORD;
END_STRUCT;
Err_DW_str AT Err_DW : STRUCT
SWLimitMinExceeded : BOOL; //1=SW-Endschalter Anfang berfahren
SWLimitMaxExceeded : BOOL; //1=SW-Endschalter Ende berfahren
TargetErr : BOOL; //1=Ziel auerhalb des Verfahrbereichs
NoSync : BOOL; //1=Achse nicht synchronisiert
DirectionErr : BOOL; //1=Fahrt in vorgegebene Richtung unzulssig
DataErr : BOOL; //1=Parameter eines Fahr-FBs unzulssig
StartErr : BOOL; //1=Start im aktuellen Achszustand nicht mglich
DistanceErr : BOOL; //1=max. Fahrweg berschritten (Fahrt ohne Ziel)
MasterErr : BOOL; //1=Leitachse mit hartem Fehler oder in falschem Achsstatus
bit09 : BOOL;
bit10 : BOOL;
bit11 : BOOL;
bit12 : BOOL;
bit13 : BOOL;
bit14 : BOOL;
bit15 : BOOL;
StoppedMotion : BOOL; //1=Achse im quittierpflichtigen Stop-Zustand
EnableDriveErr : BOOL; //1=Antriebsfreigabe fehlt
FollowingDistErr : BOOL; //1=Max. Schleppabstand berschritten
StandstillErr : BOOL; //1=Stillstandsbereich verlassen
TargetApproachErr : BOOL; //1=Fehler beim Zieleinlauf
EncoderErr : BOOL; //1=Geberfehler
OutputErr : BOOL; //1=Fehler am Ausgangstreiber
ConfigErr : BOOL; //1=Achsdaten fehlerhaft parametriert
DriveErr : BOOL; //1=Antriebsfehler
bit25 : BOOL;
bit26 : BOOL;
bit27 : BOOL;
bit28 : BOOL;
bit29 : BOOL;
bit30 : BOOL;
bit31 : BOOL;
END_STRUCT ;
Init_DW : DWORD; //internal use
Init_DW_str AT Init_DW : STRUCT //Initialisierungsbits fr 32 Bausteine
I0 : BOOL; //Init bit 0
I1 : BOOL; //Init bit 1
I2 : BOOL; //Init bit 2
I3 : BOOL; //Init bit 3
I4 : BOOL; //Init bit 4
I5 : BOOL; //Init bit 5
I6 : BOOL; //Init bit 6
I7 : BOOL; //Init bit 7
I8 : BOOL; //Init bit 8
I9 : BOOL; //Init bit 9
I10 : BOOL; //Init bit 10
I11 : BOOL; //Init bit 11
I12 : BOOL; //Init bit 12
I13 : BOOL; //Init bit 13
I14 : BOOL; //Init bit 14
I15 : BOOL; //Init bit 15
I16 : BOOL; //Init bit 16
I17 : BOOL; //Init bit 17
I18 : BOOL; //Init bit 18
I19 : BOOL; //Init bit 19
I20 : BOOL; //Init bit 20
I21 : BOOL; //Init bit 21
I22 : BOOL; //Init bit 22
I23 : BOOL; //Init bit 23
I24 : BOOL; //Init bit 24
I25 : BOOL; //Init bit 25
I26 : BOOL; //Init bit 26
I27 : BOOL; //Init bit 27
I28 : BOOL; //Init bit 28
I29 : BOOL; //Init bit 29
I30 : BOOL; //Init bit 30
I31 : BOOL; //Init bit 31
END_STRUCT ;
END_VAR
//========================================================
BEGIN
 
IF Init THEN
Init := false;
ErrorAck_alt := false;
Sim_alt := Axis.Sim;
t := 0.0;
Axis.OutVelocity := 0.0;
Axis.FollowingDistance := 0.0;
ta := Axis.Sample_T;
END_IF;
 
IF Axis.Error THEN
Err_DW_str := Axis.Err;
Err_DW_str.ConfigErr := false;
Err_DW_str.SWLimitMinExceeded := false;
Err_DW_str.SWLimitMaxExceeded := false;
IF (Err_DW <> DW#16#0) THEN
IF (NOT ErrorAck_alt AND Axis.ErrorAck) THEN
ErrorAck_alt := true;
ELSIF ((Axis.ErrorAck AND ErrorAck_alt) AND
NOT Axis.EinInitLaeuft AND NOT Axis.AusInitLaeuft AND
NOT Axis.EinQuittungLaeuft AND NOT Axis.AusQuittungLaeuft AND
(Axis.AxisState <= 2)) THEN
IF (Err_w.w1 <> W#16#0) THEN
Axis.InternalNomPosition := Axis.InternalActPos;
Axis.LastNomPosition := Axis.InternalActPos;
END_IF;
Err_DW := DW#16#0;
Err_DW_str.ConfigErr := Axis.Err.ConfigErr;
Err_DW_str.SWLimitMinExceeded := Axis.Err.SWLimitMinExceeded;
Err_DW_str.SWLimitMaxExceeded := Axis.Err.SWLimitMaxExceeded;
Axis.Err := Err_DW_str;
Axis.Error := (Err_DW <> DW#16#0);
Axis.ErrorAck := false;
ErrorAck_alt := false;
Axis.AxisState := 2;
END_IF;
ELSE
Axis.ErrorAck := false;
ErrorAck_alt := false;
END_IF;
ELSE
Axis.ErrorAck := false;
ErrorAck_alt := false;
END_IF;
 
IF NOT EnableDrive THEN
Axis.Err.EnableDriveErr := true;
Axis.Error := true;
END_IF;
 
IF (Axis.Sim XOR Sim_alt) THEN
Axis.AxisState := 1;
Axis.Error := true;
Axis.Err.StoppedMotion := true;
Sim_alt := Axis.Sim;
Init_DW := DW#16#FFFFFFFF;
Axis.Init := Init_DW_str;
END_IF;
 
IF Axis.Error THEN
Err_DW_str := Axis.Err;
IF (Err_w.w1 <> W#16#0) THEN
IF (Axis.AxisState <= 2) THEN
Axis.AxisState := 1;
ELSE
IF (Axis.AxisState <> 7) THEN
Axis.AxisState := 7;
Axis.Coord := Axis.Coord + 1;
IF (ABS(Axis.OutVelocity) >= ABS(Axis.NomVelocity)) THEN
Axis.OutVelocity := Axis.NomVelocity;
END_IF;
END_IF;
END_IF;
END_IF;
END_IF;
 
IF NOT Axis.ManEnable THEN
IF (Axis.AxisState <> 1 AND Axis.AxisState <> 7) THEN
Axis.FollowingDistance := Axis.InternalNomPosition - Axis.InternalActPos;
IF (ABS(Axis.FollowingDistance) > Axis.MaxFollowingDist) THEN
Axis.Err.FollowingDistErr := true;
Axis.Error := true;
END_IF;
Axis.OutVelocity := ABS(Axis.FactorP) * Axis.FollowingDistance;
IF (Axis.AxisState = 2) THEN
IF (ABS(Axis.FollowingDistance) > Axis.StandstillRange) THEN
Axis.Err.StandstillErr := true;
Axis.Error := true;
END_IF;
ELSIF (Axis.AxisState = 5 OR Axis.AxisState = 8) THEN
IF Axis.MonitorTargetAppr THEN
IF (ABS(Axis.FollowingDistance) > Axis.TargetRange) THEN
IF (t > Axis.MonTimeTargetAppr) THEN
Axis.Err.TargetApproachErr := true;
Axis.Error := true;
t := 0.0;
ELSE
t := t + ta;
END_IF;
ELSE
t := 0.0;
Axis.AxisState := 2;
END_IF;
ELSE
t := 0.0;
Axis.AxisState := 2;
END_IF;
END_IF;
ELSIF (Axis.AxisState = 7) THEN
Axis.RemainingDistance := 0.0;
Axis.FollowingDistance := 0.0;
IF (Axis.OutVelocity > 0.0) THEN
Axis.OutVelocity := (Axis.OutVelocity - (Axis.EmergencyDec * ta));
IF (Axis.OutVelocity <= 0.0) THEN
Axis.OutVelocity := 0.0;
Axis.AxisState := 1;
END_IF;
ELSE
Axis.OutVelocity := ((Axis.EmergencyDec * ta) + Axis.OutVelocity);
IF (Axis.OutVelocity >= 0.0) THEN
Axis.OutVelocity := 0.0;
Axis.AxisState := 1;
END_IF;
END_IF;
ELSIF (Axis.AxisState = 1) THEN
Axis.InternalNomPosition := Axis.InternalActPos;
Axis.LastNomPosition := Axis.InternalActPos;
Axis.NomVelocity := 0.0;
Axis.OutVelocity := 0.0;
Axis.RemainingDistance := 0.0;
Axis.FollowingDistance := 0.0;
Axis.Error := true;
Axis.Err.StoppedMotion := true;
END_IF;
DriveEnabled := ((Axis.AxisState <> 1) AND EnableDrive);
ELSE
Axis.OutVelocity := Axis.ManVelocity;
IF (ABS(Axis.ManVelocity) > Axis.MaxVelocity) THEN
Axis.OutVelocity := ((Axis.ManVelocity / ABS(Axis.ManVelocity)) * Axis.MaxVelocity);
END_IF;
Axis.InternalNomPosition := Axis.InternalActPos;
Axis.LastNomPosition := Axis.InternalActPos;
Axis.NomVelocity := 0.0;
Axis.LastNomVelocity := 0.0;
Axis.RemainingDistance := 0.0;
Axis.FollowingDistance := 0.0;
DriveEnabled := EnableDrive;
END_IF;
 
Axis.VLastNomPosition := Axis.LastNomPosition;
Axis.LastNomPosition := Axis.InternalNomPosition;
Axis.VLastNomVelocity := Axis.LastNomVelocity;
Axis.LastNomVelocity := Axis.NomVelocity;
Axis.LastInternalPosOffset := Axis.InternalPosOffset;
Axis.LastRundKorrektur := Axis.RundKorrektur;
Axis.RundKorrektur := 0.0;
 
END_FUNCTION_BLOCK



emc

Block checksum .





FB12 MC_Simulation - Simulate axis



Name: MC_Sim
Symbolic Name: MC_Simulation
Symbol Comment: Simulate axis
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 410
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB112
TITLE ='Simulate axis'
AUTHOR : EMC
FAMILY : EMC2
NAME : MC_Sim
VERSION : '2.0'
 
 
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
Raster : BOOL ; //internal use
Bitanzahl : INT := 12; //internal use
LageDiff_R : REAL ; //internal use
Faktor : REAL ; //internal use
AusFaktor : REAL ; //internal use
WertBei10V : REAL ; //internal use
END_VAR
VAR_TEMP
Geber_DI_alt : DINT ; //internal use
v : REAL ; //internal use
v_B AT v : ARRAY[0..31] OF BOOL;
vz : BOOL ; //internal use
END_VAR
BEGIN
 
IF Init THEN
Init:=false;
Axis.OutVelocity:=0.0;
LageDiff_R:=0.0;
WertBei10V:=EXP(INT_TO_REAL(Bitanzahl)*6.931472e-001);
Faktor:=Axis.DriveInputAtMaxVel/Axis.MaxVelocity*WertBei10V/Axis.DriveInputAt100;
AusFaktor:=Faktor*INT_TO_REAL(Axis.PolarityEncoder);
END_IF;
 
IF Axis.Sim THEN
v:=Axis.OutVelocity*AusFaktor;
IF ABS(v)>WertBei10V THEN
vz:=v_B[7];
v:=WertBei10V;
v_B[7]:=vz;
END_IF;

IF Raster THEN
v:=DINT_TO_REAL(ROUND(v))/Faktor;
ELSE
v:=v/Faktor;
END_IF;
LageDiff_R:=v*Axis.Sample_T+LageDiff_R;
Geber_DI_alt:=Axis.EncoderValue;
Axis.EncoderValue:=ROUND(LageDiff_R/Axis.aufl)+Axis.EncoderValue;
LageDiff_R:=LageDiff_R-DINT_TO_REAL(Axis.EncoderValue-Geber_DI_alt)*Axis.aufl;
IF Axis.EncoderType THEN
Axis.EncoderValue:=Axis.EncoderValue MOD Axis.AbsGeberLaenge_DI;
END_IF;
END_IF;
END_FUNCTION_BLOCK



emc

Block checksum .





FB21 EncoderIM178 - Input driver for IM178-4



Name: EncIM178
Symbolic Name: EncoderIM178
Symbol Comment: Input driver for IM178-4
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 2210
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB121
TITLE ='Input driver for IM178-4'
AUTHOR : EMC
FAMILY : EMC2
NAME : EncIM178
VERSION : '2.0'
 
 
VAR_INPUT
EncErr : BOOL ; //1=Encoder error
RefMode : BOOL ; //Referencing: 0=with I0, 1=with I1 and zero mark
END_VAR
VAR_OUTPUT
DI_0 : BOOL ; //Digital input 0
DI_1 : BOOL ; //Digital input 1
DI_2 : BOOL ; //Digital input 2
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
Status : STRUCT
REF1_Q : BOOL ; //1=Acknowledgment for actual reference value detection with I1 and zero mark
REF0_Q : BOOL ; //1=Acknowledgment for actual reference value detection with I0
SYN_Q : BOOL ; //1=Acknowledgment for synchronization
bit11 : BOOL ;
DIR : BOOL ; //Direction of movement (0=negative direction,1=positive direction)
SYNC : BOOL ; //1=Actual value synchronized
PARA : BOOL ; //1=Channel parameter assignment errorfree
bit15 : BOOL ;
DI_0 : BOOL ; //Digital input 0
DI_1 : BOOL ; //Digital input 1
DI_2 : BOOL ; //Digital input 2
bit03 : BOOL ;
EXTF0 : BOOL ; //1=Sensor signal cable error
EXTF1 : BOOL ; //1=Sensor error
EXTF2 : BOOL ; //1=Zero mark error
bit07 : BOOL ;
END_STRUCT ;
Status_w AT Status : WORD;
GeberAlt_DI : DINT ; //internal use
Addr_Status : INT ; //internal use
Addr_ActPos_DI : INT ; //internal use
Addr_RefPos_DI : INT ; //internal use
Addr_Steuer : INT ; //internal use
v_Faktor : REAL ; //internal use
MaxZahl : DINT ; //internal use
MaxZahlHalb : DINT ; //internal use
Steuer_W : WORD ; //internal use
Steuer_byte AT Steuer_W : ARRAY[0..1] OF BYTE;
Steuer_bool AT Steuer_W : ARRAY[0..15] OF BOOL;
RefPointInt : REAL ; //internal use
Richtungsanpassung : INT := 1; //internal use
aufl : REAL ; //internal use
AxisLimitMin : REAL ; //internal use
AxisLimitMax : REAL ; //internal use
PositionOffset : DINT ; //internal use
EncoderType : BOOL ; //internal use
EncErr_alt : BOOL ; //internal use
END_VAR
VAR_TEMP
Addr_InChannel : INT ; //internal use
Addr_OutChannel : INT ; //internal use
Geber_DI : DINT ; //internal use
LageDiff_DI : DINT ; //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
Sim : BOOL ; //internal use
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.InputChannelNo*12+Axis.InputModuleInAddr;
Addr_Status:=Addr_InChannel;
Addr_ActPos_DI:=Addr_InChannel+4;
Addr_RefPos_DI:=Addr_InChannel+8;
Addr_OutChannel:=Axis.OutputChannelNo*4+Axis.OutputModuleOutAddr;
Addr_Steuer:=Addr_OutChannel+1;
Steuer_byte[1]:=B#16#0;
Status_w:=W#16#0;
Axis.EinQuittungLaeuft:=false;
Axis.EintreiberFahrbereit:=false;
EncErr_alt:=false;
Axis.DoneRef:=false;
Axis.ExecRef:=0;
RefPointInt:=Axis.RefPoint;
aufl:=Axis.aufl;
v_Faktor:=aufl/Axis.Sample_T;
Richtungsanpassung:=Axis.PolarityEncoder;
AxisLimitMin:=Axis.AxisLimitMin;
AxisLimitMax:=Axis.AxisLimitMax;
EncoderType:=Axis.EncoderType;
IF NOT EncoderType THEN
Axis.Sync:=false;
ELSE
MaxZahl:=Axis.AbsGeberLaenge_DI;
MaxZahlHalb:=MaxZahl/2;
END_IF;
END_IF;

Sim:=Axis.Sim;
 
IF Sim OR EncErr THEN
Status_w:=Status_w OR W#16#4000;
ELSE
Status_w :=PIW[Addr_Status];
Axis.EncoderValue:=DWORD_TO_DINT(PID[Addr_ActPos_DI]);
END_IF;
 
DI_0:=Status.DI_0;
DI_1:=Status.DI_1;
DI_2:=Status.DI_2;
 
Geber_DI:=Axis.EncoderValue;
 
IF NOT Status.PARA OR EncErr THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
IF EncErr THEN
EncErr_alt:=true;
END_IF;
IF NOT EncoderType THEN
Axis.Sync:=false;
END_IF;
END_IF;
 
IF Status.EXTF0 OR Status.EXTF1 OR Status.EXTF2 THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
IF NOT EncoderType THEN
Axis.Sync:=false;
END_IF;
 
Steuer_byte[1]:=Steuer_byte[1] AND B#16#80;
IF NOT Steuer_bool[15] THEN
Steuer_bool[15]:=Axis.ErrorAck;
Axis.EinQuittungLaeuft:=Axis.ErrorAck;
END_IF;
ELSE
Steuer_bool[15]:=false;
Axis.EinQuittungLaeuft:=false;
END_IF;
 
IF NOT EncErr THEN
IF Init THEN
Axis.LageIstwert_DI:=Geber_DI;
GeberAlt_DI:=Geber_DI;
PositionOffset:=Axis.PositionOffset;
IF EncoderType THEN
GeberAmAchsanfang:=ROUND((AxisLimitMin-RefPointInt)/aufl*INT_TO_REAL(Richtungsanpassung)+
DINT_TO_REAL(PositionOffset)+DINT_TO_REAL(MaxZahl)) MOD MaxZahl;
GeberAmAchsende:=ROUND((AxisLimitMax-RefPointInt)/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;
ELSE
Axis.PositionOffset:=Geber_DI;
END_IF;
END_IF;
 
IF NOT Axis.Err.EncoderErr THEN
IF Axis.ExecRef=2 THEN
IF (Status.REF0_Q OR Status.REF1_Q) OR
NOT Axis.EintreiberFahrbereit AND
(NOT RefMode AND DI_0 OR RefMode AND DI_1)
THEN
Axis.DoneRef:=true;
PositionOffset:=Axis.PositionOffset;
Axis.InternalPosOffset:=(Axis.InternalPosOffset-
DINT_TO_REAL(Axis.LageIstwert_DI-PositionOffset)*aufl)-
RefPointInt+Axis.RefPoint;
RefPointInt:=Axis.RefPoint;
IF NOT Axis.EintreiberFahrbereit OR Sim THEN
PositionOffset:=Geber_DI;
ELSE
PositionOffset:=DWORD_TO_DINT(PID[Addr_RefPos_DI]);
END_IF;
Axis.LageIstwert_DI:=PositionOffset;
GeberAlt_DI:=PositionOffset;
Axis.PositionOffset:=PositionOffset;
Steuer_byte[1]:=Steuer_byte[1] AND B#16#FC;
ELSE
Steuer_bool[8]:=RefMode;
Steuer_bool[9]:= NOT RefMode;
Axis.EintreiberFahrbereit:=true;
END_IF;
 
ELSIF Axis.ExecRef=1 THEN
RefPointInt:=Axis.RefPoint;
Axis.InternalPosOffset:=RefPointInt;
Axis.InternalNomPosition:=0.0;
Axis.LastNomPosition:=0.0;
Axis.LageIstwert_DI:=Geber_DI;
Axis.PositionOffset:=Geber_DI;
Axis.DoneRef:=true;
ELSE
Axis.DoneRef:=false;
Axis.EintreiberFahrbereit:=false;
Steuer_byte[1]:=Steuer_byte[1] AND B#16#FC;
END_IF;
LageDiff_DI:=(Geber_DI-GeberAlt_DI)*INT_TO_DINT(Richtungsanpassung);
IF EncoderType THEN
IF LageDiff_DI<-MaxZahlHalb THEN
LageDiff_DI:=LageDiff_DI+MaxZahl;
END_IF;
IF LageDiff_DI>MaxZahlHalb THEN
LageDiff_DI:=LageDiff_DI-MaxZahl;
END_IF;
END_IF;
Axis.LageIstwert_DI:=Axis.LageIstwert_DI+LageDiff_DI;
Axis.ActPosition:=DINT_TO_REAL(Axis.LageIstwert_DI-Axis.PositionOffset)*
aufl+RefPointInt;
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-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<=AxisLimitMin;
Axis.Err.SWLimitMaxExceeded:=Axis.SWLimitEnable AND Axis.Sync AND
Axis.ActPosition>=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;
 
IF NOT (Sim OR EncErr) THEN
PQB[Addr_Steuer]:=Steuer_byte[1];
END_IF;
 
END_FUNCTION_BLOCK



emc

Block checksum .





FB22 EncoderFM450 - Input driver for FM450



Name: EncFM450
Symbolic Name: EncoderFM450
Symbol Comment: Input driver for FM450
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 2334
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB122
TITLE ='Input driver for FM450'
AUTHOR : EMC
FAMILY : EMC2
NAME : EncFM450
VERSION : '2.0'
 
 
VAR_INPUT
EncErr : BOOL ; //1=Encoder error
END_VAR
VAR_OUTPUT
DI_0 : BOOL ; //Digital input 0
DI_1 : BOOL ; //Digital input 1
DI_2 : BOOL ; //Digital input 2
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
ReadDiagErr : BOOL := TRUE; //1=Read data record 1 (DS1)
END_VAR
VAR
Koord : STRUCT
DA_ERR_W : WORD ; //Data error word
OT_ERR_B : BYTE ; //Operating error byte
bit0 : BOOL ;
STS_TFB : BOOL ; //Status test free
DIAG : BOOL ; //internal use
OT_ERR : BOOL ; //internal use
DATA_ERR : BOOL ; //Data error bit
FM_NEUSTQ : BOOL ; //internal use
FM_NEUST : BOOL ; //internal use
PARA : BOOL ; //1=Module configured
END_STRUCT ;
Koord_dw AT Koord : DWORD;
Status : STRUCT
byte0 : BYTE ;
STS_RUN : BOOL ; //Status counter working
STS_DIR : BOOL ; //Status count direction
STS_ZERO : BOOL ; //Status zero pass
STS_OFLW : BOOL ; //Status overflow
STS_UFLW : BOOL ; //Status underflow
STS_SYNC : BOOL ; //Status counter synchronized
STS_GATE : BOOL ; //Status internal gate
STS_SW_G : BOOL ; //Status software gate
STS_SET : BOOL ; //Status software gate
bit9 : BOOL ;
STS_STA : BOOL ; //Status digital input START
STS_STP : BOOL ; //Status digital input STOP
STS_CMP1 : BOOL ; //Status output comparison value 1
STS_CMP2 : BOOL ; //Status output comparison value 2
bit14 : BOOL ;
bit15 : BOOL ;
LoadInCounter : BOOL ; //internal use
LoadInLoad : BOOL ; //internal use
Comp1Set : BOOL ; //internal use
Comp2Set : BOOL ; //internal use
NoSync : BOOL ; //internal use
NoZero : BOOL ; //internal use
bit6 : BOOL ;
bit7 : BOOL ;
END_STRUCT ;
Status_dw AT Status : DWORD;
JobErr : INT ; //Error reported while reading DS1
DiagStatus : STRUCT
MDL_DEFECT : BOOL ; //Module defective
INT_FAULT : BOOL ; //Internal fault
EXT_FAULT : BOOL ; //External fault
PNT_INFO : BOOL ; //Point information
EXT_VOLTAGE : BOOL ; //External voltage low
FLD_CONNCTR : BOOL ; //Field wiring connector missing
NO_CONFIG : BOOL ; //Module has no configuration data
CONFIG_ERR : BOOL ; //Module has configuration error
MDL_TYPE : BYTE ; //Type of module
SUB_NDL_ERR : BOOL ; //Sub-Module is missing or has error
COMM_FAULT : BOOL ; //Communication fault
MDL_STOP : BOOL ; //Module is stopped
WTCH_DOG_FLT : BOOL ; //Watch dog timer stopped module
INT_PS_FLT : BOOL ; //Internal power supply fault
PRIM_BATT_FLT : BOOL ; //Primary battery is in fault
BCKUP_BATT_FLT : BOOL ; //Backup battery is in fault
RESERVED_2 : BOOL ; //Reserved for system
RACK_FLT : BOOL ; //Rack fault, only for bus interface module
PROC_FLT : BOOL ; //Processor fault
EPROM_FLT : BOOL ; //EPROM fault
RAM_FLT : BOOL ; //RAM fault
ADU_FLT : BOOL ; //ADU fault
FUSE_FLT : BOOL ; //Fuse fault
HW_INTR_FLT : BOOL ; //Hardware interupt input in fault
RESERVED_3 : BOOL ; //Reserved for system
CH_TYPE : BYTE ; //Channel type
LGTH_DIA : BYTE ; //Diagnostics data length per channel
CH_NO : BYTE ; //Channel number
GRP_ERR1 : BOOL ; //Group error channel 1
GRP_ERR2 : BOOL ; //Group error channel 2
bit7_2 : BOOL ;
bit7_3 : BOOL ;
bit7_4 : BOOL ;
bit7_5 : BOOL ;
bit7_6 : BOOL ;
bit7_7 : BOOL ;
CH1_SIGA : BOOL ; //Channel 1, error signal A
CH1_SIGB : BOOL ; //Channel 1, error signal B
CH1_SIGZ : BOOL ; //Channel 1, error signal zero
CH1_BETW : BOOL ; //Channel 1, error between channels
CH1_5V2 : BOOL ; //Channel 1, error in 5.2 V encoder supply
byte9 : BYTE ;
CH2_SIGA : BOOL ; //Channel 2, error signal A
CH2_SIGB : BOOL ; //Channel 2, error signal B
CH2_SIGZ : BOOL ; //Channel 2, error signal zero
CH2_BETW : BOOL ; //Channel 2, error between channels
CH2_5V2 : BOOL ; //Channel 2, error in 5.2 V encoder supply
byte11 : BYTE ;
END_STRUCT ;
DiagStatus_dw AT DiagStatus : DWORD ;
DiagStatus_b AT DiagStatus : ARRAY[0..31] OF BOOL ;
 
GeberAlt_DI : DINT ; //internal use
Addr_Status : INT ; //internal use
Addr_Koord : INT ; //internal use
Addr_ActPos_DI : INT ; //internal use
Addr_LoadVal_DI : INT ; //internal use
Addr_Steuer : INT ; //internal use
KanalNr : INT ; //internal use
v_Faktor : REAL ; //internal use
PASteuer : DWORD ; //internal use
PASteuer_b AT PASteuer : ARRAY[0..31] OF BOOL ;
RefPointInt : REAL ; //internal use
Richtungsanpassung : INT := 1; //internal use
LoadVal_DW : DWORD ; //internal use
RefZustand : INT ; //internal use
DiagFehler : BOOL ; //internal use
EncErr_alt : BOOL ; //internal use
Diag_alt : 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 ;
 
Sim : BOOL ; //internal use
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
KanalNr:=Axis.InputChannelNo;
Addr_InChannel:=Axis.InputChannelNo*32+AXis.InputModuleInAddr;
Addr_Status:=Addr_InChannel+12;
Addr_ActPos_DI:=Addr_InChannel+4;
Addr_Koord:=Addr_InChannel+8;
Addr_LoadVal_DI:=Addr_InChannel;
Addr_Steuer:=Addr_LoadVal_DI+12;
v_Faktor:=Axis.aufl/Axis.Sample_T;
Koord_dw:=DW#16#0;
Status_dw:=DW#16#0;
PASteuer:=DW#16#0;
DiagFehler:=false;
ReadDiagErr:=true;
Axis.EinInitLaeuft:=true;
Axis.EintreiberFahrbereit:=false;
EncErr_alt:=false;
Diag_alt:=false;
Axis.DoneRef:=false;
RefPointInt:=Axis.RefPoint;
Axis.ExecRef:=0;
Axis.Sync:=false;
Axis.EncoderType:=false;
Richtungsanpassung:=Axis.PolarityEncoder;
RefZustand:=0;
END_IF;
 
Sim:=Axis.Sim;
 
IF Sim OR EncErr THEN
Koord_dw:=DW#16#80;
PASteuer:=DW#16#0;
Axis.EinInitLaeuft:=false;
ReadDiagErr:=false;
ELSE
Status_dw:=PID[Addr_Status];
Koord_dw:=PID[Addr_Koord];
IF (Koord_dw AND DW#16#60) = DW#16#40 THEN
PASteuer:=DW#16#40000000;
Init:=true;
Axis.EinInitLaeuft:=true;
Axis.Sync:=false;
Axis.EintreiberFahrbereit:=false;
ELSE
PASteuer_b[6]:=false;
Axis.EncoderValue:=DWORD_TO_DINT(PID[Addr_ActPos_DI]);
IF Koord.DIAG XOR Diag_alt THEN
ReadDiagErr:=true;
Diag_alt:=Koord.DIAG;
END_IF;
IF ReadDiagErr THEN
JobErr:=RD_REC(REQ := true
,IOID := B#16#54
,LADDR := INT_TO_WORD(Axis.InputModuleInAddr)
,RECNUM := 1
,BUSY := ReadDiagErr
,RECORD := DiagStatus
);

IF (INT_TO_DINT(JobErr)>=32930 AND
INT_TO_DINT(JobErr)<=32932) OR
(INT_TO_DINT(JobErr)>=32960 AND
INT_TO_DINT(JobErr)<=32975)
THEN
ReadDiagErr:=true;
END_IF;
IF NOT ReadDiagErr THEN
IF JobErr>=0 THEN
IF DiagStatus.MDL_DEFECT THEN
IF (DW#16#F000FFFF AND DiagStatus_dw) <> 0 THEN
DiagFehler:=true;
ELSE
DiagFehler:=DiagStatus_b[INT_TO_DINT(KanalNr)+56];
END_IF;
ELSE
DiagFehler:=false;
END_IF;
ELSE
DiagFehler:=true;
END_IF;
END_IF;
END_IF;
END_IF;
END_IF;
 
Geber_DI:=Axis.EncoderValue;
DI_0:=Status.STS_STA;
DI_1:=Status.STS_STP;
DI_2:=Status.STS_SET;
 
IF EncErr THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
EncErr_alt:=true;
Axis.Sync:=false;
ELSE
IF Init THEN
GeberAlt_DI:=Geber_DI;
Axis.LageIstwert_DI:=Geber_DI;
Axis.PositionOffset:=Geber_DI;
END_IF;
IF (Koord.OT_ERR OR Koord.DATA_ERR) OR
(NOT Koord.PARA AND Koord.FM_NEUSTQ) OR
DiagFehler
THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
Axis.Sync:=false;
ELSE
IF Axis.ExecRef=2 THEN
IF RefZustand=0 THEN
LoadVal_DW:=DINT_TO_DWORD(Geber_DI) XOR DW#16#50000000;
LoadVal_DW:=LoadVal_DW AND DW#16#F0000000;
LoadVal_DW:=LoadVal_DW OR DW#16#2000000;
IF Status.STS_SET THEN
RefZustand:=5;
LoadVal_DW:=DINT_TO_DWORD(Geber_DI);
ELSE
IF Sim THEN
RefZustand:=5;
ELSE
PQD[Addr_LoadVal_DI]:=LoadVal_DW;
PASteuer_b[25]:=true;
RefZustand:=3;
END_IF;
END_IF;
ELSIF RefZustand=3 THEN
IF Status.LoadInLoad THEN
PASteuer_b[25]:=false;
RefZustand:=4;
END_IF;
ELSIF RefZustand=4 THEN
IF PID[Addr_LoadVal_DI]=LoadVal_DW THEN
RefZustand:=5;
END_IF;
ELSIF RefZustand=5 THEN
IF (LoadVal_DW AND DW#16#F0000000)=(DINT_TO_DWORD(Geber_DI) AND DW#16#F0000000) THEN
Axis.DoneRef:=true;
Axis.InternalPosOffset:=(Axis.InternalPosOffset-DINT_TO_REAL(Axis.LageIstwert_DI-Axis.PositionOffset)*Axis.aufl)
-RefPointInt+Axis.RefPoint;
RefPointInt:=Axis.RefPoint;
Axis.PositionOffset:=DWORD_TO_DINT(LoadVal_DW);
Axis.LageIstwert_DI:=DWORD_TO_DINT(LoadVal_DW);
GeberAlt_DI:=DWORD_TO_DINT(LoadVal_DW);

PASteuer:=PASteuer AND DW#16#FFFCFFFF;
RefZustand:=0;
ELSE
PASteuer:=PASteuer OR DW#16#30000;
Axis.EintreiberFahrbereit:=true;
END_IF;
END_IF;
ELSIF Axis.ExecRef=1 THEN
RefPointInt:=Axis.RefPoint;
Axis.InternalPosOffset:=RefPointInt;
Axis.InternalNomPosition:=0.0;
Axis.LastNomPosition:=0.0;
Axis.LageIstwert_DI:=Geber_DI;
Axis.PositionOffset:=Geber_DI;
Axis.DoneRef:=true;
ELSE
Axis.DoneRef:=false;
Axis.EintreiberFahrbereit:=false;
PASteuer:=PASteuer AND DW#16#FFFCFFFF;
END_IF;

LageDiff_DI:=(Geber_DI-GeberAlt_DI)*INT_TO_DINT(Richtungsanpassung);
Axis.LageIstwert_DI:=LageDiff_DI+Axis.LageIstwert_DI;
Axis.ActPosition:=DINT_TO_REAL(Axis.LageIstwert_DI-Axis.PositionOffset)
*Axis.aufl+RefPointInt;
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
Axis.InternalNomPosition:=Axis.InternalActPos;
Axis.LastNomPosition:=Axis.InternalActPos;
Axis.VLastNomPosition:=Axis.InternalActPos;
IF PASteuer=DW#16#0 THEN
IF NOT ReadDiagErr THEN
Init:=false;
Axis.EinInitLaeuft:=false;
END_IF;
END_IF;
END_IF;
 
IF NOT (Sim OR EncErr) THEN
PQD[Addr_Steuer]:=PASteuer;
END_IF;
 
END_FUNCTION_BLOCK



emc

Block checksum .





FB23 EncoderET200S1SSI - Input driver for ET200S 1SSI



Name: Enc1SSI
Symbolic Name: EncoderET200S1SSI
Symbol Comment: Input driver for ET200S 1SSI
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 1566
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



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 ; //Rckgabewert 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



emc

Block checksum .





FB24 EncoderAbsSensorDP - Input driver for DP-sensor



Name: EncAbsDP
Symbolic Name: EncoderAbsSensorDP
Symbol Comment: Input driver for DP-sensor
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 1420
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB24
TITLE ='Input driver for DP-sensor'
AUTHOR : EMC
FAMILY : EMC2
NAME : EncAbsDP
VERSION : '2.0'
 
 
VAR_INPUT
EncErr : BOOL ; //1=Encoder error
END_VAR
VAR_IN_OUT
Axis : UDT1 ;
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
Status : STRUCT
bit24 : BOOL ;
Ready : BOOL ; //1=Module ready
Mode : BOOL ; //Operating mode: 0=commissioning, 1=normal mode
SWLimitExceeded : BOOL ; //1=SW limit switch min. or max. exceeded
Dir : BOOL ; //Rotational direction: 0=clockwise, 1=counterclockwise
bit29 : BOOL ;
bit30 : BOOL ;
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
aufl : REAL ; //internal use
AxisLimitMin : REAL ; //internal use
AxisLimitMax : REAL ; //internal use
PositionOffset : DINT ; //internal use
PositionOffset_r AT PositionOffset : REAL;
EncErr_alt : 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
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;
Status_dw:=DW#16#0;
Axis.EintreiberFahrbereit:=false;
EncErr_alt:=false;
Axis.DoneRef:=false;
Axis.ExecRef:=0;
aufl:=Axis.aufl;
v_Faktor:=aufl/Axis.Sample_T;
Richtungsanpassung:=Axis.PolarityEncoder;
AxisLimitMin:=Axis.AxisLimitMin;
AxisLimitMax:=Axis.AxisLimitMax;
Axis.EncoderType:=true;
MaxZahl:=Axis.AbsGeberLaenge_DI;
MaxZahlHalb:=MaxZahl/2;
END_IF;
 
IF Axis.Sim OR EncErr
THEN
Status_dw:=DW#16#2000000;
ELSE
Status_dw:=PID[Addr_ActPos_DI];
Axis.EncoderValue:=DWORD_TO_DINT(Status_dw AND DW#16#FFFFFF);
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((AxisLimitMin-Axis.RefPoint)/aufl*INT_TO_REAL(Richtungsanpassung)+
DINT_TO_REAL(PositionOffset)+DINT_TO_REAL(MaxZahl)) MOD MaxZahl;
GeberAmAchsende:=ROUND((AxisLimitMax-Axis.RefPoint)/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 NOT Status.Ready
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)*
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-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-(TruncKorr_DI*Axis.AchsLaenge);
ELSE
Axis.Err.SWLimitMinExceeded:=Axis.SWLimitEnable AND
Axis.Sync AND
Axis.ActPosition<=AxisLimitMin;
Axis.Err.SWLimitMaxExceeded:=Axis.SWLimitEnable AND
Axis.Sync AND
Axis.ActPosition>=AxisLimitMax;

Err_DW_str:=Axis.Err;
Axis.Error:=Err_DW<>DW#16#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
 



emc

Block checksum .





FB25 EncoderSM338 - Input driver for SM338

Name: EncSM338
Symbolic Name: EncoderSM338
Symbol Comment: Input driver for SM338
Family: EMC2
Version: 2.1
Author: EMC
Last modified: 03/12/2009
Use: UDT1,SFC6,SFB52
Size in work memory: 2028
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCLCOMP K05.03.05.00_01.03.00.01 release

FUNCTION_BLOCK FB125
TITLE ='Input driver for SM338'
AUTHOR : EMC
FAMILY : EMC2
NAME : EncSM338
VERSION : '2.1'
 
 
VAR_INPUT
EncErr : BOOL ; //1=Encoder error
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
ReadDiagErr : BOOL := TRUE; //1=Read data record 1 (DS1)
END_VAR
VAR
Freeze : BOOL ; //1=Freeze at encoder input
JobErr : INT ; //Error reported while reading DS1
DiagStatus : STRUCT
MDL_DEFECT : BOOL ; //Module defective
INT_FAULT : BOOL ; //Internal fault
EXT_FAULT : BOOL ; //External fault
PNT_INFO : BOOL ; //Point information
EXT_VOLTAGE : BOOL ; //External voltage low
FLD_CONNCTR : BOOL ; //Field wiring connector missing
NO_CONFIG : BOOL ; //Module has no configuration data
CONFIG_ERR : BOOL ; //Module has configuration error
MDL_TYPE : BYTE ; //Type of module
SUB_NDL_ERR : BOOL ; //Sub-Module is missing or has error
COMM_FAULT : BOOL ; //Communication fault
MDL_STOP : BOOL ; //Module is stopped
WTCH_DOG_FLT : BOOL ; //Watch dog timer stopped module
INT_PS_FLT : BOOL ; //Internal power supply fault
PRIM_BATT_FLT : BOOL ; //Primary battery is in fault
BCKUP_BATT_FLT : BOOL ; //Backup battery is in fault
RESERVED_2 : BOOL ; //Reserved for system
RACK_FLT : BOOL ; //Rack fault, only for bus interface module
PROC_FLT : BOOL ; //Processor fault
EPROM_FLT : BOOL ; //EPROM fault
RAM_FLT : BOOL ; //RAM fault
ADU_FLT : BOOL ; //ADU fault
FUSE_FLT : BOOL ; //Fuse fault
HW_INTR_FLT : BOOL ; //Hardware interupt input in fault
RESERVED_3 : BOOL ; //Reserved for system
CH_TYPE : BYTE ; //Channel type
LGTH_DIA : BYTE ; //Diagnostics data length per channel
CH_NO : BYTE ; //Channel number
GRP_ERR0 : BOOL ; //Group error channel 0
GRP_ERR1 : BOOL ; //Group error channel 1
GRP_ERR2 : BOOL ; //Group error channel 2
bit7_3 : BOOL ;
bit7_4 : BOOL ;
bit7_5 : BOOL ;
bit7_6 : BOOL ;
bit7_7 : BOOL ;
CH0_INTF : BOOL ; //Configuration or parameterization error (internal channel malfunction)
CH0_EXTF : BOOL ; //Encoder malfunction (external channel malfunction)
bit8_2 : BOOL ;
bit8_3 : BOOL ;
bit8_4 : BOOL ;
bit8_5 : BOOL ;
bit8_6 : BOOL ;
bit8_7 : BOOL ;
CH1_INTF : BOOL ; //Configuration or parameterization error (internal channel malfunction)
CH1_EXTF : BOOL ; //Encoder malfunction (external channel malfunction)
bit9_2 : BOOL ;
bit9_3 : BOOL ;
bit9_4 : BOOL ;
bit9_5 : BOOL ;
bit9_6 : BOOL ;
bit9_7 : BOOL ;
CH2_INTF : BOOL ; //Configuration or parameterization error (internal channel malfunction)
CH2_EXTF : BOOL ; //Encoder malfunction (external channel malfunction)
bit10_2 : BOOL ;
bit10_3 : BOOL ;
bit10_4 : BOOL ;
bit10_5 : BOOL ;
bit10_6 : BOOL ;
bit10_7 : BOOL ;
byte11 : BYTE ;
dword12 : DWORD ;
END_STRUCT ;
DiagStatus_dw AT DiagStatus : DWORD;
DiagStatus_b AT DiagStatus : ARRAY[0..127] OF BOOL;
GeberAlt_DI : DINT ; //internal use
Addr_ActPos_DI : INT ; //internal use
v_Faktor : REAL ; //internal use
MaxZahl : DINT ; //internal use
MaxZahlHalb : DINT ; //internal use
PEStatus : DWORD ; //internal use
PEStatus_b AT PEStatus : ARRAY[0..31] OF BOOL;
Richtungsanpassung : INT := 1; //internal use
PositionOffset : DINT ; //internal use
BGAdr : WORD ; //internal use
BGTyp : BYTE ; //internal use
DiagFehler : BOOL ; //internal use
EncErr_alt : BOOL ; //internal use
Taktsynchron : BOOL ; //internal use
sRDREC : SFB52;
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 ; //Rckgabewert 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.InputChannelNo*4+Axis.InputModuleInAddr;
Addr_ActPos_DI:=Addr_InChannel;
v_Faktor:=Axis.aufl/Axis.Sample_T;
PEStatus:=DW#16#0;
DiagFehler:=false;
ReadDiagErr:=true;
Axis.EinInitLaeuft:=true;
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;
IF Axis.InputModuleInAddr<=Axis.InputModuleOutAddr THEN
BGAdr:=INT_TO_WORD(Axis.InputModuleInAddr);
BGTyp:=B#16#54;
ELSE
BGAdr:=INT_TO_WORD(Axis.InputModuleOutAddr);
BGAdr:=BGAdr OR W#16#8000;
BGTyp:=B#16#55;
END_IF;

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
PEStatus:=DW#16#0;
ReadDiagErr:=false;
ELSE
IF Taktsynchron THEN
PEStatus:=ID[Addr_ActPos_DI];
ELSE
PEStatus:=PID[Addr_ActPos_DI];
END_IF;
 
 
Axis.EncoderValue:=DWORD_TO_DINT(PEStatus AND DW#16#1FFFFFF);

IF ReadDiagErr THEN
sRDREC (REQ := true
,ID :=BGAdr
,INDEX := 1
,MLEN := 16
,RECORD := DiagStatus
);
ReadDiagErr:=sRDREC.BUSY;
JobErr:=DWORD_TO_INT(SHR(IN:=sRDREC.STATUS,N:=8));
IF (INT_TO_DINT(JobErr)>=32930 AND
INT_TO_DINT(JobErr)<=32932) OR
(INT_TO_DINT(JobErr)>=32960 AND
INT_TO_DINT(JobErr)<=32975)
THEN
ReadDiagErr:=true;
END_IF;
IF NOT ReadDiagErr THEN
IF JobErr>=0 THEN
IF DiagStatus.MDL_DEFECT THEN
IF (DW#16#F000FFFF AND DiagStatus_dw) <> 0 THEN
DiagFehler:=true;
ELSE
DiagFehler:=DiagStatus_b[INT_TO_DINT(Axis.InputChannelNo)+56];
END_IF;
ELSE
DiagFehler:=false;
END_IF;
ELSE
DiagFehler:=true;
END_IF;
END_IF;
END_IF;
END_IF;
 
Geber_DI:=Axis.EncoderValue;
Freeze:=PEStatus_b[7];
 
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 DiagFehler 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
IF NOT ReadDiagErr THEN
Init:=false;
Axis.EinInitLaeuft:=false;
END_IF;
Axis.InternalNomPosition:=Axis.InternalActPos;
Axis.LastNomPosition:=Axis.InternalActPos;
Axis.VLastNomPosition:=Axis.InternalActPos;
END_IF;
END_FUNCTION_BLOCK



emc

Block checksum .





FB26 EncoderET200S1Count - Input driver for ET200S 1Count

Name: Enc1CTR
Symbolic Name: EncoderET200S1Count
Symbol Comment: Input driver for ET200S 1Count
Family: EMC2
Version: 2.1
Author: EMC
Last modified: 03/12/2009
Use: UDT1,SFC6
Size in work memory: 2456
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCLCOMP K05.03.05.00_01.03.00.01 release

FUNCTION_BLOCK FB126
TITLE ='Input driver for ET200S 1Count'
AUTHOR : EMC
FAMILY : EMC2
NAME : Enc1CTR
VERSION : '2.1'
 
 
VAR_INPUT
EncErr : BOOL ; //1=Encoder error
DOut_1 : BOOL ; //Digital output 1
DOut_2 : BOOL ; //Digital output 2
END_VAR
VAR_OUTPUT
DIn : BOOL ; //Digital input
END_VAR
VAR_IN_OUT
Axis : UDT1 ;
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
Status : STRUCT
STS_LOAD : BOOL ; //1=Load function active
ERR_LOAD : BOOL ; //1=Load function error
RES_STS_A : BOOL ; //1=Resetting of status bit active
ERR_ENCODER : BOOL ; //(1COUNT5V only) 1=Short circuit / wire break of the sensor-signal
ERR_DO2 : BOOL ; //(1COUNT5V only) 1=DO2: Short circuit / wire break / overtemperature
ERR_PARA : BOOL ; //1=Parameter assignment error
ERR_DO1 : BOOL ; //DO1: 1=Short circuit / wire break / overtemperature
ERR_24V : BOOL ; //1=Short circuit of the sensor supply
STS_GATE : BOOL ; //Internal gate status
STS_DI : BOOL ; //DI status
bit18 : BOOL ;
STS_DO1 : BOOL ; //DO1 status
STS_DO2 : BOOL ; //DO2 status
bit21 : BOOL ;
STS_C_UP : BOOL ; //Up direction status
STS_C_DN : BOOL ; //Down direction status
STS_SYN : BOOL ; //Synchronization status
bit09 : BOOL ;
bit10 : BOOL ;
STS_CMP1 : BOOL ; //Comparator status 1
STS_CMP2 : BOOL ; //Comparator status 2
STS_OFLW : BOOL ; //Upper count limit
STS_UFLW : BOOL ; //Lower count limit
STS_ND : BOOL ; //Zero-crossing in the count range in counting without a main counting direction
byte3 : BYTE ;
END_STRUCT ;
Status_dw AT Status : DWORD;
GeberAlt_DI : DINT ; //internal use
Addr_ActPos_DI : INT ; //internal use
Addr_Status : INT ; //internal use
Addr_LoadVal_DI : INT ; //internal use
Addr_Steuer : INT ; //internal use
Richtungsanpassung : INT := 1; //internal use
v_Faktor : REAL ; //internal use
MaxZahl : DINT ; //internal use
MaxZahlHalb : DINT ; //internal use
PASteuer : WORD ; //internal use #AJ
PASteuer_b AT PASteuer : ARRAY[0..15] OF BOOL;
RefPointInt : REAL ; //internal use
LoadVal_DW : DWORD ; //internal use
EncoderZustand : INT ; //internal use
RefZustand : INT ; //internal use
aufl : REAL ; //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
Addr_OutChannel : 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 ;

SFCErr : INT ; //Rckgabewert 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 ;
Sim : BOOL ; //internal use
END_VAR
BEGIN
 
Sim:=Axis.Sim;
 
 
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_Status:=Addr_InChannel+4;
Addr_ActPos_DI:=Addr_InChannel;
Addr_OutChannel:=Axis.InputModuleOutAddr;
Addr_LoadVal_DI:=Addr_OutChannel;
Addr_Steuer:=Addr_LoadVal_DI+4;
Status_dw:=DW#16#0;
PASteuer:=W#16#0;
Axis.EinInitLaeuft:=true;
Axis.EinQuittungLaeuft:=false;
Axis.EintreiberFahrbereit:=false;
EncErr_alt:=false;
Axis.DoneRef:=false;
Axis.ExecRef:=0;
RefPointInt:=Axis.RefPoint;
aufl:=Axis.aufl;
v_Faktor:=aufl/Axis.Sample_T;
Richtungsanpassung:=Axis.PolarityEncoder;
Axis.EncoderType:=false;
Axis.Sync:=false;
EncoderZustand:=0;

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;
LoadVal_DW:=DW#16#0;

IF NOT (Sim OR EncErr) THEN
IF Taktsynchron THEN
QD[Addr_LoadVal_DI]:=LoadVal_DW;
ELSE
PQD[Addr_LoadVal_DI]:=LoadVal_DW;
END_IF;
END_IF;
RefZustand:=0;
END_IF;

IF Sim OR EncErr THEN
PASteuer:=W#16#0;
EncoderZustand:=4;
Axis.EinInitLaeuft:=false;
ELSE
IF Taktsynchron THEN
Status_dw:=ID[Addr_Status];
Axis.EncoderValue:=DWORD_TO_DINT(ID[Addr_ActPos_DI]);
ELSE
Status_dw:=PID[Addr_Status];
Axis.EncoderValue:=DWORD_TO_DINT(PID[Addr_ActPos_DI]);
END_IF;
END_IF;
 
IF EncErr THEN
Status_dw:=DW#16#0;
END_IF;
 
Geber_DI:=Axis.EncoderValue;
DIn:=Status.STS_DI;
IF EncoderZustand<>4 OR Init THEN
GeberAlt_DI:=Geber_DI;
Axis.LageIstwert_DI:=Geber_DI;
Axis.PositionOffset:=Geber_DI;
Axis.InternalNomPosition:=Axis.InternalActPos;
Axis.LastNomPosition:=Axis.InternalActPos;
Axis.VLastNomPosition:=Axis.InternalActPos;
Init:=false;
END_IF;
 
IF EncoderZustand=4 THEN
IF EncErr THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
EncErr_alt:=true;
Axis.Sync:=false;
END_IF;
IF Status.ERR_ENCODER OR
Status.ERR_DO2 OR
Status.ERR_PARA OR
Status.ERR_DO1 OR
Status.ERR_24V
THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
Axis.Sync:=false;
IF NOT PASteuer_b[7] THEN
PASteuer_b[7]:=Axis.ErrorAck;
Axis.EinQuittungLaeuft:=Axis.ErrorAck;
END_IF;
ELSE
PASteuer_b[7]:=false;
Axis.EinQuittungLaeuft:=false;
END_IF;
 
IF NOT EncErr THEN
IF Axis.ExecRef=2 THEN
IF RefZustand=0 THEN
IF Status.STS_DI THEN
LoadVal_DW:=DINT_TO_DWORD(Geber_DI);
RefZustand:=5;
ELSE
IF Sim THEN
RefZustand:=2;
ELSE
PASteuer_b[2]:=true;
RefZustand:=1;
END_IF;
END_IF;
ELSIF RefZustand=1 THEN
IF Status.RES_STS_A THEN
PASteuer_b[2]:=false;
RefZustand:=2;
END_IF;
ELSIF RefZustand=2 THEN
LoadVal_DW:=DINT_TO_DWORD(Geber_DI) XOR DW#16#50000000;
LoadVal_DW:=LoadVal_DW AND DW#16#F0000000;
LoadVal_DW:=LoadVal_DW OR DW#16#2000000;
IF Sim THEN
RefZustand:=5;
ELSE
IF Taktsynchron THEN
QD[Addr_LoadVal_DI]:=LoadVal_DW;
ELSE
PQD[Addr_LoadVal_DI]:=LoadVal_DW;
END_IF;
PASteuer_b[9]:=true;
RefZustand:=3;
END_IF;
ELSIF RefZustand=3 THEN
IF Status.STS_LOAD THEN
PASteuer_b[9]:=false;
RefZustand:=4;
END_IF;

ELSIF RefZustand=4 THEN
IF NOT Status.STS_LOAD THEN
PASteuer_b[1]:=true;
RefZustand:=5;
END_IF;
ELSIF RefZustand=5 THEN
IF (LoadVal_DW AND DW#16#F0000000) = (DINT_TO_DWORD(Geber_DI) AND DW#16#F0000000) THEN
Axis.DoneRef:=true;
Axis.InternalPosOffset:=Axis.InternalPosOffset-DINT_TO_REAL(Axis.LageIstwert_DI-Axis.PositionOffset)*aufl;
Axis.InternalPosOffset:=Axis.InternalPosOffset-RefPointInt+Axis.RefPoint;
RefPointInt:=Axis.RefPoint;
Axis.PositionOffset:=DWORD_TO_DINT(LoadVal_DW);
Axis.LageIstwert_DI:=DWORD_TO_DINT(LoadVal_DW);
GeberAlt_DI:=DWORD_TO_DINT(LoadVal_DW);
PASteuer_b[1]:=false;
RefZustand:=0;
ELSE
Axis.EintreiberFahrbereit:=true;
END_IF;
END_IF;
ELSIF Axis.ExecRef=1 THEN
RefPointInt:=Axis.RefPoint;
Axis.InternalPosOffset:=RefPointInt;
Axis.InternalNomPosition:=0.0;
Axis.LastNomPosition:=0.0;
Axis.LageIstwert_DI:=Geber_DI;
Axis.PositionOffset:=Geber_DI;
Axis.DoneRef:=true;
ELSE
Axis.DoneRef:=false;
Axis.EintreiberFahrbereit:=false;
PASteuer_b[1]:=false;
RefZustand:=0;
END_IF;
END_IF;
ELSIF EncoderZustand=0 THEN
IF NOT Status.STS_LOAD THEN
PASteuer_b[8]:=true;
EncoderZustand:=1;
END_IF;
ELSIF EncoderZustand=1 THEN
IF Status.STS_LOAD THEN
PASteuer_b[8]:=false;
EncoderZustand:=2;
END_IF;
ELSIF EncoderZustand=2 THEN
IF NOT Status.STS_LOAD THEN
PASteuer_b[0]:=true;
EncoderZustand:=3;
END_IF;
ELSIF EncoderZustand=3 THEN
IF Status.STS_GATE THEN
Axis.EinInitLaeuft:=false;
EncoderZustand:=4;
PASteuer_b[4]:=true;
PASteuer_b[6]:=true;
END_IF;
END_IF;
 
LageDiff_DI:=(Geber_DI-GeberAlt_DI)*INT_TO_DINT(Richtungsanpassung);
Axis.LageIstwert_DI:=LageDiff_DI+Axis.LageIstwert_DI;
Axis.ActPosition:=DINT_TO_REAL(Axis.LageIstwert_DI-Axis.PositionOffset)*
aufl+RefPointInt;
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;

IF NOT (Sim OR EncErr) THEN
PASteuer_b[3]:=DOut_1;
PASteuer_b[5]:=DOut_2;
IF Taktsynchron THEN
QW[Addr_Steuer]:=PASteuer;
ELSE
PQW[Addr_Steuer]:=PASteuer;
END_IF;
END_IF;
 
END_FUNCTION_BLOCK



emc

Block checksum .





FB27 EncoderFM350 - Input driver for FM350

Name: EncFM350
Symbolic Name: EncoderFM350
Symbol Comment: Input driver for FM350
Family: EMC2
Version: 2.1
Author: EMC
Last modified: 03/12/2009
Use: UDT1,SFB52,SFC6
Size in work memory: 2634
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCLCOMP K05.03.05.00_01.03.00.01 release

FUNCTION_BLOCK FB127
TITLE ='Input driver for FM350'
AUTHOR : EMC
FAMILY : EMC2
NAME : EncFM350
VERSION : '2.1'
 
 
VAR_INPUT
EncErr : BOOL ; //1=Encoder error
END_VAR
VAR_OUTPUT
DI_0 : BOOL ; //Digital input 0
DI_1 : BOOL ; //Digital input 1
DI_2 : BOOL ; //Digital input 2
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
ReadDiagErr : BOOL := TRUE; //1=Read data record 1 (DS1)
END_VAR
VAR
Koord : STRUCT
DA_ERR_W : WORD ; //Data error word
OT_ERR_B : BYTE ; //Operating error byte
bit0 : BOOL ;
STS_TFB : BOOL ; //Status test free
DIAG : BOOL ; //internal use
OT_ERR : BOOL ; //internal use
DATA_ERR : BOOL ; //Data error bit
FM_NEUSTQ : BOOL ; //internal use
FM_NEUST : BOOL ; //internal use
PARA : BOOL ; //1=Module configured
END_STRUCT ;
Koord_dw AT Koord : DWORD ;
Status : STRUCT
byte0 : BYTE ;
STS_RUN : BOOL ; //Status counter working
STS_DIR : BOOL ; //Status count direction
STS_ZERO : BOOL ; //Status zero pass
STS_OFLW : BOOL ; //Status overflow
STS_UFLW : BOOL ; //Status underflow
STS_SYNC : BOOL ; //Status counter synchronized
STS_GATE : BOOL ; //Status internal gate
STS_SW_G : BOOL ; //Status software gate
STS_SET : BOOL ; //Status software gate
bit9 : BOOL ;
STS_STA : BOOL ; //Status digital input START
STS_STP : BOOL ; //Status digital input STOP
STS_CMP1 : BOOL ; //Status output comparison value 1
STS_CMP2 : BOOL ; //Status output comparison value 2
bit14 : BOOL ;
bit15 : BOOL ;
LoadInCounter : BOOL ; //internal use
LoadInLoad : BOOL ; //internal use
Comp1Set : BOOL ; //internal use
Comp2Set : BOOL ; //internal use
NoSync : BOOL ; //internal use
NoZero : BOOL ; //internal use
bit6 : BOOL ;
bit7 : BOOL ;
END_STRUCT ;
Status_dw AT Status : DWORD ;
JobErr : INT ; //Error reported while reading DS1
DiagStatus : STRUCT
MDL_DEFECT : BOOL ; //Module defective
INT_FAULT : BOOL ; //Internal fault
EXT_FAULT : BOOL ; //External fault
PNT_INFO : BOOL ; //Point information
EXT_VOLTAGE : BOOL ; //External voltage low
FLD_CONNCTR : BOOL ; //Field wiring connector missing
NO_CONFIG : BOOL ; //Module has no configuration data
CONFIG_ERR : BOOL ; //Module has configuration error
MDL_TYPE : BYTE ; //Type of module
SUB_NDL_ERR : BOOL ; //Sub-Module is missing or has error
COMM_FAULT : BOOL ; //Communication fault
MDL_STOP : BOOL ; //Module is stopped
WTCH_DOG_FLT : BOOL ; //Watch dog timer stopped module
INT_PS_FLT : BOOL ; //Internal power supply fault
PRIM_BATT_FLT : BOOL ; //Primary battery is in fault
BCKUP_BATT_FLT : BOOL ; //Backup battery is in fault
RESERVED_2 : BOOL ; //Reserved for system
RACK_FLT : BOOL ; //Rack fault, only for bus interface module
PROC_FLT : BOOL ; //Processor fault
EPROM_FLT : BOOL ; //EPROM fault
RAM_FLT : BOOL ; //RAM fault
ADU_FLT : BOOL ; //ADU fault
FUSE_FLT : BOOL ; //Fuse fault
HW_INTR_FLT : BOOL ; //Hardware interupt input in fault
RESERVED_3 : BOOL ; //Reserved for system
CH_TYPE : BYTE ; //Channel type
LGTH_DIA : BYTE ; //Diagnostics data length per channel
CH_NO : BYTE ; //Channel number
GRP_ERR1 : BOOL ; //Group error channel 1
bit7_1 : BOOL ;
bit7_2 : BOOL ;
bit7_3 : BOOL ;
bit7_4 : BOOL ;
bit7_5 : BOOL ;
bit7_6 : BOOL ;
bit7_7 : BOOL ;
CH1_SIGA : BOOL ; //Error signal A
CH1_SIGB : BOOL ; //Error signal B
CH1_SIGZ : BOOL ; //Error signal zero
CH1_BETW : BOOL ; //Error between channels
CH1_5V2 : BOOL ; //Error in 5.2 V encoder supply
byte9 : BYTE ;
byte10 : BYTE ;
byte11 : BYTE ;
dword12 : DWORD ;
END_STRUCT ;
DiagStatus_dw AT DiagStatus : DWORD;
GeberAlt_DI : DINT ; //internal use
Addr_Status : INT ; //internal use
Addr_Koord : INT ; //internal use
Addr_ActPos_DI : INT ; //internal use
Addr_LoadVal_DI : INT ; //internal use
Addr_Steuer : INT ; //internal use
v_Faktor : REAL ; //internal use
PASteuer : DWORD ; //internal use
PASteuer_b AT PASteuer : ARRAY[0..31] OF BOOL;
RefPointInt : REAL ; //internal use
Richtungsanpassung : INT := 1; //internal use
LoadVal_DW : DWORD ; //internal use
RefZustand : INT ; //internal use
DiagFehler : BOOL ; //internal use
EncErr_alt : BOOL ; //internal use
Diag_alt : BOOL ; //internal use
Taktsynchron : BOOL ; //internal use
sRDREC : SFB52;
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 ;
 
SFCErr : INT ; //Rckgabewert 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 ;
Sim : BOOL ; //internal use
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_Status:=Addr_InChannel+12;
Addr_ActPos_DI:=Addr_InChannel+4;
Addr_Koord:=Addr_InChannel+8;
Addr_LoadVal_DI:=Addr_InChannel;
Addr_Steuer:=Addr_LoadVal_DI+12;
v_Faktor:=Axis.aufl/Axis.Sample_T;
Koord_dw:=DW#16#0;
Status_dw:=DW#16#0;
PASteuer:=DW#16#0;
DiagFehler:=false;
ReadDiagErr:=true;
Axis.EinInitLaeuft:=true;
Axis.EintreiberFahrbereit:=false;
EncErr_alt:=false;
Diag_alt:=false;
Axis.DoneRef:=false;
RefPointInt:=Axis.RefPoint;
Axis.ExecRef:=0;
Axis.Sync:=false;
Axis.EncoderType:=false;
Richtungsanpassung:=Axis.PolarityEncoder;
RefZustand:=0;

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;
 
Sim:=Axis.Sim;
 
IF Sim OR EncErr THEN
Koord_dw:=DW#16#80;
PASteuer:=DW#16#0;
Axis.EinInitLaeuft:=false;
ReadDiagErr:=false;
ELSE
IF Taktsynchron THEN
Status_dw:=ID[Addr_Status];
Koord_dw:=ID[Addr_Koord];
ELSE
Status_dw:=PID[Addr_Status];
Koord_dw:=PID[Addr_Koord];
END_IF;

IF (Koord_dw AND DW#16#60) = DW#16#40 THEN
PASteuer:=DW#16#40000000;
Init:=true;
Axis.EinInitLaeuft:=true;
Axis.Sync:=false;
Axis.EintreiberFahrbereit:=false;
ELSE
PASteuer_b[6]:=false;
IF Taktsynchron THEN
Axis.EncoderValue:=DWORD_TO_DINT(ID[Addr_ActPos_DI]);
ELSE
Axis.EncoderValue:=DWORD_TO_DINT(PID[Addr_ActPos_DI]);
END_IF;

IF Koord.DIAG XOR Diag_alt THEN
ReadDiagErr:=true;
Diag_alt:=Koord.DIAG;
END_IF;
 
IF ReadDiagErr THEN
sRDREC(REQ := true
,ID := INT_TO_DWORD(Axis.InputModuleInAddr)
,INDEX := 1
,MLEN := 16
,RECORD := DiagStatus
);
ReadDiagErr:=sRDREC.BUSY;
JobErr:=DWORD_TO_INT(SHR(IN:=sRDREC.STATUS,N:=8));
IF (JobErr>=32930 AND JobErr<=32932) OR (JobErr>=32960 AND JobErr<=32975)
THEN
ReadDiagErr:=true;
END_IF;

IF NOT ReadDiagErr THEN
IF JobErr>=0 THEN
IF DiagStatus.MDL_DEFECT THEN
IF (DW#16#F000FFFF AND DiagStatus_dw)<>0 THEN
DiagFehler:=true;
ELSE
DiagFehler:=DiagStatus.GRP_ERR1;
END_IF;
ELSE
DiagFehler:=false;
END_IF;
ELSE
DiagFehler:=true;
END_IF;
END_IF;
END_IF;
END_IF;
END_IF;
 
Geber_DI:=Axis.EncoderValue;
DI_0:=Status.STS_STA;
DI_1:=Status.STS_STP;
DI_2:=Status.STS_SET;
 
IF EncErr THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
EncErr_alt:=true;
Axis.Sync:=false;
ELSE
IF Init THEN
GeberAlt_DI:=Geber_DI;
Axis.LageIstwert_DI:=Geber_DI;
Axis.PositionOffset:=Geber_DI;
END_IF;
 
IF (Koord.OT_ERR OR Koord.DATA_ERR) OR
(NOT Koord.PARA AND Koord.FM_NEUSTQ) OR
DiagFehler
THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
Axis.Sync:=false;
ELSE
IF Axis.ExecRef=2 THEN
IF RefZustand=0 THEN
LoadVal_DW:=DINT_TO_DWORD(Geber_DI) XOR DW#16#50000000;
LoadVal_DW:=LoadVal_DW AND DW#16#F0000000;
LoadVal_DW:=LoadVal_DW OR DW#16#2000000;
IF Status.STS_SET THEN
RefZustand:=5;
LoadVal_DW:=DINT_TO_DWORD(Geber_DI);
ELSE
IF Sim THEN
RefZustand:=5;
ELSE
IF Taktsynchron THEN
QD[Addr_LoadVal_DI]:=LoadVal_DW;
ELSE
PQD[Addr_LoadVal_DI]:=LoadVal_DW;
END_IF;
PASteuer_b[25]:=true;
RefZustand:=3;
END_IF;
END_IF;
ELSIF RefZustand=3 THEN
IF Status.LoadInLoad THEN
PASteuer_b[25]:=false;
RefZustand:=4;
END_IF;
ELSIF RefZustand=4 THEN
IF Taktsynchron THEN
IF ID[Addr_LoadVal_DI]=LoadVal_DW THEN
RefZustand:=5;
END_IF;
ELSE
IF PID[Addr_LoadVal_DI]=LoadVal_DW THEN
RefZustand:=5;
END_IF;
END_IF;
ELSIF RefZustand=5 THEN
IF (LoadVal_DW AND DW#16#F0000000)=(DINT_TO_DWORD(Geber_DI) AND DW#16#F0000000) THEN
Axis.DoneRef:=true;
Axis.InternalPosOffset:=(Axis.InternalPosOffset-DINT_TO_REAL(Axis.LageIstwert_DI-Axis.PositionOffset)*Axis.aufl)
-RefPointInt+Axis.RefPoint;
RefPointInt:=Axis.RefPoint;
Axis.PositionOffset:=DWORD_TO_DINT(LoadVal_DW);
Axis.LageIstwert_DI:=DWORD_TO_DINT(LoadVal_DW);
GeberAlt_DI:=DWORD_TO_DINT(LoadVal_DW);

PASteuer:=PASteuer AND DW#16#FFFCFFFF;
RefZustand:=0;
ELSE
PASteuer:=PASteuer OR DW#16#30000;
Axis.EintreiberFahrbereit:=true;
END_IF;
END_IF;
ELSIF Axis.ExecRef=1 THEN
RefPointInt:=Axis.RefPoint;
Axis.InternalPosOffset:=RefPointInt;
Axis.InternalNomPosition:=0.0;
Axis.LastNomPosition:=0.0;
Axis.LageIstwert_DI:=Geber_DI;
Axis.PositionOffset:=Geber_DI;
Axis.DoneRef:=true;
ELSE
Axis.DoneRef:=false;
Axis.EintreiberFahrbereit:=false;
PASteuer:=PASteuer AND DW#16#FFFCFFFF;
END_IF;

LageDiff_DI:=(Geber_DI-GeberAlt_DI)*INT_TO_DINT(Richtungsanpassung);
Axis.LageIstwert_DI:=LageDiff_DI+Axis.LageIstwert_DI;
Axis.ActPosition:=DINT_TO_REAL(Axis.LageIstwert_DI-Axis.PositionOffset)
*Axis.aufl+RefPointInt;
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
Axis.InternalNomPosition:=Axis.InternalActPos;
Axis.LastNomPosition:=Axis.InternalActPos;
Axis.VLastNomPosition:=Axis.InternalActPos;

IF PASteuer=DW#16#0 THEN
IF NOT ReadDiagErr THEN
Init:=false;
Axis.EinInitLaeuft:=false;
END_IF;
END_IF;
END_IF;
 
IF NOT (Sim OR EncErr) THEN
IF Taktsynchron THEN
QD[Addr_Steuer]:=PASteuer;
ELSE
PQD[Addr_Steuer]:=PASteuer;
END_IF;
END_IF;
END_FUNCTION_BLOCK



emc

Block checksum .





FB28 EncoderCPU314C - Input driver for CPU 314C counter

Name: Enc314C
Symbolic Name: EncoderCPU314C
Symbol Comment: Input driver for CPU 314C counter
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 12/11/2002
Use: UDT1,SFB47,SFC58
Size in work memory: 1416
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.15)

FUNCTION_BLOCK FB128
TITLE ='Input driver for CPU 314C counter'
AUTHOR : EMC
FAMILY : EMC2
NAME : Enc314C
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
GeberAlt_DI : DINT ; //internal use
Addr_ActPos_DI : INT ; //internal use
v_Faktor : REAL ; //internal use
MaxZahl : DINT ; //internal use
MaxZahlHalb : DINT ; //internal use
Steuer_W : WORD ; //internal use
RefPointInt : REAL ; //internal use
DSNummer : BYTE ; //internal use
SFC_err : INT ; //internal use
SFB_Count : SFB47;
Richtungsanpassung : INT := 1; //internal use
aufl : REAL ; //internal use
AxisLimitMin : REAL ; //internal use
AxisLimitMax : REAL ; //internal use
PositionOffset : DINT ; //internal use
EncErr_alt : BOOL ; //internal use
END_VAR
VAR_TEMP
Geber_DI : DINT ; //internal use
LageDiff_DI : DINT ; //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
DStorauf : STRUCT
TF_Kennung : WORD ;
Job_ID : INT ;
CtrlVal : DWORD ;
END_STRUCT ;
Sim : BOOL ; //internal use
SFC_busy : BOOL ; //internal use
END_VAR
BEGIN
 
Sim:=Axis.Sim;
 
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_ActPos_DI:=Axis.InputChannelNo*4+Axis.InputModuleInAddr;
DSNummer:=int_to_byte(Axis.InputChannelNo*2+3);
Axis.EinInitLaeuft:=true;
Axis.EintreiberFahrbereit:=false;
EncErr_alt:=false;
Axis.DoneRef:=false;
Axis.ExecRef:=0;
RefPointInt:=Axis.RefPoint;
aufl:=Axis.aufl;
v_Faktor:=aufl/Axis.Sample_T;
Richtungsanpassung:=Axis.PolarityEncoder;
AxisLimitMin:=Axis.AxisLimitMin;
AxisLimitMax:=Axis.AxisLimitMax;
Axis.EncoderType:=false;
Axis.Sync:=false;

SFB_Count.LADDR:=int_to_word(Axis.InputModuleInAddr);
SFB_Count.CHANNEL:=Axis.InputChannelNo;
SFB_Count.SW_GATE:=true;
SFB_Count.JOB_REQ:=false;
SFC_busy:=false;

IF NOT Sim THEN
DStorauf.TF_Kennung:=W#16#21;
DStorauf.Job_ID:=32;
DStorauf.CtrlVal:=DW#16#1;


SFC_err:=SFC58(REQ := true
,IOID := B#16#54
,LADDR := INT_TO_WORD(Axis.InputModuleInAddr)
,RECNUM := DSNummer
,RECORD := DStorauf
,BUSY := SFC_busy
);
END_IF;
END_IF;
 
IF NOT(Sim OR EncErr) THEN
Axis.EncoderValue:=DWORD_TO_DINT(PID[Addr_ActPos_DI]);
END_IF;
 
Geber_DI:=Axis.EncoderValue;
 
IF EncErr THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
EncErr_alt:=true;
Axis.Sync:=false;
END_IF;
 
IF NOT EncErr THEN
IF Init THEN
Axis.LageIstwert_DI:=Geber_DI;
GeberAlt_DI:=Geber_DI;
PositionOffset:=Axis.PositionOffset;
Axis.PositionOffset:=GeberAlt_DI;
END_IF;
IF NOT Axis.Err.EncoderErr THEN
IF Axis.ExecRef=2 THEN
IF NOT Sim THEN
SFB_Count();
END_IF;
IF SFB_Count.STS_LTCH THEN
Axis.DoneRef:=true;
PositionOffset:=Axis.PositionOffset;
Axis.InternalPosOffset:=(Axis.InternalPosOffset-
DINT_TO_REAL(Axis.LageIstwert_DI-PositionOffset)*aufl)-
RefPointInt+Axis.RefPoint;
RefPointInt:=Axis.RefPoint;
IF NOT Axis.EintreiberFahrbereit OR Sim THEN
PositionOffset:=Geber_DI;
ELSE
PositionOffset:=SFB_Count.LATCHVAL;
END_IF;
Axis.LageIstwert_DI:=PositionOffset;
GeberAlt_DI:=PositionOffset;
Axis.PositionOffset:=PositionOffset;
ELSE
Axis.EintreiberFahrbereit:=true;
END_IF;

ELSIF Axis.ExecRef=1 THEN
RefPointInt:=Axis.RefPoint;
Axis.InternalPosOffset:=RefPointInt;
Axis.InternalNomPosition:=0.0;
Axis.LastNomPosition:=0.0;
Axis.LageIstwert_DI:=Geber_DI;
Axis.PositionOffset:=Geber_DI;
Axis.DoneRef:=true;
ELSE
Axis.DoneRef:=false;
Axis.EintreiberFahrbereit:=false;
END_IF;
LageDiff_DI:=(Geber_DI-GeberAlt_DI)*INT_TO_DINT(Richtungsanpassung);
Axis.LageIstwert_DI:=LageDiff_DI+Axis.LageIstwert_DI;
Axis.ActPosition:=DINT_TO_REAL(Axis.LageIstwert_DI-Axis.PositionOffset)*
aufl+RefPointInt;
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-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<=AxisLimitMin;
Axis.Err.SWLimitMaxExceeded:=Axis.SWLimitEnable and Axis.Sync AND
Axis.ActPosition>=AxisLimitMax;
Err_DW_str:=Axis.Err;
Axis.Error:=Err_DW<>0;
END_IF;
END_IF;
END_IF;
 
IF Init THEN
Axis.InternalNomPosition:=Axis.InternalActPos;
Axis.LastNomPosition:=Axis.InternalActPos;
Axis.VLastNomPosition:=Axis.InternalActPos;
Init:=SFC_busy;
Axis.EinInitLaeuft:=SFC_busy;
END_IF;
END_FUNCTION_BLOCK


emc

Block checksum .





FB29 EncoderUniversal - Universal input driver

Name: EncUniv
Symbolic Name: EncoderUniversal
Symbol Comment: Universal input driver
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 1416
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)

FUNCTION_BLOCK FB129
TITLE ='Universal input driver'
AUTHOR : EMC
FAMILY : EMC2
NAME : EncUniv
VERSION : '2.0'
 
 
VAR_INPUT
EncErr : BOOL ; //1=Encoder error
EncoderValue : DINT ; //Current encoder value [step]
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
GeberAlt_DI : DINT ; //internal use
v_Faktor : REAL ; //internal use
MaxZahl : DINT ; //internal use
MaxZahlHalb : DINT ; //internal use
RefPointInt : REAL ; //internal use
Richtungsanpassung : INT := 1; //internal use
PositionOffset : DINT ; //internal use
EncoderType : BOOL ; //internal use
EncErr_alt : BOOL ; //internal use
END_VAR
VAR_TEMP
Geber_DI : DINT ; //internal use
LageDiff_DI : DINT ; //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
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
Axis.EintreiberFahrbereit:=false;
EncErr_alt:=false;
Axis.DoneRef:=false;
Axis.ExecRef:=0;
RefPointInt:=Axis.RefPoint;
v_Faktor:=Axis.aufl/Axis.Sample_T;
Richtungsanpassung:=Axis.PolarityEncoder;
EncoderType:=Axis.EncoderType;
IF NOT EncoderType THEN
Axis.Sync:=false;
ELSE
MaxZahl:=Axis.AbsGeberLaenge_DI;
MaxZahlHalb:=MaxZahl/2;
END_IF;
END_IF;
 
IF EncErr THEN
Axis.Err.EncoderErr:=true;
Axis.Error:=true;
EncErr_alt:=true;
IF NOT EncoderType THEN Axis.Sync:=false; END_IF;
END_IF;

IF NOT (Axis.Sim OR EncErr) THEN
Axis.EncoderValue:=EncoderValue;
END_IF;
 
Geber_DI:=Axis.EncoderValue;
 
IF NOT EncErr THEN
IF Init THEN
Axis.LageIstwert_DI:=Geber_DI;
GeberAlt_DI:=Geber_DI;
PositionOffset:=Axis.PositionOffset;
IF EncoderType THEN
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;
ELSE
Axis.PositionOffset:=Geber_DI;
END_IF;
END_IF;

IF Axis.ExecRef=2 OR Axis.ExecRef=1 THEN
RefPointInt:=Axis.RefPoint;
Axis.InternalPosOffset:=RefPointInt;
Axis.InternalNomPosition:=0.0;
Axis.LastNomPosition:=0.0;
Axis.LageIstwert_DI:=Geber_DI;
Axis.PositionOffset:=Geber_DI;
Axis.DoneRef:=true;
Axis.EintreiberFahrbereit:=true;
ELSE
Axis.DoneRef:=false;
Axis.EintreiberFahrbereit:=false;
END_IF;

LageDiff_DI:=(Geber_DI-GeberAlt_DI)*int_to_dint(Richtungsanpassung);

IF EncoderType THEN
IF LageDiff_DI<-MaxZahlHalb THEN
LageDiff_DI:=LageDiff_DI+MaxZahl;
END_IF;
IF LageDiff_DI>MaxZahlHalb THEN
LageDiff_DI:=LageDiff_DI-MaxZahl;
END_IF;
END_IF;
Axis.LageIstwert_DI:=Axis.LageIstwert_DI+LageDiff_DI;
Axis.ActPosition:=DINT_TO_REAL(Axis.LageIstwert_DI-Axis.PositionOffset)*
Axis.aufl+RefPointInt;
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;
 
IF Init THEN
Init:=false;
Axis.InternalNomPosition:=Axis.InternalActPos;
Axis.LastNomPosition:=Axis.InternalActPos;
Axis.VLastNomPosition:=Axis.InternalActPos;
END_IF;
END_FUNCTION_BLOCK



emc

Block checksum .





FB31 OutputIM178 - Output driver for IM178-4



Name: OutIM178
Symbolic Name: OutputIM178
Symbol Comment: Output driver for IM178-4
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 436
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)

FUNCTION_BLOCK FB31 // OutputIM178
TITLE ='Output driver for IM178-4'
AUTHOR : EMC
FAMILY : EMC2
NAME : OutIM178
VERSION : '2.0'
 
 
VAR_INPUT
EnableDrive : BOOL ; //1=Enable analog output
Q0 : BOOL ; //1=Set digital output 0
Q1 : BOOL ; //1=Set digital output 1
Q2 : BOOL ; //1=Set digital output 2
OutErr : BOOL ; //1=Output module error
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
Faktor : REAL ; //internal use
Addr_OutChannel : INT ; //internal use
Addr_NomVelocity_I : INT ; //internal use
END_VAR
VAR_TEMP
OutputValue_R : REAL ; //internal use
OutputValue_B AT OutputValue_R : ARRAY[0..31] OF BOOL;
OutputValue : INT ; //internal use
Steuer_W : WORD ; //internal use
Steuer_Byte AT Steuer_W : BYTE ;
Steuer_Bool AT Steuer_W : ARRAY[0..7] OF BOOL ;
vz : BOOL ; //internal use
END_VAR
BEGIN
 
IF Init
THEN
Init:=false;
Faktor:=INT_TO_REAL(Axis.PolarityDrive)*Axis.DriveInputAtMaxVel/Axis.MaxVelocity;
Addr_OutChannel:=Axis.OutputChannelNo*4+Axis.OutputModuleOutAddr;
Addr_NomVelocity_I:=Addr_OutChannel+2;
END_IF;
 
IF OutErr
THEN
Axis.Error:=true;
Axis.Err.OutputErr:=true;
END_IF;
 
OutputValue_R:=(Axis.OutVelocity*Faktor+Axis.OffsetCompensation)*3.044000e+003;
 
IF ABS(OutputValue_R)>3.044000e+004
THEN
vz:=OutputValue_B[7]; // save sign of real number : cool trick
OutputValue_R:=3.044000e+004;
OutputValue_B[7]:=vz; // restore sign
END_IF;
 
OutputValue:=REAL_TO_INT(OutputValue_R);
 
Steuer_Byte:=BOOL_TO_BYTE(Q0);
Steuer_Bool[1]:=Q1;
Steuer_Bool[2]:=Q2;
Steuer_Bool[7]:=EnableDrive;
 
IF NOT EnableDrive
THEN
OutputValue:=0;
END_IF;

IF NOT (Axis.Sim OR OutErr)
THEN
PQB[Addr_OutChannel]:=Steuer_Byte;
PQW[Addr_NomVelocity_I]:=INT_TO_WORD(OutputValue);
END_IF;
 
END_FUNCTION_BLOCK



emc

Block checksum .





FB32 OutputSM432 - Output driver for SM432



Name: OutSM432
Symbolic Name: OutputSM432
Symbol Comment: Output driver for SM432
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 352
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB32 //OutputSM432
TITLE ='Output driver for SM432'
AUTHOR : EMC
FAMILY : EMC2
NAME : OutSM432
VERSION : '2.0'
 
VAR_INPUT
EnableDrive : BOOL ; //1=Enable analog output
OutErr : BOOL ; //1=Output module error
END_VAR
VAR_IN_OUT
Axis : UDT1;
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
Faktor : REAL ; //internal use
Addr_NomVelocity_I : INT ; //internal use
END_VAR
VAR_TEMP
OutputValue_R : REAL ; //internal use
OutputValue_B AT OutputValue_R : ARRAY[0..31] OF BOOL;
OutputValue : INT ; //internal use
vz : BOOL ; //internal use
END_VAR
BEGIN
IF Init
THEN
Init:=false;
Faktor:=INT_TO_REAL(Axis.PolarityDrive)*Axis.DriveInputAtMaxVel/Axis.MaxVelocity;
Addr_NomVelocity_I:=Axis.OutputChannelNo*2+Axis.OutputModuleOutAddr;
END_IF;
 
IF OutErr
THEN
Axis.Error:=true;
Axis.Err.OutputErr:=true;
END_IF;
OutputValue_R:=(Axis.OutVelocity*Faktor+Axis.OffsetCompensation)*2.764800e+003;
IF ABS(OutputValue_R)>2.764800e+004
THEN
vz:=OutputValue_B[7]; // save sign of real number : cool trick
OutputValue_R:=2.764800e+004;
OutputValue_B[7]:=vz; // restore sign
END_IF;
OutputValue:=REAL_TO_INT(OutputValue_R);
IF NOT EnableDrive
THEN
OutputValue:=0;
END_IF;

IF NOT (Axis.Sim OR OutErr)
THEN
PQW[Addr_NomVelocity_I]:=INT_TO_WORD(OutputValue);
END_IF;
 
END_FUNCTION_BLOCK



emc

Block checksum .



FB33 OutputET200S2AO - Output driver for ET200S 2AO



Name: OutET2AO
Symbolic Name: OutputET200S2AO
Symbol Comment: Output driver for ET200S 2AO
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 498
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB33 // OutputET200S2AO
TITLE ='Output driver for ET200S 2AO'
AUTHOR : EMC
FAMILY : EMC2
NAME : OutET2AO
VERSION : '2.0'
 
 
VAR_INPUT
EnableDrive : BOOL ; //1=Enable analog output
OutErr : BOOL ; //1=Output module error
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
Faktor : REAL ; //internal use
Addr_NomVelocity_I : INT ; //internal use
Taktsynchron : BOOL ; //internal use
END_VAR
VAR_TEMP
OutputValue_R : REAL ; //internal use
OutputValue_B AT OutputValue_R : ARRAY[0..31] OF BOOL;
OutputValue : INT ; //internal use
vz : BOOL ; //internal use
SFCErr : INT ; //Rckgabewert 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 Init
THEN
Init:=false;
Faktor:=INT_TO_REAL(Axis.PolarityDrive)*Axis.DriveInputAtMaxVel/Axis.MaxVelocity;
Addr_NomVelocity_I:=Axis.OutputChannelNo*2+Axis.OutputModuleOutAddr;
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 OutErr
THEN
Axis.Error:=true;
Axis.Err.OutputErr:=true;
END_IF;
 
OutputValue_R:=(Axis.OutVelocity*Faktor+Axis.OffsetCompensation)*2.764800e+003;
 
IF ABS(OutputValue_R)>2.764800e+004
THEN
vz:=OutputValue_B[7]; // save sign of real number : cool trick
OutputValue_R:=2.764800e+004;
OutputValue_B[7]:=vz; // restore sign
END_IF;

OutputValue:=REAL_TO_INT(OutputValue_R);
 
IF NOT EnableDrive
THEN
OutputValue:=0;
END_IF;

IF NOT (Axis.Sim OR OutErr)
THEN
IF Taktsynchron
THEN
QW[Addr_NomVelocity_I]:=INT_TO_WORD(OutputValue);
ELSE
PQW[Addr_NomVelocity_I]:=INT_TO_WORD(OutputValue);
END_IF;
END_IF;
 
END_FUNCTION_BLOCK



emc

Block checksum .



FB34 OutputCPU314C - Output driver for CPU 314C



Name: OutIM178
Symbolic Name: Out314C
Symbol Comment: Output driver for CPU 314C
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 356
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB34 // OutputCPU314C
TITLE ='Output driver for CPU 314C'
AUTHOR : EMC
FAMILY : EMC2
NAME : Out314C
VERSION : '2.0'
 
 
VAR_INPUT
EnableDrive : BOOL ; //1=Enable analog output
OutErr : BOOL ; //1=Output module error
END_VAR
VAR_IN_OUT
Axis : UDT1;
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
Faktor : REAL ; //internal use
Addr_NomVelocity_I : INT ; //internal use
END_VAR
VAR_TEMP
OutputValue_R : REAL ; //internal use
OutputValue_B AT OutputValue_R : ARRAY[0..31] OF BOOL;
OutputValue : INT ; //internal use
Addr_OutChannel : INT ; //internal use
vz : BOOL ; //internal use
END_VAR
BEGIN
 
IF Init
THEN
Init:=false;
Faktor:=INT_TO_REAL(Axis.PolarityDrive)*Axis.DriveInputAtMaxVel/Axis.MaxVelocity;
Addr_OutChannel:=Axis.OutputChannelNo*2+Axis.OutputModuleOutAddr;
Addr_NomVelocity_I:=Addr_OutChannel;
END_IF;
 
IF OutErr
THEN
Axis.Error:=true;
Axis.Err.OutputErr:=true;
END_IF;
OutputValue_R:=(Axis.OutVelocity*Faktor+Axis.OffsetCompensation)*2.764800e+003;
IF ABS(OutputValue_R)>2.764800e+004
THEN
vz:=OutputValue_B[7]; // save sign of real number : cool trick
OutputValue_R:=2.764800e+004;
OutputValue_B[7]:=vz; // restore sign
END_IF;
OutputValue:=REAL_TO_INT(OutputValue_R);
IF NOT EnableDrive
THEN
OutputValue:=0;
END_IF;

IF NOT (Axis.Sim OR OutErr)
THEN
PQW[Addr_NomVelocity_I]:=INT_TO_WORD(OutputValue);
END_IF;

END_FUNCTION_BLOCK



emc

Block checksum .



FB35 OutputSM332 - Output driver for SM332



Name: OutSM332
Symbolic Name: OutputSM332
Symbol Comment: Output driver for SM332
Family: EMC2
Version: 2.0
Author: EMC
Last modified: 11/26/2002
Use: UDT1
Size in work memory: 498
Object name: emc2
Signature: generiert vom SCL Übersetzer Version: SCL K5.1.4 (C5.1.8.13)



FUNCTION_BLOCK FB35 // OutputSM332
TITLE ='Output driver for SM332'
AUTHOR : EMC
FAMILY : EMC2
NAME : OutSM332
VERSION : '2.0'
 
 
VAR_INPUT
EnableDrive : BOOL ; //1=Enable analog output
OutErr : BOOL ; //1=Output module error
END_VAR
VAR_IN_OUT
Axis : UDT1 ; //Global axis data
Init : BOOL := TRUE; //1=Initialize block
END_VAR
VAR
Faktor : REAL ; //internal use
Addr_NomVelocity_I : INT ; //internal use
Taktsynchron : BOOL ; //internal use
END_VAR
VAR_TEMP
OutputValue_R : REAL ; //internal use
OutputValue_B AT OutputValue_R : ARRAY[0..31] OF BOOL;
OutputValue : INT ; //internal use
vz : BOOL ; //internal use
SFCErr : INT ; //Rckgabewert 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 :