odrive_rs/commands/
mod.rs

1use std::fmt::Display;
2use std::io::{BufReader, Error, Read, Write};
3use std::io;
4use std::time::Instant;
5
6use crate::enumerations::{AxisID, AxisState, ControlMode, EncoderMode};
7use crate::enumerations::errors::{ODriveError, ODriveResult};
8
9#[cfg(test)]
10#[cfg_attr(tarpaulin, skip)]
11mod command_tests;
12
13/// The `ODrive` struct manages a connection with an ODrive motor over the ASCII protocol.
14/// It acts as a newtype around a connection stream.
15/// This has been tested using serial types from `serialport-rs`.
16#[derive(Debug)]
17pub struct ODrive<T> where T: Read {
18    io_stream: BufReader<T>,
19}
20
21impl<T> ODrive<T> where T: Read {
22    /// Although any type can be passed in here, it is suggested that the supplied type `T` be
23    /// `Read + Write`. Doing so will unlock the full API.
24    pub fn new(io_stream: T) -> Self {
25        Self {
26            io_stream: BufReader::new(io_stream)
27        }
28    }
29}
30
31/// An implementation of `Write` has been provided as an escape hatch to enable the usage of
32/// operations not yet supported by this library.
33impl<T> Write for ODrive<T> where T: Write + Read {
34    fn write(&mut self, buf: &[u8]) -> Result<usize, Error> {
35        self.io_stream.get_mut().write(buf)
36    }
37
38    fn flush(&mut self) -> Result<(), Error> {
39        self.io_stream.get_mut().flush()
40    }
41}
42
43/// An implementation of `Write` has been provided as an escape hatch to enable the usage of
44/// operations not yet supported by this library. Be advised that using this implementation may
45/// place the connection into an inconsistent state.
46impl<T> Read for ODrive<T> where T: Read {
47    fn read(&mut self, buf: &mut [u8]) -> Result<usize, Error> {
48        self.io_stream.read(buf)
49    }
50}
51
52impl<T> ODrive<T> where T: Read {
53    /// Reads the next message sent by the ODrive as a string.
54    /// If their is no message, this function should return `None`
55    ///
56    /// It is suggested that you only use this if you are directly using the `Write` implementation
57    /// and are expecting a response, as normally the supplied for the ODrive can directly support
58    /// reading any response.
59    pub fn read_string(&mut self) -> io::Result<Option<String>> {
60        let mut string = String::with_capacity(20);
61        let duration = Instant::now();
62        loop {
63            let mut buffer = [0; 1];
64            while self.read(&mut buffer).unwrap_or_default() == 0 {
65                if duration.elapsed().as_millis() >= 1_000 {
66                    return Ok(None);
67                }
68            }
69            let ch = buffer[0];
70            if ch as char == '\n' {
71                break;
72            }
73
74            string.push(ch as char);
75        }
76
77        Ok(Some(string.trim().to_owned()))
78    }
79
80    pub fn read_odrive_response(&mut self) -> ODriveResult<String> {
81        let mut string = String::with_capacity(20);
82        let duration = Instant::now();
83        loop {
84            let mut buffer = [0; 1];
85            while self.read(&mut buffer).unwrap_or_default() == 0 {
86                if duration.elapsed().as_millis() >= 1_000 {
87                    return Err(ODriveError::NoMessageReceived);
88                }
89            }
90            let ch = buffer[0];
91            if ch as char == '\n' {
92                break;
93            }
94
95            string.push(ch as char);
96        }
97
98        Ok(string.trim().to_owned())
99    }
100
101    /// Reads the next message as a float. This will return zero if the message is not a valid
102    /// float.
103    ///
104    /// It is suggested that you only use this if you are directly using the `Write` implementation
105    /// and are expecting a response, as normally the supplied for the ODrive can directly support
106    /// reading any response.
107    pub fn read_float(&mut self) -> io::Result<Option<f32>> {
108        Ok(self.read_string()?.map(|s| s.parse().unwrap_or_default()))
109    }
110
111    /// Reads the next message as an int. This will return zero if the message is not a valid int.
112    ///
113    /// It is suggested that you only use this if you are directly using the `Write` implementation
114    /// and are expecting a response, as normally the supplied for the ODrive can directly support
115    /// reading any response.
116    pub fn read_int(&mut self) -> io::Result<Option<i32>> {
117        Ok(self.read_string()?.map(|s| s.parse().unwrap_or_default()))
118    }
119}
120
121impl<T> ODrive<T> where T: Write + Read {
122    /// Move the motor to a position. Use this command if you have a real-time controller which
123    /// is streaming setpoints and tracking a trajectory.
124    /// `axis` The motor to be used for the operation.
125    /// `position` is the desired position, in encoder counts.
126    /// `velocity_feed_forward` is the velocity feed forward term, in encoder counts per second.
127    /// `current_feed_forward` is the current feed forward term, in amps.
128    /// If `None` is supplied for a feed forward input, zero will be provided as a default.
129    pub fn set_position_p(&mut self, axis: AxisID, position: f32, velocity_feed_forward: Option<f32>,
130                          current_feed_forward: Option<f32>) -> io::Result<()> {
131        let velocity_feed_forward = velocity_feed_forward.unwrap_or_default();
132        let current_feed_forward = current_feed_forward.unwrap_or_default();
133        writeln!(self, "p {} {} {} {}", axis as u8, position, velocity_feed_forward, current_feed_forward)?;
134        self.flush()
135    }
136
137    /// Move the motor to a position. Use this command if you are sending one setpoint at a time.
138    /// `axis` The motor to be used for the operation.
139    /// `position` is the desired position, in encoder counts.
140    /// `velocity_limit` is the velocity limit, in encoder counts per second.
141    /// `current_limit` is the current limit, in amps.
142    /// If `None` is supplied for a limit, zero will be provided as a default.
143    pub fn set_position_q(&mut self, axis: AxisID, position: f32, velocity_limit: Option<f32>,
144                          current_limit: Option<f32>) -> io::Result<()> {
145        let velocity_limit = velocity_limit.unwrap_or_default();
146        let current_limit = current_limit.unwrap_or_default();
147        writeln!(self, "q {} {} {} {}", axis as u8, position, velocity_limit, current_limit)?;
148        self.flush()
149    }
150
151    /// Specifies a velocity setpoint for the motor.
152    /// `axis` The motor to be used for the operation.
153    /// `velocity` is the velocity setpoint, in encoder counts per second.
154    /// `current_feed_forward` is the current feed forward term, in amps.
155    /// If `None` is supplied for a feed forward input, zero will be provided as a default.
156    pub fn set_velocity(&mut self, axis: AxisID, velocity: f32, current_feed_forward: Option<f32>) -> io::Result<()> {
157        let current_feed_forward = current_feed_forward.unwrap_or_default();
158        writeln!(self, "v {} {} {}", axis as u8, velocity, current_feed_forward)?;
159        self.flush()
160    }
161
162    /// Specifies a velocity setpoint for the motor.
163    /// `axis` The motor to be used for the operation.
164    /// `current` is the current to be supplied, in amps.
165    pub fn set_current(&mut self, axis: AxisID, current: f32) -> io::Result<()> {
166        writeln!(self, "c {} {}", axis as u8, current)?;
167        self.flush()
168    }
169
170    /// Moves a motor to a given position
171    /// For general movement, this is the best command.
172    /// `axis` The motor to be used for the operation.
173    /// `position` is the desired position, in encoder counts.
174    pub fn set_trajectory(&mut self, axis: AxisID, position: f32) -> io::Result<()> {
175        writeln!(self, "t {} {}", axis as u8, position)?;
176        self.flush()
177    }
178}
179
180impl<T> ODrive<T> where T: Read + Write {
181    /// Retrieves the velocity of a motor, in counts per second.
182    pub fn get_velocity(&mut self, axis: AxisID) -> io::Result<Option<f32>> {
183        writeln!(self, "r axis{} .encoder.vel_estimate", axis as u8)?;
184        self.flush()?;
185        self.read_float()
186    }
187
188    /// Changes the state of an axis.
189    /// The `wait` flag indicates whether this command should block until the state is updated.
190    /// Returns true unless we are in blocking mode and the operation times out.
191    /// The current timeout is 10 seconds.
192    ///
193    /// This command will likely be deprecated and reworked in a future release.
194    pub fn run_state(&mut self, axis: AxisID, requested_state: AxisState, wait: bool) -> io::Result<bool> {
195        let timer = Instant::now();
196        writeln!(self, "w axis{}.requested_state {}", axis as u8, requested_state as u8)?;
197        self.flush()?;
198        if wait {
199            while {
200                writeln!(self, "r axis{}.current_state", axis as u8)?;
201                self.flush()?;
202
203                self.read_int()?.unwrap_or_default() != AxisState::Idle as i32
204                    && timer.elapsed().as_millis() < 10_000 // exit
205            } {}
206        }
207
208        Ok(timer.elapsed().as_millis() < 10_000)
209    }
210}
211
212// Implement private helper methods
213impl<T> ODrive<T> where T: Read + Write {
214    fn set_config_property<D: Display>(&mut self, param: &str, value: D) -> ODriveResult<()> {
215        writeln!(self, "w {} {}", param, value).map_err(ODriveError::Io)?;
216        self.flush().map_err(ODriveError::Io)
217    }
218
219    fn get_config_property(&mut self, param: &str) -> ODriveResult<String> {
220        writeln!(self, "r {}", param).map_err(ODriveError::Io)?;
221        self.flush().map_err(ODriveError::Io)?;
222        self.read_odrive_response()
223    }
224
225    fn set_axis_property<D: Display>(&mut self, axis: AxisID, property: &str, value: D) -> ODriveResult<()> {
226        let config = format!("axis{}.{}", axis as u8, property);
227        self.set_config_property(&config, value)
228    }
229
230    fn get_axis_property(&mut self, axis: AxisID, property: &str) -> ODriveResult<String> {
231        let config = format!("axis{}.{}", axis as u8, property);
232        self.get_config_property(&config)
233    }
234
235    fn set_axis_config_property<D: Display>(&mut self, axis: AxisID, name: &str, value: D) -> ODriveResult<()> {
236        let config = format!("axis{}.config.{}", axis as u8, name);
237        self.set_config_property(&config, value)
238    }
239
240    fn get_axis_config_property(&mut self, axis: AxisID, name: &str) -> ODriveResult<String> {
241        let config = format!("axis{}.config.{}", axis as u8, name);
242        self.get_config_property(&config)
243    }
244}
245
246/// # Startup Configuration
247/// The ODrive motor controllers have several optional startup procedures which can be enabled.
248/// Each of them has an associated getter and setter which can be invoked to read to and write from
249/// their value.
250///
251/// From the official documentation:
252/// > By default the ODrive takes no action at startup and goes to idle immediately.
253/// > In order to change what startup procedures are used, set the startup procedures you want to `true`.
254/// > The ODrive will sequence all enabled startup actions selected in the order shown below.
255///
256/// > 1. `<axis>.config.startup_motor_calibration`
257/// > 2. `<axis>.config.startup_encoder_index_search`
258/// > 3. `<axis>.config.startup_encoder_offset_calibration`
259/// > 4. `<axis>.config.startup_closed_loop_control`
260/// > 5. `<axis>.config.startup_sensorless_control`
261///
262/// For further information, see the documentation for `AxisState`.
263impl<T> ODrive<T> where T: Read + Write {
264    pub fn set_startup_motor_calibration(&mut self, axis: AxisID, value: bool) -> ODriveResult<()> {
265        self.set_axis_config_property(axis, "startup_motor_calibration", value as u8)
266    }
267
268    pub fn set_startup_encoder_index_search(&mut self, axis: AxisID, value: bool) -> ODriveResult<()> {
269        self.set_axis_config_property(axis, "startup_encoder_index_search", value as u8)
270    }
271
272    pub fn set_startup_encoder_offset_calibration(&mut self, axis: AxisID, value: bool) -> ODriveResult<()> {
273        self.set_axis_config_property(axis, "startup_encoder_offset_calibration", value as u8)
274    }
275
276    pub fn set_startup_closed_loop_control(&mut self, axis: AxisID, value: bool) -> ODriveResult<()> {
277        self.set_axis_config_property(axis, "startup_closed_loop_control", value as u8)
278    }
279
280    pub fn set_startup_sensorless_control(&mut self, axis: AxisID, value: bool) -> ODriveResult<()> {
281        self.set_axis_config_property(axis, "startup_sensorless_control", value as u8)
282    }
283
284    pub fn read_startup_motor_calibration(&mut self, axis: AxisID) -> ODriveResult<bool> {
285        let response = self.get_axis_config_property(axis, "startup_motor_calibration")?;
286        match response.parse::<u8>() {
287            Ok(val) => match val {
288                0 => Ok(false),
289                1 => Ok(true),
290                _ => Err(ODriveError::InvalidMessageReceived(response))
291            },
292            Err(_error) => {
293                Err(ODriveError::InvalidMessageReceived(response))
294            }
295        }
296    }
297
298    pub fn read_startup_encoder_index_search(&mut self, axis: AxisID, value: bool) -> ODriveResult<bool> {
299        let response = self.get_axis_config_property(axis, "startup_encoder_index_search")?;
300        match response.parse::<u8>() {
301            Ok(val) => match val {
302                0 => Ok(false),
303                1 => Ok(true),
304                _ => Err(ODriveError::InvalidMessageReceived(response))
305            },
306            Err(_error) => {
307                Err(ODriveError::InvalidMessageReceived(response))
308            }
309        }
310    }
311
312    pub fn read_startup_encoder_offset_calibration(&mut self, axis: AxisID, value: bool) -> ODriveResult<bool> {
313        let response = self.get_axis_config_property(axis, "startup_encoder_offset_calibration")?;
314        match response.parse::<u8>() {
315            Ok(val) => match val {
316                0 => Ok(false),
317                1 => Ok(true),
318                _ => Err(ODriveError::InvalidMessageReceived(response))
319            },
320            Err(_error) => {
321                Err(ODriveError::InvalidMessageReceived(response))
322            }
323        }
324    }
325
326    pub fn read_startup_closed_loop_control(&mut self, axis: AxisID, value: bool) -> ODriveResult<bool> {
327        let response = self.get_axis_config_property(axis, "startup_closed_loop_control")?;
328        match response.parse::<u8>() {
329            Ok(val) => match val {
330                0 => Ok(false),
331                1 => Ok(true),
332                _ => Err(ODriveError::InvalidMessageReceived(response))
333            },
334            Err(_error) => {
335                Err(ODriveError::InvalidMessageReceived(response))
336            }
337        }
338    }
339
340    pub fn read_startup_sensorless_control(&mut self, axis: AxisID, value: bool) -> ODriveResult<bool> {
341        let response = self.get_axis_config_property(axis, "startup_sensorless_control")?;
342        match response.parse::<u8>() {
343            Ok(val) => match val {
344                0 => Ok(false),
345                1 => Ok(true),
346                _ => Err(ODriveError::InvalidMessageReceived(response))
347            },
348            Err(_error) => {
349                Err(ODriveError::InvalidMessageReceived(response))
350            }
351        }
352    }
353}
354
355/// Configuration management.
356impl<T> ODrive<T> where T: Read + Write {
357    /// Saves the current configuration of properties to the ODrives non-volatile memory, allowing
358    /// the configuration to persist after reboots.
359    pub fn save_configuration(&mut self) -> ODriveResult<()> {
360        writeln!(self, "ss").map_err(ODriveError::Io)?;
361        self.flush().map_err(ODriveError::Io)
362    }
363
364    /// Reset the current configuration to the factory default settings.
365    pub fn erase_configuration(&mut self) -> ODriveResult<()> {
366        writeln!(self, "se").map_err(ODriveError::Io)?;
367        self.flush().map_err(ODriveError::Io)
368    }
369}
370
371/// Motor configuration
372impl<T> ODrive<T> where T: Read + Write {
373    pub fn set_motor_pole_pairs(&mut self, axis: AxisID, value: u16) -> ODriveResult<()> {
374        self.set_axis_property(axis, "motor.config.pole_pairs", value)
375    }
376
377    pub fn set_motor_resistance_calib_max_voltage(&mut self, axis: AxisID, value: f32) -> ODriveResult<()> {
378        self.set_axis_property(axis, "motor.config.resistance_calib_max_voltage", value)
379    }
380
381    pub fn set_motor_requested_current_range(&mut self, axis: AxisID, value: f32) -> ODriveResult<()> {
382        self.set_axis_property(axis, "motor.config.requested_current_range", value)
383    }
384
385    pub fn set_motor_current_control_bandwidth(&mut self, axis: AxisID, value: f32) -> ODriveResult<()> {
386        self.set_axis_property(axis, "motor.config.current_control_bandwidth", value)
387    }
388
389    pub fn set_motor_pre_calibrated(&mut self, axis: AxisID, value: bool) -> ODriveResult<()> {
390        self.set_axis_property(axis, "motor.config.pre_calibrated", value as u8)
391    }
392}
393
394/// Encoder configuration
395impl<T> ODrive<T> where T: Read + Write {
396    pub fn set_encoder_mode(&mut self, axis: AxisID, value: EncoderMode) -> ODriveResult<()> {
397        self.set_axis_property(axis, "encoder.config.mode", value as u8)
398    }
399
400    pub fn set_encoder_cpr(&mut self, axis: AxisID, value: u16) -> ODriveResult<()> {
401        self.set_axis_property(axis, "encoder.config.cpr", value)
402    }
403
404    pub fn set_encoder_bandwidth(&mut self, axis: AxisID, value: f32) -> ODriveResult<()> {
405        self.set_axis_property(axis, "encoder.config.bandwidth", value)
406    }
407
408    pub fn set_encoder_pre_calibrated(&mut self, axis: AxisID, value: bool) -> ODriveResult<()> {
409        self.set_axis_property(axis, "encoder.config.pre_calibrated", value as u8)
410    }
411}
412
413/// Controller configuration
414impl<T> ODrive<T> where T: Read + Write {
415    pub fn set_position_gain(&mut self, axis: AxisID, value: f32) -> ODriveResult<()> {
416        self.set_axis_property(axis, "controller.config.pos_gain", value)
417    }
418
419    pub fn set_velocity_gain(&mut self, axis: AxisID, value: f32) -> ODriveResult<()> {
420        self.set_axis_property(axis, "controller.config.vel_gain", value)
421    }
422
423    pub fn set_velocity_integrator_gain(&mut self, axis: AxisID, value: f32) -> ODriveResult<()> {
424        self.set_axis_property(axis, "controller.config.vel_integrator_gain", value)
425    }
426
427    pub fn set_velocity_limit(&mut self, axis: AxisID, value: f32) -> ODriveResult<()> {
428        self.set_axis_property(axis, "controller.config.vel_limit", value)
429    }
430
431    pub fn set_control_mode(&mut self, axis: AxisID, mode: ControlMode) -> ODriveResult<()> {
432        self.set_axis_property(axis, "controller.config.control_mode", mode as u8)
433    }
434}
odrive_rs/commands/mod.rs

odrive_rs/commands/
mod.rs
