autocore-std 3.3.30

/// Seek Probe — Jog an axis in the negative direction until a sensor triggers.
///
/// This function block implements a simple seek-to-probe state machine:
/// 1. On a rising edge of `execute`, the axis begins jogging in the negative direction
///    using the jog speed/accel/decel from [`AxisConfig`](super::AxisConfig).
/// 2. When the sensor input goes high, the axis is halted.
/// 3. Once the axis has fully stopped, the operation is complete.
///
/// The axis must be enabled and at a position > 0 before executing.
/// Jog velocity, acceleration, and deceleration are taken from
/// [`AxisConfig::jog_speed`](super::AxisConfig::jog_speed),
/// [`AxisConfig::jog_accel`](super::AxisConfig::jog_accel), and
/// [`AxisConfig::jog_decel`](super::AxisConfig::jog_decel).
///
/// # Example
///
/// ```ignore
/// use autocore_std::motion::{Axis, AxisConfig, SeekProbe};
/// use autocore_std::motion::axis_view::AxisView;
///
/// struct MyProgram {
///     axis: Axis,
///     seek: SeekProbe,
/// }
///
/// fn process_tick(&mut self, ctx: &mut TickContext<Self::Memory>) {
///     let mut view = ctx.gm.axis_view();
///     self.axis.tick(&mut view, ctx.client);
///
///     self.seek.call(
///         &mut self.axis,
///         &mut view,
///         ctx.gm.start_seek,    // execute: bool
///         ctx.gm.ball_sensor,   // sensor: bool
///     );
///
///     if self.seek.done {
///         log::info!("Probe found at position {:.3}", self.axis.position);
///     }
///     if self.seek.state.is_error() {
///         log::error!("Seek failed: error_code={}", self.seek.state.error_code);
///     }
/// }
/// ```
///
/// # Error Codes
///
/// | Code | Meaning |
/// |------|---------|
/// | 1 | Panic/abort while motion was active |
/// | 100 | Axis position is not > 0 at start |
/// | 120 | Axis error or control disabled during motion |
/// | 200 | Axis reported error when stopping |
///
/// # State Diagram
///
/// ```text
/// ┌──────────┐  execute ↑  ┌────────────┐ position>0  ┌────────────────┐
/// │ 10: Idle │──────────►│ 100: Start │────────────►│ 120: Jogging   │
/// └──────────┘           └────────────┘             │  (negative)    │
///       ▲                      │ pos<=0             └───────┬────────┘
///       │                      ▼                   sensor │  │ axis error
///       │                 error_code=100                   ▼  ▼
///       │                                          ┌────────────────┐
///       │◄──── done=true ◄─────────────────────────│ 200: Stopping  │
///       │◄──── error=true ◄────────────────────────│                │
///       │                                          └────────────────┘
///       │◄──── error=true ◄── 250: Motion Error
/// ```

use crate::fb::{RTrig, StateMachine};
use super::axis::Axis;
use super::axis_view::AxisView;

/// Seek Probe function block.
///
/// Jogs an axis in the negative direction until a sensor triggers,
/// then halts and reports completion.
#[derive(Debug, Clone)]
pub struct SeekProbe {
    /// Output: operation completed successfully.
    pub done: bool,
    /// Output: operation failed — check `state.error_code`.
    pub error: bool,
    /// State machine with index, error_code, timers, and messages.
    pub state: StateMachine,

    rt_execute: RTrig,
}

impl SeekProbe {
    /// Create a new SeekProbe in the idle state.
    pub fn new() -> Self {
        Self {
            done: false,
            error: false,
            state: StateMachine::new(),
            rt_execute: RTrig::new(),
        }
    }

    /// Execute one scan cycle of the seek-probe state machine.
    ///
    /// # Arguments
    ///
    /// * `axis` — The axis to jog. Must have been ticked already this cycle.
    /// * `view` — The axis PDO view for issuing motion commands.
    /// * `execute` — Rising edge triggers the seek operation.
    /// * `sensor` — When this goes high during jogging, the axis halts.
    pub fn call(
        &mut self,
        axis: &mut Axis,
        view: &mut impl AxisView,
        execute: bool,
        sensor: bool,
    ) {
        let execute_edge = self.rt_execute.call(execute);

        match self.state.index {
            0 => {
                // Reset / initialize
                self.done = false;
                self.state.index = 10;
            }

            10 => {
                // Idle — wait for rising edge of execute
                if execute_edge {
                    self.state.clear_error();
                    self.done = false;
                    self.error = false;
                    self.state.index = 100;
                }
            }

            100 => {
                // Start motion — verify position > 0, then jog negative
                if axis.position > 0.0 {
                    let speed = axis.config().jog_speed;
                    let accel = axis.config().jog_accel;
                    let decel = axis.config().jog_decel;
                    // Jog negative: move toward 0 at jog speed.
                    // The sensor trigger will halt before reaching the target.
                    axis.move_relative(view, -axis.position, speed, accel, decel);
                    self.state.index = 120;
                } else {
                    self.done = false;
                    self.error = true;
                    self.state.set_error(100, "Axis position must be > 0 to start seek");
                    self.state.index = 10;
                }
            }

            120 => {
                // Jogging negative — wait for sensor or error
                if sensor {
                    axis.halt(view);
                    self.state.index = 200;
                } else if axis.is_error || !axis.motor_on {
                    self.state.set_error(120, "Axis error or control disabled during seek");
                    self.state.index = 250;
                }
            }

            200 => {
                // Wait for axis to come to a complete stop
                if !axis.is_busy {
                    if axis.is_error {
                        self.done = false;
                        self.error = true;
                        self.state.set_error(200, "Axis error while stopping");
                    } else {
                        self.done = true;
                        self.error = false;
                    }
                    self.state.index = 10;
                }
            }

            250 => {
                // Motion error during jog — halt and return to idle
                axis.halt(view);
                self.done = false;
                self.error = true;
                self.state.index = 10;
            }

            _ => {
                self.state.index = 0;
            }
        }

        self.state.call();
    }

    /// Abort the current operation immediately.
    ///
    /// If motion is active (states > 10), the axis is halted and
    /// an error is set with code 1.
    pub fn abort(&mut self, axis: &mut Axis, view: &mut impl AxisView) {
        if self.state.index > 10 {
            axis.halt(view);
            self.error = true;
            self.state.set_error(1, "Seek aborted");
        }
        self.state.index = 10;
        self.done = false;
    }

    /// Returns `true` while the seek operation is in progress.
    pub fn is_busy(&self) -> bool {
        self.state.index > 10
    }
}

impl Default for SeekProbe {
    fn default() -> Self {
        Self::new()
    }
}