FUNCTION_BLOCK "FB_EncoderTR"
{ S7_Optimized_Access := 'TRUE' }
VERSION : 0.1
   VAR_INPUT 
      nInPosAct : DInt;
      xInPresetAdjQuit : Bool;
      xInPresetAdjSet : Bool;
      nInPresetAdjSetValue : DInt;   // Abweichung in mm
      nInPitch : DInt := 254;
      nInEncoderMax : DInt;
      nInHooksSorterMax : DInt;
      stInSensorReference { ExternalAccessible := 'False'; ExternalVisible := 'False'; ExternalWritable := 'False'} : "UDT_Sensor";
   END_VAR

   VAR_OUTPUT 
      xOutPresetAdjSet : Bool;
      nOutPresetAdjSetValue : DInt;
      nOutPosAct : DInt;
      nOutPosActDif : DInt;
      bOutReferenced : Bool;
      nOutPosPitch : DInt;
      nOutPosPitchDif : DInt;
      nOutPosPitchPart : DInt;   // Skaliert [0-100]%
   END_VAR

   VAR 
      arIncremente : Array[0..40] of DInt;
      nPos : Int;
      xSensorFlag : Bool;
      nStateAdjustment : Int;
      nPosActDif { ExternalAccessible := 'False'; ExternalVisible := 'False'; ExternalWritable := 'False'} : DInt;
      nPosPitchDif : DInt;
      nPosAct : DInt;
      nPosPitchAct : DInt;
      nTest : Array[0..40] of DInt;
      nPosPitchPart : DInt;
   END_VAR

   VAR CONSTANT 
      cReferenceValue : DInt;
      cEncoderScaling : DInt := 100;   // Scalierung des Encoders pro Teilung
   END_VAR


