component turret "4-position pneumatic step toolchanger with fault detection and spindle interlock";

# --- Input pins ---
pin in bit sensor0;
pin in bit sensor1;
pin in bit sensor2;
pin in bit sensor3;

pin in s32 target;      // target pocket from tool table
pin in bit start;       // trigger to start tool change

# --- Output pins ---
pin out bit valve;      // pneumatic valve control
pin out s32 position;   // current turret pocket (0-3)
pin out bit busy;       // turret moving
pin out bit done;       // turret finished move
pin out bit error;      // turret fault

# --- Parameters ---
param float stepTime = 2.0;        // seconds to lift + rotate
param float settleTime = 0.5;      // seconds to settle after drop
param float sensorTimeout = 2.5;   // max wait for sensor after step

# --- Internal variables ---
variable int state = 0;
variable int stepsRemaining = 0;
variable float timer = 0;

function _;

license "GPL";

;;

FUNCTION(_) {

    // Count active sensors
    int sensorCount = sensor0 + sensor1 + sensor2 + sensor3;

    // Determine current position only if exactly one sensor active
    if (sensorCount == 1) {
        if (sensor0) position = 0;
        else if (sensor1) position = 1;
        else if (sensor2) position = 2;
        else if (sensor3) position = 3;
    }

    switch(state) {

        case 0: // IDLE
            valve = 0;
            busy = 0;
            done = 0;

            // Start condition: rising edge + spindle interlock handled in HAL
            if (start) {
                error = 0;

                // Fault if sensor reading invalid
                if (sensorCount != 1) {
                    error = 1;
                    break;
                }

                // Ignore redundant tool changes
                if (target == position) {
                    done = 1;
                    break;
                }

                // Compute steps to move (unidirectional)
                int diff = target - position;
                if (diff < 0) diff += 4;

                stepsRemaining = diff;
                busy = 1;
                state = 1;
                timer = 0;
            }
            break;

        case 1: // LIFT + ROTATE
            valve = 1;          // energize pneumatic valve
            timer += fperiod;

            if (timer >= stepTime) {
                timer = 0;
                state = 2;
            }
            break;

        case 2: // DROP
            valve = 0;          // de-energize valve
            timer += fperiod;

            if (timer >= settleTime) {
                timer = 0;
                state = 3;
            }
            break;

        case 3: // WAIT FOR SENSOR
            timer += fperiod;

            // Sensor confirmed
            if (sensorCount == 1) {
                stepsRemaining--;

                if (stepsRemaining > 0) {
                    state = 1;
                    timer = 0;
                } else {
                    state = 0;
                    done = 1;
                }
            }

            // Timeout fault
            if (timer > sensorTimeout) {
                error = 1;
                state = 0;
                valve = 0;
            }

            // Multiple sensors fault
            if (sensorCount > 1) {
                error = 1;
                state = 0;
                valve = 0;
            }
            break;
    }
}