1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120

//! Flat motion profile
//!
//! See [`Flat`].

use fixed::FixedU32;

use crate::MotionProfile;

/// Flat motion profile
///
/// This is the simplest possible motion profile, as it produces just a constant
/// velocity. Please note that this is of limited use, and should probably be
/// restricted to testing.
///
/// Theoretically, this profile produces infinite acceleration/deceleration at
/// the beginning and end of the movement. In practice, you might get away with
/// this, if the velocity and the load on the motor are low enough. Otherwise,
/// this will definitely produce missed steps.
///
/// Create an instance of this struct using [`Flat::new`], then use the API
/// defined by [`MotionProfile`] (which this struct implements) to generate the
/// acceleration ramp.
///
/// # Unit of Time
///
/// This code is agnostic on which unit of time is used. If you provide the
/// target velocity in steps per second, the unit of the delay returned will be
/// seconds.
///
/// This allows you to pass the target velocity in steps per number of timer
/// counts for the timer you're using, completely eliminating any conversion
/// overhead for the delay.
///
/// # Type Parameter
///
/// The type parameter `Num` defines the type that is used to represent the
/// target velocity and the delay per step. It is set to a 32-bit fixed-point
/// number type by default.
///
/// This default is appropriate for 32-bit microcontrollers, but it might not
/// be ideal for 8- or 16-bit microcontrollers, or target platforms where
/// hardware support for floating point numbers is available. You can override
/// it with other types from the `fixed` crate, or `f32`/`f64`, for example.
pub struct Flat<Num = DefaultNum> {
    delay: Option<Num>,
    num_steps: u32,
}

impl<Num> Flat<Num> {
    /// Create a new instance of `Flat`
    pub fn new() -> Self {
        Self {
            delay: None,
            num_steps: 0,
        }
    }
}

impl Default for Flat<f32> {
    fn default() -> Self {
        Self::new()
    }
}

impl<Num> MotionProfile for Flat<Num>
where
    Num: Copy + num_traits::Zero + num_traits::Inv<Output = Num>,
{
    type Velocity = Num;
    type Delay = Num;

    fn enter_position_mode(
        &mut self,
        max_velocity: Self::Velocity,
        num_steps: u32,
    ) {
        self.delay = if max_velocity.is_zero() {
            None
        } else {
            Some(max_velocity.inv())
        };

        self.num_steps = num_steps;
    }

    fn next_delay(&mut self) -> Option<Self::Delay> {
        if self.num_steps == 0 {
            return None;
        }

        self.num_steps -= 1;

        self.delay
    }
}

/// The default numeric type used by [`Flat`]
pub type DefaultNum = FixedU32<typenum::U16>;

#[cfg(test)]
mod tests {
    use crate::{Flat, MotionProfile as _};

    #[test]
    fn flat_should_pass_motion_profile_tests() {
        crate::util::testing::test::<Flat<f32>>();
    }

    #[test]
    fn flat_should_produce_constant_velocity() {
        let mut flat = Flat::new();

        let max_velocity = 1000.0;
        flat.enter_position_mode(max_velocity, 200);

        for velocity in flat.velocities() {
            assert_eq!(velocity, max_velocity);
        }
    }
}