From d5f54cc53a632d873eb5cd82bbe235198c5961a9 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 31 Dec 2023 02:24:24 -0600 Subject: [PATCH 01/24] feat: initial IMU support --- pros/src/sensors/imu.rs | 320 ++++++++++++++++++++++++++++++++++++++++ pros/src/sensors/mod.rs | 3 +- 2 files changed, 322 insertions(+), 1 deletion(-) create mode 100644 pros/src/sensors/imu.rs diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs new file mode 100644 index 00000000..3c97af00 --- /dev/null +++ b/pros/src/sensors/imu.rs @@ -0,0 +1,320 @@ +use pros_sys::{PROS_ERR, PROS_ERR_F}; +use snafu::Snafu; + +use crate::error::{bail_on, map_errno, PortError}; + +#[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] +pub struct InertialStatus { + pub calibrating: bool, + pub error: bool, +} + +impl From for InertialStatus { + fn from(value: pros_sys::imu_status_e_t) -> Self { + Self { + calibrating: (value & pros_sys::E_IMU_STATUS_CALIBRATING) == 0b001, + error: (value & pros_sys::E_IMU_STATUS_ERROR) == 0b010, + } + } +} + +#[derive(Debug)] +pub struct InertialSensor { + port: u8, +} + +impl InertialSensor { + pub fn new(port: u8) -> Result { + let sensor = Self { port }; + sensor.rotation()?; + Ok(sensor) + } + + // Calibrate IMU. + // This takes approximately 2 seconds, and is blocking until the IMU status flag is set properly. + pub fn calibrate(&self) -> Result<(), InertialError> { + unsafe { + bail_on!(PROS_ERR, pros_sys::imu_reset(self.port)); + } + Ok(()) + } + + /// Get the total number of degrees the Inertial Sensor has spun about the z-axis. + + /// This value is theoretically unbounded. Clockwise rotations are represented with positive degree values, + /// while counterclockwise rotations are represented with negative ones. + pub fn rotation(&self) -> Result { + unsafe { Ok(bail_on!(PROS_ERR_F, pros_sys::imu_get_rotation(self.port))) } + } + + /// Get the Inertial Sensor’s heading relative to the initial direction of its x-axis. + /// + /// This value is bounded by [0, 360) degrees. Clockwise rotations are represented with positive degree values, + /// while counterclockwise rotations are represented with negative ones. + pub fn heading(&self) -> Result { + unsafe { Ok(bail_on!(PROS_ERR_F, pros_sys::imu_get_heading(self.port))) } + } + + /// Get the Inertial Sensor’s pitch angle bounded by (-180, 180) degrees. + pub fn pitch(&self) -> Result { + unsafe { Ok(bail_on!(PROS_ERR_F, pros_sys::imu_get_pitch(self.port))) } + } + + /// Get the Inertial Sensor’s roll angle bounded by (-180, 180) degrees. + pub fn roll(&self) -> Result { + unsafe { Ok(bail_on!(PROS_ERR_F, pros_sys::imu_get_roll(self.port))) } + } + + /// Get the Inertial Sensor’s yaw angle bounded by (-180, 180) degrees. + pub fn yaw(&self) -> Result { + unsafe { Ok(bail_on!(PROS_ERR_F, pros_sys::imu_get_yaw(self.port))) } + } + + /// Get the Inertial Sensor’s yaw angle bounded by (-180, 180) degrees. + pub fn status(&self) -> Result { + unsafe { Ok(bail_on!(PROS_ERR as _, pros_sys::imu_get_status(self.port) as pros_sys::imu_status_e_t).into()) } + } + + /// Get a quaternion representing the Inertial Sensor’s orientation. + pub fn quaternion(&self) -> Result { + unsafe { pros_sys::imu_get_quaternion(self.port).try_into() } + } + + /// Get the Euler angles representing the Inertial Sensor’s orientation. + pub fn euler(&self) -> Result { + unsafe { pros_sys::imu_get_euler(self.port).try_into() } + } + + // // Get the Inertial Sensor’s raw gyroscope values. + // pub fn gyro_rate(&self) -> Result { + // unsafe { pros_sys::imu_get_gyro_rate(self.port).try_into() } + // } + + // // Get the Inertial Sensor’s raw accelerometer values. + // pub fn accel(&self) -> Result { + // unsafe { pros_sys::imu_get_accel(self.port).try_into() } + // } + + /// Resets the current reading of the Inertial Sensor’s heading to zero. + pub fn zero_heading(&self) -> Result<(), InertialError> { + unsafe { + bail_on!(PROS_ERR, pros_sys::imu_tare_heading(self.port)); + } + Ok(()) + } + + /// Resets the current reading of the Inertial Sensor’s rotation to zero. + pub fn zero_rotation(&self) -> Result<(), InertialError> { + unsafe { + bail_on!(PROS_ERR, pros_sys::imu_tare_rotation(self.port)); + } + Ok(()) + } + + /// Resets the current reading of the Inertial Sensor’s pitch to zero. + pub fn zero_pitch(&self) -> Result<(), InertialError> { + unsafe { + bail_on!(PROS_ERR, pros_sys::imu_tare_pitch(self.port)); + } + Ok(()) + } + + /// Resets the current reading of the Inertial Sensor’s roll to zero. + pub fn zero_roll(&self) -> Result<(), InertialError> { + unsafe { + bail_on!(PROS_ERR, pros_sys::imu_tare_roll(self.port)); + } + Ok(()) + } + + /// Resets the current reading of the Inertial Sensor’s yaw to zero. + pub fn zero_yaw(&self) -> Result<(), InertialError> { + unsafe { + bail_on!(PROS_ERR, pros_sys::imu_tare_yaw(self.port)); + } + Ok(()) + } + + /// Reset all 3 euler values of the Inertial Sensor to 0. + pub fn zero_euler(&self) -> Result<(), InertialError> { + unsafe { + bail_on!(PROS_ERR, pros_sys::imu_tare_euler(self.port)); + } + Ok(()) + } + + /// Resets all 5 values of the Inertial Sensor to 0. + pub fn zero(&self) -> Result<(), InertialError> { + unsafe { + bail_on!(PROS_ERR, pros_sys::imu_tare(self.port)); + } + Ok(()) + } + + // Sets the current reading of the Inertial Sensor’s euler values to target euler values. Will default to +/- 180 if target exceeds +/- 180. + pub fn set_euler(&self, euler: Euler) -> Result<(), InertialError> { + unsafe { + bail_on!( + PROS_ERR, + pros_sys::imu_set_euler(self.port, euler.into()) + ); + } + Ok(()) + } + + // Sets the current reading of the Inertial Sensor’s rotation to target value. + pub fn set_rotation(&self, rotation: f64) -> Result<(), InertialError> { + unsafe { + bail_on!( + PROS_ERR, + pros_sys::imu_set_rotation(self.port, rotation) + ); + } + Ok(()) + } + + // Sets the current reading of the Inertial Sensor’s heading to target value Target will default to 360 if above 360 and default to 0 if below 0. + pub fn set_heading(&self, heading: f64) -> Result<(), InertialError> { + unsafe { + bail_on!( + PROS_ERR, + pros_sys::imu_set_heading(self.port, heading) + ); + } + Ok(()) + } + + // Sets the current reading of the Inertial Sensor’s pitch to target value Will default to +/- 180 if target exceeds +/- 180. + pub fn set_pitch(&self, pitch: f64) -> Result<(), InertialError> { + unsafe { + bail_on!( + PROS_ERR, + pros_sys::imu_set_pitch(self.port, pitch) + ); + } + Ok(()) + } + + // Sets the current reading of the Inertial Sensor’s roll to target value Will default to +/- 180 if target exceeds +/- 180. + pub fn set_roll(&self, roll: f64) -> Result<(), InertialError> { + unsafe { + bail_on!( + PROS_ERR, + pros_sys::imu_set_roll(self.port, roll) + ); + } + Ok(()) + } + + // Sets the current reading of the Inertial Sensor’s yaw to target value Will default to +/- 180 if target exceeds +/- 180. + pub fn set_yaw(&self, yaw: f64) -> Result<(), InertialError> { + unsafe { + bail_on!( + PROS_ERR, + pros_sys::imu_set_yaw(self.port, yaw) + ); + } + Ok(()) + } +} + +#[derive(Default, Debug, Clone, Copy, PartialEq)] +pub struct Quaternion { + pub x: f64, + pub y: f64, + pub z: f64, + pub w: f64, +} + +impl TryFrom for Quaternion { + type Error = InertialError; + + fn try_from(value: pros_sys::quaternion_s_t) -> Result { + bail_on!(value.x, PROS_ERR_F); + + Ok(Self { + x: value.x, + y: value.y, + z: value.z, + w: value.w, + }) + } +} + +impl Into for Quaternion { + fn into(self) -> pros_sys::quaternion_s_t { + pros_sys::quaternion_s_t { + x: self.x, + y: self.y, + z: self.z, + w: self.w, + } + } +} + +#[derive(Default, Debug, Clone, Copy, PartialEq)] +pub struct Euler { + pub pitch: f64, + pub roll: f64, + pub yaw: f64, +} + +impl TryFrom for Euler { + type Error = InertialError; + + fn try_from(value: pros_sys::euler_s_t) -> Result { + bail_on!(value.pitch, PROS_ERR_F); + + Ok(Self { + pitch: value.pitch, + roll: value.roll, + yaw: value.yaw, + }) + } +} + +impl Into for Euler { + fn into(self) -> pros_sys::euler_s_t { + pros_sys::euler_s_t { + pitch: self.pitch, + roll: self.roll, + yaw: self.yaw, + } + } +} + +// #[derive(Clone, Copy, Debug, PartialEq)] +// pub struct InertialRaw { +// pub x: f64, +// pub y: f64, +// pub z: f64, +// } + +// impl TryFrom for InertialRaw { +// type Error = InertialError; + +// fn try_from(value: pros_sys::imu_raw_s) -> Result { +// bail_on!(value.x, PROS_ERR_F); + +// Ok(Self { +// x: value.x, +// y: value.y, +// z: value.z, +// }) +// } +// } + +#[derive(Debug, Snafu)] +pub enum InertialError { + #[snafu(display("Inertial sensor is still calibrating."))] + StillCalibrating, + #[snafu(display("{source}"), context(false))] + Port { source: PortError }, +} + +map_errno! { + InertialError { + EAGAIN => Self::StillCalibrating, + } + inherit PortError; +} \ No newline at end of file diff --git a/pros/src/sensors/mod.rs b/pros/src/sensors/mod.rs index afbd4d0e..b99c01cc 100644 --- a/pros/src/sensors/mod.rs +++ b/pros/src/sensors/mod.rs @@ -1,4 +1,5 @@ pub mod distance; pub mod gps; +pub mod imu; pub mod rotation; -pub mod vision; +pub mod vision; \ No newline at end of file From 9dc143c8e34496cbc9a9bc08d390164e9a4ab860 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 31 Dec 2023 02:25:25 -0600 Subject: [PATCH 02/24] chore: update todo --- TODO.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TODO.md b/TODO.md index 428ceb68..0f669436 100644 --- a/TODO.md +++ b/TODO.md @@ -23,7 +23,7 @@ This is the todo list for the eventual 1.0.0 release of pros-rs * [ ] Sensors * [X] Distance * [X] GPS - * [ ] Inertial (IMU) + * [x] Inertial (IMU) * [ ] Optical * [X] Rotational * [X] Vision From 1994dcdd918af0bbe984ba45aa7b3d68d4337a2a Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 31 Dec 2023 02:37:38 -0600 Subject: [PATCH 03/24] fmt --- pros/src/sensors/imu.rs | 52 ++++++++++++++++------------------------- 1 file changed, 20 insertions(+), 32 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 3c97af00..be4e13ff 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -40,7 +40,7 @@ impl InertialSensor { } /// Get the total number of degrees the Inertial Sensor has spun about the z-axis. - + /// /// This value is theoretically unbounded. Clockwise rotations are represented with positive degree values, /// while counterclockwise rotations are represented with negative ones. pub fn rotation(&self) -> Result { @@ -48,7 +48,7 @@ impl InertialSensor { } /// Get the Inertial Sensor’s heading relative to the initial direction of its x-axis. - /// + /// /// This value is bounded by [0, 360) degrees. Clockwise rotations are represented with positive degree values, /// while counterclockwise rotations are represented with negative ones. pub fn heading(&self) -> Result { @@ -72,7 +72,13 @@ impl InertialSensor { /// Get the Inertial Sensor’s yaw angle bounded by (-180, 180) degrees. pub fn status(&self) -> Result { - unsafe { Ok(bail_on!(PROS_ERR as _, pros_sys::imu_get_status(self.port) as pros_sys::imu_status_e_t).into()) } + unsafe { + Ok(bail_on!( + PROS_ERR as _, + pros_sys::imu_get_status(self.port) as pros_sys::imu_status_e_t + ) + .into()) + } } /// Get a quaternion representing the Inertial Sensor’s orientation. @@ -154,10 +160,7 @@ impl InertialSensor { // Sets the current reading of the Inertial Sensor’s euler values to target euler values. Will default to +/- 180 if target exceeds +/- 180. pub fn set_euler(&self, euler: Euler) -> Result<(), InertialError> { unsafe { - bail_on!( - PROS_ERR, - pros_sys::imu_set_euler(self.port, euler.into()) - ); + bail_on!(PROS_ERR, pros_sys::imu_set_euler(self.port, euler.into())); } Ok(()) } @@ -165,21 +168,15 @@ impl InertialSensor { // Sets the current reading of the Inertial Sensor’s rotation to target value. pub fn set_rotation(&self, rotation: f64) -> Result<(), InertialError> { unsafe { - bail_on!( - PROS_ERR, - pros_sys::imu_set_rotation(self.port, rotation) - ); + bail_on!(PROS_ERR, pros_sys::imu_set_rotation(self.port, rotation)); } Ok(()) } - + // Sets the current reading of the Inertial Sensor’s heading to target value Target will default to 360 if above 360 and default to 0 if below 0. pub fn set_heading(&self, heading: f64) -> Result<(), InertialError> { unsafe { - bail_on!( - PROS_ERR, - pros_sys::imu_set_heading(self.port, heading) - ); + bail_on!(PROS_ERR, pros_sys::imu_set_heading(self.port, heading)); } Ok(()) } @@ -187,10 +184,7 @@ impl InertialSensor { // Sets the current reading of the Inertial Sensor’s pitch to target value Will default to +/- 180 if target exceeds +/- 180. pub fn set_pitch(&self, pitch: f64) -> Result<(), InertialError> { unsafe { - bail_on!( - PROS_ERR, - pros_sys::imu_set_pitch(self.port, pitch) - ); + bail_on!(PROS_ERR, pros_sys::imu_set_pitch(self.port, pitch)); } Ok(()) } @@ -198,10 +192,7 @@ impl InertialSensor { // Sets the current reading of the Inertial Sensor’s roll to target value Will default to +/- 180 if target exceeds +/- 180. pub fn set_roll(&self, roll: f64) -> Result<(), InertialError> { unsafe { - bail_on!( - PROS_ERR, - pros_sys::imu_set_roll(self.port, roll) - ); + bail_on!(PROS_ERR, pros_sys::imu_set_roll(self.port, roll)); } Ok(()) } @@ -209,10 +200,7 @@ impl InertialSensor { // Sets the current reading of the Inertial Sensor’s yaw to target value Will default to +/- 180 if target exceeds +/- 180. pub fn set_yaw(&self, yaw: f64) -> Result<(), InertialError> { unsafe { - bail_on!( - PROS_ERR, - pros_sys::imu_set_yaw(self.port, yaw) - ); + bail_on!(PROS_ERR, pros_sys::imu_set_yaw(self.port, yaw)); } Ok(()) } @@ -220,9 +208,9 @@ impl InertialSensor { #[derive(Default, Debug, Clone, Copy, PartialEq)] pub struct Quaternion { - pub x: f64, - pub y: f64, - pub z: f64, + pub x: f64, + pub y: f64, + pub z: f64, pub w: f64, } @@ -317,4 +305,4 @@ map_errno! { EAGAIN => Self::StillCalibrating, } inherit PortError; -} \ No newline at end of file +} From 209cb27b341cee61f39c9f646185796e46acb93c Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 31 Dec 2023 04:01:00 -0600 Subject: [PATCH 04/24] use correct bitflag comparison --- pros/src/sensors/imu.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index be4e13ff..a1e4ebff 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -12,8 +12,8 @@ pub struct InertialStatus { impl From for InertialStatus { fn from(value: pros_sys::imu_status_e_t) -> Self { Self { - calibrating: (value & pros_sys::E_IMU_STATUS_CALIBRATING) == 0b001, - error: (value & pros_sys::E_IMU_STATUS_ERROR) == 0b010, + calibrating: (value & pros_sys::E_IMU_STATUS_CALIBRATING) == pros_sys::E_IMU_STATUS_CALIBRATING, + error: (value & pros_sys::E_IMU_STATUS_ERROR) == pros_sys::E_IMU_STATUS_ERROR, } } } From c57ad0bb54338e3bd756fa13512b3476fa5ce750 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 31 Dec 2023 04:02:12 -0600 Subject: [PATCH 05/24] add is_calibrating method --- pros/src/sensors/imu.rs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index a1e4ebff..48053181 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -39,6 +39,10 @@ impl InertialSensor { Ok(()) } + pub fn is_calibrating(&self) -> Result { + Ok(self.status()?.calibrating) + } + /// Get the total number of degrees the Inertial Sensor has spun about the z-axis. /// /// This value is theoretically unbounded. Clockwise rotations are represented with positive degree values, From 43df0c14515db8595563b66bb3f1e2b2f16d58bb Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 31 Dec 2023 10:54:59 -0600 Subject: [PATCH 06/24] fix: doc comments --- pros/src/sensors/imu.rs | 38 ++++++++++++++++++++++++++------------ 1 file changed, 26 insertions(+), 12 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 48053181..0d7cad0f 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -5,6 +5,7 @@ use crate::error::{bail_on, map_errno, PortError}; #[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] pub struct InertialStatus { + pub raw: pros_sys::imu_status_e_t, pub calibrating: bool, pub error: bool, } @@ -12,8 +13,9 @@ pub struct InertialStatus { impl From for InertialStatus { fn from(value: pros_sys::imu_status_e_t) -> Self { Self { - calibrating: (value & pros_sys::E_IMU_STATUS_CALIBRATING) == pros_sys::E_IMU_STATUS_CALIBRATING, - error: (value & pros_sys::E_IMU_STATUS_ERROR) == pros_sys::E_IMU_STATUS_ERROR, + raw: value, + calibrating: (value & pros_sys::E_IMU_STATUS_CALIBRATING) != 0, + error: (value & pros_sys::E_IMU_STATUS_ERROR) != 0, } } } @@ -24,14 +26,15 @@ pub struct InertialSensor { } impl InertialSensor { + /// Create a new inertial sensor from a smart port index. pub fn new(port: u8) -> Result { let sensor = Self { port }; sensor.rotation()?; Ok(sensor) } - // Calibrate IMU. - // This takes approximately 2 seconds, and is blocking until the IMU status flag is set properly. + /// Calibrate IMU. + /// This takes approximately 2 seconds, and is blocking until the IMU status flag is set properly. pub fn calibrate(&self) -> Result<(), InertialError> { unsafe { bail_on!(PROS_ERR, pros_sys::imu_reset(self.port)); @@ -39,6 +42,7 @@ impl InertialSensor { Ok(()) } + /// Check if the Intertial Sensor is currently calibrating. pub fn is_calibrating(&self) -> Result { Ok(self.status()?.calibrating) } @@ -95,12 +99,12 @@ impl InertialSensor { unsafe { pros_sys::imu_get_euler(self.port).try_into() } } - // // Get the Inertial Sensor’s raw gyroscope values. + // /// Get the Inertial Sensor’s raw gyroscope values. // pub fn gyro_rate(&self) -> Result { // unsafe { pros_sys::imu_get_gyro_rate(self.port).try_into() } // } - // // Get the Inertial Sensor’s raw accelerometer values. + // /// Get the Inertial Sensor’s raw accelerometer values. // pub fn accel(&self) -> Result { // unsafe { pros_sys::imu_get_accel(self.port).try_into() } // } @@ -161,7 +165,9 @@ impl InertialSensor { Ok(()) } - // Sets the current reading of the Inertial Sensor’s euler values to target euler values. Will default to +/- 180 if target exceeds +/- 180. + /// Sets the current reading of the Inertial Sensor’s euler values to target euler values. + /// + /// Will default to +/- 180 if target exceeds +/- 180. pub fn set_euler(&self, euler: Euler) -> Result<(), InertialError> { unsafe { bail_on!(PROS_ERR, pros_sys::imu_set_euler(self.port, euler.into())); @@ -169,7 +175,7 @@ impl InertialSensor { Ok(()) } - // Sets the current reading of the Inertial Sensor’s rotation to target value. + /// Sets the current reading of the Inertial Sensor’s rotation to target value. pub fn set_rotation(&self, rotation: f64) -> Result<(), InertialError> { unsafe { bail_on!(PROS_ERR, pros_sys::imu_set_rotation(self.port, rotation)); @@ -177,7 +183,9 @@ impl InertialSensor { Ok(()) } - // Sets the current reading of the Inertial Sensor’s heading to target value Target will default to 360 if above 360 and default to 0 if below 0. + /// Sets the current reading of the Inertial Sensor’s heading to target value. + /// + /// Target will default to 360 if above 360 and default to 0 if below 0. pub fn set_heading(&self, heading: f64) -> Result<(), InertialError> { unsafe { bail_on!(PROS_ERR, pros_sys::imu_set_heading(self.port, heading)); @@ -185,7 +193,9 @@ impl InertialSensor { Ok(()) } - // Sets the current reading of the Inertial Sensor’s pitch to target value Will default to +/- 180 if target exceeds +/- 180. + /// Sets the current reading of the Inertial Sensor’s pitch to target value. + /// + /// Will default to +/- 180 if target exceeds +/- 180. pub fn set_pitch(&self, pitch: f64) -> Result<(), InertialError> { unsafe { bail_on!(PROS_ERR, pros_sys::imu_set_pitch(self.port, pitch)); @@ -193,7 +203,9 @@ impl InertialSensor { Ok(()) } - // Sets the current reading of the Inertial Sensor’s roll to target value Will default to +/- 180 if target exceeds +/- 180. + /// Sets the current reading of the Inertial Sensor’s roll to target value + /// + /// Will default to +/- 180 if target exceeds +/- 180. pub fn set_roll(&self, roll: f64) -> Result<(), InertialError> { unsafe { bail_on!(PROS_ERR, pros_sys::imu_set_roll(self.port, roll)); @@ -201,7 +213,9 @@ impl InertialSensor { Ok(()) } - // Sets the current reading of the Inertial Sensor’s yaw to target value Will default to +/- 180 if target exceeds +/- 180. + /// Sets the current reading of the Inertial Sensor’s yaw to target value. + /// + /// Will default to +/- 180 if target exceeds +/- 180. pub fn set_yaw(&self, yaw: f64) -> Result<(), InertialError> { unsafe { bail_on!(PROS_ERR, pros_sys::imu_set_yaw(self.port, yaw)); From 656ac00039fe7f4189ec077e64fee620d85e6a06 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 31 Dec 2023 11:45:14 -0600 Subject: [PATCH 07/24] fix(pros_sys): make `imu_status_e_t` field visibility public --- pros-sys/src/imu.rs | 8 +++--- pros/src/sensors/imu.rs | 56 ++++++++++++++++++++--------------------- 2 files changed, 32 insertions(+), 32 deletions(-) diff --git a/pros-sys/src/imu.rs b/pros-sys/src/imu.rs index 57df53d6..69e15b60 100644 --- a/pros-sys/src/imu.rs +++ b/pros-sys/src/imu.rs @@ -16,10 +16,10 @@ pub struct quaternion_s_t { #[repr(C)] pub struct imu_raw_s { - x: f64, - y: f64, - z: f64, - w: f64, + pub x: f64, + pub y: f64, + pub z: f64, + pub w: f64, } pub type imu_gyro_s_t = imu_raw_s; diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 0d7cad0f..dbd35d33 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -99,15 +99,15 @@ impl InertialSensor { unsafe { pros_sys::imu_get_euler(self.port).try_into() } } - // /// Get the Inertial Sensor’s raw gyroscope values. - // pub fn gyro_rate(&self) -> Result { - // unsafe { pros_sys::imu_get_gyro_rate(self.port).try_into() } - // } + /// Get the Inertial Sensor’s raw gyroscope values. + pub fn gyro_rate(&self) -> Result { + unsafe { pros_sys::imu_get_gyro_rate(self.port).try_into() } + } - // /// Get the Inertial Sensor’s raw accelerometer values. - // pub fn accel(&self) -> Result { - // unsafe { pros_sys::imu_get_accel(self.port).try_into() } - // } + /// Get the Inertial Sensor’s raw accelerometer values. + pub fn accel(&self) -> Result { + unsafe { pros_sys::imu_get_accel(self.port).try_into() } + } /// Resets the current reading of the Inertial Sensor’s heading to zero. pub fn zero_heading(&self) -> Result<(), InertialError> { @@ -289,26 +289,26 @@ impl Into for Euler { } } -// #[derive(Clone, Copy, Debug, PartialEq)] -// pub struct InertialRaw { -// pub x: f64, -// pub y: f64, -// pub z: f64, -// } - -// impl TryFrom for InertialRaw { -// type Error = InertialError; - -// fn try_from(value: pros_sys::imu_raw_s) -> Result { -// bail_on!(value.x, PROS_ERR_F); - -// Ok(Self { -// x: value.x, -// y: value.y, -// z: value.z, -// }) -// } -// } +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct InertialRaw { + pub x: f64, + pub y: f64, + pub z: f64, +} + +impl TryFrom for InertialRaw { + type Error = InertialError; + + fn try_from(value: pros_sys::imu_raw_s) -> Result { + bail_on!(value.x, PROS_ERR_F); + + Ok(Self { + x: value.x, + y: value.y, + z: value.z, + }) + } +} #[derive(Debug, Snafu)] pub enum InertialError { From bea9c18685f9ae3f778d8e4670e6c989913ad713 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 31 Dec 2023 13:44:32 -0600 Subject: [PATCH 08/24] chore: fmt --- pros/src/sensors/imu.rs | 10 +++++----- pros/src/sensors/mod.rs | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index dbd35d33..42f57d6c 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -166,7 +166,7 @@ impl InertialSensor { } /// Sets the current reading of the Inertial Sensor’s euler values to target euler values. - /// + /// /// Will default to +/- 180 if target exceeds +/- 180. pub fn set_euler(&self, euler: Euler) -> Result<(), InertialError> { unsafe { @@ -194,7 +194,7 @@ impl InertialSensor { } /// Sets the current reading of the Inertial Sensor’s pitch to target value. - /// + /// /// Will default to +/- 180 if target exceeds +/- 180. pub fn set_pitch(&self, pitch: f64) -> Result<(), InertialError> { unsafe { @@ -204,7 +204,7 @@ impl InertialSensor { } /// Sets the current reading of the Inertial Sensor’s roll to target value - /// + /// /// Will default to +/- 180 if target exceeds +/- 180. pub fn set_roll(&self, roll: f64) -> Result<(), InertialError> { unsafe { @@ -213,8 +213,8 @@ impl InertialSensor { Ok(()) } - /// Sets the current reading of the Inertial Sensor’s yaw to target value. - /// + /// Sets the current reading of the Inertial Sensor’s yaw to target value. + /// /// Will default to +/- 180 if target exceeds +/- 180. pub fn set_yaw(&self, yaw: f64) -> Result<(), InertialError> { unsafe { diff --git a/pros/src/sensors/mod.rs b/pros/src/sensors/mod.rs index b99c01cc..68c125b2 100644 --- a/pros/src/sensors/mod.rs +++ b/pros/src/sensors/mod.rs @@ -2,4 +2,4 @@ pub mod distance; pub mod gps; pub mod imu; pub mod rotation; -pub mod vision; \ No newline at end of file +pub mod vision; From aaf42170dfedd743a876a0bfd83d156b1a880d11 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 31 Dec 2023 17:00:21 -0600 Subject: [PATCH 09/24] remove `InertialStatus::raw` --- pros/src/sensors/imu.rs | 2 -- 1 file changed, 2 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 42f57d6c..fa1af4bb 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -5,7 +5,6 @@ use crate::error::{bail_on, map_errno, PortError}; #[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] pub struct InertialStatus { - pub raw: pros_sys::imu_status_e_t, pub calibrating: bool, pub error: bool, } @@ -13,7 +12,6 @@ pub struct InertialStatus { impl From for InertialStatus { fn from(value: pros_sys::imu_status_e_t) -> Self { Self { - raw: value, calibrating: (value & pros_sys::E_IMU_STATUS_CALIBRATING) != 0, error: (value & pros_sys::E_IMU_STATUS_ERROR) != 0, } From 2c6b597c31ce2d0114a3461f2f7128fe3b207a29 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 31 Dec 2023 17:42:33 -0600 Subject: [PATCH 10/24] use `imu_reset_blocking` for calibration --- pros/src/sensors/imu.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index fa1af4bb..136ba38d 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -35,7 +35,7 @@ impl InertialSensor { /// This takes approximately 2 seconds, and is blocking until the IMU status flag is set properly. pub fn calibrate(&self) -> Result<(), InertialError> { unsafe { - bail_on!(PROS_ERR, pros_sys::imu_reset(self.port)); + bail_on!(PROS_ERR, pros_sys::imu_reset_blocking(self.port)); } Ok(()) } From 02ebb46891e097c829476f9d555da748d5819f2f Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Mon, 1 Jan 2024 16:39:42 -0600 Subject: [PATCH 11/24] make `InertialStatus` API consistent with `CompetitionStatus` --- pros/src/sensors/imu.rs | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 136ba38d..480d8ff2 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -3,18 +3,21 @@ use snafu::Snafu; use crate::error::{bail_on, map_errno, PortError}; -#[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] -pub struct InertialStatus { - pub calibrating: bool, - pub error: bool, +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub struct InertialStatus(pub u32); + +impl InertialStatus { + pub const fn calibrating(&self) -> bool { + self.0 & pros_sys::E_IMU_STATUS_CALIBRATING != 0 + } + pub const fn error(&self) -> bool { + self.0 & pros_sys::E_IMU_STATUS_ERROR != 0 + } } impl From for InertialStatus { fn from(value: pros_sys::imu_status_e_t) -> Self { - Self { - calibrating: (value & pros_sys::E_IMU_STATUS_CALIBRATING) != 0, - error: (value & pros_sys::E_IMU_STATUS_ERROR) != 0, - } + Self(value) } } @@ -27,7 +30,7 @@ impl InertialSensor { /// Create a new inertial sensor from a smart port index. pub fn new(port: u8) -> Result { let sensor = Self { port }; - sensor.rotation()?; + sensor.status()?; Ok(sensor) } @@ -42,7 +45,7 @@ impl InertialSensor { /// Check if the Intertial Sensor is currently calibrating. pub fn is_calibrating(&self) -> Result { - Ok(self.status()?.calibrating) + Ok(self.status()?.calibrating()) } /// Get the total number of degrees the Inertial Sensor has spun about the z-axis. From 1963e361041c3489851905a6ee6f84ad8b2b1a1b Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Tue, 2 Jan 2024 00:15:34 -0600 Subject: [PATCH 12/24] refactor: inline `bail_on!` for structs returning errors --- pros/src/sensors/imu.rs | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 480d8ff2..08ab608f 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -79,7 +79,7 @@ impl InertialSensor { unsafe { Ok(bail_on!(PROS_ERR_F, pros_sys::imu_get_yaw(self.port))) } } - /// Get the Inertial Sensor’s yaw angle bounded by (-180, 180) degrees. + /// Read the inertial sensor's status code. pub fn status(&self) -> Result { unsafe { Ok(bail_on!( @@ -237,10 +237,8 @@ impl TryFrom for Quaternion { type Error = InertialError; fn try_from(value: pros_sys::quaternion_s_t) -> Result { - bail_on!(value.x, PROS_ERR_F); - Ok(Self { - x: value.x, + x: bail_on!(value.x, PROS_ERR_F), y: value.y, z: value.z, w: value.w, @@ -270,10 +268,8 @@ impl TryFrom for Euler { type Error = InertialError; fn try_from(value: pros_sys::euler_s_t) -> Result { - bail_on!(value.pitch, PROS_ERR_F); - Ok(Self { - pitch: value.pitch, + pitch: bail_on!(value.pitch, PROS_ERR_F), roll: value.roll, yaw: value.yaw, }) @@ -301,10 +297,8 @@ impl TryFrom for InertialRaw { type Error = InertialError; fn try_from(value: pros_sys::imu_raw_s) -> Result { - bail_on!(value.x, PROS_ERR_F); - Ok(Self { - x: value.x, + x: bail_on!(value.x, PROS_ERR_F), y: value.y, z: value.z, }) From e4bec23598146e1f590777fbb7dd025ee3495c30 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Tue, 2 Jan 2024 00:44:11 -0600 Subject: [PATCH 13/24] chore: document all the things --- pros/src/sensors/imu.rs | 66 ++++++++++++++++++++++++++++++----------- 1 file changed, 48 insertions(+), 18 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 08ab608f..9a6a0a4f 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -3,24 +3,7 @@ use snafu::Snafu; use crate::error::{bail_on, map_errno, PortError}; -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub struct InertialStatus(pub u32); - -impl InertialStatus { - pub const fn calibrating(&self) -> bool { - self.0 & pros_sys::E_IMU_STATUS_CALIBRATING != 0 - } - pub const fn error(&self) -> bool { - self.0 & pros_sys::E_IMU_STATUS_ERROR != 0 - } -} - -impl From for InertialStatus { - fn from(value: pros_sys::imu_status_e_t) -> Self { - Self(value) - } -} - +/// Represents a smart port configured as a V5 inertial sensor (IMU) #[derive(Debug)] pub struct InertialSensor { port: u8, @@ -35,6 +18,7 @@ impl InertialSensor { } /// Calibrate IMU. + /// /// This takes approximately 2 seconds, and is blocking until the IMU status flag is set properly. pub fn calibrate(&self) -> Result<(), InertialError> { unsafe { @@ -225,11 +209,20 @@ impl InertialSensor { } } +/// Standard quaternion consisting of a vector defining an axis of rotation +/// and a rotation value about the axis. #[derive(Default, Debug, Clone, Copy, PartialEq)] pub struct Quaternion { + /// The x-component of the axis of rotation. pub x: f64, + + /// The y-component of the axis of rotation. pub y: f64, + + /// The z-component of the axis of rotation. pub z: f64, + + /// The magnitude of rotation about the axis. pub w: f64, } @@ -257,10 +250,16 @@ impl Into for Quaternion { } } +/// A 3-axis set of euler angles. #[derive(Default, Debug, Clone, Copy, PartialEq)] pub struct Euler { + /// The angle measured along the pitch axis. pub pitch: f64, + + /// The angle measured along the roll axis. pub roll: f64, + + /// The angle measured along the yaw axis. pub yaw: f64, } @@ -286,10 +285,19 @@ impl Into for Euler { } } +/// Represents raw data reported by the IMU. +/// +/// This is effectively a 3D vector containing either angular velocity or +/// acceleration values depending on the type of data requested.. #[derive(Clone, Copy, Debug, PartialEq)] pub struct InertialRaw { + /// The x component of the raw data. pub x: f64, + + /// The y component of the raw data. pub y: f64, + + /// The z component of the raw data. pub z: f64, } @@ -305,6 +313,28 @@ impl TryFrom for InertialRaw { } } +/// Represents a status code returned by the Inertial Sensor. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub struct InertialStatus(pub u32); + +impl InertialStatus { + /// Determine if the sensor is currently calibrating. + pub const fn calibrating(&self) -> bool { + self.0 & pros_sys::E_IMU_STATUS_CALIBRATING != 0 + } + + /// Determine if an error state was reached when trying to get the IMU's status. + pub const fn error(&self) -> bool { + self.0 & pros_sys::E_IMU_STATUS_ERROR != 0 + } +} + +impl From for InertialStatus { + fn from(value: pros_sys::imu_status_e_t) -> Self { + Self(value) + } +} + #[derive(Debug, Snafu)] pub enum InertialError { #[snafu(display("Inertial sensor is still calibrating."))] From 474f407b781ebedc041ab55373ce3088234797ac Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Thu, 4 Jan 2024 17:57:03 -0600 Subject: [PATCH 14/24] include imu in prelude, add example --- pros/examples/imu.rs | 28 ++++++++++++++++++++++++++++ pros/src/lib.rs | 1 + pros/src/sensors/imu.rs | 4 ++-- 3 files changed, 31 insertions(+), 2 deletions(-) create mode 100644 pros/examples/imu.rs diff --git a/pros/examples/imu.rs b/pros/examples/imu.rs new file mode 100644 index 00000000..ec4c6eb5 --- /dev/null +++ b/pros/examples/imu.rs @@ -0,0 +1,28 @@ +#![no_std] +#![no_main] + +use pros::prelude::*; +use core::time::Duration; + +#[derive(Default)] +pub struct Robot; + +impl SyncRobot for Robot { + fn opcontrol(&mut self) -> pros::Result { + let imu = InertialSensor::new(1)?; + + imu.calibrate()?; + + loop { + let euler = imu.euler()?; + + println!("Pitch: {} Roll: {} Yaw: {}", euler.pitch, euler.roll, euler.yaw); + + pros::task::delay(Duration::from_secs(1)); + } + + Ok(()) + } +} + +sync_robot!(Robot); diff --git a/pros/src/lib.rs b/pros/src/lib.rs index b7e4ceaf..2c845aa7 100644 --- a/pros/src/lib.rs +++ b/pros/src/lib.rs @@ -318,5 +318,6 @@ pub mod prelude { pub use crate::sensors::gps::*; pub use crate::sensors::rotation::*; pub use crate::sensors::vision::*; + pub use crate::sensors::imu::*; pub use crate::task::{sleep, spawn}; } diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 9a6a0a4f..80431fa7 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -268,7 +268,7 @@ impl TryFrom for Euler { fn try_from(value: pros_sys::euler_s_t) -> Result { Ok(Self { - pitch: bail_on!(value.pitch, PROS_ERR_F), + pitch: bail_on!(PROS_ERR_F, value.pitch), roll: value.roll, yaw: value.yaw, }) @@ -306,7 +306,7 @@ impl TryFrom for InertialRaw { fn try_from(value: pros_sys::imu_raw_s) -> Result { Ok(Self { - x: bail_on!(value.x, PROS_ERR_F), + x: bail_on!(PROS_ERR_F, value.x), y: value.y, z: value.z, }) From fea6c8d1d4f68fa3659e9e51dc7cdc8e6167fd7d Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Thu, 4 Jan 2024 18:02:55 -0600 Subject: [PATCH 15/24] fix: quaternion using incorrect `bail_on!` order --- pros/examples/imu.rs | 31 ++++++++++++++++++++++++++++--- pros/src/sensors/imu.rs | 2 +- 2 files changed, 29 insertions(+), 4 deletions(-) diff --git a/pros/examples/imu.rs b/pros/examples/imu.rs index ec4c6eb5..6c670bc0 100644 --- a/pros/examples/imu.rs +++ b/pros/examples/imu.rs @@ -14,9 +14,34 @@ impl SyncRobot for Robot { imu.calibrate()?; loop { - let euler = imu.euler()?; - - println!("Pitch: {} Roll: {} Yaw: {}", euler.pitch, euler.roll, euler.yaw); + // let euler = imu.euler()?; + + // println!("Pitch: {} Roll: {} Yaw: {}", euler.pitch, euler.roll, euler.yaw); + + let delay = Duration::from_secs(1); + + println!("{:?}", imu.is_calibrating()); + pros::task::delay(delay); + println!("{:?}", imu.rotation()); + pros::task::delay(delay); + println!("{:?}", imu.heading()); + pros::task::delay(delay); + println!("{:?}", imu.pitch()); + pros::task::delay(delay); + println!("{:?}", imu.roll()); + pros::task::delay(delay); + println!("{:?}", imu.yaw()); + pros::task::delay(delay); + println!("{:?}", imu.status()); + pros::task::delay(delay); + println!("{:?}", imu.quaternion()); + pros::task::delay(delay); + println!("{:?}", imu.euler()); + pros::task::delay(delay); + println!("{:?}", imu.gyro_rate()); + pros::task::delay(delay); + println!("{:?}", imu.accel()); + pros::task::delay(delay); pros::task::delay(Duration::from_secs(1)); } diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 80431fa7..fa73769b 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -231,7 +231,7 @@ impl TryFrom for Quaternion { fn try_from(value: pros_sys::quaternion_s_t) -> Result { Ok(Self { - x: bail_on!(value.x, PROS_ERR_F), + x: bail_on!(PROS_ERR_F, value.x), y: value.y, z: value.z, w: value.w, From 2ba4141c3344ed42cc43df14c725d522609fae86 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Thu, 4 Jan 2024 18:06:13 -0600 Subject: [PATCH 16/24] fix: revert test code in example --- pros/examples/imu.rs | 31 +++---------------------------- 1 file changed, 3 insertions(+), 28 deletions(-) diff --git a/pros/examples/imu.rs b/pros/examples/imu.rs index 6c670bc0..ec4c6eb5 100644 --- a/pros/examples/imu.rs +++ b/pros/examples/imu.rs @@ -14,34 +14,9 @@ impl SyncRobot for Robot { imu.calibrate()?; loop { - // let euler = imu.euler()?; - - // println!("Pitch: {} Roll: {} Yaw: {}", euler.pitch, euler.roll, euler.yaw); - - let delay = Duration::from_secs(1); - - println!("{:?}", imu.is_calibrating()); - pros::task::delay(delay); - println!("{:?}", imu.rotation()); - pros::task::delay(delay); - println!("{:?}", imu.heading()); - pros::task::delay(delay); - println!("{:?}", imu.pitch()); - pros::task::delay(delay); - println!("{:?}", imu.roll()); - pros::task::delay(delay); - println!("{:?}", imu.yaw()); - pros::task::delay(delay); - println!("{:?}", imu.status()); - pros::task::delay(delay); - println!("{:?}", imu.quaternion()); - pros::task::delay(delay); - println!("{:?}", imu.euler()); - pros::task::delay(delay); - println!("{:?}", imu.gyro_rate()); - pros::task::delay(delay); - println!("{:?}", imu.accel()); - pros::task::delay(delay); + let euler = imu.euler()?; + + println!("Pitch: {} Roll: {} Yaw: {}", euler.pitch, euler.roll, euler.yaw); pros::task::delay(Duration::from_secs(1)); } From 962d9d821e5c905f7d07df2da527f185330ef5c3 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Thu, 4 Jan 2024 22:11:04 -0600 Subject: [PATCH 17/24] fix: don't return Ok(()) in the example --- pros/examples/imu.rs | 2 -- 1 file changed, 2 deletions(-) diff --git a/pros/examples/imu.rs b/pros/examples/imu.rs index ec4c6eb5..5529fec2 100644 --- a/pros/examples/imu.rs +++ b/pros/examples/imu.rs @@ -20,8 +20,6 @@ impl SyncRobot for Robot { pros::task::delay(Duration::from_secs(1)); } - - Ok(()) } } From 02a4bc23aecfd9cd370d9f683138c5f5dd3f0fcc Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Thu, 4 Jan 2024 23:17:35 -0600 Subject: [PATCH 18/24] feat: calibration future, data rate setter --- pros/src/sensors/imu.rs | 62 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 59 insertions(+), 3 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index fa73769b..9a430400 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -1,10 +1,18 @@ +use core::{ + time::Duration, + task::{Poll, Context}, + pin::Pin, +}; use pros_sys::{PROS_ERR, PROS_ERR_F}; use snafu::Snafu; use crate::error::{bail_on, map_errno, PortError}; +pub const IMU_RESET_TIMEOUT: Duration = Duration::from_secs(3); +pub const IMU_MIN_DATA_RATE: Duration = Duration::from_millis(5); + /// Represents a smart port configured as a V5 inertial sensor (IMU) -#[derive(Debug)] +#[derive(Debug, Clone, Copy)] pub struct InertialSensor { port: u8, } @@ -22,11 +30,19 @@ impl InertialSensor { /// This takes approximately 2 seconds, and is blocking until the IMU status flag is set properly. pub fn calibrate(&self) -> Result<(), InertialError> { unsafe { - bail_on!(PROS_ERR, pros_sys::imu_reset_blocking(self.port)); + bail_on!(PROS_ERR, pros_sys::imu_reset(self.port)); } Ok(()) } + /// Returns a future that completes when the motor reports that it has stopped. + pub fn wait_until_calibrated(&self) -> InertialCalibrationFuture { + InertialCalibrationFuture { + imu: *self, + timestamp: Duration::from_micros(unsafe { pros_sys::rtos::micros() }), + } + } + /// Check if the Intertial Sensor is currently calibrating. pub fn is_calibrating(&self) -> Result { Ok(self.status()?.calibrating()) @@ -207,6 +223,19 @@ impl InertialSensor { } Ok(()) } + + pub fn set_data_rate(&self, data_rate: Duration) -> Result<(), InertialError> { + unsafe { + let rate_ms = if let Ok(rate) = u32::try_from(data_rate.as_millis()) { + rate + } else { + return Err(InertialError::InvalidDataRate); + }; + + bail_on!(PROS_ERR, pros_sys::imu_set_data_rate(self.port, rate_ms)); + } + Ok(()) + } } /// Standard quaternion consisting of a vector defining an axis of rotation @@ -335,10 +364,37 @@ impl From for InertialStatus { } } +pub struct InertialCalibrationFuture { + imu: InertialSensor, + timestamp: Duration, // TODO: Change this to [`Instant`] if/when that's implemented +} + +impl core::future::Future for InertialCalibrationFuture { + type Output = Result<(), InertialError>; + + fn poll( + self: Pin<&mut Self>, + cx: &mut Context<'_>, + ) -> Poll { + let elapsed = Duration::from_micros(unsafe { pros_sys::rtos::micros() }) - self.timestamp; + + if elapsed > IMU_RESET_TIMEOUT { + Poll::Ready(Err(InertialError::StillCalibrating)) + } else if self.imu.is_calibrating()? { + cx.waker().wake_by_ref(); + Poll::Pending + } else { + Poll::Ready(Ok(())) + } + } +} + #[derive(Debug, Snafu)] pub enum InertialError { - #[snafu(display("Inertial sensor is still calibrating."))] + #[snafu(display("Inertial sensor is still calibrating, but exceeded calibration timeout."))] StillCalibrating, + #[snafu(display("Sensor data rate has a minimum duration of 5 milliseconds."))] + InvalidDataRate, #[snafu(display("{source}"), context(false))] Port { source: PortError }, } From f7f6f4a733675fe1fe86a37ed82393b5b14c50d7 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Thu, 4 Jan 2024 23:59:38 -0600 Subject: [PATCH 19/24] feat: proper async calibration --- pros/src/sensors/imu.rs | 73 ++++++++++++++++++++++------------------- 1 file changed, 40 insertions(+), 33 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 9a430400..a5eb2aaf 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -1,15 +1,15 @@ use core::{ - time::Duration, - task::{Poll, Context}, pin::Pin, + task::{Context, Poll}, + time::Duration, }; use pros_sys::{PROS_ERR, PROS_ERR_F}; use snafu::Snafu; use crate::error::{bail_on, map_errno, PortError}; -pub const IMU_RESET_TIMEOUT: Duration = Duration::from_secs(3); -pub const IMU_MIN_DATA_RATE: Duration = Duration::from_millis(5); +pub const IMU_RESET_TIMEOUT: Duration = Duration::from_secs(3); +pub const IMU_MIN_DATA_RATE: Duration = Duration::from_millis(5); /// Represents a smart port configured as a V5 inertial sensor (IMU) #[derive(Debug, Clone, Copy)] @@ -26,21 +26,21 @@ impl InertialSensor { } /// Calibrate IMU. - /// + /// /// This takes approximately 2 seconds, and is blocking until the IMU status flag is set properly. - pub fn calibrate(&self) -> Result<(), InertialError> { + /// There is additionally a 3 second timeout that will return [`InertialError::CalibrationTimedOut`] if the timeout is exceeded. + pub fn calibrate_blocking(&self) -> Result<(), InertialError> { unsafe { - bail_on!(PROS_ERR, pros_sys::imu_reset(self.port)); + bail_on!(PROS_ERR, pros_sys::imu_reset_blocking(self.port)); } Ok(()) } - /// Returns a future that completes when the motor reports that it has stopped. - pub fn wait_until_calibrated(&self) -> InertialCalibrationFuture { - InertialCalibrationFuture { - imu: *self, - timestamp: Duration::from_micros(unsafe { pros_sys::rtos::micros() }), - } + /// Calibrate IMU asynchronously. + /// + /// There a 3 second timeout that will return [`InertialError::CalibrationTimedOut`] if the timeout is exceeded. + pub fn wait_until_calibrated(&self) -> InertialCalibrateFuture { + InertialCalibrateFuture::Calibrate(*self) } /// Check if the Intertial Sensor is currently calibrating. @@ -315,7 +315,7 @@ impl Into for Euler { } /// Represents raw data reported by the IMU. -/// +/// /// This is effectively a 3D vector containing either angular velocity or /// acceleration values depending on the type of data requested.. #[derive(Clone, Copy, Debug, PartialEq)] @@ -364,27 +364,34 @@ impl From for InertialStatus { } } -pub struct InertialCalibrationFuture { - imu: InertialSensor, - timestamp: Duration, // TODO: Change this to [`Instant`] if/when that's implemented +pub enum InertialCalibrateFuture { + Calibrate(InertialSensor), + Waiting(InertialSensor, Duration), } -impl core::future::Future for InertialCalibrationFuture { +impl core::future::Future for InertialCalibrateFuture { type Output = Result<(), InertialError>; - fn poll( - self: Pin<&mut Self>, - cx: &mut Context<'_>, - ) -> Poll { - let elapsed = Duration::from_micros(unsafe { pros_sys::rtos::micros() }) - self.timestamp; - - if elapsed > IMU_RESET_TIMEOUT { - Poll::Ready(Err(InertialError::StillCalibrating)) - } else if self.imu.is_calibrating()? { - cx.waker().wake_by_ref(); - Poll::Pending - } else { - Poll::Ready(Ok(())) + fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll { + loop { + match &mut *self { + Self::Calibrate(imu) => { + unsafe { bail_on!(PROS_ERR, pros_sys::imu_reset(imu.port)); } + *self = Self::Waiting(*imu, Duration::from_micros(unsafe { pros_sys::rtos::micros() })); + }, + Self::Waiting(imu, timestamp) => { + let elapsed = Duration::from_micros(unsafe { pros_sys::rtos::micros() }) - *timestamp; + + return if elapsed > IMU_RESET_TIMEOUT { + Poll::Ready(Err(InertialError::CalibrationTimedOut)) + } else if imu.is_calibrating()? { + cx.waker().wake_by_ref(); + Poll::Pending + } else { + Poll::Ready(Ok(())) + } + }, + } } } } @@ -392,7 +399,7 @@ impl core::future::Future for InertialCalibrationFuture { #[derive(Debug, Snafu)] pub enum InertialError { #[snafu(display("Inertial sensor is still calibrating, but exceeded calibration timeout."))] - StillCalibrating, + CalibrationTimedOut, #[snafu(display("Sensor data rate has a minimum duration of 5 milliseconds."))] InvalidDataRate, #[snafu(display("{source}"), context(false))] @@ -401,7 +408,7 @@ pub enum InertialError { map_errno! { InertialError { - EAGAIN => Self::StillCalibrating, + EAGAIN => Self::CalibrationTimedOut, } inherit PortError; } From fb3f57e487ecfbc0504b8304340b36973cd03a30 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Fri, 5 Jan 2024 08:47:25 -0600 Subject: [PATCH 20/24] refactor: rename `wait_until_calibrated` to `calibrate_async` --- pros/src/sensors/imu.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index a5eb2aaf..e38a5c17 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -29,7 +29,7 @@ impl InertialSensor { /// /// This takes approximately 2 seconds, and is blocking until the IMU status flag is set properly. /// There is additionally a 3 second timeout that will return [`InertialError::CalibrationTimedOut`] if the timeout is exceeded. - pub fn calibrate_blocking(&self) -> Result<(), InertialError> { + pub fn calibrate(&self) -> Result<(), InertialError> { unsafe { bail_on!(PROS_ERR, pros_sys::imu_reset_blocking(self.port)); } @@ -39,7 +39,7 @@ impl InertialSensor { /// Calibrate IMU asynchronously. /// /// There a 3 second timeout that will return [`InertialError::CalibrationTimedOut`] if the timeout is exceeded. - pub fn wait_until_calibrated(&self) -> InertialCalibrateFuture { + pub fn calibrate_async(&self) -> InertialCalibrateFuture { InertialCalibrateFuture::Calibrate(*self) } From 71081a41c5f3293f93a2f9753239b2c9edf83c38 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Fri, 5 Jan 2024 14:24:27 -0600 Subject: [PATCH 21/24] fix: calibration future --- pros/src/sensors/imu.rs | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index e38a5c17..225b5bd6 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -6,7 +6,7 @@ use core::{ use pros_sys::{PROS_ERR, PROS_ERR_F}; use snafu::Snafu; -use crate::error::{bail_on, map_errno, PortError}; +use crate::error::{bail_on, map_errno, take_errno, FromErrno, PortError}; pub const IMU_RESET_TIMEOUT: Duration = Duration::from_secs(3); pub const IMU_MIN_DATA_RATE: Duration = Duration::from_millis(5); @@ -372,15 +372,25 @@ pub enum InertialCalibrateFuture { impl core::future::Future for InertialCalibrateFuture { type Output = Result<(), InertialError>; - fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll { + fn poll(mut self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll { loop { - match &mut *self { - Self::Calibrate(imu) => { - unsafe { bail_on!(PROS_ERR, pros_sys::imu_reset(imu.port)); } - *self = Self::Waiting(*imu, Duration::from_micros(unsafe { pros_sys::rtos::micros() })); + match *self { + Self::Calibrate(imu) => match unsafe { pros_sys::imu_reset(imu.port) } { + PROS_ERR => { + let errno = take_errno(); + return Poll::Ready(Err(InertialError::from_errno(take_errno()) + .unwrap_or_else(|| panic!("Unknown errno code {errno}")))); + } + _ => { + *self = Self::Waiting( + imu, + Duration::from_micros(unsafe { pros_sys::rtos::micros() }), + ); + } }, Self::Waiting(imu, timestamp) => { - let elapsed = Duration::from_micros(unsafe { pros_sys::rtos::micros() }) - *timestamp; + let elapsed = + Duration::from_micros(unsafe { pros_sys::rtos::micros() }) - timestamp; return if elapsed > IMU_RESET_TIMEOUT { Poll::Ready(Err(InertialError::CalibrationTimedOut)) @@ -389,8 +399,8 @@ impl core::future::Future for InertialCalibrateFuture { Poll::Pending } else { Poll::Ready(Ok(())) - } - }, + }; + } } } } From b3b990cd09b79fa069f9bfae61b97b4da05720d3 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 7 Jan 2024 21:53:31 -0600 Subject: [PATCH 22/24] docs: document `set_data_rate` --- pros/src/sensors/imu.rs | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 225b5bd6..c8eb4092 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -224,10 +224,17 @@ impl InertialSensor { Ok(()) } + /// Sets the update rate of the IMU. + /// + /// This duration must be above [`IMU_MIN_DATA_RATE`] (5 milliseconds). pub fn set_data_rate(&self, data_rate: Duration) -> Result<(), InertialError> { unsafe { - let rate_ms = if let Ok(rate) = u32::try_from(data_rate.as_millis()) { - rate + let rate_ms = if data_rate > IMU_MIN_DATA_RATE { + if let Ok(rate) = u32::try_from(data_rate.as_millis()) { + rate + } else { + return Err(InertialError::InvalidDataRate); + } } else { return Err(InertialError::InvalidDataRate); }; From 54f834e181512f7b4fdc5aa3c3ef012bc6b0b34d Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 7 Jan 2024 22:03:49 -0600 Subject: [PATCH 23/24] refactor: swap `calibrate` terminology --- pros/src/sensors/imu.rs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index c8eb4092..611c5ddd 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -29,7 +29,7 @@ impl InertialSensor { /// /// This takes approximately 2 seconds, and is blocking until the IMU status flag is set properly. /// There is additionally a 3 second timeout that will return [`InertialError::CalibrationTimedOut`] if the timeout is exceeded. - pub fn calibrate(&self) -> Result<(), InertialError> { + pub fn calibrate_blocking(&self) -> Result<(), InertialError> { unsafe { bail_on!(PROS_ERR, pros_sys::imu_reset_blocking(self.port)); } @@ -38,8 +38,10 @@ impl InertialSensor { /// Calibrate IMU asynchronously. /// + /// Returns an [`InertialCalibrationFuture`] that is be polled until the IMU status flag reports the sensor as + /// no longer calibrating. /// There a 3 second timeout that will return [`InertialError::CalibrationTimedOut`] if the timeout is exceeded. - pub fn calibrate_async(&self) -> InertialCalibrateFuture { + pub fn calibrate(&self) -> InertialCalibrateFuture { InertialCalibrateFuture::Calibrate(*self) } From e61a5e156746b6c49d896358bcfdb5e48b1a9961 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 7 Jan 2024 22:13:17 -0600 Subject: [PATCH 24/24] chore: fmt --- pros/examples/imu.rs | 7 +++++-- pros/src/lib.rs | 2 +- pros/src/sensors/imu.rs | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/pros/examples/imu.rs b/pros/examples/imu.rs index 5529fec2..53653c3d 100644 --- a/pros/examples/imu.rs +++ b/pros/examples/imu.rs @@ -1,8 +1,8 @@ #![no_std] #![no_main] -use pros::prelude::*; use core::time::Duration; +use pros::prelude::*; #[derive(Default)] pub struct Robot; @@ -16,7 +16,10 @@ impl SyncRobot for Robot { loop { let euler = imu.euler()?; - println!("Pitch: {} Roll: {} Yaw: {}", euler.pitch, euler.roll, euler.yaw); + println!( + "Pitch: {} Roll: {} Yaw: {}", + euler.pitch, euler.roll, euler.yaw + ); pros::task::delay(Duration::from_secs(1)); } diff --git a/pros/src/lib.rs b/pros/src/lib.rs index c7203e3f..23f936eb 100644 --- a/pros/src/lib.rs +++ b/pros/src/lib.rs @@ -370,8 +370,8 @@ pub mod prelude { pub use crate::position::*; pub use crate::sensors::distance::*; pub use crate::sensors::gps::*; + pub use crate::sensors::imu::*; pub use crate::sensors::rotation::*; pub use crate::sensors::vision::*; - pub use crate::sensors::imu::*; pub use crate::task::{sleep, spawn}; } diff --git a/pros/src/sensors/imu.rs b/pros/src/sensors/imu.rs index 611c5ddd..43a40956 100644 --- a/pros/src/sensors/imu.rs +++ b/pros/src/sensors/imu.rs @@ -227,7 +227,7 @@ impl InertialSensor { } /// Sets the update rate of the IMU. - /// + /// /// This duration must be above [`IMU_MIN_DATA_RATE`] (5 milliseconds). pub fn set_data_rate(&self, data_rate: Duration) -> Result<(), InertialError> { unsafe {