ev3dev_rs/pupdevices/
motor.rs

1use fixed::types::I32F32;
2use scopeguard::defer;
3use tokio::time::interval;
4
5use crate::{
6    attribute::AttributeName,
7    enum_str,
8    enum_string::AsStr,
9    error::{Ev3Error, Ev3Result},
10    motor_driver::MotorDriver,
11    parameters::{Direction, MotorPort, Stop},
12};
13use std::{cell::Cell, collections::HashSet, str::FromStr, time::Duration};
14
15enum_str! {
16    Command,
17
18    (RunForever, "run-forever"),
19    (RunToAbsolutePosition, "run-to-abs-pos"),
20    (RunToRelativePosition, "run-to-rel-pos"),
21    (RunTimed, "run-timed"),
22    (RunDirect, "run-direct"),
23    (Stop, "stop"),
24    (Reset, "reset"),
25}
26
27enum_str! {
28    State,
29
30    (Running, "running"),
31    (Ramping, "ramping"),
32    (Holding, "holding"),
33    (Overloaded, "overloaded"),
34    (Stalled, "stalled"),
35}
36
37/// NXT motor, EV3 large and medium motors
38#[allow(dead_code)]
39pub struct Motor {
40    driver: MotorDriver,
41    direction: Direction,
42    last_command: Cell<Option<Command>>,
43    count_per_rot: u32,
44    count_per_degree: u32,
45    pub(crate) max_speed: I32F32,
46}
47
48impl Motor {
49    /// Tries to find a `Motor` on the given port
50    ///
51    /// If no motor is found, returns `MotorNotFound`.
52    ///
53    /// Note that the motor is not reset upon initialization.
54    ///
55    /// #Examples
56    ///
57    /// ``` no_run
58    /// use ev3dev_rs::pupdevices::Motor;
59    /// use ev3dev_rs::parameters::{MotorPort, Direction};
60    ///
61    /// let motor = Motor::new(MotorPort::OutA, Direction::Clockwise);
62    /// motor.reset()?;
63    /// motor.run_target(300, 360)?;
64    /// ```
65    pub fn new(port: MotorPort, direction: Direction) -> Ev3Result<Self> {
66        let driver = MotorDriver::new(port)?;
67
68        // reset the motor upon initialization
69        driver.set_attribute_enum(AttributeName::Command, Command::Reset)?;
70
71        driver.set_attribute_enum(AttributeName::Polarity, direction)?;
72
73        let count_per_rot: u32 = driver
74            .read_attribute(AttributeName::CountPerRotation)?
75            .parse()?;
76
77        Ok(Self {
78            driver,
79            direction,
80            last_command: Cell::new(None),
81            count_per_rot,
82            count_per_degree: count_per_rot / 360,
83            max_speed: I32F32::from_num(1000),
84        })
85    }
86
87    fn get_states(&self) -> Ev3Result<HashSet<State>> {
88        let mut states = HashSet::new();
89
90        for flag in self
91            .driver
92            .read_attribute(AttributeName::State)?
93            .split_ascii_whitespace()
94        {
95            if let Ok(state) = State::from_str(flag) {
96                states.insert(state);
97            }
98        }
99
100        Ok(states)
101    }
102
103    fn send_command(&self, command: Command) -> Ev3Result<()> {
104        self.driver
105            .set_attribute_enum(AttributeName::Command, command)?;
106        self.last_command.set(Some(command));
107        Ok(())
108    }
109
110    /// sets the stop action for the `Motor`
111    pub fn set_stop_action(&self, action: Stop) -> Ev3Result<()> {
112        self.driver
113            .set_attribute_enum(AttributeName::StopAction, action)
114    }
115
116    async fn wait_for_stop(&self) -> Ev3Result<()> {
117        defer! {
118            _ = self.send_command(Command::Stop);
119        }
120
121        let mut timer = interval(Duration::from_millis(5));
122
123        // the first tick completes immediately
124        timer.tick().await;
125
126        while self.get_states()?.contains(&State::Running) {
127            timer.tick().await;
128        }
129
130        Ok(())
131    }
132
133    fn set_speed(&self, speed: i32) -> Ev3Result<()> {
134        self.driver
135            .set_attribute(AttributeName::SpeedSetpoint, speed)
136    }
137
138    /// Units are in milliseconds and must be positive.
139    ///
140    /// When set to a non-zero value, the motor speed will increase from 0 to 100% of max_speed over the span of this setpoint.
141    ///
142    /// This is especially useful for avoiding wheel slip.
143    pub fn set_ramp_up_setpoint(&self, sp: u32) -> Ev3Result<()> {
144        self.driver.set_attribute(AttributeName::RampUpSetpoint, sp)
145    }
146
147    /// Units are in milliseconds and must be positive.
148    ///
149    /// When set to a non-zero value, the motor speed will decrease from 0 to 100% of max_speed over the span of this setpoint.
150    ///
151    /// This is especially useful for avoiding wheel slip.
152    pub fn set_ramp_down_setpoint(&self, sp: u32) -> Ev3Result<()> {
153        self.driver
154            .set_attribute(AttributeName::RampDownSetpoint, sp)
155    }
156
157    /// Resets all the motor parameters to their default values.
158    ///
159    /// This also has the effect of stopping the motor.
160    pub fn reset(&self) -> Ev3Result<()> {
161        self.send_command(Command::Reset)?;
162        self.driver
163            .set_attribute_enum(AttributeName::Polarity, self.direction)
164    }
165
166    /// Stops the motor with the previously selected stop action.
167    pub fn stop_prev_action(&self) -> Ev3Result<()> {
168        self.send_command(Command::Stop)
169    }
170
171    /// Stops the motor and lets it spin freely.
172    ///
173    /// This also has the effect of setting the motor's stop action to `coast`.
174    pub fn stop(&self) -> Ev3Result<()> {
175        self.set_stop_action(Stop::Coast)?;
176        self.send_command(Command::Stop)
177    }
178
179    /// Stops the motor and passively brakes it by generating current.
180    ///
181    /// This also has the effect of setting the motor's stop action to `brake`.
182    pub fn brake(&self) -> Ev3Result<()> {
183        self.set_stop_action(Stop::Brake)?;
184        self.send_command(Command::Stop)
185    }
186
187    /// Stops the motor and actively holds it at its current angle.
188    ///
189    /// This also has the effect of setting the motor's stop action to `hold`.
190    pub fn hold(&self) -> Ev3Result<()> {
191        self.set_stop_action(Stop::Hold)?;
192        self.send_command(Command::Stop)
193    }
194
195    /// Gets the rotation angle of the motor.
196    pub fn angle(&self) -> Ev3Result<i32> {
197        Ok(self
198            .driver
199            .read_attribute(AttributeName::Position)?
200            .parse::<i32>()?
201            / self.count_per_degree as i32)
202    }
203
204    /// Runs the motor at a constant speed by a given angle.
205    pub async fn run_angle(&self, speed: i32, rotation_angle: i32) -> Ev3Result<()> {
206        self.set_speed(speed)?;
207
208        self.driver
209            .set_attribute(AttributeName::PositionSetpoint, rotation_angle)?;
210
211        self.send_command(Command::RunToRelativePosition)?;
212
213        self.wait_for_stop().await
214    }
215
216    /// Runs the motor at a constant speed to a towards a target angle.
217    ///
218    /// Note that the angle is continuous and does not wrap around at 360 degrees.
219    ///
220    /// Additionally, the motor's position is not necessarily zero at the start of your program.
221    ///
222    /// To guarantee that the starting position is zero, you can use the `reset` method.
223    pub async fn run_target(&self, speed: i32, target_angle: i32) -> Ev3Result<()> {
224        self.set_speed(speed)?;
225
226        self.driver
227            .set_attribute(AttributeName::PositionSetpoint, target_angle)?;
228
229        self.send_command(Command::RunToAbsolutePosition)?;
230
231        self.wait_for_stop().await
232    }
233
234    /// Runs the motor at a constant speed.
235    ///
236    /// The motor will run at this speed until manually stopped, or you give it a new command.
237    pub fn run(&self, speed: i32) -> Ev3Result<()> {
238        self.set_speed(speed)?;
239
240        self.send_command(Command::RunForever)
241    }
242
243    /// Runs the motor at a constant speed for a given duration.
244    pub async fn run_time(&self, speed: i32, time: Duration) -> Ev3Result<()> {
245        self.set_speed(speed)?;
246
247        self.driver
248            .set_attribute(AttributeName::TimeSetpoint, time.as_millis())?;
249
250        self.send_command(Command::RunTimed)?;
251
252        self.wait_for_stop().await
253    }
254
255    /// Runs at a given duty cycle percentage (-100 to 100) until stalled.
256    pub async fn run_until_stalled(&self, power: i32) -> Ev3Result<()> {
257        defer! {
258            _ = self.send_command(Command::Stop)
259        }
260
261        self.dc(power)?;
262        let mut timer = interval(Duration::from_millis(5));
263
264        let mut states = self.get_states()?;
265
266        // the first tick completes immediately
267        timer.tick().await;
268
269        while states.contains(&State::Running) && !states.contains(&State::Stalled) {
270            timer.tick().await;
271            states = self.get_states()?;
272        }
273
274        Ok(())
275    }
276
277    /// Rotates the motor at a given duty cycle percentage (-100 to 100) until stopped, or you give it a new command.
278    pub fn dc(&self, duty: i32) -> Ev3Result<()> {
279        if self.last_command.get() != Some(Command::RunDirect) {
280            self.send_command(Command::RunDirect)?;
281        }
282
283        self.driver
284            .set_attribute(AttributeName::DutyCycleSetpoint, duty)
285    }
286}
ev3dev_rs/pupdevices/motor.rs

ev3dev_rs/pupdevices/
motor.rs
