fll_rs/
robot.rs

1use anyhow::{bail, Context};
2use ev3dev_lang_rust::motors::TachoMotor;
3use ev3dev_lang_rust::sensors::SensorPort;
4
5use crate::error::Result;
6use crate::input::Input;
7use crate::movement::spec::RobotSpec;
8use crate::types::{Distance, Heading, Percent, Speed};
9use std::cell::RefMut;
10use std::time::Duration;
11
12/// How the robot should turn
13#[derive(Eq, PartialEq, Copy, Clone, Debug)]
14pub enum TurnType {
15    /// Centered on right wheel
16    Right,
17
18    /// Centered on left wheel
19    Left,
20
21    /// Turn using both wheels
22    Center,
23}
24
25impl TurnType {
26    pub fn wheels(&self) -> (bool, bool) {
27        match self {
28            TurnType::Right => (false, true),
29            TurnType::Left => (true, false),
30            TurnType::Center => (true, true),
31        }
32    }
33}
34
35/// Identifies a motor
36#[derive(Eq, PartialEq, Copy, Clone, Hash, Debug)]
37pub enum MotorId {
38    DriveRight,
39    DriveLeft,
40    Attachment1,
41    Attachment2,
42}
43// todo support set-points
44/// A motor movement
45#[derive(Clone, Debug, PartialEq)]
46pub enum Command {
47    /// Begin spinning forever
48    /// Takes speed param
49    On(Speed),
50
51    /// Stop spinning
52    Stop(StopAction),
53
54    /// Spin for a set distance (relative)
55    /// Takes distance and speed params
56    Distance(Distance, Speed),
57
58    /// Spin to a set angle (absolute)
59    /// Takes position and speed params
60    ///
61    /// Position is in distance from last reset
62    To(Distance, Speed),
63
64    /// Spin for a set time
65    /// Takes time and speed params
66    Time(Duration, Speed),
67
68    /// Queues a command to be ran when a `Execute` command is sent
69    /// Queuing a command could allow it to be executed faster
70    /// This could minimize "kick" when using 2 motors
71    ///
72    /// Not guranteed to be kept if additional commands are sent to this motor
73    /// between this command and the next `Execute` command
74    Queue(Box<Command>),
75
76    /// Executes the queued command
77    Execute,
78}
79
80/// A Stop Action
81#[derive(Copy, Clone, Debug, Eq, PartialEq)]
82pub enum StopAction {
83    /// Freely coast to a stop
84    Coast,
85    /// Cause motors to stop more quickly than `Coast` but without holding its position
86    Break,
87    /// Actively holds a motor's position
88    Hold,
89}
90
91impl StopAction {
92    pub fn to_str(&self) -> &'static str {
93        match self {
94            StopAction::Coast => TachoMotor::STOP_ACTION_COAST,
95            StopAction::Break => TachoMotor::STOP_ACTION_BRAKE,
96            StopAction::Hold => TachoMotor::STOP_ACTION_HOLD,
97        }
98    }
99}
100
101/// Identifies a sensor
102#[derive(Eq, PartialEq, Copy, Clone, Hash, Debug)]
103pub enum SensorId {
104    DriveLeft,
105    AttachmentRight,
106    AttachmentLeft,
107}
108
109/// Represents a simple robot with 2 wheels to move and a method to sense direction
110pub trait Robot: AngleProvider {
111    /// Drive the robot using the gyro to correct for errors
112    ///
113    /// # Returns
114    ///
115    /// Returns an error if an interrupt has been requested
116    /// Otherwise, returns Ok(())
117    ///
118    fn drive(&self, distance: impl Into<Distance>, speed: impl Into<Speed>) -> Result<()>;
119
120    /// Turns the robot using the gyro sensor
121    /// Guesses the turn type from the turn direction
122    ///
123    /// # Inputs
124    ///
125    /// `speed` should be greater than `0.0`
126    ///
127    /// # Returns
128    ///
129    /// Returns an error if an interrupt has been requested
130    /// Otherwise, returns Ok(())
131    ///
132    /// # Panics
133    ///
134    /// This function may panic if `speed` is less than or equal to 0
135    fn turn(&self, angle: Heading, speed: impl Into<Speed>) -> Result<()>;
136
137    /// Turns the robot
138    /// Uses specified turn type using the gyro sensor
139    ///
140    /// # Inputs
141    ///
142    /// `speed` should be greater than `0.0`
143    ///
144    /// # Returns
145    ///
146    /// Returns an error if an interrupt has been requested
147    /// Otherwise, returns Ok(())
148    ///
149    /// # Panics
150    ///
151    /// This function may panic if `speed` is less than or equal to 0
152    fn turn_named(&self, angle: Heading, speed: impl Into<Speed>, turn: TurnType) -> Result<()>;
153
154    fn set_heading(&self, angle: Heading) -> Result<()>;
155
156    /// Retereives a motor
157    fn motor(&self, motor: MotorId) -> Option<RefMut<dyn Motor>>;
158
159    fn color_sensor(&self, port: SensorPort) -> Option<RefMut<dyn ColorSensor>>;
160
161    /// Returns the status of the robot's buttons
162    fn process_buttons(&self) -> Result<Input>;
163
164    /// Returns an error if an interrupt has been requested
165    /// Otherwise, returns Ok(())
166    fn handle_interrupt(&self) -> Result<()> {
167        let input = self.process_buttons().context("Process buttons")?;
168
169        if input.is_left() {
170            bail!("Interupt requested")
171        }
172
173        Ok(())
174    }
175
176    /// Retrieves the battery percentage
177    /// Ranges from 0.0 -> 1.0
178    fn battery(&self) -> Result<Percent>;
179
180    /// Waits for the status of the robot's buttons and returns that new status
181    fn await_input(&self, timeout: Option<Duration>) -> Result<Input>;
182
183    /// Stops the robot's motors
184    fn stop(&self) -> Result<()>;
185
186    /// Resets the robot's state
187    fn reset(&self) -> Result<()>;
188
189    /// Returns information about the robot
190    fn spec(&self) -> &RobotSpec;
191}
192
193pub trait ColorSensor {
194    /// Retrieves the light reflected into the color sensor
195    fn reflected_light(&self) -> Result<Percent>;
196
197    /// Sets the white (1.0) definition
198    fn cal_white(&mut self) -> Result<()>;
199
200    /// Sets the black (0.0) definition
201    fn cal_black(&mut self) -> Result<()>;
202
203    /// Resets calibration data
204    fn reset(&mut self) -> Result<()>;
205}
206
207pub trait Motor {
208    /// Start a motor movement
209    fn raw(&mut self, command: Command) -> Result<()>;
210
211    /// Resets the angle of a motor to 0
212    /// this method sets the stopping action for implicit stops
213    fn motor_reset(&mut self, stop_action: Option<StopAction>) -> Result<()>;
214
215    /// Waits until a motor is finished moving
216    fn wait(&self, timeout: Option<Duration>) -> Result<()>;
217
218    /// Retrieves the speed of a motor
219    fn speed(&self) -> Result<Speed>;
220
221    /// Retrieves the angle of a motor in distance from the last reset
222    fn motor_angle(&self) -> Result<Distance>;
223}
224
225pub trait MotorExt {
226    /// Spin for a set distance (relative)
227    fn dist(&mut self, dist: impl Into<Distance>, speed: impl Into<Speed>) -> Result<()>;
228
229    /// Spin to a set angle (absolute)
230    ///
231    /// Position is in distance from last reset
232    fn to_pos(&mut self, position: impl Into<Distance>, speed: impl Into<Speed>) -> Result<()>;
233
234    /// Spin for a set time
235    fn time(&mut self, duration: Duration, speed: impl Into<Speed>) -> Result<()>;
236}
237
238impl MotorExt for dyn Motor + '_ {
239    fn dist(&mut self, dist: impl Into<Distance>, speed: impl Into<Speed>) -> Result<()> {
240        self.raw(Command::Distance(dist.into(), speed.into()))?;
241
242        self.wait(None)
243    }
244
245    fn to_pos(&mut self, position: impl Into<Distance>, speed: impl Into<Speed>) -> Result<()> {
246        self.raw(Command::To(position.into(), speed.into()))?;
247
248        self.wait(None)
249    }
250
251    fn time(&mut self, duration: Duration, speed: impl Into<Speed>) -> Result<()> {
252        self.raw(Command::Time(duration, speed.into()))?;
253
254        self.wait(None)
255    }
256}
257
258impl<M: Motor> MotorExt for M {
259    fn dist(&mut self, dist: impl Into<Distance>, speed: impl Into<Speed>) -> Result<()> {
260        self.raw(Command::Distance(dist.into(), speed.into()))?;
261
262        self.wait(None)
263    }
264
265    fn to_pos(&mut self, position: impl Into<Distance>, speed: impl Into<Speed>) -> Result<()> {
266        self.raw(Command::To(position.into(), speed.into()))?;
267
268        self.wait(None)
269    }
270
271    fn time(&mut self, duration: Duration, speed: impl Into<Speed>) -> Result<()> {
272        self.raw(Command::Time(duration, speed.into()))?;
273
274        self.wait(None)
275    }
276}
277
278pub trait AngleProvider {
279    /// Retrieves the robot's current heading
280    fn angle(&self) -> Result<Heading>;
281}
fll_rs/robot.rs

fll_rs/
robot.rs
