FUNCTION_BLOCK "FB_DistanceFoot"
{ S7_Optimized_Access := 'TRUE' }
VERSION : 0.1
   VAR_INPUT 
      xInUp : Bool;   // true = fährt gerade hoch
      xInDown : Bool;   // true = fährt gerade runter
      wInDistance { ExternalAccessible := 'False'; ExternalVisible := 'False'; ExternalWritable := 'False'} : Word;   // Rohwerte von Sensor
      nInStartSlow { ExternalAccessible := 'False'; ExternalVisible := 'False'; ExternalWritable := 'False'} : Int;   // Unterhalb diesen Wertes wird langsam gefahren
      nInStartFast { ExternalAccessible := 'False'; ExternalVisible := 'False'; ExternalWritable := 'False'} : Int;   // Oberhalb diesen Wertes wird schnell gefahren
      nInDistanceZero : Int := 2460;   // Abstand der für Bodenkontakt steht
      nInDiff : Int := 250;   // Dieser Wert +- ist die Grenze ab der ein Fehler generiert wird bei Bodenkontakt
      tInDiff : Time := T#1500ms;   // Zeit wie lange die Differenz ansteht bis ein Fehler generiert wird
      xInComp : Bool;   // Teach Bit um die Werte beizubringen
      xInOver2m : Bool;   // Signal dass sich Ladeschleife über 2m Auslage befindet
   END_VAR

   VAR_OUTPUT 
      nOutDistance { ExternalAccessible := 'False'; ExternalVisible := 'False'; ExternalWritable := 'False'} : Int;   // Distanzwert INT
      xOutBelowThreshold : Bool;   // Unter Schwelle Kriechgang
      xOutAboveThreshold : Bool;   // Über Schwelle Kriechgang
      xOutError : Bool;   // Fehler allgemein
      xOutGroundContact : Bool;   // Bodenkontakt ermittelt
      xOutTeachNec : Bool;   // Teachen notwendig
   END_VAR

   VAR RETAIN
      xCrawl : Bool;   // Kriechgang aktiv
      nDistance { ExternalAccessible := 'False'; ExternalVisible := 'False'; ExternalWritable := 'False'} : Int;   // Abstandswert INT
      nDistanceZero : Int;   // Eingestellter Sollwert der für Bodenkontakt steht
      nDiff : Int;   // Wert +- um DistanceZero Sollwert der die gültige Toleranz darstellt
      tOnDiff {InstructionName := 'TON_TIME'; LibVersion := '1.0'; S7_SetPoint := 'False'} : TON_TIME;   // Solange muss Sollwert abweichen damit Fehler kommt
      tOnrUp {InstructionName := 'TONR_TIME'; LibVersion := '1.0'; S7_SetPoint := 'False'} : TONR_TIME;   // NIU
      tOnrDown {InstructionName := 'TONR_TIME'; LibVersion := '1.0'} : TONR_TIME;   // NIU
      xGroundContact : Bool;   // Es besteht Bodenkontakt
      xError : Bool;   // Fehlerbit allgemein
   END_VAR


BEGIN
	REGION Umwandeln
	    #nOutDistance := #nDistance := WORD_TO_INT(#wInDistance);
	END_REGION
	
	REGION Kriechgang ermitteln
	    IF #nDistance < #nInStartSlow THEN
	        #xCrawl := TRUE;
	    ELSIF #nDistance > #nInStartFast THEN
	        #xCrawl := FALSE;
	    END_IF;
	    
	    IF #nDistance <= #nDistanceZero + #nDiff THEN
	        #xGroundContact := TRUE;
	    ELSIF #nDistance > #nDistanceZero + #nDiff + 50 THEN
	        #xGroundContact := FALSE;
	    END_IF;
	END_REGION
	
	
	
	REGION Zero-Wert Teachen
	    IF #xInComp THEN
	        IF #nInDistanceZero > 0 AND #nInDiff > 0 THEN
	            #nDistanceZero := #nInDistanceZero;
	            #nDiff := #nInDiff;
	        END_IF;
	    END_IF;
	END_REGION
	
	REGION Timer
	    #tOnDiff(IN:=#nDistance = 0 OR
	             (#xGroundContact AND #xInOver2m AND
	             ((#nDistance > #nDistanceZero + #nDiff) OR (#nDistance < #nDistanceZero - #nDiff))),
	             PT:=#tInDiff);
	END_REGION
	
	REGION Fehler
	    IF #tOnDiff.Q OR
	        (#xInOver2m AND NOT #xGroundContact AND NOT #xInUp AND NOT #xInDown)
	    THEN
	        #xError := TRUE;
	    ELSE
	        #xError := FALSE;
	    END_IF;
	END_REGION
	
	REGION Ausgänge
	    #xOutBelowThreshold := #xCrawl;
	    #xOutAboveThreshold := NOT #xCrawl;
	    #xOutError := #xError;
	    #xOutGroundContact := #xGroundContact;
	    #xOutTeachNec := #nDistanceZero = 0 OR #nDiff = 0;
	END_REGION
	
END_FUNCTION_BLOCK

