Advanced Search

VAR
    // PDO mapping inputs (from the drive)
    wStatusword : WORD; // 0x6041
    iActualMode : SINT; // 0x6060
    
    // PDO mapping outputs (to the PLC)
    wControlword : WORD; // 0x6040
    iModeOfOp    : SINT; // 0x6060
    
    // Control / Monitoring
    bStartHoming : BOOL;    // Trigger
    bHomingActive: BOOL;    // Status
    bHomed       : BOOL;    // Target reached
    bError       : BOOL;    // Monitoring: Error
    iState       : INT := 0; // Step chain
    
    // Monitoring: Timer
    fbTimeout : TON;
END_VAR

// --- Read status bits from status word (0x6041) ---
// Bit 12: Homing process running / successful (depending on method)
// Bit 13: Homing error
// Bit 10: Target reached (often used during homing)
bHomingActive := (wStatusword.12); // Homing operation in progress
bHomed        := (wStatusword.12) AND (wStatusword.10); 
bError        := (wStatusword.13); // Homing error

// --- Monitoring (timeout) ---
fbTimeout(IN := bHomingActive AND NOT bHomed, PT := T#10S);

// --- State machine for homing (CiA 402) ---
CASE iState OF
    0: // Wait for start
        IF bStartHoming AND NOT bError THEN
            bStartHoming := FALSE;
            iState := 10;
        END_IF;
 10: // Set mode of operation to “Homing” (6)
        iModeOfOp := 6;
        IF iActualMode = 6 THEN
            iState := 20;
        END_IF;

    20: // Set drive to “Operation Enabled” (standard CiA402 state machine)
        // Simple example: Set control word to 0x0F (Enable)
        wControlword := 16#000F;
        IF (wStatusword AND 16#006F) = 16#0027 THEN // Ready to operate
            iState := 30;
        END_IF;

    30: // Start homing: Set bit 4 in control word (start homing procedure)
        wControlword := 16#001F; // Bit 4 high, enable op
        IF bHomingActive THEN
            iState := 40;
        END_IF;
        IF fbTimeout.Q OR bError THEN iState := 999; END_IF; // Monitoring

    40: // Wait for completion
        IF bHomed THEN
            wControlword := 16#000F; // Reset homing start bit
            iState := 0;
        END_IF;
        IF fbTimeout.Q OR bError THEN iState := 999; END_IF; // Monitoring

    999: // Error state
        // Error handling: Cancel homing, set message
        wControlword := 16#000F; // Stop
        bError := TRUE;
        IF NOT bStartHoming THEN iState := 0; bError := FALSE; END_IF;
END_CASE;