Search L mehrere Flanken benötigt

CONST robtarget rtRobtarget0 := [[0,0,0],[1,0,0,0],[1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

PERS robtarget rtTrapPos1 := [[0,0,0],[1,0,0,0],[1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS robtarget rtTrapPos2 := [[0,0,0],[1,0,0,0],[1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

VAR intnum inSearchLMS1;
VAR intnum inSearchLMS2;

!****************************************************************

FUNC byte SearchLMS(VAR signaldi pSignal1, \switch fallingFlank1 | switch anyFlank1,
                    VAR signaldi pSignal2, \switch fallingFlank2 | switch anyFlank2,
                    robtarget pEndOfSearch, INOUT robtarget pFound1, INOUT robtarget pFound2,
                    speeddata pSpeed, PERS tooldata pTool, \PERS wobjdata pWObj)
    ! Führt eine Suche wie SearchL aus, kann aber auf 2 Signale
    ! reagieren. Startposition der Suche ist die aktuelle Position,
    ! die Endposition ist anzugeben.
    ! Standard ist es auf steigende Flanke zu reagieren. Das kann
    ! per switch aber auf fallende Flanke oder einen beliebigen
    ! Flankenwechsel (1 -> 0, 0 -> 1) geändert werden.
    ! Entweder sucht man auf 2 unterschiedliche Signale oder auf
    ! 2 Flanken des gleichen Signals:
    !
    ! Auf die steigenden Flanken zweier Signale (z.B. Positionsmessung)
    !bSearchLMSSensorsSet := SearchLMS(diSeitenmessung_P1, diSeitenmessung_P2, rtEndPos, rtFoundPos1, rtFoundPos2, v100, tVK, \pWObj:=WObj);
    !
    ! Erst auf die steigende, danach auf die fallende Flanke eines Signals (z.B. Abstands- oder Dickenmessung)
    !bSearchLMSSensorsSet := SearchLMS(diSeitenmessung_P2, diSeitenmessung_P2 \fallingFlankSig2, rtEndPos, rtFoundPos1, rtFoundPos2, v100, tVK, \pWObj:=WObj);
    !
    ! Es werden die beiden gefundenen Positionen UND ein Byte
    ! zurückgegeben. Das Byte kann man nutzen, wenn man nur wissen
    ! muss, ob die Sensoren belegt wurden, aber die genaue Position
    ! nicht braucht (z.B. Erkennung eines Bauteils mit variabler Position).
    
    VAR dionum dioEdgeSig1 := high;
    VAR dionum dioEdgeSig2 := high;
    VAR byte   bSignalsSet := 0;
    
    ! Standardverhalten (steigende Flanke) ggf. per switch ändern
    IF Present(anyFlank1)     dioEdgeSig1 := edge;
    IF Present(fallingFlank1) dioEdgeSig1 := low;
    
    IF Present(anyFlank2)     dioEdgeSig2 := edge;
    IF Present(fallingFlank2) dioEdgeSig2 := low;
    
    ! Interrupts
    IDelete inSearchLMS1;
    CONNECT inSearchLMS1 WITH trSearchLMS1;
    ISignalDI \single, pSignal1, dioEdgeSig1, inSearchLMS1;
    IWatch inSearchLMS1;
    
    IDelete inSearchLMS2;
    CONNECT inSearchLMS2 WITH trSearchLMS2;
    ISignalDI \single, pSignal2, dioEdgeSig2, inSearchLMS2;
    IWatch inSearchLMS2;
    
    ! Robtargets ablöschen
    rtTrapPos1 := rtRobtarget0;
    rtTrapPos2 := rtRobtarget0;
    
    ! Bewegung zur Endposition
    MoveL pEndOfSearch, pSpeed, fine, pTool, \WObj?pWObj;
    
    ! Interrupts temporär deaktivieren
    ISleep inSearchLMS1;
    ISleep inSearchLMS2;
    
    ! Return-Wert bilden
    IF (rtTrapPos1 <> rtRobtarget0) THEN
        BitSet bSignalsSet, 1;
        pFound1 := rtTrapPos1;
    ENDIF
    
    IF (rtTrapPos2 <> rtRobtarget0) THEN
        BitSet bSignalsSet, 2;
        pFound2 := rtTrapPos2;
    ENDIF
    
    RETURN bSignalsSet;
    
    !FUNC byte SearchLMS(...)
ENDFUNC

!****************************************************************

TRAP trSearchLMS1
    
    rtTrapPos1 := CRobT();
    
    ! TRAP trSearchLMS1
ENDTRAP

!****************************************************************

TRAP trSearchLMS2
    
    rtTrapPos2 := CRobT();
    
    ! TRAP trSearchLMS2
ENDTRAP