Advanced Search

// uncomment pins if enabled
/* --- CiA 402 Controlword Commands --- */
#define CMD_SHUTDOWN                0x0006 // Transition to Ready to Switch On
#define CMD_SWITCH_ON               0x0007 // Transition to Switched On
#define CMD_DISABLE_VOLTAGE         0x0000 // Disable power stage
#define CMD_QUICK_STOP              0x0002 // Emergency deceleration
#define CMD_DISABLE_OPERATION       0x0007 // Disable motor drive
#define CMD_ENABLE_OPERATION        0x000F // Enable motor drive (Power On)
#define CMD_FAULT_RESET             0x0080 // Reset fault on rising edge

/* --- CiA 402 Statusword States --- */
// Masked with 0x006F to extract the specific state bits
#define STATE_NOT_READY             0x0000
#define STATE_SWITCH_ON_DISABLED    0x0040
#define STATE_READY_TO_SWITCH_ON    0x0021
#define STATE_SWITCHED_ON           0x0023
#define STATE_OPERATION_ENABLED     0x0027
#define STATE_QUICK_STOP_ACTIVE     0x0007
#define STATE_FAULT_REACTION_ACTIVE 0x000F
#define STATE_FAULT                 0x0008


uint16_t update_cia402_state(config_data_t *d) {  // receives status.word ( d->status.raw)

        
	unsigned short currentState = d->status.raw & 0x006F; // removes all the bits we are not interested in
	
    switch (currentState) {
        case STATE_SWITCH_ON_DISABLED:
            d->control.raw = CMD_SHUTDOWN;
            break;

        case STATE_READY_TO_SWITCH_ON:
            d->control.raw = CMD_SWITCH_ON;
            break;

        case STATE_SWITCHED_ON:
            d->control.raw = CMD_ENABLE_OPERATION;
            break;

        case STATE_OPERATION_ENABLED:
            // Drive is active. Keep it enabled.
            d->control.raw = CMD_ENABLE_OPERATION;
            return 1;
            break;

        case STATE_FAULT:
            // Attempt to clear the fault
            d->control.raw = CMD_FAULT_RESET;
            break;

        case STATE_QUICK_STOP_ACTIVE:
            // Move back to disabled if quick stop was triggered
            d->control.raw = CMD_DISABLE_VOLTAGE;
            break;

        default:
            // For any unknown or intermediate state, default to a safe "Voltage Disable"
            d->control.raw = CMD_DISABLE_VOLTAGE;
            break;
    }

    return 0;
}