FUNCTION_BLOCK "FB_LoadingBoom_INBOUND"
{ S7_Optimized_Access := 'TRUE' }
VERSION : 0.1
   VAR_INPUT 
      stInControl : Struct   // Bedienelemente
         xBtnReleaseMT1 : Bool;   // Taster 1 Freigabe für MT
         xSwitchLoadingPos2 : Bool;   // Knebelschalter Loading Pos 1 oder 2, true=2, false=1
         xBtnBack : Bool;   // Taster "Einfahren"
         xBtnForth : Bool;   // Taster "Ausfahren"
         xBtnReleaseMT2 : Bool;   // Taster 2 Freigabe für MT
         xSwitchFootUp : Bool;   // Knebelschalter "Hochfahren"
         xSwitchFootDown : Bool;   // Knebelschalter "Runterfahren"
         xSwitchLightOnOff : Bool;   // Knebelschalter Licht AN/AUS
         xBtnBoomActive : Bool;   // Taster Ladeschleife AN/AUS
         xBtnReset : Bool;   // Taster Reset
         xSwitchFootManual : Bool;   // Schlüsselschalter "Stützfuß verfahren"
      END_STRUCT;
      stInBoom : Struct   // In Signale für Ausleger
         xMotorProtection1 : Bool;   // Meldekontakt Motorschutzschalter Motor 1
         xMotorProtection2 : Bool;   // **NIU**Meldekontakt Motorschutzschalter Motor 2
         xLimitForth : Bool;   // Endlage Vorne normal
         xLimitForthEmergency : Bool;   // Endlage Vorne NOT (normal überschritten)
         xLimitBack : Bool;   // Endlage Hinten normal
         xLimitBackEmergency : Bool;   // Endlage Hinten NOT (normal überschritten)
         xBumperOk : Bool;   // Bumper Stirnseitig
      END_STRUCT;
      stInFoot : Struct   // In Signale für Stützfuß
         xMotorProtection : Bool;   // Meldekontakt Motorschutzschalter Stützfuß
         xLimitUp : Bool;   // Obere Endlage
         xLimitDown : Bool;   // Untere Endlage
      END_STRUCT;
      stInSettings : Struct
         tBoomInput : Time := T#3500ms;   // Zeit für Bediener-Eingabe Boom
         tWheelInput : Time := T#3500ms;   // Zeit für Bediener-Eingabe Wheel
         tWaitForMove : Time := T#2500ms;   // Zeit die bei Bewegungswunsch gewartet wird
         tSenWaitStopper1 : Time := T#300ms;   // Wartezeit zwischen Sensor und Stopper Clamp
         tWaitAfterReleaseStopper1 : Time := T#4000ms;   // Wartezeit nach MT Release
         tWaitBeforeSwitchStopper2 : Time := T#4000ms;
         tTrailingStopper2 : Time := T#1500ms;   // Zeit wie lange ohne Belegung offen sein muss
      END_STRUCT;
      stInTilt : Struct   // Neigungssensorik
         wTilt : Word;   // Eingangswert von Sensor
         xClock : Bool;   // Frequenz für Mittelwert Ermittlung
         tDiff : Time;   // Solange muss Abweichung bestehen damit nachgeregelt wird
         tZero : Time;   // Solange muss ca. Null bestehen damit gestoppt wird
         nDiff : Int;   // Abweichung > oder <  als nDiff? ---> Nachregeln
         nZeroOk : Int;   // Bereich um Null in welchem nachregeln aufgehört wird
         xComp : Bool;   // Flanke um Kompensationswert einzustellen
         nTiltComp : Int;   // Wert von User um Tilt zu kompensieren, +X um größer zu werden, -X um kleiner zu werden
      END_STRUCT;
      stInDistance : Struct   // Signale für Distanzsensor
         wDistance : Word;   // Rohwerte von Sensor
         nStartSlow : Int;   // Unterhalb diesen Wertes wird langsam gefahren
         nStartFast : Int;   // Oberhalb diesen Wertes wird schnell gefahren
         nDistanceZero : Int;   // Abstand der für Bodenkontakt steht
         nDiff : Int;   // Dieser Wert +- ist die Grenze ab der ein Fehler generiert wird bei Bodenkontakt
         tDiff : Time;   // Zeit wie lange die Differenz ansteht bis ein Fehler generiert wird
         xComp : Bool;   // Teach Bit um die Werte beizubringen
      END_STRUCT;
      stInSen2mThreshold : "UDT_Sensor";   // Induktiver Sensor Position 2m, true= unter 2m. false=über 2m
      stInSenJamEntr : "UDT_Sensor";   // Sensor Stau nach Vario am Eingang
      stInSenJamExit1 : "UDT_Sensor";   // Sensor Stau vor Vario am Ausgang
      stInSenJamExit2 : "UDT_Sensor";   // Sensor Stau am Ende von Vario
      stInSenStopper1 : "UDT_Sensor";   // Sensor KURZ VOR Stopper 1 von Arbeitsplatz
      stInSenStopper2 : "UDT_Sensor";   // Sensor IN Stopper 2 von Arbeitsplatz
      stInSenGrid : "UDT_Sensor";   // Sensor "Karussell dreht sich"
      xInMachineRunning : Bool;   // Machine Running
      xInTrolleyInSep : Bool;   // Separator vor dem Stopper hat einen Trolley
      xInSafety : Bool;   // Safety ok
   END_VAR

   VAR_OUTPUT 
      xOutLedMovement : Bool;   // Orangene Warn Leuchte für Bewegung der Ladeschleife
      xOutLight : Bool;   // Beleuchtung Ladeschleife an der Decke
      xOutBoomActive : Bool;   // LS eingeschaltet, ready for use
      xOutReady : Bool;   // Bereit für TROs
      xOutMoveReq : Bool;   // Bereit um auf Bewegung der Ladeschleife umzuschalten
   END_VAR
   VAR_OUTPUT RETAIN
      stOutStopper : Struct   // Ausgänge Stopper allgemein
         xStopper1Clamp : Bool;   // Schiebe Stopper 1 vor
         xStopper1Finger : Bool;   // Schalte Stopper 1 runter
         xStopper2 : Bool;   // Öffne Stopper 2
         xStopperEmpty : Bool;   // Stopperstatus leer
         xStopperFreeEdge : Bool;   // Flanke dass Stopper frei wird
         xStopperReceive : Bool;   // Flanke dass Stopper aufnehmen will
      END_STRUCT;
   END_VAR
   VAR_OUTPUT 
      stOutBoom : Struct   // Ausgänge Ladeschleife allgemein
         xBoomForth1 : Bool;   // Rausfahren Motor 1
         xBoomForth2 : Bool;   // Rausfahren Motor 2
         xBoomBack1 : Bool;   // Zurückfahren Motor 1
         xBoomBack2 : Bool;   // Zurückfahren Motor 2
      END_STRUCT;
      stOutFoot : Struct   // Ausgänge Stützfuß allgemein
         xFootUp : Bool;   // Hochfahren
         xFootDown : Bool;   // Runterfahren
         xFootSpeed1 : Bool;   // Geschwindigkeit 1 (Kriechgang)
      END_STRUCT;
      xOutCarousel : Bool;   // Rechtslauf drehen
      nOutState : Int;   // Zustand des Bausteins
      xOutLedBumper : Bool;   // LED Bumper ausgelöst
      xOutStopperState : Bool;
   END_VAR

   VAR_IN_OUT 
      arInOutCarrierStopper : Array[1..1] of "stCarrier";   // Carrierdaten im Stopper
      arInOutCarrierExit : Array[*] of "stCarrier";   // Carrierdaten im Exitbereich
      stInOutCarrierExit : "stJamData";   // JamStatus im Exitbereich
   END_VAR

   VAR 
      fbTiltSensor : "FB_Tilt_TEST";
      fbDistanceFoot : "FB_DistanceFoot";
      fbWorkSation : "FB_BoomWorkStation";
   END_VAR
   VAR RETAIN
      nState : Int;
   END_VAR
   VAR 
      fbStateManager : "FB_StateManager";
      rTrigBoomActive {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke von Ladeschleife einschalten
      rTrigBtnForth {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke für VOR-Taster
      rTrigBtnBack {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke für ZURÜCK-Taster
      rTrigBtnReleaseMT {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke für Taster 1 und 2 am Arbeitsplatz
      rTrigBtnReset {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke für Resettaster
      rTrigLimitForth {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke für Endlage Vorne
      rTrigLimitForthEmergency {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke für Endlage Vorne Rollenschalter
      rTrigLimitBack {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke für Endlage Hinten
      rTrigLimitBackEmergency {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke für Endlage Hinten Rollenschalter
      rTrigLimitUp {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke für Endlage Oben
      rTrigLimitDown {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke für Endlage Unten
      rTrigStopperFull {InstructionName := 'R_TRIG'; LibVersion := '1.0'} : R_TRIG;   // Flanke für das Einfahren in den Stopper
      tOnBoomInput {InstructionName := 'TON_TIME'; LibVersion := '1.0'} : TON_TIME;   // Timer der abläuft wenn länger keine Eingabe durch Bediener erfolgt
      tOnFootInput {InstructionName := 'TON_TIME'; LibVersion := '1.0'} : TON_TIME;   // Timer der abläuft wenn länger keine Eingabe durch Bediener erfolgt
   END_VAR
   VAR RETAIN
      tOnStopper2 {InstructionName := 'TON_TIME'; LibVersion := '1.0'} : TON_TIME;   // Timer für Stopper 2 Release
      tOnStopper1 {InstructionName := 'TON_TIME'; LibVersion := '1.0'} : TON_TIME;   // Timer für Stopper 1 Release
      xBoomActive : Bool;   // Ladeschleife ist eingeschaltet
      xErrorLogical : Bool;   // Fehlerbit Schrittkette
      xErrorSensor : Bool;   // **NIU**Fehlerbit Sensorik
      xHightInvalid : Bool;   // Höhenanpassung notwendig
      xFloorReached : Bool;   // Bodenhöhe erreicht für Schleichfahrt
      xAbove2m : Bool;   // Ausleger 2 m überschritten
      xLight : Bool;   // Lichtbit
      xStopper1Active : Bool;   // Stopper 1 ist aktiviert
      xStopperBusy : Bool;   // jeweiliger Stopper belegt
      stTilt : Struct
         nTiltRaw : Int;
         nTiltMed : Int;
         nTiltComp : Int;
         xTiltPositive : Bool;   // Neigungssensor zu hoch
         xTiltNegative : Bool;   // Neigungssensor zu niedrig
      END_STRUCT;
   END_VAR

   VAR_TEMP 
      stTmpActuator : Struct
         xForth : Bool;
         xBack : Bool;
         xUp : Bool;
         xDown : Bool;
         xCarousel : Bool;
      END_STRUCT;
      xTmpReady : Bool;   // Signal dass Ausleger einsatzbereit bereit
      xTmpMoveReq : Bool;
      xTmpActUnivTon : Bool;
      xTmpMtOnTheWay : Bool;   // Es wird MT an Stopper geschickt
   END_VAR

   VAR CONSTANT 
      cStepOff : Int := 0;
      cStepUndefined : Int := 100;
      cStepIdle : Int := 200;
      cStepStopper : Int := 250;
      cStepWait : Int := 300;
      cStepWaitForBoom : Int := 1000;
      cStepMoveBoom : Int := 1500;
      cStepWaitForFootMan : Int := 2000;
      cStepMoveFootMan : Int := 2500;
      cStepWaitForFootAutUp : Int := 3000;
      cStepMoveFootAutUp : Int := 3500;
      cStepWaitForFootAutDown : Int := 4000;
      cStepMoveFootAutDown : Int := 4500;
      cStepError : Int := 9999;
   END_VAR


BEGIN
	(*
	Vor- und Zurückfahren: Nur manuell per Button. Nach Bumperauslösung nur zurück.
	Normale induktive Endlagen melden Limit. Werden diese überfahren, dann kommt Rollenschalter der Strom abschaltet.
	
	Hoch- und Runterfahren: 4 Bit => 1 oder 2 entspr. L oder R, beide false oder beide true entspr. AUS. 3 oder 4 entspr. v1 oder v2, 3 und 4 entspr. v1+v2
	Nach induktivem 2m-Sensor -> automatisches Ausfahren
	
	Baustein managed:
	- das Verfahren vor/zurück/hoch/runter
	- die Logik für den Arbeitsplatz
	- das Regeln des Auslegers
	- den Zustand des Auslegers (bereit aufzunehmen, Anforderung das Teile benötigt werden, busy, Position des Auslegers, Stoppen aller MTROs wenn gerade verfahren wird, Kreisel fahren/stoppen)
	
	TROs auf dem Ausleger sind separat und kommunizieren extern mit ihm. Aufwand zu groß, die Schnittstellen der MainTROs nach außen zu führen,
	sowie extra für Loading Boom alles extra mit SubTROs intern zu realisieren.
	
	-Wenn zu viel MT nach Vario sind, dann darf nicht zurückgefahren werden
	-normaler Separator vor Arbeiter hat Scanner
	-Am Arbeitsplatz NACH Separator ist ein besonderer Stopper an der Strinseite und danach zusätzlich ein einfacher Stopper (Sep mit nur einem Stachel)
	-Wird begonnen seitlich -> MT fährt an offenem ersten Stopper vorbei und Taster 1 lässt MT am zweiten Stopper raus 
	-Später wenn Boom weiter draußen ist wird am geschlossnem ersten Stopper gearbeitet und MT mit Taster 2 rausgelassen, worauf MT danach durch offenen zweiten Stopper fährt
	*)
	
	REGION Eingänge
	    (*
	    Beispiel:
	c==========================Ausleger    
	        |___ Abstand zu Boden 2 m
	        |
	        |___ Abstand zu Boden 60cm (>= 60cm schnell fahren - xFloorReachedEnd)
	        |
	        |___ Abstand zu Boden 50cm (<= 40cm langsam fahren - xFloorReachedStart)
	        |
	        O
	                        
	 ///////////////////////////////Boden       
	*)
	    
	    IF #fbDistanceFoot.xOutBelowThreshold(* #stInFoot.xFloorReachedStart *) THEN
	        #xFloorReached := TRUE;
	    ELSIF #fbDistanceFoot.xOutAboveThreshold (* #stInFoot.xFloorReachedEnd *) THEN
	        #xFloorReached := FALSE;
	    END_IF;
	    
	    //ab 2 m ist Verwendung des Stützrades notwenig
	    // #x2mReached := #stInSen2mThreshold.xDbncdSensor; //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Regelung für Foot außerhalb von Schrittkette mit FB aufrufen!!!!!!!!!!!!!
	    IF (#stInSen2mThreshold.xDbncdEdgeNeg AND #stInControl.xBtnForth) OR (NOT #stInSen2mThreshold.xDbncdSensor AND #stInSen2mThreshold.tDirectChangeLast > T#500ms) THEN
	        #xAbove2m := TRUE; //provisorisch
	    ELSIF (#stInSen2mThreshold.xDbncdEdgePos AND #stInControl.xBtnBack) OR (#stInSen2mThreshold.xDbncdSensor AND #stInSen2mThreshold.tDirectChangeLast > T#500ms) THEN
	        #xAbove2m := FALSE;
	    // ELSIF #rTrigBtnBack.Q OR #rTrigBtnForth.Q THEN
	    //     #xAbove2m := FALSE; //provisorisch
	    END_IF;
	    
	    REGION Flanken
	        
	        #rTrigBoomActive(CLK:=#stInControl.xBtnBoomActive); //Ladeschleife einschalten
	        
	        #rTrigBtnForth(CLK := #stInControl.xBtnForth); //Flanke für VOR-Taster
	        #rTrigBtnBack(CLK := #stInControl.xBtnBack); //Flanke für ZURÜCK-Taster
	        #rTrigBtnReleaseMT(CLK := #stInControl.xBtnReleaseMT1 OR #stInControl.xBtnReleaseMT2); //Flanke für Taster 1 und 2 am Arbeitsplatz
	        #rTrigBtnReset(CLK := #stInControl.xBtnReset); //Flanke für Resettaster
	        
	        #rTrigLimitForth(CLK := #stInBoom.xLimitForth); //Flanke für Endlage Vorne
	        #rTrigLimitForthEmergency(CLK := #stInBoom.xLimitForthEmergency); //Flanke für Endlage Vorne Rollenschalter
	        #rTrigLimitBack(CLK := #stInBoom.xLimitBack); //Flanke für Endlage Hinten
	        #rTrigLimitBackEmergency(CLK := #stInBoom.xLimitBackEmergency); //Flanke für Endlage Hinten Rollenschalter
	        #rTrigLimitUp(CLK := #stInFoot.xLimitUp); //Flanke für Endlage Oben
	        #rTrigLimitDown(CLK := #stInFoot.xLimitDown); //Flanke für Endlage Unten
	    END_REGION
	
	END_REGION
	
	REGION Timer
	    //In Schritt MoveBoom wird Eingabe erwartet, wenn nichts kommt -> Schrittwechsel
	    #tOnBoomInput(IN := #nState = #cStepMoveBoom AND NOT #stInControl.xBtnForth AND NOT #stInControl.xBtnBack,
	                  PT := #stInSettings.tBoomInput);
	    
	    //In Schritt MoveWheel wird Eingabe erwartet, wenn nichts kommt -> Schrittwechsel
	    #tOnFootInput(IN := #nState = #cStepMoveFootMan AND NOT #stInControl.xSwitchFootUp AND NOT #stInControl.xSwitchFootDown,
	                  PT := #stInSettings.tWheelInput);
	    
	    // #tOnStopper1(IN:=#xStopper1Active AND #fbStopper1.nOutState >= 500,
	    //              PT:=_time_in_);
	    
	END_REGION
	
	REGION Distance Sensor
	    //IBN
	    // IF #fbDistanceFoot.nDistanceZero = 0 THEN
	    //     //Entsrpicht in LL ungefähr dem Distance Abstand am Boden
	    //     #fbDistanceFoot.nDistanceZero := 2460;
	    // END_IF;
	    // IF #fbDistanceFoot.nDiff = 0 THEN
	    //     #fbDistanceFoot.nDiff := 50;
	    // END_IF;
	    //IBN
	    
	    #fbDistanceFoot(xInUp:=#stOutFoot.xFootUp,
	                    xInDown:=#stOutFoot.xFootDown,
	                    wInDistance := #stInDistance.wDistance,
	                    nInStartSlow := #stInDistance.nStartSlow, // Format z.B. 2799 für 2799mm
	                    nInStartFast := #stInDistance.nStartFast,
	                    nInDistanceZero := #stInDistance.nDistanceZero,
	                    nInDiff := #stInDistance.nDiff,
	                    tInDiff := #stInDistance.tDiff,
	                    xInComp := #stInDistance.xComp,
	                    xInOver2m := NOT #stInSen2mThreshold.xDbncdSensor);
	                    // nOutDistance => "IBN".nDistance); VERSCHALTEN!!!!!!!!
	END_REGION
	
	REGION Logik Regelung / Neigung
	    
	    //Ab Winkel +-X starte Nachregeln
	    // #xHightInvalid := "cTodoFalse"; //Höhenanpassung notwendig - to do
	    
	    #fbTiltSensor.stInSettings.tDiff := #stInTilt.tDiff;//"DB_Interface_HMI_INBOUND".stBoomData1.tDiff;
	    #fbTiltSensor.stInSettings.tZero := #stInTilt.tZero;//"DB_Interface_HMI_INBOUND".stBoomData1.tZero;
	    #fbTiltSensor.stInSettings.nDiff := #stInTilt.nDiff;//"DB_Interface_HMI_INBOUND".stBoomData1.nTiltAbweichung;
	    #fbTiltSensor.stInSettings.nZeroOk := #stInTilt.nZeroOk;//"DB_Interface_HMI_INBOUND".stBoomData1.nTiltBereichZero;
	    #fbTiltSensor.stInSettings.xComp := #stInTilt.xComp;//"DB_Interface_HMI_INBOUND".stBoomData1.xComp;
	    #fbTiltSensor.stInSettings.nTiltComp := #stInTilt.nTiltComp;//"DB_Interface_HMI_INBOUND".stBoomData1.nTiltNullen;
	    #fbTiltSensor(wInTilt := #stInTilt.wTilt, //"wTiltSensor"
	                  xInClock := #stInTilt.xClock, //"Clock_10Hz",//
	                  xInTiltActive:= NOT #stInSen2mThreshold.xDbncdSensor,//"DB_Interface_HMI_INBOUND".stBoomData1.xTiltÜberwachungActive,
	                  xInTiltAdjust:= #nState = #cStepWait);
	    
	    #stTilt.nTiltMed := #fbTiltSensor.stOutTilt.nTiltCompMed;
	    
	    #xHightInvalid := #fbTiltSensor.xOutToHigh OR #fbTiltSensor.xOutToLow;
	    
	    
	    //Format 1/100 Grad
	    //Beispiel:
	    //+4    =   +0.04 Grad
	    //+14   =   +0.14 Grad
	    //+114  =   +1.14 Grad
	    //etc.
	    
	END_REGION
	
	REGION Schrittkette
	    CASE #nState OF
	        #cStepOff:
	            REGION 000: Off
	                #nOutState := #cStepOff;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := FALSE;
	                #stTmpActuator.xDown := FALSE;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := FALSE;
	            END_REGION
	            
	        #cStepUndefined:
	            REGION 100: Undefined
	                #nOutState := #cStepUndefined;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := FALSE;
	                #stTmpActuator.xDown := FALSE;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := FALSE;
	                //////////////////////////////////////
	                
	                
	                IF NOT #xErrorLogical AND #fbStateManager.tOutTimeInStep > T#1000ms THEN
	                    #nState := #cStepIdle;
	                END_IF;
	            END_REGION
	            
	        #cStepIdle:
	            REGION 200: Idle
	                //Idle Zustand in welchem Kreisel bereit ist und die MTROs normal arbeiten können
	                #nOutState := #cStepIdle;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := FALSE;
	                #stTmpActuator.xDown := FALSE;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := TRUE;
	                #xTmpMoveReq := FALSE;
	                ///////////////////////////////////////
	                
	                IF ((#rTrigBtnBack.Q AND NOT #stInBoom.xLimitBack) OR (#rTrigBtnForth.Q AND NOT #stInBoom.xLimitForth) OR NOT #stInBoom.xBumperOk) AND
	                    NOT #fbWorkSation.xOutStationMovement THEN
	                    #nState := #cStepWaitForBoom;
	                END_IF;
	                
	                IF (#stInControl.xSwitchFootManual AND ((#stInControl.xSwitchFootUp AND NOT #stInFoot.xLimitUp) OR (#stInControl.xSwitchFootDown AND NOT #stInFoot.xLimitDown (* AND NOT #fbDistanceFoot.xOutGroundContact *)))) AND
	                    NOT #fbWorkSation.xOutStationMovement THEN
	                    #nState := #cStepWaitForFootMan;
	                END_IF;
	                
	                // IF #xStopperBusy THEN
	                //     #xTmpMtOnTheWay := TRUE;
	                //     #nState := #cStepStopper;
	                // END_IF;
	                    
	                // IF #xHightInvalid THEN
	                //     #nState := #cStepWait;
	                // END_IF;
	                
	                IF #xErrorLogical THEN
	                    #nState := #cStepError;
	                END_IF;
	            END_REGION
	            
	        // #cStepStopper:
	        //     REGION 250: Stopper arbeitet
	        //         // Warten für Stopperarbeit
	        //         #nOutState := #cStepStopper;
	                
	        //         #stTmpActuator.xForth := FALSE;
	        //         #stTmpActuator.xBack := FALSE;
	        //         #stTmpActuator.xUp := FALSE;
	        //         #stTmpActuator.xDown := FALSE;
	        //         #stTmpActuator.xCarousel := FALSE;
	                
	        //         #xTmpReady := TRUE;
	        //         #xTmpMoveReq := FALSE;
	        //         //////////////////////////////////////////////
	                
	        //         // IF NOT #xHightInvalid THEN
	        //         //     #nState := #cStepMoveBoom;//#cStepUndefined;
	        //         // END_IF;
	                
	        //         // AND NOT #fbWorkSation.xOutStationMovement ....
	                
	                
	        //         // IF ((#rTrigBtnBack.Q AND NOT #stInBoom.xLimitBack) OR (#rTrigBtnForth.Q AND NOT #stInBoom.xLimitForth) OR NOT #stInBoom.xBumperOk) AND
	        //         //     ((#xStopper1Active AND NOT #fbStopper1.xOutStopper) OR (NOT #xStopper1Active AND NOT #fbStopper2.xOutStopper)) THEN
	        //         //     #nState := #cStepWaitForBoom;
	        //         // END_IF;
	                
	        //         // IF (#stInControl.xSwitchFootManual AND ((#stInControl.xSwitchFootUp AND NOT #stInFoot.xLimitUp) OR (#stInControl.xSwitchFootDown AND NOT #stInFoot.xLimitDown AND NOT #fbDistanceFoot.xOutGroundContact))) AND
	        //         //     ((#xStopper1Active AND NOT #fbStopper1.xOutStopper) OR (NOT #xStopper1Active AND NOT #fbStopper2.xOutStopper)) THEN
	        //         //     #nState := #cStepWaitForFootMan;
	        //         // END_IF;
	                
	        //         // IF (NOT #xStopper1Active AND #fbStopper2.xOutReleaseDone) OR
	        //         //     (#xStopper1Active AND #fbStopper1.xOutReleaseDone) THEN
	        //         //     #xStopperBusy := FALSE;
	        //         //     #xTmpStopperFreeEdge := TRUE;
	        //         //     #nState := #cStepIdle;
	        //         // END_IF;
	                
	        //         IF #xErrorLogical THEN
	        //             #nState := #cStepError;
	        //         END_IF;
	        //     END_REGION
	            
	        #cStepWait:
	            REGION 300: Wait
	                // Warten für Zustandswechsel
	                #nOutState := #cStepWait;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := #fbTiltSensor.xOutToHigh;
	                #stTmpActuator.xDown := #fbTiltSensor.xOutToLow;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := FALSE;
	                //////////////////////////////////////////////
	                
	                IF NOT #xHightInvalid THEN
	                    #nState := #cStepMoveBoom;//#cStepUndefined;
	                END_IF;
	                
	                IF #xErrorLogical THEN
	                    #nState := #cStepError;
	                END_IF;
	            END_REGION
	            
	        #cStepWaitForBoom:
	            REGION 1000: Wait for Boom
	                #nOutState := #cStepWaitForBoom;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := FALSE;
	                #stTmpActuator.xDown := FALSE;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := TRUE;
	                ///////////////////////////////////////////////
	                
	                
	                IF #fbStateManager.tOutTimeInStep > #stInSettings.tWaitForMove OR NOT #stInBoom.xBumperOk THEN
	                    #nState := #cStepMoveBoom;
	                END_IF;
	                
	                // IF #xHightInvalid THEN
	                //     #nState := #cStepWait;
	                // END_IF;
	                
	                IF #xErrorLogical THEN
	                    #nState := #cStepError;
	                END_IF;
	            END_REGION
	            
	        #cStepMoveBoom:
	            REGION 1500: Move Boom
	                #nOutState := #cStepMoveBoom;
	                
	                #stTmpActuator.xForth := #stInControl.xBtnForth AND NOT #stInControl.xBtnBack AND NOT #stInBoom.xLimitForth AND NOT #fbDistanceFoot.xOutError AND ((NOT #xAbove2m AND #stInFoot.xLimitUp) OR (#xAbove2m AND #fbDistanceFoot.xOutGroundContact));
	                #stTmpActuator.xBack := #stInControl.xBtnBack AND NOT #stInControl.xBtnForth AND NOT #stInBoom.xLimitBack AND NOT #fbDistanceFoot.xOutError AND ((NOT #xAbove2m AND #stInFoot.xLimitUp) OR (#xAbove2m AND #fbDistanceFoot.xOutGroundContact));
	                #stTmpActuator.xUp := FALSE;
	                #stTmpActuator.xDown := FALSE;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := FALSE;
	                ////////////////////////////////////////////
	                
	                // IF #stInLimitBoom.stSenThreshold.xDbncdSensor
	                
	                IF #tOnBoomInput.Q AND #stInBoom.xBumperOk THEN
	                    #nState := #cStepIdle;
	                END_IF;
	                
	                IF #stInSen2mThreshold.xDbncdEdgeNeg AND #stInControl.xBtnForth THEN
	                    #nState := #cStepWaitForFootAutDown;
	                END_IF;
	                
	                IF (#stInSen2mThreshold.xDbncdEdgePos AND #stInControl.xBtnBack) OR
	                    (#stInSen2mThreshold.xDirectSensor AND NOT #stInFoot.xLimitUp AND #stInControl.xBtnBack) THEN
	                    #nState := #cStepWaitForFootAutUp;
	                END_IF;
	                
	                IF #xHightInvalid THEN
	                    #nState := #cStepWait;
	                END_IF;
	                
	                IF #xErrorLogical THEN
	                    #nState := #cStepError;
	                END_IF;
	            END_REGION
	            
	        #cStepWaitForFootMan:
	            REGION 2000: Wait for Foot Manually
	                #nOutState := #cStepWaitForFootMan;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := FALSE;//#stInControl.xSwitchFootUp AND NOT #stInControl.xSwitchFootDown AND NOT #stInFoot.xLimitUp;
	                #stTmpActuator.xDown := FALSE;//#stInControl.xSwitchFootDown AND NOT #stInControl.xSwitchFootUp AND NOT #stInFoot.xLimitDown;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := TRUE;
	                ///////////////////////////////////////////////
	                
	                IF NOT #stInControl.xSwitchFootManual THEN
	                    #nState := #cStepIdle;
	                END_IF;
	                
	                IF #fbStateManager.tOutTimeInStep > #stInSettings.tWaitForMove THEN
	                    #nState := #cStepMoveFootMan;
	                END_IF;
	                
	                // IF #xHightInvalid THEN
	                //     #nState := #cStepWait;
	                // END_IF;
	                
	                IF #xErrorLogical THEN
	                    #nState := #cStepError;
	                END_IF;
	            END_REGION
	            
	        #cStepMoveFootMan:
	            REGION 2500: Move Foot Manually
	                #nOutState := #cStepMoveFootMan;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := #stInControl.xSwitchFootUp AND NOT #stInControl.xSwitchFootDown AND NOT #stInFoot.xLimitUp;
	                #stTmpActuator.xDown := #stInControl.xSwitchFootDown AND NOT #stInControl.xSwitchFootUp AND NOT #stInFoot.xLimitDown(* AND NOT #fbDistanceFoot.xOutGroundContact *);
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := FALSE;
	                /////////////////////////////////////////////
	                
	                
	                IF #tOnFootInput.Q OR NOT #stInControl.xSwitchFootManual THEN
	                    #nState := #cStepIdle;
	                END_IF;
	                
	                // IF #xHightInvalid THEN
	                //     #nState := #cStepWait;
	                // END_IF;
	                
	                IF #xErrorLogical THEN
	                    #nState := #cStepError;
	                END_IF;
	            END_REGION
	            
	            
	        #cStepWaitForFootAutUp:
	            REGION 3000: Wait for Foot Automatic Up
	                #nOutState := #cStepWaitForFootAutUp;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := FALSE;
	                #stTmpActuator.xDown := FALSE;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := FALSE;
	                ///////////////////////////////////////////////
	                
	                IF #fbStateManager.tOutTimeInStep > #stInSettings.tWaitForMove THEN
	                    #nState := #cStepMoveFootAutUp;
	                END_IF;
	                
	                // IF #xHightInvalid THEN
	                //     #nState := #cStepWait;
	                // END_IF;
	                
	                IF #xErrorLogical THEN
	                    #nState := #cStepError;
	                END_IF;
	            END_REGION
	            
	        #cStepMoveFootAutUp:
	            REGION 3500: Move Foot Automatic Up
	                #nOutState := #cStepMoveFootAutUp;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := NOT #stInFoot.xLimitUp;
	                #stTmpActuator.xDown := FALSE;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := FALSE;
	                /////////////////////////////////////////////
	                
	                //Nur zum Testen dieses IF
	                IF #stInFoot.xLimitUp THEN
	                    #nState := #cStepIdle;
	                END_IF;
	                
	                // IF #xHightInvalid THEN
	                //     #nState := #cStepWait;
	                // END_IF;
	                
	                IF #xErrorLogical THEN
	                    #nState := #cStepError;
	                END_IF;
	            END_REGION
	            
	        #cStepWaitForFootAutDown:
	            REGION 4000: Wait for Foot Automatic Down
	                #nOutState := #cStepWaitForFootAutDown;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := FALSE;
	                #stTmpActuator.xDown := FALSE;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := FALSE;
	                ///////////////////////////////////////////////
	                
	                IF #fbStateManager.tOutTimeInStep > #stInSettings.tWaitForMove THEN
	                    #nState := #cStepMoveFootAutDown;
	                END_IF;
	                
	                // IF #xHightInvalid THEN
	                //     #nState := #cStepWait;
	                // END_IF;
	                
	                IF #xErrorLogical THEN
	                    #nState := #cStepError;
	                END_IF;
	            END_REGION
	            
	        #cStepMoveFootAutDown:
	            REGION 4500: Move Foot Automatic Down
	                #nOutState := #cStepMoveFootAutDown;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := FALSE;
	                #stTmpActuator.xDown := NOT (#fbDistanceFoot.nOutDistance <= 2460 AND #fbDistanceFoot.nOutDistance >= 2455) AND #stInControl.xBtnForth;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := FALSE;
	                /////////////////////////////////////////////
	                
	                
	                IF #fbDistanceFoot.nOutDistance <= 2460 AND #fbDistanceFoot.nOutDistance >= 2455 THEN
	                    #nState := #cStepIdle;
	                END_IF;
	                
	                // IF #xHightInvalid THEN
	                //     #nState := #cStepWait;
	                // END_IF;
	                
	                IF #xErrorLogical THEN
	                    #nState := #cStepError;
	                END_IF;
	            END_REGION
	            
	        #cStepError:
	            REGION 9999: Error
	                #nOutState := #cStepError;
	                
	                #stTmpActuator.xForth := FALSE;
	                #stTmpActuator.xBack := FALSE;
	                #stTmpActuator.xUp := FALSE;
	                #stTmpActuator.xDown := FALSE;
	                #stTmpActuator.xCarousel := FALSE;
	                
	                #xTmpReady := FALSE;
	                #xTmpMoveReq := FALSE;
	                /////////////////////////////////////////////////
	                
	                
	                IF NOT #xErrorLogical THEN
	                    #nState := #cStepUndefined;
	                END_IF;
	            END_REGION
	            
	        ELSE  // Statement section ELSE
	            #stTmpActuator.xForth := FALSE;
	            #stTmpActuator.xBack := FALSE;
	            #stTmpActuator.xUp := FALSE;
	            #stTmpActuator.xDown := FALSE;
	            #stTmpActuator.xCarousel := FALSE;
	            
	            #xTmpReady := FALSE;
	            #xTmpMoveReq := FALSE;
	            
	            #nState := #cStepUndefined;
	    END_CASE;
	END_REGION
	
	REGION State Manager
	    
	    IF #rTrigBoomActive.Q AND #xInMachineRunning AND #xInSafety(* AND NOT #xAbove2m AND #stInBoom.xLimitBack AND #stInFoot.xLimitUp AND NOT #xErrorLogical *) THEN
	        // IF #nState = #cStepIdle THEN
	        #xBoomActive := NOT #xBoomActive;
	        // END_IF;
	    ELSIF NOT #xInMachineRunning OR NOT #xInSafety THEN
	        #xBoomActive := FALSE;
	    END_IF;
	    
	    #fbStateManager.stInSettings.nStartState := #cStepUndefined;
	    #fbStateManager.stInSettings.xActUnivTOn := TRUE;
	    #fbStateManager.stInSettings.xRestartAtStartState := FALSE;
	    #fbStateManager(xInFunctionEnabled := #xBoomActive,// stInControl.xBtnBoomActive
	                    xInRunUnivTOn := #xTmpActUnivTon,
	                    nInOutState := #nState);
	END_REGION
	
	
	REGION Stopper Arbeitsplatz
	    
	    #fbWorkSation(xInRunning := #xBoomActive,
	                  xInSwitchPosition2 := #stInControl.xSwitchLoadingPos2,
	                  xInReceivingMT := #xInTrolleyInSep,
	                  xInReleaseMT := #rTrigBtnReleaseMT.Q AND #nState = #cStepIdle,
	                  stInSenStopper1 := #stInSenStopper1,
	                  stInSenStopper2 := #stInSenStopper2,
	                  stInSenJamExit1:= #stInSenJamExit1,
	                  stInSenJamExit2:= #stInSenJamExit2,
	                  tInSenWaitStopper1 := #stInSettings.tSenWaitStopper1,
	                  tInWaitAfterReleaseStopper1 := #stInSettings.tWaitAfterReleaseStopper1,
	                  tInWaitBeforeSwitchStopper2:=#stInSettings.tWaitBeforeSwitchStopper2,
	                  tInTrailingStopper2 := #stInSettings.tTrailingStopper2,
	                  // xOutReadyForReceiving => #stOutStopper.xStopperReceive,
	                  xOutStopper1Clamp => #stOutStopper.xStopper1Clamp,
	                  xOutStopper1 => #stOutStopper.xStopper1Finger,
	                  xOutStopper2 => #stOutStopper.xStopper2,
	                  arInOutCarrierStopper := #arInOutCarrierStopper,
	                  arInOutCarrierExit := #arInOutCarrierExit,
	                  stInOutCarrierExit:= #stInOutCarrierExit);
	    
	       #xOutStopperState := #fbWorkSation.xStopperFull;
	    
	    // #rTrigStopperFull(CLK:=#arInOutCarrierStopper[1].bStatus.%X0 OR #xInTrolleyInSep);
	    // IF #rTrigStopperFull.Q THEN
	    //     #xStopperBusy := TRUE;
	    // //wird in Schrittkette zurückgesetzt
	    // END_IF;
	    
	    // IF #nState = #cStepIdle THEN
	        
	    //     IF #stInControl.xSwitchLoadingPos1 AND NOT #xStopperBusy THEN
	    //         #xStopper1Active := TRUE;
	    //     ELSIF NOT #stInControl.xSwitchLoadingPos1 AND NOT #xStopperBusy THEN
	    //         #xStopper1Active := FALSE;
	    //     END_IF;
	        
	    // END_IF;
	    
	    
	    
	    // #fbStopper1(stInSensor := #stInSenStopper1,
	    //             stInSensorNext:=#stInSenStopper2,
	    //             tInSenWait := #stInSettings.tStopper1SenWait,//T#300ms,
	    //             tInWaitAfterRelease := #stInSettings.tStopper1WaitAfterRelease,//T#4000ms,
	    //             xInActive:=#xStopper1Active,
	    //             xInSwitchReq:=NOT #stInControl.xSwitchLoadingPos1,
	    //             xInRelease := #rTrigBtnReleaseMT.Q AND #nState = #cStepStopper,
	    //             xInReceive := #xTmpMtOnTheWay,
	    //             xOutClamp => #stOutStopper.xStopper1Clamp,
	    //             xOutStopper => #stOutStopper.xStopper1Finger);
	    
	    // #fbStopper2(stInSensor := #stInSenStopper2,
	    //             tInTrailing := #stInSettings.tStopper2Trailing,//T#1500ms,
	    //             xInActive := NOT #xStopper1Active,
	    //             xInRelease := #rTrigBtnReleaseMT.Q  AND #nState = #cStepStopper,
	    //             xInReceive := #xTmpMtOnTheWay,
	    //             xOutStopper => #stOutStopper.xStopper2);
	        
	END_REGION
	REGION Fehler Handling
	    
	    IF (* NOT #xInSafety OR *) NOT #stInBoom.xMotorProtection1 OR NOT #stInFoot.xMotorProtection OR
	        #fbTiltSensor.xOutTeachNec OR #fbDistanceFoot.xOutTeachNec OR
	        (#stInBoom.xLimitForth AND #stInBoom.xLimitBack) OR
	        (#stInBoom.xLimitForthEmergency AND #stInBoom.xLimitBackEmergency) OR
	        (#stInFoot.xLimitUp AND #stInFoot.xLimitDown) OR
	        (#stTmpActuator.xForth AND (#rTrigLimitBack.Q OR #rTrigLimitBackEmergency.Q OR #rTrigLimitForthEmergency.Q)) OR
	        (#stTmpActuator.xBack AND (#rTrigLimitForth.Q OR #rTrigLimitForthEmergency.Q OR #rTrigLimitBackEmergency.Q)) OR
	        (#stTmpActuator.xUp AND #rTrigLimitDown.Q) OR
	        (#stTmpActuator.xDown AND #rTrigLimitUp.Q) THEN
	        #xErrorLogical := TRUE; //Fehler
	    ELSIF #xInSafety AND #stInBoom.xMotorProtection1 AND #stInFoot.xMotorProtection AND #stInControl.xBtnReset THEN
	        #xErrorLogical := FALSE;
	    END_IF;
	    
	END_REGION
	
	REGION Ausgänge
	    
	    REGION Licht / Leuchtmelder
	        // IF #rTrigBtnLight.Q THEN
	        //     #xLight := NOT #xLight;
	        // END_IF;
	        
	        //Licht an-/ausschalten am Arbeitsplatz
	        #xOutLight := #xLight := #stInControl.xSwitchLightOnOff;
	        
	        //LED Boom ready for use
	        #xOutBoomActive := #xBoomActive;
	        
	        #xOutLedMovement :=
	        //Dauerleuchten=Boom wartet und Sep stoppt
	        //Langsam blinken=Nivellieren
	        //Schnell blinken=Boom oder Foot fährt
	        //Sehr schnell blinken = Endlage bereits erreicht
	        (#nState = #cStepWaitForBoom OR #nState = #cStepWaitForFootMan OR #nState = #cStepWaitForFootAutDown OR #nState = #cStepWaitForFootAutUp) OR
	        (#nState = #cStepWait AND "Clock_1Hz") OR
	        ((#nState = #cStepMoveBoom OR #nState = #cStepMoveFootMan OR #nState = #cStepMoveFootAutUp OR #nState = #cStepMoveFootAutDown) AND "Clock_2.5Hz") OR
	        (((#nState = #cStepIdle (* OR #nState = #cStepStopper *)) AND ((#stInControl.xBtnBack AND #stInBoom.xLimitBack) OR (#stInControl.xBtnForth AND #stInBoom.xLimitForth) OR (#stInControl.xSwitchFootDown AND #fbDistanceFoot.xOutGroundContact AND #stInControl.xSwitchFootManual) OR (#stInControl.xSwitchFootUp AND #stInFoot.xLimitUp AND #stInControl.xSwitchFootManual))) AND "Clock_10Hz");
	        
	        #xOutLedBumper := NOT #stInBoom.xBumperOk;
	    END_REGION
	    
	    REGION Antriebe
	        
	        #xOutCarousel := #stTmpActuator.xCarousel;
	        
	        #stOutBoom.xBoomForth2 := #stOutBoom.xBoomForth1 := #stTmpActuator.xForth AND #stInBoom.xBumperOk;
	        #stOutBoom.xBoomBack2 := #stOutBoom.xBoomBack1 := #stTmpActuator.xBack;
	        
	        // #xOutFoot400V := "cTodoFalse"; //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	        #stOutFoot.xFootUp := #stTmpActuator.xUp;
	        #stOutFoot.xFootDown := #stTmpActuator.xDown;
	        IF #xFloorReached THEN //Boden erreicht 
	            #stOutFoot.xFootSpeed1 := TRUE; //langsam
	            // #xOutFootSpeed2 := FALSE; //schnell NIU
	        ELSE //Boden noch nicht erreicht
	            #stOutFoot.xFootSpeed1 := FALSE; //langsam
	            // #xOutFootSpeed2 := TRUE; //schnell NIU
	        END_IF;
	    END_REGION
	    
	    #xOutReady := #xTmpReady;
	    #xOutMoveReq := #xTmpMoveReq;
	    
	    #stOutStopper.xStopperEmpty := NOT #xStopperBusy;
	    #stOutStopper.xStopperReceive := #nState = #cStepIdle AND NOT #fbWorkSation.xOutStationMovement AND #fbWorkSation.xOutReadyForReceiving;
	    
	END_REGION
	
END_FUNCTION_BLOCK

