Aufgabe_10_11

; Aufgabe 10+11
; Lager und Dorne Zählen und Am Ende Ausgeben

;;einfuege kopfsr60

programm Wagen10

; Ein und Ausgänge definieren
Eingang: 1 = Freigabe, 19 = Dorn, 20 = Lager
Ausgang: 5 = Greifer_Auf, 6 = Greifer_Zu

; Benötigte Punkte definieren
Mk_punkt: @start, @hpos
Punkt: Aufnehmen1, Aufnehmen2, Wagen, Hoch, diff_x, diff_y, diff_x2, diff_y2, Sensor_Dorn, Sensor_Lager, Sensor_Hoch

; Aufgabe 11: zusätzliche Punkte
; Punkt: Aufnehmen3, Aufnehmen4, Aufnehmen5, Aufnehmen6
; Ansicht Zwei Paletten mit den zusätzlichen Punkten
;
; Palette 1                                 Palette 2
; ______________________________________________        _________________________________________________
; | Aufnehmen4  *       *       *       |       | Aufnehmen6    *       *       *       |
; |   *     *       *       *       |       |   *           *       *       *       |
; | Aufnehmen1  *       *     Aufnehmen3  |     | Aufnehmen2    *       *     Aufnehmen5  |
; |_____________________________________________|       |_______________________________________________|


; Integer definieren
Ganz: Zaehler_Dorn, Zaehler_Lager

; Hauptprogramm Beginn
Anfang

; Variablen erstmal zu Null
Zaehler_Dorn = 0
Zaehler_Lager = 0

; Aufgabe 10
; Feste Abstände, sowie Hoch Punkte definieren
diff_x = (75,0,0,0)
diff_y = (0,75,0,0)
diff_x2 = (75,0,0,0)
diff_y2 = (0,75,0,0)
Hoch = (0,0,90,0)
Sensor_Hoch = (0,0,50,0)

; Aufgabe 11
; Mittels 3 Punkte Triangulation
; diff_x = (Aufnehmen1 - Aufnehmen3)/3
; diff_y = (Aufnehmen1 - Aufnehmen4)/2
; diff_x2 = (Aufnehmen2 - Aufnehmen5)/3
; diff_y2 = (Aufnehmen2 - Aufnehmen6)/2

; Geschwindigkeit und Sicherheitsposition einstellen
V = 100
v_ptp = 0.2
@hpos = @ipos
@hpos.a_3 = 182
fahre nach @hpos
warte 1

; Roboter referenzieren
REF_PKT(3)
REF_PKT(1,2,4)
warte 1

; Zum Startpunkt fahren und Greifer öffnen
fahre nach @start
warte 1
GRFAUF

; Text das Start Position erreicht wurde
Text_Ausgabe('Startposition erreicht')

; Auf die Freigabe warten
Freigabe_Marke:
        WENN Freigabe = 1
        DANN Anfang         ; Freigabe erteilt

            ; Gebe Meldung aus, dass Freigabe erteilt wurde
            Text_Ausgabe('Freigabe erteilt, führe Aufnahme aus...')

            ; Aufnahme und Ablage in Wagen aus Palette 1
            Palette_Aufnehmen(Aufnehmen1)

            ; Aufnahme und Ablage in wagen aus Palette 2
            Palette_Aufnehmen(Aufnehmen2)

            ; Ausgabe der beförderten Lager und Dorne
            Text_Ausgabe('Es wurden [',Zaehler_Dorn,'] Dorne und [',Zaehler_Lager,'] in den Waagen einsortiert.')
        Ende

        SONST Anfang        ; Freigabe noch nicht erteilt

            ; Gebe Meldung aus, dass Freigabe fehlt
            Text_Ausgabe('Freigabe nicht erteilt, warte...')

            ; Springe zur Freigabe Sprung Marke zurück
            SPRUNG Freigabe_Marke
        Ende

; Fahre nach Beendigung wieder zurück
fahre nach @start
warte 1

