stepper_motion/trajectory/
builder.rs

1//! Trajectory builder for programmatic trajectory creation.
2
3use heapless::String;
4
5use crate::config::{TrajectoryConfig, WaypointTrajectory};
6use crate::config::units::{Degrees, DegreesPerSecSquared};
7use crate::error::{Error, Result, TrajectoryError};
8
9/// Builder for creating single-target trajectories.
10#[derive(Debug, Clone)]
11pub struct TrajectoryBuilder {
12    motor: Option<String<32>>,
13    target_degrees: Option<Degrees>,
14    velocity_percent: u8,
15    acceleration_percent: u8,
16    acceleration: Option<DegreesPerSecSquared>,
17    deceleration: Option<DegreesPerSecSquared>,
18    dwell_ms: Option<u32>,
19}
20
21impl Default for TrajectoryBuilder {
22    fn default() -> Self {
23        Self::new()
24    }
25}
26
27impl TrajectoryBuilder {
28    /// Create a new trajectory builder.
29    pub fn new() -> Self {
30        Self {
31            motor: None,
32            target_degrees: None,
33            velocity_percent: 100,
34            acceleration_percent: 100,
35            acceleration: None,
36            deceleration: None,
37            dwell_ms: None,
38        }
39    }
40
41    /// Set the target motor name.
42    pub fn motor(mut self, name: &str) -> Self {
43        self.motor = String::try_from(name).ok();
44        self
45    }
46
47    /// Set the target position in degrees.
48    pub fn target(mut self, position: Degrees) -> Self {
49        self.target_degrees = Some(position);
50        self
51    }
52
53    /// Set velocity as percentage of motor's max (1-200).
54    pub fn velocity_percent(mut self, percent: u8) -> Self {
55        self.velocity_percent = percent.clamp(1, 200);
56        self
57    }
58
59    /// Set acceleration as percentage of motor's max (1-200).
60    pub fn acceleration_percent(mut self, percent: u8) -> Self {
61        self.acceleration_percent = percent.clamp(1, 200);
62        self
63    }
64
65    /// Set absolute acceleration rate in degrees/sec².
66    pub fn acceleration(mut self, accel: DegreesPerSecSquared) -> Self {
67        self.acceleration = Some(accel);
68        self
69    }
70
71    /// Set absolute deceleration rate in degrees/sec².
72    pub fn deceleration(mut self, decel: DegreesPerSecSquared) -> Self {
73        self.deceleration = Some(decel);
74        self
75    }
76
77    /// Set asymmetric acceleration/deceleration rates.
78    pub fn asymmetric(mut self, accel: DegreesPerSecSquared, decel: DegreesPerSecSquared) -> Self {
79        self.acceleration = Some(accel);
80        self.deceleration = Some(decel);
81        self
82    }
83
84    /// Set dwell time at target in milliseconds.
85    pub fn dwell(mut self, dwell_ms: u32) -> Self {
86        self.dwell_ms = Some(dwell_ms);
87        self
88    }
89
90    /// Build the trajectory configuration.
91    ///
92    /// # Errors
93    ///
94    /// Returns an error if required fields are missing.
95    pub fn build(self) -> Result<TrajectoryConfig> {
96        let motor = self.motor.ok_or_else(|| {
97            Error::Trajectory(TrajectoryError::InvalidName(
98                String::try_from("motor not specified").unwrap(),
99            ))
100        })?;
101
102        let target_degrees = self.target_degrees.ok_or_else(|| {
103            Error::Trajectory(TrajectoryError::InvalidName(
104                String::try_from("target not specified").unwrap(),
105            ))
106        })?;
107
108        Ok(TrajectoryConfig {
109            motor,
110            target_degrees,
111            velocity_percent: self.velocity_percent,
112            acceleration_percent: self.acceleration_percent,
113            acceleration: self.acceleration,
114            deceleration: self.deceleration,
115            dwell_ms: self.dwell_ms,
116        })
117    }
118}
119
120/// Maximum number of waypoints in a trajectory.
121pub const MAX_WAYPOINTS: usize = 32;
122
123/// Builder for creating waypoint trajectories.
124#[derive(Debug, Clone)]
125pub struct WaypointTrajectoryBuilder {
126    motor: Option<String<32>>,
127    waypoints: heapless::Vec<Degrees, MAX_WAYPOINTS>,
128    velocity_percent: u8,
129    dwell_ms: u32,
130}
131
132impl Default for WaypointTrajectoryBuilder {
133    fn default() -> Self {
134        Self::new()
135    }
136}
137
138impl WaypointTrajectoryBuilder {
139    /// Create a new waypoint trajectory builder.
140    pub fn new() -> Self {
141        Self {
142            motor: None,
143            waypoints: heapless::Vec::new(),
144            velocity_percent: 100,
145            dwell_ms: 0,
146        }
147    }
148
149    /// Set the target motor name.
150    pub fn motor(mut self, name: &str) -> Self {
151        self.motor = String::try_from(name).ok();
152        self
153    }
154
155    /// Add a waypoint at the given position.
156    pub fn waypoint(mut self, position: Degrees) -> Self {
157        let _ = self.waypoints.push(position);
158        self
159    }
160
161    /// Add multiple waypoints.
162    pub fn waypoints(mut self, positions: &[Degrees]) -> Self {
163        for pos in positions {
164            let _ = self.waypoints.push(*pos);
165        }
166        self
167    }
168
169    /// Set velocity as percentage of motor's max (1-200).
170    pub fn velocity_percent(mut self, percent: u8) -> Self {
171        self.velocity_percent = percent.clamp(1, 200);
172        self
173    }
174
175    /// Set dwell time at each waypoint in milliseconds.
176    pub fn dwell(mut self, dwell_ms: u32) -> Self {
177        self.dwell_ms = dwell_ms;
178        self
179    }
180
181    /// Build the waypoint trajectory configuration.
182    ///
183    /// # Errors
184    ///
185    /// Returns an error if required fields are missing or waypoints are empty.
186    pub fn build(self) -> Result<WaypointTrajectory> {
187        let motor = self.motor.ok_or_else(|| {
188            Error::Trajectory(TrajectoryError::InvalidName(
189                String::try_from("motor not specified").unwrap(),
190            ))
191        })?;
192
193        if self.waypoints.is_empty() {
194            return Err(Error::Trajectory(TrajectoryError::Empty));
195        }
196
197        Ok(WaypointTrajectory {
198            motor,
199            waypoints: self.waypoints,
200            velocity_percent: self.velocity_percent,
201            dwell_ms: self.dwell_ms,
202        })
203    }
204}
stepper_motion/trajectory/builder.rs

stepper_motion/trajectory/
builder.rs
