autocore_std/motion/

seek_probe.rs

/// 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::StateMachine;
use super::axis::Axis;
use super::axis_view::{AxisView, AxisHandle};

/// Blanket implementation to allow old-style (Axis, AxisView) calls.
impl<'a, V: AxisView> AxisHandle for (&'a mut Axis, &'a mut V) {
    fn position(&self) -> f64 { self.0.position }
    fn config(&self) -> &super::axis_config::AxisConfig { self.0.config() }
    fn move_relative(&mut self, distance: f64, vel: f64, accel: f64, decel: f64) {
        self.0.move_relative(self.1, distance, vel, accel, decel);
    }
    fn move_absolute(&mut self, position: f64, vel: f64, accel: f64, decel: f64) {
        self.0.move_absolute(self.1, position, vel, accel, decel);
    }
    fn halt(&mut self) { self.0.halt(self.1); }
    fn is_busy(&self) -> bool { self.0.is_busy }
    fn is_error(&self) -> bool { self.0.is_error }
    fn error_message(&self) -> String { self.0.error_message.clone() }
    fn motor_on(&self) -> bool { self.0.motor_on }
}

/// 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,
}

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

    /// The FB is busy running.
    pub fn is_busy(&self) -> bool {
        self.state.index > 10
    }

    /// The last requested command resulted in an error.
    pub fn is_error(&self) -> bool {
        self.error || self.state.is_error()
    }

    /// Return the current error code.
    pub fn error_code(&self) -> i32 {
        self.state.error_code
    }

    /// Start the FB processing a seek cycle.
    pub fn start(&mut self) {
        self.state.clear_error();
        self.done = false;
        self.error = false;
        self.state.index = 100;
    }

    /// Stop the FB from processing and send back to idle.
    /// Safely halts the axis if it was currently moving.
    pub fn reset(&mut self, handle: &mut impl AxisHandle) {
        if self.state.index > 10 {
            handle.halt();
        }
        self.done = false;
        self.error = false;
        self.state.index = 10;
    }

    /// Execute one scan cycle of the seek-probe state machine.
    ///
    /// # Arguments
    ///
    /// * `handle` — The axis handle (e.g. `AxisLift` or `(&mut axis, &mut view)`).
    /// * `sensor` — When this goes high during jogging, the axis halts.
    pub fn tick(
        &mut self,
        handle: &mut impl AxisHandle,
        sensor: bool,
    ) {
        match self.state.index {
            0 => {
                // Reset / initialize
                self.done = false;
                self.state.index = 10;
            }

            10 => {
                // Idle — wait for start() to be called
            }

            100 => {
                // Start motion — verify position > 0, then jog negative
                if handle.position() > 0.0 {
                    let speed = handle.config().jog_speed;
                    let accel = handle.config().jog_accel;
                    let decel = handle.config().jog_decel;
                    // Jog negative: move toward 0 at jog speed.
                    // The sensor trigger will halt before reaching the target.
                    handle.move_relative(-handle.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 {
                    handle.halt();
                    self.state.index = 200;
                } else if handle.is_error() || !handle.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 !handle.is_busy() {
                    if handle.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
                handle.halt();
                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, handle: &mut impl AxisHandle) {
        self.reset(handle);
        self.error = true;
        self.state.set_error(1, "Seek aborted");
    }
}

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