EncoderCPU314C.scl

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