syact/act/stepper/
motor.rs

1use syunit::*;
2
3use crate::{SyncActuator, Setup, Dismantle};
4use crate::act::{InterruptReason, Interruptible, Interruptor, SyncActuatorError, SyncDriveFuture};
5use crate::act::stepper::{StepperActuator, StepperController, StepperBuilder, BuilderError, DriveMode};
6use crate::data::{StepperConfig, StepperConst, MicroSteps}; 
7use crate::math::movements::DefinedActuator;
8
9/// A stepper motor
10/// 
11/// Controlled by two pins, one giving information about the direction, the other about the step signal (PWM)
12pub struct StepperMotor<B : StepperBuilder, C : StepperController + Setup + Dismantle + Send + 'static> {
13    builder : B,
14    ctrl : C, 
15
16    // Atomics
17        /// The current absolute position
18        _gamma : Gamma,
19    // 
20
21    _limit_min : Option<Gamma>,
22    _limit_max : Option<Gamma>,
23
24    // Interrupters
25    interruptors : Vec<Box<dyn Interruptor + Send>>,
26    _intr_reason : Option<InterruptReason>,
27}
28
29// Inits
30impl<B : StepperBuilder + Send + 'static + 'static, C : StepperController + Setup + Dismantle + Send + 'static> StepperMotor<B, C> {   
31    /// Creates a new stepper controller with the given stepper motor constants `consts`
32    #[allow(unused_must_use)]
33    pub fn new(ctrl : C, consts : StepperConst) -> Result<Self, BuilderError> {
34        Ok(Self {
35            builder: B::new(consts)?,
36            ctrl,
37
38            _gamma : Gamma::ZERO,
39
40            _limit_min: None,
41            _limit_max: None,
42
43            interruptors : vec![],
44            _intr_reason: None
45        })
46    }
47
48    pub fn limit_min(&self) -> Option<Gamma> {
49        self._limit_min
50    }
51
52    pub fn limit_max(&self) -> Option<Gamma> {
53        self._limit_max
54    }
55}
56
57impl<B : StepperBuilder + Send + 'static, C : StepperController + Setup + Dismantle + Send + 'static> StepperMotor<B, C> {
58    /// Returns the current direction of the motor
59    pub fn dir(&self) -> Direction {
60        self.builder.dir()
61    }
62}
63
64impl<B : StepperBuilder + Send + 'static, C : StepperController + Setup + Dismantle + Send + 'static> Setup for StepperMotor<B, C> {
65    fn setup(&mut self) -> Result<(), crate::Error> {
66        self.ctrl.setup()?;
67        Ok(())
68    }
69}
70
71impl<B : StepperBuilder + Send + 'static, C : StepperController + Setup + Dismantle + Send + 'static> Dismantle for StepperMotor<B, C> {
72    fn dismantle(&mut self) -> Result<(), crate::Error> {
73        self.ctrl.dismantle()?;
74        Ok(())
75    }
76}
77
78impl<B : StepperBuilder + Send + 'static, C : StepperController + Setup + Dismantle + Send + 'static> SyncActuator for StepperMotor<B, C> {
79    // Movement
80        fn drive_rel(&mut self, delta : Delta, speed_f : Factor) -> SyncDriveFuture {
81            if !delta.is_finite() {
82                return SyncDriveFuture::Done(Err(SyncActuatorError::InvaldDeltaDistance(delta)));
83            }
84
85            // Set drive mode, return mapped error if one occurs
86            if let Err(e) = self.builder.set_drive_mode(DriveMode::FixedDistance(delta, Velocity::ZERO, speed_f), &mut self.ctrl) {
87                return SyncDriveFuture::Done(Err(SyncActuatorError::StepperBuilderError(e)));
88            }
89
90            while let Some(node) = self.builder.next() {
91                let dir_val = self.builder.dir();
92
93                // Check all interruptors
94                for intr in self.interruptors.iter_mut() {
95                    // Check if the direction is right
96                    if let Some(i_dir) = intr.dir() {
97                        if i_dir != dir_val {
98                            continue;
99                        }
100                    }
101
102                    if let Some(reason) = intr.check(self._gamma) {
103                        intr.set_temp_dir(Some(dir_val));
104                        self._intr_reason.replace(reason);
105                        
106                        if let Err(e) = self.builder.set_drive_mode(DriveMode::Stop, &mut self.ctrl) {
107                            return SyncDriveFuture::Done(Err(SyncActuatorError::StepperBuilderError(e)));
108                        }
109                    } else {
110                        // Clear temp direction
111                        intr.set_temp_dir(None);
112                    }
113                }
114
115                // Make step and return error if occured
116                if let Err(e) = self.ctrl.step_no_wait(node) {
117                    return SyncDriveFuture::Done(Err(SyncActuatorError::StepperCtrlError(e)));
118                }
119
120                // Check if the gamma value exeeds any limits
121                if dir_val.as_bool() {
122                    self._gamma += self.builder.step_angle();
123
124                    if self.gamma() > self.limit_max().unwrap_or(Gamma::INFINITY) {
125                        if let Err(e) = self.builder.set_drive_mode(DriveMode::Stop, &mut self.ctrl) {
126                            return SyncDriveFuture::Done(Err(SyncActuatorError::StepperBuilderError(e)))
127                        }
128                    } 
129                } else {
130                    self._gamma = self._gamma - self.builder.step_angle();
131
132                    if self.gamma() < self.limit_min().unwrap_or(Gamma::NEG_INFINITY) {
133                        if let Err(e) = self.builder.set_drive_mode(DriveMode::Stop, &mut self.ctrl) {
134                            return SyncDriveFuture::Done(Err(SyncActuatorError::StepperBuilderError(e)))
135                        }
136                    } 
137                }
138            }
139
140            SyncDriveFuture::Done(Ok(()))
141        }
142
143        fn drive_abs(&mut self, gamma : Gamma, speed : Factor) -> SyncDriveFuture {
144            let delta = gamma - self.gamma();
145            self.drive_rel(delta, speed)
146        }
147    // 
148
149    // Position
150        #[inline]
151        fn gamma(&self) -> Gamma {
152            self._gamma
153        }   
154
155        #[inline]
156        fn set_gamma(&mut self, gamma : Gamma) {
157            self._gamma = gamma;
158        }
159
160        #[inline]
161        fn velocity_max(&self) -> Velocity {
162            self.builder.velocity_max()
163        }
164
165        fn set_velocity_max(&mut self, velocity_max : Velocity) {
166            self.builder.set_velocity_cap(velocity_max).unwrap();      // TODO
167        }
168
169        #[inline]
170        fn set_limits(&mut self, min : Option<Gamma>, max : Option<Gamma>) {
171            if let Some(min) = min {
172                self._limit_min = Some(min)
173            }
174
175            if let Some(max) = max {
176                self._limit_max = Some(max);
177            }
178        }
179
180        #[inline]
181        fn overwrite_limits(&mut self, min : Option<Gamma>, max : Option<Gamma>) {
182            self._limit_min = min;
183            self._limit_max = max;
184        }
185
186        fn limits_for_gamma(&self, gamma : Gamma) -> Delta {
187            if let Some(ang) = self.limit_min() {
188                if gamma < ang {
189                    gamma - ang
190                } else {
191                    if let Some(ang) = self.limit_max() {
192                        if gamma > ang {
193                            gamma - ang
194                        } else { 
195                            Delta::ZERO 
196                        }
197                    } else {
198                        Delta::ZERO
199                    }
200                }
201            } else {
202                if let Some(ang) = self.limit_max() {
203                    if gamma > ang {
204                        gamma - ang
205                    } else { 
206                        Delta::ZERO 
207                    }
208                } else {
209                    Delta::NAN
210                }
211            }
212        }
213
214        fn set_end(&mut self, set_gamma : Gamma) {
215            self.set_gamma(set_gamma);
216
217            let dir = self.dir().as_bool();
218    
219            self.set_limits(
220                if dir { None } else { Some(set_gamma) },
221                if dir { Some(set_gamma) } else { None }
222            )
223        }
224    //
225
226    // Loads
227        fn force_gen(&self) -> Force {
228            self.builder.vars().force_load_gen
229        }
230
231        fn force_dir(&self) -> Force {
232            self.builder.vars().force_load_dir
233        }
234
235        fn apply_gen_force(&mut self, force : Force) -> Result<(), crate::Error> {
236            self.builder.apply_gen_force(force)?;
237            Ok(())
238        }
239
240        fn apply_dir_force(&mut self, force : Force) -> Result<(), crate::Error> {
241            self.builder.apply_dir_force(force)?;
242            Ok(())
243        }
244
245        fn inertia(&self) -> Inertia {
246            self.builder.vars().inertia_load
247        }
248
249        #[inline(always)]
250        fn apply_inertia(&mut self, inertia : Inertia) -> () {
251            self.builder.apply_inertia(inertia).unwrap()
252        }
253    //
254}
255
256impl<B : StepperBuilder + Send + 'static, C : StepperController + Setup + Dismantle + Send + 'static> StepperActuator for StepperMotor<B, C> 
257where
258    B : DefinedActuator 
259{
260    // Data
261        fn consts(&self) -> &StepperConst {
262            self.builder.consts()
263        }
264
265        fn config(&self) -> &StepperConfig {
266            self.builder.config()
267        }
268
269        fn set_config(&mut self, config : StepperConfig) -> Result<(), BuilderError> {
270            self.builder.set_config(config)
271        }
272
273        fn microsteps(&self) -> MicroSteps {
274            self.builder.microsteps()
275        }
276
277        fn set_microsteps(&mut self, microsteps : MicroSteps) -> Result<(), BuilderError> {
278            self.builder.set_microsteps(microsteps)
279            
280        }
281    //
282
283    fn step_ang(&self) -> Delta {
284        self.builder.step_angle()
285    }
286}
287
288impl<B : StepperBuilder + Send + 'static, C : StepperController + Setup + Dismantle + Send + 'static> Interruptible for StepperMotor<B, C> {
289    // Interruptors
290        fn add_interruptor(&mut self, interruptor : Box<dyn Interruptor + Send>) {
291            self.interruptors.push(interruptor);
292        }
293
294        fn intr_reason(&mut self) -> Option<InterruptReason> {
295            // Return the value and replace it with `None`
296            std::mem::replace(&mut self._intr_reason, None)
297        }
298    // 
299}
300
301impl<B : StepperBuilder + Send + 'static, C : StepperController + Setup + Dismantle + Send + 'static> DefinedActuator for StepperMotor<B, C> 
302where
303    B : DefinedActuator 
304{
305    fn ptp_time_for_distance(&self, gamma_0 : Gamma, gamma_t : Gamma) -> Time {
306        self.builder.ptp_time_for_distance(gamma_0, gamma_t)
307    }
308}
syact/act/stepper/motor.rs

syact/act/stepper/
motor.rs
