ev3dev_rs/robotics/
gyro_controller.rs

1use std::cell::RefCell;
2
3use fixed::types::I32F32;
4
5use crate::error::Ev3Result;
6use crate::pupdevices::GyroSensor;
7
8/// A structure that can get the average reading from multiple gyro sensors in a single command.
9///
10/// This is also how you allow a `DriveBase` to use your gyro sensor(s)
11///
12/// # Examples
13///
14/// ```
15/// use ev3dev_rs::robotics::GyroController;
16/// use ev3dev_rs::pupdevices::GyroSensor;
17/// use ev3dev_rs::parameters::SensorPort;
18///
19/// let gyro1 = GyroSensor::new(SensorPort::In1)?;
20/// let gyro2 = GyroSensor::new(SensorPort::In2)?;
21///
22/// let controller = GyroController::new(vec![&gyro1, &gyro2]).await?;
23///
24/// let heading = controller.heading().await?;
25/// let angular_velocity = controller.angular_velocity().await?;
26///
27/// println!("Heading: {}", heading);
28/// println!("Angular Velocity: {}", angular_velocity);
29/// ```
30pub struct GyroController<'a> {
31    pub(crate) gyros: RefCell<Vec<(&'a GyroSensor, i16)>>,
32}
33
34impl<'a> GyroController<'a> {
35    /// Create a new `GyroController` with the given gyro sensors.
36    ///
37    /// # Examples
38    ///
39    /// ``` no_run
40    /// use ev3dev_rs::pupdevices::GyroSensor;
41    /// use ev3dev_rs::robotics::GyroController;
42    /// use ev3dev_rs::parameters::SensorPort;
43    ///
44    /// let left_gyro = GyroSensor::new(SensorPort::In1)?;
45    /// let right_gyro = GyroSensor::new(SensorPort::In4)?;
46    ///
47    /// let controller = GyroController::new(vec![&left_gyro, &right_gyro]).await?;
48    /// ```
49    pub async fn new(gyros: Vec<&'a GyroSensor>) -> Ev3Result<Self> {
50        let mut gyros_with_offsets = Vec::new();
51        for gyro in gyros {
52            let heading = gyro.heading().await?;
53            gyros_with_offsets.push((gyro, heading));
54        }
55        Ok(Self {
56            gyros: RefCell::new(gyros_with_offsets),
57        })
58    }
59
60    /// Gets the average heading of all contained gyros.
61    pub async fn heading(&self) -> Ev3Result<I32F32> {
62        let mut sum = I32F32::from_num(0.0);
63        for (gyro, offset) in self.gyros.borrow().iter() {
64            sum += I32F32::from_num(gyro.heading().await? - offset);
65        }
66
67        Ok(sum / I32F32::from_num(self.gyros.borrow().len()))
68    }
69
70    /// Gets the average angular velocity of all contained gyros.
71    pub async fn angular_velocity(&self) -> Ev3Result<I32F32> {
72        let mut sum = I32F32::from_num(0.0);
73        for (gyro, _) in self.gyros.borrow().iter() {
74            sum += I32F32::from_num(gyro.angular_velocity().await?);
75        }
76
77        Ok(sum / I32F32::from_num(self.gyros.borrow().len()))
78    }
79
80    /// Resets the heading of the controller to zero.
81    pub async fn reset(&self) -> Ev3Result<()> {
82        for (gyro, heading) in self.gyros.borrow_mut().iter_mut() {
83            *heading = gyro.heading().await?;
84        }
85        Ok(())
86    }
87}
ev3dev_rs/robotics/gyro_controller.rs

ev3dev_rs/robotics/
gyro_controller.rs
