autocore_std/motion/seek_probe.rs
1/// Seek Probe — Jog an axis in the negative direction until a sensor triggers.
2///
3/// This function block implements a simple seek-to-probe state machine:
4/// 1. On a rising edge of `execute`, the axis begins jogging in the negative direction
5/// using the jog speed/accel/decel from [`AxisConfig`](super::AxisConfig).
6/// 2. When the sensor input goes high, the axis is halted.
7/// 3. Once the axis has fully stopped, the operation is complete.
8///
9/// The axis must be enabled and at a position > 0 before executing.
10/// Jog velocity, acceleration, and deceleration are taken from
11/// [`AxisConfig::jog_speed`](super::AxisConfig::jog_speed),
12/// [`AxisConfig::jog_accel`](super::AxisConfig::jog_accel), and
13/// [`AxisConfig::jog_decel`](super::AxisConfig::jog_decel).
14///
15/// # Example
16///
17/// ```ignore
18/// use autocore_std::motion::{Axis, AxisConfig, SeekProbe};
19/// use autocore_std::motion::axis_view::AxisView;
20///
21/// struct MyProgram {
22/// axis: Axis,
23/// seek: SeekProbe,
24/// }
25///
26/// fn process_tick(&mut self, ctx: &mut TickContext<Self::Memory>) {
27/// let mut view = ctx.gm.axis_view();
28/// self.axis.tick(&mut view, ctx.client);
29///
30/// self.seek.call(
31/// &mut self.axis,
32/// &mut view,
33/// ctx.gm.start_seek, // execute: bool
34/// ctx.gm.ball_sensor, // sensor: bool
35/// );
36///
37/// if self.seek.done {
38/// log::info!("Probe found at position {:.3}", self.axis.position);
39/// }
40/// if self.seek.state.is_error() {
41/// log::error!("Seek failed: error_code={}", self.seek.state.error_code);
42/// }
43/// }
44/// ```
45///
46/// # Error Codes
47///
48/// | Code | Meaning |
49/// |------|---------|
50/// | 1 | Panic/abort while motion was active |
51/// | 100 | Axis position is not > 0 at start |
52/// | 120 | Axis error or control disabled during motion |
53/// | 200 | Axis reported error when stopping |
54///
55/// # State Diagram
56///
57/// ```text
58/// ┌──────────┐ execute ↑ ┌────────────┐ position>0 ┌────────────────┐
59/// │ 10: Idle │──────────►│ 100: Start │────────────►│ 120: Jogging │
60/// └──────────┘ └────────────┘ │ (negative) │
61/// ▲ │ pos<=0 └───────┬────────┘
62/// │ ▼ sensor │ │ axis error
63/// │ error_code=100 ▼ ▼
64/// │ ┌────────────────┐
65/// │◄──── done=true ◄─────────────────────────│ 200: Stopping │
66/// │◄──── error=true ◄────────────────────────│ │
67/// │ └────────────────┘
68/// │◄──── error=true ◄── 250: Motion Error
69/// ```
70
71use crate::fb::StateMachine;
72use super::axis::Axis;
73use super::axis_view::{AxisView, AxisHandle};
74
75/// Blanket implementation to allow old-style (Axis, AxisView) calls.
76impl<'a, V: AxisView> AxisHandle for (&'a mut Axis, &'a mut V) {
77 fn position(&self) -> f64 { self.0.position }
78 fn config(&self) -> &super::axis_config::AxisConfig { self.0.config() }
79 fn move_relative(&mut self, distance: f64, vel: f64, accel: f64, decel: f64) {
80 self.0.move_relative(self.1, distance, vel, accel, decel);
81 }
82 fn move_absolute(&mut self, position: f64, vel: f64, accel: f64, decel: f64) {
83 self.0.move_absolute(self.1, position, vel, accel, decel);
84 }
85 fn halt(&mut self) { self.0.halt(self.1); }
86 fn is_busy(&self) -> bool { self.0.is_busy }
87 fn is_error(&self) -> bool { self.0.is_error }
88 fn error_message(&self) -> String { self.0.error_message.clone() }
89 fn motor_on(&self) -> bool { self.0.motor_on }
90}
91
92/// Seek Probe function block.
93///
94/// Jogs an axis in the negative direction until a sensor triggers,
95/// then halts and reports completion.
96#[derive(Debug, Clone)]
97pub struct SeekProbe {
98 /// Output: operation completed successfully.
99 pub done: bool,
100 /// Output: operation failed — check `state.error_code`.
101 pub error: bool,
102
103 /// Target position for the seek move. this should be below the expected height of the object, but
104 /// must be greater than the minimum position limit
105 pub target_position : f64,
106 /// Target speed for the seek move
107 pub target_speed : f64,
108 /// Target accel for the seek move
109 pub target_accel : f64,
110 /// Target decel for the seek move. Higher is probably better.
111 pub target_decel : f64,
112
113 /// State machine with index, error_code, timers, and messages.
114 pub state: StateMachine,
115}
116
117impl SeekProbe {
118 /// Create a new SeekProbe in the idle state.
119 pub fn new() -> Self {
120 Self {
121 done: false,
122 error: false,
123 target_position : 0.0,
124 target_speed : 0.0,
125 target_accel : 0.0,
126 target_decel : 0.0,
127 state: StateMachine::new(),
128 }
129 }
130
131 /// The FB is busy running.
132 pub fn is_busy(&self) -> bool {
133 self.state.index > 10
134 }
135
136 /// The last requested command resulted in an error.
137 pub fn is_error(&self) -> bool {
138 self.error || self.state.is_error()
139 }
140
141 /// Return the current error code.
142 pub fn error_code(&self) -> i32 {
143 self.state.error_code
144 }
145
146 /// Start the FB processing a seek cycle.
147 pub fn start(&mut self,
148 target_position : f64,
149 target_speed : f64,
150 target_accel : f64,
151 target_decel : f64
152 ) {
153
154 self.target_position = target_position;
155 self.target_speed = target_speed;
156 self.target_accel = target_accel;
157 self.target_decel = target_decel;
158
159 self.state.clear_error();
160 self.done = false;
161 self.error = false;
162 self.state.index = 100;
163 }
164
165 /// Stop the FB from processing and send back to idle.
166 /// Safely halts the axis if it was currently moving.
167 pub fn reset(&mut self, handle: &mut impl AxisHandle) {
168 if self.state.index > 10 {
169 handle.halt();
170 }
171 self.done = false;
172 self.error = false;
173 self.state.index = 10;
174 }
175
176 /// Execute one scan cycle of the seek-probe state machine.
177 ///
178 /// # Arguments
179 ///
180 /// * `handle` — The axis handle (e.g. `AxisLift` or `(&mut axis, &mut view)`).
181 /// * `sensor` — When this goes high during jogging, the axis halts.
182 pub fn tick(
183 &mut self,
184 handle: &mut impl AxisHandle,
185 sensor: bool,
186 ) {
187 match self.state.index {
188 0 => {
189 // Reset / initialize
190 self.done = false;
191 self.state.index = 10;
192 }
193
194 10 => {
195 // Idle — wait for start() to be called
196 }
197
198 100 => {
199 // Start motion — verify position > 0, then jog negative
200 let target = self.target_position;
201 if handle.position() > target {
202 let speed = self.target_speed;
203 let accel = self.target_accel;
204 let decel = self.target_decel;
205 // Jog negative: move toward 0 at jog speed.
206 // The sensor trigger will halt before reaching the target.
207 // handle.move_relative(-handle.position(), speed, accel, decel);
208 handle.move_absolute(target, speed, accel, decel);
209 self.state.index = 120;
210 } else {
211 self.done = false;
212 self.error = true;
213 self.state.set_error(100,
214 format!("Axis position must be > min position limit {} to start seek",
215 target)
216 );
217 self.state.index = 10;
218 }
219 }
220
221 120 => {
222 // Jogging negative — wait for sensor or error
223 if sensor {
224 handle.halt();
225 self.state.index = 200;
226 } else if handle.is_error() || !handle.motor_on() {
227 self.state.set_error(120, "Axis error or control disabled during seek");
228 self.state.index = 250;
229 }
230 }
231
232 200 => {
233 // Wait for axis to come to a complete stop
234 if !handle.is_busy() {
235 if handle.is_error() {
236 self.done = false;
237 self.error = true;
238 self.state.set_error(200, "Axis error while stopping");
239 } else {
240 self.done = true;
241 self.error = false;
242 }
243 self.state.index = 10;
244 }
245 }
246
247 250 => {
248 // Motion error during jog — halt and return to idle
249 handle.halt();
250 self.done = false;
251 self.error = true;
252 self.state.index = 10;
253 }
254
255 _ => {
256 self.state.index = 0;
257 }
258 }
259
260 self.state.call();
261 }
262
263 /// Abort the current operation immediately.
264 ///
265 /// If motion is active (states > 10), the axis is halted and
266 /// an error is set with code 1.
267 pub fn abort(&mut self, handle: &mut impl AxisHandle) {
268 self.reset(handle);
269 self.error = true;
270 self.state.set_error(1, "Seek aborted");
271 }
272}
273
274impl Default for SeekProbe {
275 fn default() -> Self {
276 Self::new()
277 }
278}