PROGRAM PLC_PRG
VAR
    pSlave: POINTER TO ETCSlave;
    eStep : INT := 0;
    tonDelay : TON;
    bEnable : BOOL := TRUE;
    nCycles : UDINT;
    wCurrentState : WORD;
    xError : BOOL; // Flag to indicate error
END_VAR

//---------------------------------------------------------------------------------------//
//------------------------------END OF VARIABLE DECLARATION------------------------------//
//---------------------------------------------------------------------------------------//

// Get the first slave pointer
pSlave := EtherCAT_Master.FirstSlave;

// Safety check - ensure pointer is valid before using
IF pSlave = 0 THEN
    xError := TRUE;
    RETURN;
END_IF

// Only proceed if enabled and pointer is valid
IF bEnable AND NOT xError THEN
    CASE eStep OF
        0: // Set INIT
            pSlave^.SetOpMode(1); // 1 = INIT
            eStep := 1;
            
        1: // Wait for INIT state and delay
            wCurrentState := pSlave^.wState;
            tonDelay(IN := TRUE, PT := T#200MS);
            IF tonDelay.Q THEN
                tonDelay(IN := FALSE);
                eStep := 2;
            END_IF;
            
        2: // Set preOP
            pSlave^.SetOpMode(2); // 2 = preOP
            eStep := 3;
            
        3: // Wait for preOP state and delay
            wCurrentState := pSlave^.wState;
            tonDelay(IN := TRUE, PT := T#200MS);
            IF tonDelay.Q THEN
                tonDelay(IN := FALSE);
                nCycles := nCycles + 1;
                eStep := 0; // Loop back to INIT (start cycle again)
            END_IF;
    END_CASE;
ELSE
    eStep := 0;
    tonDelay(IN := FALSE);
END_IF;
