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])?;
23///
24/// let heading = controller.heading()?;
25/// let angular_velocity = controller.angular_velocity()?;
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    ///
43    /// let left_gyro = GyroSensor::new(SensorPort::In1)?;
44    /// let right_gyro = GyroSensor::new(SensorPort::In4)?;
45    ///
46    /// let controller = GyroController::new(vec![gyro1, gyro2])?;
47    /// ```
48    pub fn new(gyros: Vec<&'a GyroSensor>) -> Ev3Result<Self> {
49        let mut gyros_with_offsets = Vec::new();
50        for gyro in gyros {
51            let heading = gyro.heading()?;
52            gyros_with_offsets.push((gyro, heading));
53        }
54        Ok(Self {
55            gyros: RefCell::new(gyros_with_offsets),
56        })
57    }
58
59    /// Gets the average heading of all contained gyros.
60    pub fn heading(&self) -> Ev3Result<I32F32> {
61        let mut sum = I32F32::from_num(0.0);
62        for (gyro, offset) in self.gyros.borrow().iter() {
63            sum += I32F32::from_num(gyro.heading()? - offset);
64        }
65
66        Ok(sum / I32F32::from_num(self.gyros.borrow().len()))
67    }
68
69    /// Gets the average angular velocity of all contained gyros.
70    pub fn angular_velocity(&self) -> Ev3Result<I32F32> {
71        let mut sum = I32F32::from_num(0.0);
72        for (gyro, _) in self.gyros.borrow().iter() {
73            sum += I32F32::from_num(gyro.angular_velocity()?);
74        }
75
76        Ok(sum / I32F32::from_num(self.gyros.borrow().len()))
77    }
78
79    /// Resets the heading of the controller to zero.
80    pub fn reset(&self) -> Ev3Result<()> {
81        for (gyro, heading) in self.gyros.borrow_mut().iter_mut() {
82            *heading = gyro.heading()?;
83        }
84        Ok(())
85    }
86}
ev3dev_rs/robotics/gyro_controller.rs

ev3dev_rs/robotics/
gyro_controller.rs