; Ausgabe das Programm zu Ende
Ausgabe_Text('Programm wurde zu Ende bearbeitet.')

; Hauptprogramm zu Ende
Programm_Ende

; Generisches Unterprogramm zum Aufnehmen
; Übergabe Variable: Uebergabe -> Von der Palette der Aufnahme Punkt
UP Palette1_Aufnehmen(WERT Punkt: Uebergabe)
GANZ: M, N
Anfang

    ; Zaehler zuruecksetzen
    M = 0
    N = 0

    WDH 3 Mal           ; Pro Y-Achse 3 Werkstücke
        WDH 4 Mal       ; Pro X-Achse 4 Werkstücke

        ; Aufnahme des Werkstücks
        Uebergabe = Uebergabe + M*diff_x + N*diff_y
        fahre nach Uebergabe1 + Hoch
        warte 1
        fahre nach Uebergabe
        GRFZU
        fahre nach Uebergabe + Hoch
        warte 1
        M = M + 1

        ; Sensor Abfrage
        ; Schaue was es ist: Lager ?
        fahre nach Sensor_Lager + Sensor_Hoch
        warte 1
        fahre nach Sensor_Lager
        warte 1
        WENN Lager = 1
        DANN Ablage(0)
        SONST Anfang
            fahre nach Sensor_Lager + Sensor_Hoch
            warte 1

            ; Schaue was es ist: Dorn ?
            fahre nach Sensor_Dorn + Sensor_Hoch
            warte 1
            fahre nach Sensor_Dorn
            warte 1
            WENN Dorn = 1
            DANN Ablage(1)
            SONST fahre nach Sensor_Dorn + Sensor_Hoch
        Ende

        WDH_ENDE

        M = 0
        N = N + 1

    WDH_ENDE
UP_Ende

; Generisches Programm zur Ablage der Werkstuecke
; Übergabe Variable: Ablageort -> 1= Ablage Dorne, 0=Ablage Lager
UP Ablage(WERT BINAER: Ablageort)
Anfang

    WENN Ablageort = 0

    ; Lager ablegen
    DANN Anfang

        ; Ablegen
        fahre ueber Sensor_Lager + Sensor_Hoch nach Wagen + Hoch
        warte 1
        fahre nach Wagen
        GRFAUF
        fahre nach Wagen + Hoch

        ; Ausgabe einer Information
        Ausgabe_Text('Werkstueck in Wagen: Lager abgelegt!')

        ; Zaehler hochzaehlen
        Zaehler_Lager = Zaehler_Dorn + 1
    Ende

    ; Dorne Ablegen
    SONST Anfang

        ; Ablegen
        fahre ueber Sensor_Dorn + Sensor_Hoch nach Wagen + Hoch
        warte 1
        fahre nach Wagen
        GRFAUF
        fahre nach Wagen + Hoch

        ; Ausgabe einer Information
        Ausgabe('Werkstueck in Wagen: Dorn abgelegt!')

        ; Zaehler hochzaehlen
        Zaehler_Dorn = Zaehler_Dorn + 1
    Ende
UP_Ende

; Unterprogramm zur Ablage der Dorne in den Wagen_Dorn
UP Ablage_Dorn
Anfangg

; Unterprogramm zum Text Aufrufen
UP Text_Ausgabe(WERT TEXT: Ausgabe)
Anfang
    SCHREIBE PHG, CLS
    WDH 10 Mal
        SCHREIBE PHG, Ausgabe
    WDH_ENDE
    SCHREIBE PHG, CLS
UP_Ende


; Unterprogramm Greifer Öffnen
UP GRFAUF
Anfang
    warte 1
    Greifer_Auf = 1
    Greifer_Zu = 0
    warte 1
UP_Ende

; Unteprogramm Greifer Schließen
UP GRFZU
Anfang
    warte 1
    Greifer_Auf = 0
    Greifer_Zu = 1
    warte 1
UP_Ende