BEGIN
	REGION GeberParameter
	(*
	Einstellungen für Pinband ILS Testanlage
	Anzahl Taschen: 54 Stück (länge des Bandes)
	Durchmesser: 278mm
	Kettenteilung (pro Tasche): 250mm
	Eingabe im Geber:
	Gesamtmesslänge = 13500 (Bandteilung * Anzahl Taschen)
	Umdrehungen Zähler = 154575 
	Umdrehungen Nenner = 10000 
	-> PosAct = Position in mm
	
	Einstellungen für Fingerband ILS Testanlage
	Anzahl Taschen: 26 Stück (länge der Kette)
	Kettenrad: 17 Zähne
	Kettenteilung (pro Tasche): 254mm
	Kettenglieder pro Tasche: 32 Stück
	Eingabe im Geber:
	Gesamtmesslänge = 6604 (Kettenteilung * Anzahl Taschen)
	Umdrehungen Zähler = 244706 (Anzahl Taschen * Kettenglieder pro Tasche)
	Umdrehungen Nenner = 10000 (Kettenrad)
	-> PosAct = Position in mm
	*)
	END_REGION
	
	REGION Adjustment
	    CASE #nStateAdjustment OF
	        0:      // Undefined
	            #bOutReferenced := 0;
	            #nStateAdjustment := 10;
	        10:     // Wait for Reference Sensor 1st Time
	            IF #stInSensorReference.xDirectEdgePos THEN
	                #nStateAdjustment := 15;
	            END_IF;
	        15:     // Wait for Reference Sensor 2nd Time 
	            IF #stInSensorReference.xDirectEdgePos THEN
	                #nStateAdjustment := 20;
	                #xOutPresetAdjSet := 1;
	                IF #xInPresetAdjSet THEN
	                    #nOutPresetAdjSetValue := #nInPresetAdjSetValue;
	                ELSE
	                    #nOutPresetAdjSetValue := #cReferenceValue;
	                END_IF;
	            END_IF;
	        20:     // Wait for Quit
	            #xOutPresetAdjSet := 1;
	            IF #xInPresetAdjSet THEN
	                #nOutPresetAdjSetValue := #nInPresetAdjSetValue;
	            ELSE
	                #nOutPresetAdjSetValue := #cReferenceValue;
	            END_IF;
	            IF #xInPresetAdjQuit THEN
	                #xOutPresetAdjSet := 0;
	                #nStateAdjustment := 30;
	            END_IF;
	        30:     // Wait for Reset the #xInPresetAdjSet Input
	            IF NOT #xInPresetAdjSet THEN
	                #nStateAdjustment := 100;
	                #nTest[1] := 0;
	            END_IF;
	        100:   // First Refernce Set
	            #bOutReferenced := 1;
	            IF #xInPresetAdjSet THEN
	                #nStateAdjustment := 0;
	            END_IF;
	    END_CASE;
	END_REGION
	
	REGION Positionsänderung ermitteln
	    #nPosActDif := #nInPosAct - #nPosAct;
	    IF (#nInPosAct < #nPosAct) AND                        // Wenn die neue Position kleiner ist als der alte Wert
	        ABS(#nPosActDif) > (#nInEncoderMax / 2) THEN       // Und die differenz Größer als die Hälfte des Encoderendwertes
	        #nPosActDif := #nInEncoderMax - #nPosAct;
	        #nPosActDif += #nInPosAct;
	        #nPosAct := #nInPosAct;
	    ELSIF #nPosActDif > 0 THEN
	        #nPosAct := #nInPosAct;
	    ELSE
	        #nPosActDif := 0;
	        #nPosAct := #nInPosAct;
	    END_IF;
	    (*
	    IF #nPosActDif > 0 THEN
	        #nPosAct := #nInPosAct;
	    ELSIF ABS(#nPosActDif) > ("cEncoderMax" / 2) THEN
	        #nPosActDif := "cEncoderMax" - #nPosAct;
	        #nPosActDif += #nInPosAct;
	        #nPosAct := #nInPosAct;
	    ELSE
	        #nPosActDif := 0;
	        #nPosAct := #nInPosAct;
	    END_IF;
	    *)
	END_REGION
	
	REGION Pitch
	    #nOutPosPitch := #nInPosAct / #nInPitch;                                                // Taschennummer nach Encoder
	    //#nOutPosPitchPart := REAL_TO_DINT(((#nInPosAct MOD #nInPitch)));
	    //TEST MOLINA ANFANG
	    #nOutPosPitchPart := (#"cEncoderScaling" * (#nInPosAct MOD #nInPitch)) / #nInPitch;      // Skalierter Restwert Taschenzwischenschritt
	    //TEST MOLINA ENDE
	    #nPosPitchDif := #nOutPosPitch - #nPosPitchAct;
	    
	    IF ABS(#nPosPitchDif) > (#nInHooksSorterMax / 2) THEN
	        #nPosPitchDif := #nInHooksSorterMax - #nPosPitchAct;
	        #nPosPitchDif += #nOutPosPitch;
	        #nPosPitchAct := #nOutPosPitch;
	    ELSIF #nPosPitchDif > 0 THEN
	        #nPosPitchAct := #nOutPosPitch;
	    ELSE
	        #nPosPitchDif := 0;
	        #nPosPitchAct := #nOutPosPitch;
	    END_IF;
	    
	END_REGION
	
	REGION Outputs
	    IF #nStateAdjustment = 100 THEN
	        #nOutPosAct := #nInPosAct;
	    END_IF;
	    #nOutPosActDif := #nPosActDif;
	    #nOutPosPitchDif := #nPosPitchDif;
	    #nTest[0] += #nPosPitchDif;
	    IF #stInSensorReference.xDirectEdgePos THEN
	        #nTest[1] += 1;
	    END_IF;
	END_REGION
	
	REGION Statistic
	    #arIncremente[#nPos] += #nPosActDif;
	    IF #stInSensorReference.xDirectEdgePos THEN
	        (*
	        #nPosActDif := #nInPosAct - #nPosAct;
	        IF #nPosActDif > 0 THEN
	            #nPosAct := #nInPosAct;
	        ELSIF ABS(#nPosActDif) > ("cEncoderMax" / 2) THEN
	            #nPosActDif := "cEncoderMax" - #nPosAct;
	            #nPosActDif += #nInPosAct;
	            #nPosAct := #nInPosAct;
	        ELSE
	            #nPosActDif := 0;
	            #nPosAct := #nInPosAct;
	        END_IF;
	        *)
	        #nPos += 1;
	        IF #nPos > 40 THEN
	            #nPos := 0;
	        END_IF;
	    END_IF;
	END_REGION
END_FUNCTION_BLOCK

