autocore_std/motion/
move_to_load.rs

1use crate::fb::StateMachine;
2use super::axis_view::AxisHandle;
3
4/// Move To Load function block.
5///
6/// Moves an axis towards a target load (e.g., from a load cell) and stops
7/// as quickly as possible once the edge of that load is reached.
8/// 
9/// - If `current_load > target_load`, it moves in the negative direction.
10/// - If `current_load < target_load`, it moves in the positive direction.
11///
12/// A hysteresis value (minimum 1.0) is used to handle noise spikes. The axis
13/// will halt as soon as `current_load` crosses the `target_load +/- hysteresis` threshold.
14///
15/// If the axis reaches the `position_limit` before the load is achieved, the block
16/// halts the axis and enters an error state.
17#[derive(Debug, Clone)]
18pub struct MoveToLoad {
19    /// Output: operation completed successfully.
20    pub done: bool,
21    /// Output: block is currently executing motion.
22    pub active: bool,
23    /// Output: operation failed. Check `state.error_code`.
24    pub error: bool,
25    /// Internal state machine.
26    pub state: StateMachine,
27    
28    moving_negative: bool,
29}
30
31impl Default for MoveToLoad {
32    fn default() -> Self {
33        Self {
34            done: false,
35            active: false,
36            error: false,
37            state: StateMachine::new(),
38            moving_negative: false,
39        }
40    }
41}
42
43impl MoveToLoad {
44    pub fn new() -> Self {
45        Self::default()
46    }
47
48    /// Abort the operation immediately.
49    pub fn abort(&mut self, axis: &mut impl AxisHandle) {
50        axis.halt();
51        self.error = true;
52        self.active = false;
53        self.state.set_error(1, "Abort called");
54        self.state.index = 100;
55    }
56
57    /// Execute the function block.
58    ///
59    /// - `axis`: The handle to the PP axis.
60    /// - `execute`: Set to true to start the move. On a falling edge, the axis halts.
61    /// - `target_load`: The target load to reach.
62    /// - `current_load`: The raw, instantaneous reading from the load cell.
63    /// - `position_limit`: The absolute position boundary. If reached before the load, an error occurs.
64    /// - `hysteresis`: The acceptable noise band (minimum 1.0) to prevent premature stopping.
65    pub fn call(
66        &mut self,
67        axis: &mut impl AxisHandle,
68        execute: bool,
69        target_load: f64,
70        current_load: f64,
71        position_limit: f64,
72        hysteresis: f64,
73    ) {
74        let hyst = hysteresis.max(1.0);
75
76        if !execute {
77            if self.active {
78                axis.halt();
79            }
80            self.done = false;
81            self.active = false;
82            self.error = false;
83            self.state.index = 0;
84            return;
85        }
86
87        // Safety: Check for axis faults
88        if axis.is_error() && self.state.index != 100 {
89            self.error = true;
90            self.active = false;
91            self.state.set_error(120, "Axis is in error state");
92            self.state.index = 100;
93        }
94
95        match self.state.index {
96            0 => { // Idle -> Start
97                self.done = false;
98                self.error = false;
99                self.active = true;
100                self.state.clear_error();
101
102                self.moving_negative = current_load > target_load;
103
104                // Check if we're already there
105                let reached = if self.moving_negative {
106                    current_load <= target_load + hyst
107                } else {
108                    current_load >= target_load - hyst
109                };
110
111                if reached {
112                    self.done = true;
113                    self.active = false;
114                    self.state.index = 30;
115                } else {
116                    // Are we already out of bounds?
117                    if (self.moving_negative && axis.position() <= position_limit) ||
118                       (!self.moving_negative && axis.position() >= position_limit) {
119                        self.error = true;
120                        self.active = false;
121                        self.state.set_error(110, "Axis already past position limit before starting");
122                        self.state.index = 100;
123                    } else {
124                        let vel = axis.config().jog_speed;
125                        let acc = axis.config().jog_accel;
126                        let dec = axis.config().jog_decel;
127                        axis.move_absolute(position_limit, vel, acc, dec);
128                        self.state.index = 10;
129                    }
130                }
131            }
132            10 => { // Moving
133                let reached = if self.moving_negative {
134                    current_load <= target_load + hyst
135                } else {
136                    current_load >= target_load - hyst
137                };
138
139                if reached {
140                    axis.halt();
141                    self.state.index = 20; // Wait to stop
142                } else {
143                    // Check limit
144                    let hit_limit = if self.moving_negative {
145                        axis.position() <= position_limit + 0.0001
146                    } else {
147                        axis.position() >= position_limit - 0.0001
148                    };
149
150                    if hit_limit || !axis.is_busy() {
151                        axis.halt();
152                        self.error = true;
153                        self.active = false;
154                        self.state.set_error(150, "Reached position limit without hitting target load");
155                        self.state.index = 100;
156                    }
157                }
158            }
159            20 => { // Stopping
160                if !axis.is_busy() {
161                    self.done = true;
162                    self.active = false;
163                    self.state.index = 30;
164                }
165            }
166            30 => { // Done
167                // Wait for execute to drop
168            }
169            100 => { // Error
170                // Wait for execute to drop
171            }
172            _ => {
173                self.state.index = 0;
174            }
175        }
176        
177        self.state.call();
178    }
179}
autocore_std/motion/move_to_load.rs

autocore_std/motion/
move_to_load.rs
