RAPID

%%%
  VERSION:1
  LANGUAGE:ENGLISH
%%%

MODULE RA_HM
  !***************************************************************************
  !* Aktueller Stand: Keihsler 11:40 17.08.2013                              *
  !* Datei: RA_HM.MOD Getrag Neuenstein OP450                                *
  !***************************************************************************

  !***************************************************************************
  !* Enthaelt die folgenden Funktionen und Prozeduren:          [1405061147] *
  !*                                                                         *
  !* Proc. CheckGreifer     Prufen ob benotigter Greifer gleich akt. Greifer *
  !* Proc. ZentrierungVor   fahre Greifer nach innen                         *
  !* Proc. ZentrierungZur   fahre Greifer nach aussen                        *
  !* Proc. GreiferZu        Diese Prozedur schliesst den Greifer             *
  !* Proc. GreiferAuf       Diese Prozedur offnet den Greifer                *
  !* Proc. WaitGreiferZu    warte auf Greifer geschlossen (mit Timeout)      *
  !* Proc. WaitGreiferAuf   warte auf Greifer offen (mit Timeout)            *
  !* Proc. WaitZentrierungZurwarte auf Greifer aussen (mit Timeout)           *
  !* Proc. WaitZentrierungVorwarte auf Greifer innen (mit Timeout)            *
  !* Proc. GreiferTest      teste alle Greiferfunktionen                     *
  !* Proc. prufen_PBGk      Belegung von Kupplungsgehause in Dichtmittel-Pru *
  !* --> CASE "gka":          Kupplungsgehause Rezept 1                      *
  !*                                                                         *
  !* Proc. prufen_NBGk      Belegung von Kupplungsgehause in Dichtmittel-Nac *
  !* --> CASE "gka":          Kupplungsgehause Rezept 1                      *
  !*                                                                         *
  !* Proc. aufnahme_BKgk    Aufnahme Kupplungsgehause von Band Kupplungsgeha *
  !* Proc. ablage_FEgk      Ablage Kupplungsgehause auf Fugeeinheit          *
  !* Proc. ablage_NTgk      Ablage Kupplungsgehause auf Notablage            *
  !* Proc. auftragen_RaupeG Auftragen von Kleber auf Kupplungsgehause        *
  !* Proc. Typ_ErkennungG   Auslesen der Kupplungsgehause Typ-Nummer         *
  !* Proc. FE_losen         Losen der Spannvorrichtung Fugeeinheit           *
  !* Proc. FE_spannen       Schliessen der Spannvorrichtung Fugeeinheit      *
  !* Proc. Auftrag10        Kupplungsgehause von Band Kupplungsgehause Loop  *
  !* Proc. Auftrag200       Roboter zu Service-Position fahren               *
  !* Proc. chkStartFrei     Diese Prozedur pruft die Startbedingungen        *
  !* Proc. initialisiere    Diese Prozedur initialisiert Ausgange sowie glob *
  !* Proc. bereitschaft     Diese Prozedur testet den Greifer und faehrt in  *
  !* Proc. grundstellung    Diese Prozedur testet ob Greifer leer ist und fa *
  !* Proc. main             Diese Prozedur ist die Hauptprozedur und steuert *
  !* Proc. a__Homing        $internal_use                                    *
  !* Proc. a__CheckRevCounter$internal_use                                    *
  !***************************************************************************

  !***************************************************************************
  !* Wartezeiten:                                                            *
  !***************************************************************************
  CONST num ntWartezeit:=0.5;
  CONST num ntGreifer:=1;
  !***************************************************************************
  !* Globale boolsche Variablen:                                             *
  !***************************************************************************
  VAR bool bGreiferOK:=FALSE;
  VAR bool bSPSDatenOK:=FALSE;
  VAR bool bDDDatenOK:=FALSE;
  VAR bool bPBDatenOK:=FALSE;
  VAR bool bNBDatenOK:=FALSE;
  VAR bool bHome:=FALSE;
  !***************************************************************************
  !* Globale pos Variablen:                                                  *
  !***************************************************************************
  !***************************************************************************
  !* Globale numerische Variablen:                                           *
  !***************************************************************************
  VAR num nAuftrag:=0;
  !VAR clock clCycle;
  VAR clock clTimeOut;
  VAR clock clProzessZeit;

  !***************************************************************************
  !* Prozedur CheckGreifer                                                   *
  !*                                                                         *
  !* Prufen ob benotigter Greifer gleich akt. Greifer, ggf. Greiferwechel    *
  !***************************************************************************
  !* check if needed gripper equals current gripper, change gripper if nec.  *
  !***************************************************************************
  PROC CheckGreifer(
    string sCheckGreifer)

    TEST sCheckGreifer
    CASE "1":
      IF sStartPos="" THEN
        message fmSystem;
      ELSE
        IF StrPart(sStartPos,StrLen(sStartPos),1)="1" THEN
          !alles ok
        ELSE
          MoveToPos("Bereit"+StrPart(sStartPos,StrLen(sStartPos),1));
          MoveToPos("Bereit1");
        ENDIF
      ENDIF
    CASE "2":
      IF sStartPos="" THEN
        message fmSystem;
      ELSE
        IF StrPart(sStartPos,StrLen(sStartPos),1)="2" THEN
          !alles ok
        ELSE
          MoveToPos("Bereit"+StrPart(sStartPos,StrLen(sStartPos),1));
          MoveToPos("Bereit2");
        ENDIF
      ENDIF
    CASE "V":
      IF sStartPos="" THEN
        message fmSystem;
      ELSE
        IF StrPart(sStartPos,StrLen(sStartPos),1)="V" THEN
          !alles ok
        ELSE
          MoveToPos("Bereit"+StrPart(sStartPos,StrLen(sStartPos),1));
          MoveToPos("BereitV");
        ENDIF
      ENDIF
    default:
      message fmSystem;
      !stop;
      exit;
    ENDTEST
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur ZentrierungVor                                                 *
  !*                                                                         *
  !* fahre Greifer nach innen                                                *
  !***************************************************************************
  !* move gripper inbound                                                    *
  !***************************************************************************
  PROC ZentrierungVor()
    Reset doZentrierung13Zur;
    Set doZentrierung13Vor;
    CheckSignals fmZentrierNiVor,diZentrierung13Zur,0\fmMessage2:=fmZentrierNiVor\diSign2:=diZentrierung13Vor\iCond2:=1;
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur ZentrierungZur                                                 *
  !*                                                                         *
  !* fahre Greifer nach aussen                                               *
  !***************************************************************************
  !* move gripper outbound                                                   *
  !***************************************************************************
  PROC ZentrierungZur()
    Reset doZentrierung13Vor;
    Set doZentrierung13Zur;
    CheckSignals fmZentrierNiZur,diZentrierung13Zur,1\fmMessage2:=fmZentrierNiZur\diSign2:=diZentrierung13Vor\iCond2:=0;
    GripLoad load0;
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur GreiferZu                                                      *
  !*                                                                         *
  !* Diese Prozedur schliesst den Greifer                                    *
  !***************************************************************************
  !* close gripper                                                           *
  !***************************************************************************
  PROC GreiferZu()
    Reset doSpanner13Auf;
    Set doSpanner13Zu;
    CheckSignals fmGreiferNiZu,diSpanner1Auf,0\fmMessage2:=fmGreiferNiZu\diSign2:=diSpanner1Zu\iCond2:=1;
    CheckSignals fmGreiferNiZu,diSpanner2Auf,0\fmMessage2:=fmGreiferNiZu\diSign2:=diSpanner2Zu\iCond2:=1;
    !CheckSignals fmGreiferNiZu,diSpanner3Auf,0\fmMessage2:=fmGreiferNiZu\diSign2:=diSpanner3Zu\iCond2:=1;
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur GreiferAuf                                                     *
  !*                                                                         *
  !* Diese Prozedur offnet den Greifer                                       *
  !***************************************************************************
  !* open gripper                                                            *
  !***************************************************************************
  PROC GreiferAuf()
    Reset doSpanner13Zu;
    Set doSpanner13Auf;
    CheckSignals fmGreiferNiAuf,diSpanner1Auf,1\fmMessage2:=fmGreiferNiAuf\diSign2:=diSpanner1Zu\iCond2:=0;
    CheckSignals fmGreiferNiAuf,diSpanner2Auf,1\fmMessage2:=fmGreiferNiAuf\diSign2:=diSpanner2Zu\iCond2:=0;
    CheckSignals fmGreiferNiAuf,diSpanner3Auf,1\fmMessage2:=fmGreiferNiAuf\diSign2:=diSpanner3Zu\iCond2:=0;
    GripLoad load0;
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur WaitGreiferZu                                                  *
  !*                                                                         *
  !* warte auf Greifer geschlossen (mit Timeout)                             *
  !***************************************************************************
  !* wait for gripper closed w/ timeout                                      *
  !***************************************************************************
  PROC WaitGreiferZu()

    ClkReset clTimeOut;
    ClkStart clTimeOut;

    WHILE (diSpanner1Zu=0 OR diSpanner2Zu=0 OR diSpanner3Zu=0) AND ClkRead(clTimeOut)<ntGreifer DO
      WaitTime 0.01;
    ENDWHILE

    ClkStop clTimeOut;
  ENDPROC

  !***************************************************************************
  !* Prozedur WaitGreiferAuf                                                 *
  !*                                                                         *
  !* warte auf Greifer offen (mit Timeout)                                   *
  !***************************************************************************
  !* wait for gripper opened w/ timeout                                      *
  !***************************************************************************
  PROC WaitGreiferAuf()

    ClkReset clTimeOut;
    ClkStart clTimeOut;

    WHILE (diSpanner1Auf=0 OR diSpanner2Auf=0 OR diSpanner3Auf=0) AND ClkRead(clTimeOut)<ntGreifer DO
      WaitTime 0.01;
    ENDWHILE

    ClkStop clTimeOut;
  ENDPROC

  !***************************************************************************
  !* Prozedur WaitZentrierungVor                                             *
  !*                                                                         *
  !* warte auf Greifer innen (mit Timeout)                                   *
  !***************************************************************************
  !* wait for gripper moved inbound w/ timeout                               *
  !***************************************************************************
  PROC WaitZentrierungVor()

    ClkReset clTimeOut;
    ClkStart clTimeOut;

    WHILE (diZentrierung1Vor=0 OR diZentrierung2Vor=0 OR diZentrierung3Vor=0) AND ClkRead(clTimeOut)<ntGreifer DO
      WaitTime 0.01;
    ENDWHILE

    ClkStop clTimeOut;
  ENDPROC

  !***************************************************************************
  !* Prozedur GreiferTest                                                    *
  !*                                                                         *
  !* teste alle Greiferfunktionen                                            *
  !***************************************************************************
  !* test all gripper functions                                              *
  !***************************************************************************
  PROC GreiferTest()

    GreiferAuf;
    GreiferZu;
    GreiferAuf;
    ZentrierungZur;
    ZentrierungVor;
    ZentrierungZur;

    bGreiferOK:=TRUE;
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur prufen_PBGk                                                    *
  !*                                                                         *
  !* Belegung von Kupplungsgehause in Dichtmittel-Pruf-Bereich prufen        *
  !***************************************************************************
  !* check clutch case position in Sealing Manual Check Area                 *
  !***************************************************************************
  PROC prufen_PBGk(
    string Typ)

    VAR bool bPBbereit:=FALSE;
    VAR bool bPBfertig:=FALSE;

    IF diPBactive=1 THEN
      IF waitSignalChange(fmFrPB_,fmFrPBQ,diFrPB,doPFPB) THEN
        Reset doPFPB;
        TEST Typ
        CASE "gka":
          mvVPB1_CkPBgka1;
          WHILE bPBbereit=FALSE DO
            Reset doPBstart;
            IF waitSignalAND(fmPBbereit,diPBendOK,0,diPBendNOK,0\NOGST) THEN
              bPBbereit:=TRUE;
            ENDIF
          ENDWHILE
          Set doPBstart;
          WHILE bPBfertig=FALSE DO
            IF waitSignalXOR(fmPBfertig,diPBendOK,1,diPBendNOK,1) THEN
              bPBfertig:=TRUE;
              IF diPBendOK=1 AND diPBendNOK=0 THEN
                bPBDatenOK:=TRUE;
              ELSE
                bPBDatenOK:=FALSE;
              ENDIF
              !IF diReqHome=1 THEN
              !  bHome:=TRUE;
              !ENDIF
            ELSE
              bPBfertig:=TRUE;
              bHome:=TRUE;
            ENDIF
          ENDWHILE
          Reset doPBstart;
          mvCkPBgka1_VPB1;
          Set doPFPB;
          IF bHome THEN
            !message fmSystem;
            !stop;
            !exit;
            mvVPB1_VNT1;
            ablage_NTgk Typ;
          ENDIF
        default:
          message fmSystem;
          stop;
          exit;
        ENDTEST
      ELSE
        bHome:=TRUE;
        mvVPB1_VNT1;
        ablage_NTgk Typ;
        mvFNT1_Bereit1;
      ENDIF
    ELSE
      !PB abgewahlt => alles IO
      bPBDatenOK:=TRUE;
    ENDIF
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur prufen_NBGk                                                    *
  !*                                                                         *
  !* Belegung von Kupplungsgehause in Dichtmittel-Nacharbeit-Bereich prufen  *
  !***************************************************************************
  !* check clutch case position in Sealing Rework Area                       *
  !***************************************************************************
  PROC prufen_NBGk(
    string Typ)

    VAR bool bNBbereit:=FALSE;
    VAR bool bNBfertig:=FALSE;

    IF diNBactive=1 THEN
      IF waitSignalChange(fmFrNB_,fmFrNBQ,diFrNB,doPFNB) THEN
        Reset doPFNB;
        TEST Typ
        CASE "gka":
          mvVNB1_CkNBgka1;
          WHILE bNBbereit=FALSE DO
            Reset doNBstart;
            IF waitSignalAND(fmNBbereit,diNBendOK,0,diNBendNOK,0\NOGST) THEN
              bNBbereit:=TRUE;
            ENDIF
          ENDWHILE
          Set doNBstart;
          WHILE bNBfertig=FALSE DO
            IF waitSignalXOR(fmNBfertig,diNBendOK,1,diNBendNOK,1) THEN
              bNBfertig:=TRUE;
              IF diNBendOK=1 AND diNBendNOK=0 THEN
                bNBDatenOK:=TRUE;
              ELSE
                bNBDatenOK:=FALSE;
              ENDIF
              !IF diReqHome=1 THEN
              !  bHome:=TRUE;
              !ENDIF
            ELSE
              bNBfertig:=TRUE;
              bHome:=TRUE;
            ENDIF
          ENDWHILE
          Reset doNBstart;
          mvCkNBgka1_VNB1;
          Set doPFNB;
          IF bHome THEN
            !message fmSystem;
            !stop;
            !exit;
            mvVNB1_VNT1;
            ablage_NTgk Typ;
          ENDIF
        default:
          message fmSystem;
          stop;
          exit;
        ENDTEST
      ELSE
        bHome:=TRUE;
        mvVNB1_VNT1;
        ablage_NTgk Typ;
        mvFNT1_Bereit1;
      ENDIF
    ELSE
      !NB abgewahlt => alles IO
      bNBDatenOK:=TRUE;
    ENDIF
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur aufnahme_BKgk                                                  *
  !*                                                                         *
  !* Aufnahme Kupplungsgehause von Band Kupplungsgehause Loop                *
  !***************************************************************************
  !* take clutch case from Clutch-Housing Loop                               *
  !***************************************************************************
  PROC aufnahme_BKgk(
    string Typ)

    VAR bool bAufnahme:=FALSE;
    VAR bool bVorhanden:=FALSE;

    IF waitSignalChange(fmFrBK_,fmFrBKQ,diFrBKE,doPFBK)THEN
      GreiferAuf;
      ZentrierungZur;
      Reset doPFBK;
      mvFBK1_AfBKgka1;
      Reset doZentrierung13Zur;
      Set doZentrierung13Vor;
      WaitZentrierungVor;
      WHILE bVorhanden=FALSE AND bHome=FALSE DO
        IF diZentrierung13Vor=1 AND diZentrierung13Zur=0 AND diTeilImGreifer=1 THEN
          bVorhanden:=TRUE;
          !GripLoad loGga;
          WaitTime ntWartezeit;
        ELSE
          message fmBelegung;
          IF diReqHome=1 THEN
            ZentrierungZur;
            bHome:=TRUE;
          ENDIF
        ENDIF
      ENDWHILE
      bVorhanden:=FALSE;
      IF bHome=FALSE THEN
        Reset doSpanner13Auf;
        Set doSpanner13Zu;
        WaitGreiferZu;
        WHILE bVorhanden=FALSE AND bHome=FALSE DO
          IF diSpanner13Zu=1 AND diSpanner13Auf=0 AND diTeilImGreifer=1 THEN
            bVorhanden:=TRUE;
            GripLoad loGka;
            WaitTime ntWartezeit;
          ELSE
            message fmBelegung;
            IF diReqHome=1 THEN
              GreiferAuf;
              ZentrierungZur;
              bHome:=TRUE;
            ENDIF
          ENDIF
        ENDWHILE
      ENDIF      
      IF bHome=FALSE THEN
        mvAfBKgka1_VBK1;
        WHILE bAufnahme=FALSE AND bHome=FALSE DO
          IF diSpanner13Zu=1 AND diSpanner13Auf=0 AND diTeilImGreifer=1 THEN
            bAufnahme:=TRUE;
            Teil_Entnommen 1;
          ELSE
            message fmAufnahme;
            IF diReqHome=1 AND diTeilImGreifer=0 THEN
              GreiferAuf;
              bHome:=TRUE;
              mvFBK1_Bereit1;
            ENDIF
          ENDIF
        ENDWHILE
        Set doPFBK;
      ELSE
        mvAfBKgka1_FBK1;
        Set doPFBK;
        mvFBK1_Bereit1;
      ENDIF
    ELSE
      bHome:=TRUE;
      mvFBK1_Bereit1;
    ENDIF
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur ablage_FEgk                                                    *
  !*                                                                         *
  !* Ablage Kupplungsgehause auf Fugeeinheit                                 *
  !***************************************************************************
  !* put clutch case in Assembly-Unit                                        *
  !***************************************************************************
  PROC ablage_FEgk(
    string Typ)

    VAR bool bAblage:=FALSE;

    IF waitSignalChange(fmFrFE_,fmFrFEQ,diFrFEB,doPFFE) THEN
      Reset doPFFE;
      FE_losen;
      mvVFE1_AbFEgka1;
      MoveL Offs(pAbFEgka1,0,0,10),v20,fine,tGreifer1\WObj:=wobjFE;
      FE_spannen;
      MoveL pAbFEgka1,v20,fine,tGreifer1\WObj:=wobjFE;
      WHILE bAblage=FALSE DO
        Reset doSpanner13Zu;
        Set doSpanner13Auf;
        WaitGreiferAuf;
        IF diSpanner13Auf=1 AND diSpanner13Zu=0 THEN
          GripLoad load0;
          ZentrierungZur;
          mvAbFEgka1_FFE1;
          WHILE diTeilImGreifer=1 DO
            message fmAblage;
          ENDWHILE
          bAblage:=TRUE;
          Teil_Beladen 2;
          Set doPFFE;
        ELSE
          message fmAblage;
        ENDIF
      ENDWHILE
    ELSE
      bHome:=TRUE;
      mvVFE1_VNT1;
      ablage_NTgk Typ;
      mvFNT1_Bereit1;
    ENDIF
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur ablage_NTgk                                                    *
  !*                                                                         *
  !* Ablage Kupplungsgehause auf Notablage                                   *
  !***************************************************************************
  !* put clutch case in Emergency deposit                                    *
  !***************************************************************************
  PROC ablage_NTgk(
    string Typ)

    VAR bool bAblage:=FALSE;

    IF waitSignalChange(fmFrNT_,fmFrNTQ,diFrNT,doPFNT\NOGST) THEN
      Reset doPFNT;
      mvVNT1_AbNTgka1;
      WHILE bAblage=FALSE  DO
        Reset doSpanner13Zu;
        Set doSpanner13Auf;
        WaitGreiferAuf;
        IF diSpanner13Auf=1 AND diSpanner13Zu=0 THEN
          GripLoad load0;
          ZentrierungZur;
          mvAbNTgka1_FNT1;
          WHILE diTeilImGreifer=1 DO
            message fmAblage;
          ENDWHILE
          bAblage:=TRUE;
          !Teil_Beladen 3;
          Set doPFNT;
        ELSE
          message fmAblage;
        ENDIF
      ENDWHILE
    ELSE
      !Grundstellung vor Notablage nicht moglich
      message fmSystem;
      !stop;
      exit;
    ENDIF
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur auftragen_RaupeG                                               *
  !*                                                                         *
  !* Auftragen von Kleber auf Kupplungsgehause                               *
  !***************************************************************************
  !* Glueing of clutch case                                                  *
  !***************************************************************************
  PROC auftragen_RaupeG(
    string Typ)

    VAR bool bDDbereit:=FALSE;
    VAR bool bDDfertig:=FALSE;

    IF waitSignalChange(fmFrDD_,fmFrDDQ,diFrDD,doPFDD) THEN
      Reset doPFDD;

      WHILE bDDbereit=FALSE DO
        Reset doDDstart;
        IF waitSignalAND(fmDDbereit,diDDendOK,0,diDDendNOK,0\NOGST) THEN
          bDDbereit:=TRUE;
        ENDIF
      ENDWHILE

      !Breite von Objekt
      AccSet 80, 50;

      MoveL Offs(pKaDDgka0001,0,0,20),vMittel,z5,tDD\WObj:=wobjDD;

      !Fahre in Startposition
      MoveL pKaDDgka0001,vLangsam,fine,tDD\WObj:=wobjDD;
      WaitTime 0.1;

      !Starte Duse
      IF bDebugNoDichtmittel=FALSE THEN
        ! This Trigg Equip command starts the Loctite sealing 
        ! at a predefined "time and distance" relative to
        ! position "pKaDDgka1001".
        !TriggData    Distance in mm [\Start] EquipLag in seconds [\DOp] | [\GOp]| [\AOp] | [\ProcID] SetValue | SetDvalue [\Inhib]
        TriggEquip SealerOn, SealerOnDist, SealerOnTime\DOp:=doDDstart, 1;
        !Start Trigger Point
        TriggL pKaDDgka1001, vRaupe, SealerOn, zDD, tDD\WObj:=wobjDD; 
      ELSE
        MoveL pKaDDgka1001, vRaupe, zDD, tDD\WObj:=wobjDD;
      ENDIF

      moveraupe_gka;
      
      IF bDebugNoDichtmittel=FALSE THEN
        ! This Trigg Equip command starts the Loctite sealing 
        ! at a predefined "time and distance" relative to
        ! position "pKaDDgka1001".
        !TriggData    Distance in mm [\Start] EquipLag in seconds [\DOp] | [\GOp]| [\AOp] | [\ProcID] SetValue | SetDvalue [\Inhib]
        TriggEquip SealerOff, SealerOffDist, SealerOffTime\DOp:=doDDstart, 0;
        !Start Trigger Point
        TriggL pKaDDgka6001, vRaupe, SealerOff, zDD, tDD\WObj:=wobjDD; 
      ELSE
        MoveL pKaDDgka6001, vRaupe, zDD, tDD\WObj:=wobjDD;
      ENDIF
      
      MoveL Offs(pKaDDgka6001,0,0,20),vlangsam,fine,tDD\WObj:=wobjDD;
      Reset doDDstart;
      
      WaitTime 1;

      WHILE bDDfertig=FALSE DO
        IF waitSignalXOR(fmDDfertig,diDDendOK,1,diDDendNOK,1) THEN
          bDDfertig:=TRUE;
          IF diDDendOK=1 AND diDDendNOK=0 THEN
            bDDDatenOK:=TRUE;
          ELSE
            bDDDatenOK:=FALSE;
          ENDIF
          !IF diReqHome=1 THEN
          !  bHome:=TRUE;
          !ENDIF
        ELSE
          bDDfertig:=TRUE;
          bHome:=TRUE;
        ENDIF
      ENDWHILE

      MoveL pVDD1, vSchnell, fine, tGreifer1\WObj:=wobj0;

      Set doPFDD;
      
      IF bDebugNoDichtmittel=TRUE THEN
        MoveL pVDD1, vSchnell, fine, tGreifer1\WObj:=wobj0;
      ENDIF

      IF bHome THEN
        !message fmSystem;
        !stop;
        !exit;
        mvVDD1_VNT1;
        ablage_NTgk Typ;
      ENDIF
    ELSE
      bHome:=TRUE;
      mvVDD1_VNT1;
      ablage_NTgk Typ;
      mvFNT1_Bereit1;
    ENDIF
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur Typ_ErkennungG                                                 *
  !*                                                                         *
  !* Auslesen der Kupplungsgehause Typ-Nummer                                *
  !***************************************************************************
  !* read clutch case type id                                                *
  !***************************************************************************
  PROC Typ_ErkennungG()
    VAR string sTyp1;
    VAR string sTyp2;
    VAR string sTypX;

    sTyp1:=ByteToStr(giType1\Char);
    sTyp2:=ByteToStr(giType2\Char);
    sTypX:=sTyp1+sTyp2;
    TPWrite sTypX;

    TEST sTypX
    CASE "01":
      sTyp:="gka";
    CASE "02":
      !sTyp:="gkb";
    CASE "03":
      !sTyp:="gkc";
    default:
      sTyp:="";
    ENDTEST
    sTyp:="gka";

    IF sTyp="gka" THEN
      !Daten OK
      bSPSDatenOK:=TRUE;
      TPWrite "TypG: "+sTyp;
    ELSE
      bSPSDatenOK:=FALSE;
      message fmSPSDatenNOK;
    ENDIF

    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur FE_losen                                                       *
  !*                                                                         *
  !* Losen der Spannvorrichtung Fugeeinheit                                  *
  !***************************************************************************
  !* unclamp Assembly-Unit                                                   *
  !***************************************************************************
  PROC FE_losen()
    VAR bool bGelost:=FALSE;

    !losen von Spannvorrichtung
    Reset doFESVzu;
    Set doFESVauf;
    WHILE bGelost=FALSE AND bHome=FALSE DO
      IF waitSignalAND(fmFENiGelost,diFESVzu,0,diFESVauf,1\NOGST)THEN
        bGelost:=TRUE;
      ENDIF
    ENDWHILE
    Reset doFESVauf;
    TPErase;
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur FE_spannen                                                     *
  !*                                                                         *
  !* Schliessen der Spannvorrichtung Fugeeinheit                             *
  !***************************************************************************
  !* clamp Assembly-Unit                                                     *
  !***************************************************************************
  PROC FE_spannen()
    VAR bool bGespannt:=FALSE;

    !spannen von Spannvorrichtung
    Reset doFESVauf;
    Set doFESVzu;
    WHILE bGespannt=FALSE AND bHome=FALSE DO
      IF waitSignalAND(fmFENiGespannt,diFESVzu,1,diFESVauf,0\NOGST)THEN
        bGespannt:=TRUE;
      ENDIF
    ENDWHILE
    Reset doFESVzu;
    TPErase;
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur Auftrag10                                                      *
  !*                                                                         *
  !* Kupplungsgehause von Band Kupplungsgehause Loop uber Dichtmittel-Duse nach Fugeeinheit
  !***************************************************************************
  !* clutch case from Clutch-Housing Loop via Seal-Feeder-Nozzle to Assembly-Unit
  !***************************************************************************
  PROC Auftrag10()

    Typ_ErkennungG;
    IF bSPSDatenOK AND bGreiferOK THEN
      CheckGreifer "1";
      Set doOrderParaOk;
      MoveToPos "FBK1";
      Reset doCurOrderStart;
      aufnahme_BKgk sTyp;
      IF NOT bHome THEN
        mvVBK1_VDD1;
        auftragen_RaupeG sTyp;
        IF NOT bHome THEN
          IF bDDDatenOK=TRUE THEN
            mvVDD1_VFE1;
            ablage_FEgk sTyp;
            IF NOT bHome THEN
              mvFFE1_FBK1;
            ENDIF
          ELSE
            mvVDD1_VPB1;
            prufen_PBGk sTyp;
            IF NOT bHome THEN
              IF bPBDatenOK=TRUE THEN
                mvVPB1_VFE1;
                ablage_FEgk sTyp;
                IF NOT bHome THEN
                  mvFFE1_FBK1;
                ENDIF
              ELSE
                mvVPB1_VNB1;
                prufen_NBGk sTyp;
                IF NOT bHome THEN
                  IF bNBDatenOK=TRUE OR bNBDatenOK=FALSE THEN
                    mvVNB1_VFE1;
                    ablage_FEgk sTyp;
                    IF NOT bHome THEN
                      mvFFE1_FBK1;
                    ENDIF
                  ENDIF
                ENDIF
              ENDIF
            ENDIF
          ENDIF
        ENDIF
      ENDIF
    ELSE
      !SPSDatenNOK or GreiferNOK
      Set doOrderEndNok;
    ENDIF
    Set doOrderEnd;
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur auftrag200                                                     *
  !*                                                                         *
  !* Roboter zu Service-Position fahren                                      *
  !***************************************************************************
  !* robot to service position                                               *
  !***************************************************************************
  PROC Auftrag200()

    sTyp:="";
    IF bGreiferOK THEN
      CheckGreifer "1";
      Set doOrderParaOk;
      MoveToPos "FSB1";
      Reset doCurOrderStart;
      mvFSB1_CeSB1;
      mvCeSB1_FSB1;
      WaitTime 1;
    ELSE
      !SPSDatenNOK or GreiferNOK
      Set doOrderEndNok;
    ENDIF
    Set doOrderEnd;
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur auftrag202                                                     *
  !*                                                                         *
  !* Roboter Testablauf fur Schlauchpaketfuhrung                             *
  !***************************************************************************
  !* robot test-sequence dress-pack-stress-test                              *
  !***************************************************************************
  PROC Auftrag202()

    sTyp:="";
    IF bGreiferOK THEN
      !Stop;
      CheckGreifer "1";
      Set doOrderParaOk;
      ClkStop clProzessZeit;
      ClkReset clProzessZeit;
      ClkStart clProzessZeit;

      MoveToPos "FBK1";
      Reset doCurOrderStart;
      
      mvFBK1_AfBKgka1;
      ZentrierungVor;
      GreiferZu;
      mvAfBKgka1_VBK1;
      mvVBK1_VDD1;
      
      !Dichtmittel
      AccSet 80, 30;
      MoveL Offs(pKaDDgka0001,0,0,20),vMittel,fine,tDD\WObj:=wobjDD;
      MoveL pKaDDgka0001,vLangsam,fine,tDD\WObj:=wobjDD;
      WaitTime 0.1;
      MoveL pKaDDgka1001,vRaupe,zDD,tDD\WObj:=wobjDD;

      moveraupe_gka;

      MoveL pKaDDgka6001,vRaupe,zDD,tDD\WObj:=wobjDD;
      MoveL Offs(pKaDDgka6001,0,0,20),vlangsam,fine,tDD\WObj:=wobjDD;
      WaitTime 1;
      MoveL pVDD1,vSchnell,fine,tGreifer1\WObj:=wobj0;
      !/!Dichtmittel
      
      mvVDD1_VFE1;

      mvVFE1_AbFEgka1;
      MoveL Offs(pAbFEgka1,0,0,10),v20,fine,tGreifer1\WObj:=wobjFE;
      WaitTime 0.5;
      MoveL pAbFEgka1,v20,fine,tGreifer1\WObj:=wobjFE;

      GreiferAuf;
      ZentrierungZur;
      mvAbFEgka1_FFE1;
      
      ClkStop clProzessZeit;
      TPWrite ""\Num:=ClkRead(clProzessZeit);
      Stop;
    ELSE
      !SPSDatenNOK or GreiferNOK
      Set doOrderEndNok;
    ENDIF
    Set doOrderEnd;
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur chkStartFrei                                                   *
  !*                                                                         *
  !* Diese Prozedur pruft die Startbedingungen                               *
  !***************************************************************************
  !* check of startconditions                                                *
  !***************************************************************************
  PROC chkStartFrei()
    WHILE diRobInHome=0 DO
      message fmIRBniGST;
    ENDWHILE
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur initialisiere                                                  *
  !*                                                                         *
  !* Diese Prozedur initialisiert Ausgange sowie globale Variablen           *
  !***************************************************************************
  !* init of outputs and global variables                                    *
  !***************************************************************************
  PROC initialisiere()
    !* Standardsignale:                    *
    Reset doOrderMirrored;
    Set doOrderEnd;
    SetGO goOrder,0;
    SetGO goMessage,0;
    SetGO goError, 0;
    setgo goCurOrder,0;
    nAuftrag:=0;
    bHome:=FALSE;
    bGreiferOK:=FALSE;
    sStartPos:="";
    Reset doCurOrderStart;
    Reset doOrderParaOk;
    !* Stationsabhaengige Signale:         *
    Set doPneumatikEin;
    Set doPFBK;
    Set doPFDD;
    Set doPFFE;
    Set doPFNB;
    Set doPFNT;
    Set doPFPB;
    Set doPFSB;
    Reset doPartUnload;
    Reset doPartLoaded;
    Reset doDDstart;
    Reset doNBstart;
    Reset doPBstart;
    Reset doFESVzu;
    Reset doFESVauf;
    !* Interrupts verbinden                *
    IDelete irAir_Error;
    CONNECT irAir_Error WITH Air_Error;
    ISignalDI diPneumatikOK,0,irAir_Error;
    IEnable;
    !VelSet 100, 5000;
    VelSet 100, 1000;
    !AccSet 80, 80;

    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur bereitschaft                                                   *
  !*                                                                         *
  !* Diese Prozedur testet den Greifer und faehrt in die Bereitschaftspos.   *
  !***************************************************************************
  !* check gripper and move to Start-Position                                *
  !***************************************************************************
  PROC bereitschaft()
    !VAR bool bBereit:=FALSE;

    WHILE diPneumatikOK=0 DO
      message fmPneuDrNiOK;
      WaitTime 1;
    ENDWHILE

    Reset doRobInHome;
    mvHome1_Bereit1;

    WHILE diTeilImGreifer=1 DO
      message fmGreiferNiLeer;
      ! Greifer nicht leer, Teile von Hand entnehmen !!!!!!
    ENDWHILE

    IF NOT bGreiferOK THEN
      GreiferTest;
    ENDIF

    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur grundstellung                                                  *
  !*                                                                         *
  !* Diese Prozedur testet ob Greifer leer ist und fahrt in Grundstellung.   *
  !***************************************************************************
  !* check gripper and move robot to homeposition                            *
  !***************************************************************************
  PROC grundstellung()
    WHILE diTeilImGreifer=1 DO
      message fmGreiferNiLeer;
      ! Greifer nicht leer, Teile von Hand entnehmen !!!!!!
    ENDWHILE
    !CheckGreifer "1";
    mvBereit1_Home1;
    TPErase;
    WaitTime\InPos,2;
    IF DInput(diRobInHome)=1 THEN
      Set doRobInHome;
    ELSE
      message fmIRBniGST;
    ENDIF
    initialisiere;
    exit;
    RETURN;
  ENDPROC

  !***************************************************************************
  !* Prozedur main                                                           *
  !*                                                                         *
  !* Diese Prozedur ist die Hauptprozedur und steuert den gesamten Ablauf.   *
  !***************************************************************************
  !* This procedure is the main-procedure and controls the complete sequence.
  !***************************************************************************
  PROC main()
  
    chkStartFrei;
    initialisiere;
    bereitschaft;
    MoveToPos "Bereit1";
    WHILE bHome=FALSE AND bGreiferOK=TRUE DO
      setgo goCurOrder,0;
      Reset doOrderParaOk;

      nAuftrag:=leseAuftrag();

      ! Rucksetze NIO-Bit
      Reset doOrderEndNok;
      VelSet 100, 1000;

      ! setze RobDC1
      setgo goCurOrder,nAuftrag;
      Set doCurOrderStart;

      TEST nAuftrag
      CASE 0: ! Grundstellung ist angefordert ! | Homing requested !
        bHome:=TRUE;
      CASE 10: ! Dichtmittelauftrag Kupplungsgehause
        Auftrag10;
      CASE 200: ! anfahren Service-Position
        Auftrag200;
      CASE 202: ! Testablauf fur Schlauchpaketfuhrung
        VelSet 100, 5000;
        Auftrag202;
      default:
        message fmAuftragA;
      ENDTEST
      
    ENDWHILE
    CheckGreifer "1";
    MoveToPos "Bereit1";
    grundstellung;
    exit;
  ENDPROC

  !***************************************************************************
  !* Prozedur a__Homing                                                      *
  PROC a__Homing() !$internal_use
    grundstellung;
  ENDPROC

  !***************************************************************************
  !* Prozedur a__CheckRevCounter                                             *
  PROC a__CheckRevCounter() !$internal_use
    
    !fahre Achse 4,5,6 in Referenzposition
    MoveAbsJ [[-38,-48,48,5,5,5],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]\NoEOffs, vLangsam, fine, tGreifer1;
    MoveAbsJ [[-38,-48,48,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]\NoEOffs, vLangsam, fine, tGreifer1;
    stop;
    !fahre Achse 1,2,3,4,6 in Referenzposition
    MoveAbsJ [[-5,-5,5,0,100,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]\NoEOffs, vLangsam, fine, tGreifer1;
    MoveAbsJ [[0,0,0,0,100,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]\NoEOffs, vLangsam, fine, tGreifer1;
    exit;
  ENDPROC
ENDMODULE