Advanced Search

    // STATE TRANSITIONS
    switch (state) {
        
        case 0x0000: 
        case 0x0040: // Switch On Disabled
            d->control.raw = 0x0006; // CMD: Shutdown
            break;

        case 0x0021: // Ready to Switch On
            d->control.raw = 0x0007; // CMD: Switch Onif (homed && !data.prev_homed) {
            break;

        case 0x0023: // Switched On
            d->control.raw = 0x000F; // CMD: Enable Operation
            break;

        case 0x0007: // Quick Stop Active
            d->control.raw = 0x0000; // CMD: Disable Voltage (Reset the stop)
            break;

        case 0x0027: // OPERATION ENABLED
            d->control.raw = 0x000F; // Maintain Enable
            return 1;                // Signal: "Drive Ready for Motion"

        default:     
            d->control.raw = 0x0000; // Safety Fallback
            break;
    }
    return 0; // Still transitioning to on state

    // 4. Drive Logic: Only command motion/homing if Operational AND Mode is correct
    if (drive_ready) {
        if (homing) {
            opmode_out = 6;
            
            // --- DYNAMIC TORQUE LIMIT ---
            // Use the HAL pin 'torque_limit', but maybe you want to 
            // force it lower specifically for homing?
            // Usually, we just use the pin value set by the user.
            drive_torque_limit_out = (int32_t)(homing_torque_limit * torque_scale);

            if (opmode_display_in == 6) {
                if (!data.status.bits.op_error) 
                    data.control.bits.op_specific1 = 1;
            }
        } else {
            opmode_out = default_opmode;
            data.control.bits.op_specific1 = 0;
            
            // Set normal torque limit (100% or as defined by HAL)
            drive_torque_limit_out = (int32_t)(torque_limit * torque_scale);

            if (mode_verified) {
                drive_target_position = (int32_t)(target_position * data.s_pos_scale);
                drive_target_velocity = (int32_t)(target_velocity * data.s_vel_scale);
            }
        }
    } 
    else {
        // Drive not ready (Faulted, Timeout, or Powering up)
        opmode_out = default_opmode;
        data.control.bits.op_specific1 = 0;
        drive_torque_limit_out = 0; // Safety clamp
    }

typedef struct {
    union {
        unsigned short raw;
        struct {   
            unsigned short ready_to_switch_on :1; // Bit 0
            unsigned short switched_on        :1; // Bit 1
            unsigned short op_enabled         :1; // Bit 2
            unsigned short fault              :1; // Bit 3
            unsigned short voltage_enabled    :1; // Bit 4
            unsigned short quick_stop         :1; // Bit 5
            unsigned short sw_on_disabled     :1; // Bit 6
            unsigned short warning            :1; // Bit 7
            unsigned short manufacturer       :1; // Bit 8
            unsigned short remote             :1; // Bit 9
            unsigned short target_reached     :1; // Bit 10
            unsigned short internal_limit     :1; // Bit 11
            unsigned short op_specific        :1; // Bit 12 (Homing Attained)
            unsigned short op_error           :1; // Bit 13 (Homing Error)
            unsigned short manufacturer_1     :1; // Bit 14
            unsigned short manufacturer_2     :1; // Bit 15
        } bits;
    } status;