pamoja_actuators/stepper/
driver.rs

1//! Stepper motors driven through pins: four coil lines, or a step and direction pair.
2
3use embedded_hal::delay::DelayNs;
4use embedded_hal::digital::OutputPin;
5use pamoja_core::Actuator;
6
7use super::{Direction, Drive, Position, Sequencer};
8use crate::error::DriverError;
9
10/// The pause between steps a driver starts with, in microseconds.
11///
12/// Two milliseconds a step is a rate every common geared hobby motor follows
13/// without stalling; a motor's own datasheet gives the fastest it can take.
14pub const DEFAULT_STEP_MICROS: u32 = 2_000;
15
16/// The width of a step pulse a step/direction driver starts with, in microseconds.
17///
18/// A step/direction driver holds the direction line this long before it raises the
19/// step line, then holds the step line high this long. An A4988 needs 200 ns of
20/// setup and a microsecond high, a DRV8825 650 ns and 1.9 microseconds; ten leaves
21/// room for a slow pin. The chip's datasheet gives its minimums.
22pub const DEFAULT_PULSE_MICROS: u32 = 10;
23
24fn direction_of(steps: i32) -> Direction {
25    if steps < 0 {
26        Direction::Backward
27    } else {
28        Direction::Forward
29    }
30}
31
32/// A four-wire stepper driven coil by coil, through a transistor array such as a
33/// ULN2003 or an H-bridge.
34///
35/// Each step energizes the coils the [`Drive`] pattern names, pin A for the
36/// pattern's high bit down to pin D for its low bit, then waits the step interval.
37/// The [`Position`] counts steps from where the motor was when the driver was
38/// built. As an [`Actuator`] the command is a signed step count.
39///
40/// # Examples
41///
42/// ```
43/// use pamoja_actuators::stepper::{Drive, FourWire};
44/// use pamoja_core::Actuator;
45/// use pamoja_hal::digital::PinState::{High, Low};
46/// use pamoja_hal::script::{block_on, DelayLog, PinScript};
47///
48/// let pins = (PinScript::new([]), PinScript::new([]), PinScript::new([]), PinScript::new([]));
49/// let mut motor = FourWire::new(pins, DelayLog::new(), Drive::FullStep);
50/// block_on(motor.apply(2))?;
51/// assert_eq!(motor.position(), 2);
52///
53/// // Each step advances to the next full-step pattern: 0110 then 0011, so coil A
54/// // stays off and coil B is on for the first step only.
55/// let ((a, b, _, _), _) = motor.release();
56/// assert_eq!(a.driven(), [Low, Low]);
57/// assert_eq!(b.driven(), [High, Low]);
58/// # Ok::<(), pamoja_core::Error>(())
59/// ```
60#[derive(Debug)]
61pub struct FourWire<A, B, C, Dp, D> {
62    coils: (A, B, C, Dp),
63    delay: D,
64    sequencer: Sequencer,
65    position: Position,
66    step_micros: u32,
67}
68
69impl<A, B, C, Dp, D> FourWire<A, B, C, Dp, D> {
70    /// Wraps the four coil lines, without driving them.
71    ///
72    /// # Arguments
73    ///
74    /// * `coils` - the lines for coils A, B, C, and D, in the motor's phase order.
75    /// * `delay` - the timer that paces the steps.
76    /// * `drive` - the coil pattern to step through.
77    ///
78    /// # Returns
79    ///
80    /// The driver, at [`DEFAULT_STEP_MICROS`] a step, counting from zero.
81    pub fn new(coils: (A, B, C, Dp), delay: D, drive: Drive) -> Self {
82        FourWire {
83            coils,
84            delay,
85            sequencer: Sequencer::new(drive),
86            position: Position::new(),
87            step_micros: DEFAULT_STEP_MICROS,
88        }
89    }
90
91    /// Sets the pause after each step, which sets the speed.
92    ///
93    /// # Arguments
94    ///
95    /// * `micros` - the interval in microseconds.
96    ///
97    /// # Returns
98    ///
99    /// The driver.
100    pub fn with_step_interval(mut self, micros: u32) -> Self {
101        self.step_micros = micros;
102        self
103    }
104
105    /// Returns the step count since the driver was built, forward positive.
106    pub fn position(&self) -> i32 {
107        self.position.steps()
108    }
109
110    /// Returns the drive pattern in use.
111    pub fn drive(&self) -> Drive {
112        self.sequencer.drive()
113    }
114
115    /// Gives back the coil lines and the delay.
116    ///
117    /// # Returns
118    ///
119    /// The pins and delay the driver was built from.
120    pub fn release(self) -> ((A, B, C, Dp), D) {
121        (self.coils, self.delay)
122    }
123}
124
125impl<A, B, C, Dp, D, E> FourWire<A, B, C, Dp, D>
126where
127    A: OutputPin<Error = E>,
128    B: OutputPin<Error = E>,
129    C: OutputPin<Error = E>,
130    Dp: OutputPin<Error = E>,
131    D: DelayNs,
132{
133    /// Takes one step and waits the step interval.
134    ///
135    /// # Arguments
136    ///
137    /// * `direction` - which way to step.
138    ///
139    /// # Errors
140    ///
141    /// Returns [`DriverError::Bus`] if a coil line cannot be driven.
142    pub fn step(&mut self, direction: Direction) -> Result<(), DriverError<E>> {
143        let coils = self.sequencer.step(direction);
144        self.energize(coils)?;
145        self.position.step(direction);
146        self.delay.delay_us(self.step_micros);
147        Ok(())
148    }
149
150    /// Takes `count` steps, backward when negative.
151    ///
152    /// # Arguments
153    ///
154    /// * `count` - the signed number of steps.
155    ///
156    /// # Errors
157    ///
158    /// Returns [`DriverError::Bus`] if a coil line cannot be driven; the steps
159    /// already taken stay counted.
160    pub fn steps(&mut self, count: i32) -> Result<(), DriverError<E>> {
161        let direction = direction_of(count);
162        for _ in 0..count.unsigned_abs() {
163            self.step(direction)?;
164        }
165        Ok(())
166    }
167
168    /// Drops every coil, so the motor holds nothing and draws nothing.
169    ///
170    /// # Errors
171    ///
172    /// Returns [`DriverError::Bus`] if a coil line cannot be driven.
173    pub fn idle(&mut self) -> Result<(), DriverError<E>> {
174        self.energize(0)
175    }
176
177    fn energize(&mut self, coils: u8) -> Result<(), DriverError<E>> {
178        let (a, b, c, d) = &mut self.coils;
179        a.set_state((coils & 0b1000 != 0).into())
180            .map_err(DriverError::Bus)?;
181        b.set_state((coils & 0b0100 != 0).into())
182            .map_err(DriverError::Bus)?;
183        c.set_state((coils & 0b0010 != 0).into())
184            .map_err(DriverError::Bus)?;
185        d.set_state((coils & 0b0001 != 0).into())
186            .map_err(DriverError::Bus)
187    }
188}
189
190impl<A, B, C, Dp, D, E> Actuator for FourWire<A, B, C, Dp, D>
191where
192    A: OutputPin<Error = E> + Send,
193    B: OutputPin<Error = E> + Send,
194    C: OutputPin<Error = E> + Send,
195    Dp: OutputPin<Error = E> + Send,
196    D: DelayNs + Send,
197    E: core::fmt::Debug,
198{
199    type Command = i32;
200
201    async fn apply(&mut self, count: i32) -> pamoja_core::Result<()> {
202        self.steps(count).map_err(pamoja_core::Error::from)
203    }
204}
205
206/// A stepper behind a step/direction driver chip such as an A4988 or a DRV8825.
207///
208/// Each step sets the direction line and holds it for the pulse width, since the
209/// chip reads the direction on the step line's rising edge and needs it settled
210/// first. Then it pulses the step line high for the pulse width, brings it low, and
211/// waits the step interval. Microstepping, current limiting, and enable are the
212/// chip's own pins and settings, outside this driver. As an [`Actuator`] the command
213/// is a signed step count.
214///
215/// # Examples
216///
217/// ```
218/// use pamoja_actuators::stepper::StepDir;
219/// use pamoja_core::Actuator;
220/// use pamoja_hal::digital::PinState::{High, Low};
221/// use pamoja_hal::script::{block_on, DelayLog, PinScript};
222///
223/// let mut motor = StepDir::new(PinScript::new([]), PinScript::new([]), DelayLog::new());
224/// block_on(motor.apply(-1))?;
225/// assert_eq!(motor.position(), -1);
226///
227/// let ((step, direction), _) = motor.release();
228/// assert_eq!(direction.driven(), [Low]);
229/// assert_eq!(step.driven(), [High, Low]);
230/// # Ok::<(), pamoja_core::Error>(())
231/// ```
232#[derive(Debug)]
233pub struct StepDir<S, R, D> {
234    step: S,
235    direction: R,
236    delay: D,
237    position: Position,
238    pulse_micros: u32,
239    step_micros: u32,
240}
241
242impl<S, R, D> StepDir<S, R, D> {
243    /// Wraps the step and direction lines, without driving them.
244    ///
245    /// # Arguments
246    ///
247    /// * `step` - the line the chip counts rising edges on.
248    /// * `direction` - the line the chip reads the direction from; high is forward.
249    /// * `delay` - the timer that shapes the pulses and paces the steps.
250    ///
251    /// # Returns
252    ///
253    /// The driver, with [`DEFAULT_PULSE_MICROS`] pulses at [`DEFAULT_STEP_MICROS`]
254    /// a step, counting from zero.
255    pub fn new(step: S, direction: R, delay: D) -> Self {
256        StepDir {
257            step,
258            direction,
259            delay,
260            position: Position::new(),
261            pulse_micros: DEFAULT_PULSE_MICROS,
262            step_micros: DEFAULT_STEP_MICROS,
263        }
264    }
265
266    /// Sets the width of the step pulse, and how long the direction is held before it.
267    ///
268    /// # Arguments
269    ///
270    /// * `micros` - the time the direction line is held before the step line rises,
271    ///   and the time the step line is then held high.
272    ///
273    /// # Returns
274    ///
275    /// The driver.
276    pub fn with_pulse_width(mut self, micros: u32) -> Self {
277        self.pulse_micros = micros;
278        self
279    }
280
281    /// Sets the pause after each step, which sets the speed.
282    ///
283    /// # Arguments
284    ///
285    /// * `micros` - the interval in microseconds.
286    ///
287    /// # Returns
288    ///
289    /// The driver.
290    pub fn with_step_interval(mut self, micros: u32) -> Self {
291        self.step_micros = micros;
292        self
293    }
294
295    /// Returns the step count since the driver was built, forward positive.
296    pub fn position(&self) -> i32 {
297        self.position.steps()
298    }
299
300    /// Gives back the lines and the delay.
301    ///
302    /// # Returns
303    ///
304    /// The step and direction pins and the delay the driver was built from.
305    pub fn release(self) -> ((S, R), D) {
306        ((self.step, self.direction), self.delay)
307    }
308}
309
310impl<S, R, D, E> StepDir<S, R, D>
311where
312    S: OutputPin<Error = E>,
313    R: OutputPin<Error = E>,
314    D: DelayNs,
315{
316    /// Takes one step and waits the step interval.
317    ///
318    /// # Arguments
319    ///
320    /// * `direction` - which way to step.
321    ///
322    /// # Errors
323    ///
324    /// Returns [`DriverError::Bus`] if a line cannot be driven.
325    pub fn step(&mut self, direction: Direction) -> Result<(), DriverError<E>> {
326        self.direction
327            .set_state((direction == Direction::Forward).into())
328            .map_err(DriverError::Bus)?;
329        self.delay.delay_us(self.pulse_micros);
330        self.step.set_high().map_err(DriverError::Bus)?;
331        self.delay.delay_us(self.pulse_micros);
332        self.step.set_low().map_err(DriverError::Bus)?;
333        self.position.step(direction);
334        self.delay.delay_us(self.step_micros);
335        Ok(())
336    }
337
338    /// Takes `count` steps, backward when negative.
339    ///
340    /// # Arguments
341    ///
342    /// * `count` - the signed number of steps.
343    ///
344    /// # Errors
345    ///
346    /// Returns [`DriverError::Bus`] if a line cannot be driven; the steps already
347    /// taken stay counted.
348    pub fn steps(&mut self, count: i32) -> Result<(), DriverError<E>> {
349        let direction = direction_of(count);
350        for _ in 0..count.unsigned_abs() {
351            self.step(direction)?;
352        }
353        Ok(())
354    }
355}
356
357impl<S, R, D, E> Actuator for StepDir<S, R, D>
358where
359    S: OutputPin<Error = E> + Send,
360    R: OutputPin<Error = E> + Send,
361    D: DelayNs + Send,
362    E: core::fmt::Debug,
363{
364    type Command = i32;
365
366    async fn apply(&mut self, count: i32) -> pamoja_core::Result<()> {
367        self.steps(count).map_err(pamoja_core::Error::from)
368    }
369}
370
371#[cfg(test)]
372mod tests {
373    use super::*;
374    use alloc::vec::Vec;
375    use core::cell::RefCell;
376    use core::convert::Infallible;
377    use embedded_hal::digital::{ErrorType, PinState};
378    use pamoja_hal::digital::PinState::{High, Low};
379    use pamoja_hal::script::{block_on, DelayLog, PinScript};
380
381    #[derive(Debug, PartialEq)]
382    enum Event {
383        Direction(PinState),
384        Step(PinState),
385        Wait(u32),
386    }
387
388    struct Line<'a> {
389        log: &'a RefCell<Vec<Event>>,
390        event: fn(PinState) -> Event,
391    }
392
393    impl ErrorType for Line<'_> {
394        type Error = Infallible;
395    }
396
397    impl OutputPin for Line<'_> {
398        fn set_low(&mut self) -> Result<(), Infallible> {
399            self.log.borrow_mut().push((self.event)(PinState::Low));
400            Ok(())
401        }
402
403        fn set_high(&mut self) -> Result<(), Infallible> {
404            self.log.borrow_mut().push((self.event)(PinState::High));
405            Ok(())
406        }
407    }
408
409    struct Timeline<'a>(&'a RefCell<Vec<Event>>);
410
411    impl DelayNs for Timeline<'_> {
412        fn delay_ns(&mut self, ns: u32) {
413            self.0.borrow_mut().push(Event::Wait(ns));
414        }
415    }
416
417    fn pins() -> (PinScript, PinScript, PinScript, PinScript) {
418        (
419            PinScript::new([]),
420            PinScript::new([]),
421            PinScript::new([]),
422            PinScript::new([]),
423        )
424    }
425
426    #[test]
427    fn full_steps_energize_the_datasheet_pairs_in_order() {
428        let mut motor =
429            FourWire::new(pins(), DelayLog::new(), Drive::FullStep).with_step_interval(1_500);
430        motor.steps(4).unwrap();
431        assert_eq!(motor.position(), 4);
432        assert_eq!(motor.drive(), Drive::FullStep);
433        let ((a, b, c, d), delay) = motor.release();
434        assert_eq!(a.driven(), [Low, Low, High, High]);
435        assert_eq!(b.driven(), [High, Low, Low, High]);
436        assert_eq!(c.driven(), [High, High, Low, Low]);
437        assert_eq!(d.driven(), [Low, High, High, Low]);
438        assert_eq!(delay.waits_ns(), [1_500_000; 4]);
439    }
440
441    #[test]
442    fn backward_steps_walk_the_pattern_the_other_way_and_count_down() {
443        let mut motor = FourWire::new(pins(), DelayLog::new(), Drive::Wave);
444        motor.steps(-2).unwrap();
445        assert_eq!(motor.position(), -2);
446        motor.idle().unwrap();
447        let ((a, _, _, d), _) = motor.release();
448        assert_eq!(a.driven(), [Low, Low, Low]);
449        assert_eq!(
450            d.driven(),
451            [High, Low, Low],
452            "wave drive backward starts at coil D"
453        );
454    }
455
456    #[test]
457    fn a_four_wire_motor_is_an_actuator_taking_signed_steps() {
458        let mut motor = FourWire::new(pins(), DelayLog::new(), Drive::HalfStep);
459        block_on(motor.apply(3)).unwrap();
460        block_on(motor.apply(-1)).unwrap();
461        assert_eq!(motor.position(), 2);
462    }
463
464    #[test]
465    fn step_dir_pulses_the_step_line_with_the_direction_set_first() {
466        let mut motor = StepDir::new(PinScript::new([]), PinScript::new([]), DelayLog::new())
467            .with_pulse_width(5)
468            .with_step_interval(1_000);
469        motor.steps(2).unwrap();
470        motor.steps(-1).unwrap();
471        assert_eq!(motor.position(), 1);
472        let ((step, direction), delay) = motor.release();
473        assert_eq!(direction.driven(), [High, High, Low]);
474        assert_eq!(step.driven(), [High, Low, High, Low, High, Low]);
475        assert_eq!(delay.waits_ns()[..3], [5_000, 5_000, 1_000_000]);
476    }
477
478    #[test]
479    fn step_dir_settles_the_direction_before_the_rising_edge_as_the_timing_diagram_asks() {
480        let log = RefCell::new(Vec::new());
481        let step = Line {
482            log: &log,
483            event: Event::Step,
484        };
485        let direction = Line {
486            log: &log,
487            event: Event::Direction,
488        };
489        let mut motor = StepDir::new(step, direction, Timeline(&log))
490            .with_pulse_width(2)
491            .with_step_interval(1_000);
492        motor.step(Direction::Backward).unwrap();
493        assert_eq!(
494            *log.borrow(),
495            [
496                Event::Direction(PinState::Low),
497                Event::Wait(2_000),
498                Event::Step(PinState::High),
499                Event::Wait(2_000),
500                Event::Step(PinState::Low),
501                Event::Wait(1_000_000),
502            ],
503            "the A4988 wants 200 ns of setup and 1 us high; the DRV8825 650 ns and 1.9 us"
504        );
505    }
506
507    #[test]
508    fn a_step_dir_motor_is_an_actuator_taking_signed_steps() {
509        let mut motor = StepDir::new(PinScript::new([]), PinScript::new([]), DelayLog::new());
510        block_on(motor.apply(-4)).unwrap();
511        assert_eq!(motor.position(), -4);
512    }
513}
pamoja_actuators/stepper/driver.rs

pamoja_actuators/stepper/
driver.rs
