From 2d4265a0cee05a44d5f57e21dc80a97a271f10e6 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 24 Mar 2024 11:09:10 -0500 Subject: [PATCH 01/14] refactor: distance sensor --- packages/pros-core/src/error.rs | 3 + packages/pros-devices/Cargo.toml | 1 + packages/pros-devices/src/smart/distance.rs | 83 +++++++++++++++------ packages/pros-devices/src/smart/imu.rs | 11 --- packages/pros-devices/src/smart/mod.rs | 23 ++++++ 5 files changed, 86 insertions(+), 35 deletions(-) diff --git a/packages/pros-core/src/error.rs b/packages/pros-core/src/error.rs index f129cede..c3bdbe93 100644 --- a/packages/pros-core/src/error.rs +++ b/packages/pros-core/src/error.rs @@ -103,6 +103,9 @@ pub enum PortError { PortCannotBeConfigured, /// The specified port is already being used or is mismatched. AlreadyInUse, + + NoDevice, + IncorrectDevice, } map_errno!(PortError { diff --git a/packages/pros-devices/Cargo.toml b/packages/pros-devices/Cargo.toml index 52bbf80d..6c47cf22 100644 --- a/packages/pros-devices/Cargo.toml +++ b/packages/pros-devices/Cargo.toml @@ -22,6 +22,7 @@ authors = [ [dependencies] pros-core = { version = "0.1.0", path = "../pros-core" } pros-sys = { path = "../pros-sys", version = "0.7.0", features = ["xapi"] } +vex_sys = { git = "https://github.com/pros-rs/vex-sys/" } snafu = { version = "0.8.0", default-features = false, features = [ "rust_1_61", "unstable-core-error", diff --git a/packages/pros-devices/src/smart/distance.rs b/packages/pros-devices/src/smart/distance.rs index 53fa0849..eaf8bf26 100644 --- a/packages/pros-devices/src/smart/distance.rs +++ b/packages/pros-devices/src/smart/distance.rs @@ -2,10 +2,12 @@ //! //! Pretty much one to one with the PROS C and CPP API, except Result is used instead of ERRNO values. -use core::ffi::c_double; - -use pros_core::{bail_on, error::PortError}; -use pros_sys::PROS_ERR; +use pros_core::error::PortError; +use snafu::Snafu; +use vex_sys::{ + vexDeviceDistanceConfidenceGet, vexDeviceDistanceDistanceGet, vexDeviceDistanceObjectSizeGet, + vexDeviceDistanceObjectVelocityGet, vexDeviceDistanceStatusGet, V5_DeviceType, +}; use super::{SmartDevice, SmartDeviceType, SmartPort}; @@ -22,20 +24,32 @@ impl DistanceSensor { Self { port } } + /// Validates that the sensor is currently connected to its port, and that its status code + /// is either 0x82 or 0x86. + /// + /// It's unknown what these status codes indicate (likely related to port status), but PROS + /// performs this check in their API, so we will too. + /// + /// + fn validate(&self) -> Result<(), DistanceError> { + match self.status()? { + 0x82 | 0x86 => Ok(()), + _ => Err(DistanceError::BadStatusCode), + } + } + /// Returns the distance to the object the sensor detects in millimeters. - pub fn distance(&self) -> Result { - Ok(bail_on!(PROS_ERR, unsafe { - pros_sys::distance_get(self.port.index()) - }) as u32) + pub fn distance(&self) -> Result { + self.validate()?; + + Ok(unsafe { vexDeviceDistanceDistanceGet(self.port.device_handle()) }) } /// Returns the velocity of the object the sensor detects in m/s - pub fn velocity(&self) -> Result { - // All VEX Distance Sensor functions return PROS_ERR on failure even though - // some return floating point values (not PROS_ERR_F) - Ok(bail_on!(PROS_ERR as c_double, unsafe { - pros_sys::distance_get_object_velocity(self.port.index()) - })) + pub fn velocity(&self) -> Result { + self.validate()?; + + Ok(unsafe { vexDeviceDistanceObjectVelocityGet(self.port.device_handle()) }) } /// Get the current guess at relative "object size". @@ -49,20 +63,27 @@ impl DistanceSensor { /// of salt. /// /// [`vex::sizeType`]: https://api.vexcode.cloud/v5/search/sizeType/sizeType/enum - pub fn relative_size(&self) -> Result { - Ok(bail_on!(PROS_ERR, unsafe { - pros_sys::distance_get_object_size(self.port.index()) - }) as u32) + pub fn relative_size(&self) -> Result { + self.validate()?; + + Ok(unsafe { vexDeviceDistanceObjectSizeGet(self.port.device_handle()) as u32 }) } /// Returns the confidence in the distance measurement from 0.0 to 1.0. - pub fn distance_confidence(&self) -> Result { - // 0 -> 63 - let confidence = bail_on!(PROS_ERR, unsafe { - pros_sys::distance_get_confidence(self.port.index()) - }) as f64; + pub fn distance_confidence(&self) -> Result { + self.validate()?; + + Ok( + unsafe { vexDeviceDistanceConfidenceGet(self.port.device_handle()) as u32 } as f64 + / 63.0, + ) + } + + /// Gets the status code of the distance sensor + pub fn status(&self) -> Result { + self.port.validate(V5_DeviceType::DistanceSensor)?; - Ok(confidence / 63.0) + Ok(unsafe { vexDeviceDistanceStatusGet(self.port.device_handle()) }) } } @@ -75,3 +96,17 @@ impl SmartDevice for DistanceSensor { SmartDeviceType::Distance } } + +#[derive(Debug, Snafu)] +/// Errors that can occur when using a distance sensor. +pub enum DistanceError { + /// The sensor's status code is not 0x82 or 0x86. + BadStatusCode, + + /// Generic port related error. + #[snafu(display("{source}"), context(false))] + Port { + /// The source of the error. + source: PortError, + }, +} diff --git a/packages/pros-devices/src/smart/imu.rs b/packages/pros-devices/src/smart/imu.rs index b76ef393..d512f8b4 100644 --- a/packages/pros-devices/src/smart/imu.rs +++ b/packages/pros-devices/src/smart/imu.rs @@ -36,17 +36,6 @@ impl InertialSensor { Self { port } } - /// Calibrate IMU. - /// - /// 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(&mut self) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_reset_blocking(self.port.index()) - }); - Ok(()) - } - /// Calibrate IMU asynchronously. /// /// Returns an [`InertialCalibrateFuture`] that is be polled until the IMU status flag reports the sensor as diff --git a/packages/pros-devices/src/smart/mod.rs b/packages/pros-devices/src/smart/mod.rs index f528aa90..09d1514a 100644 --- a/packages/pros-devices/src/smart/mod.rs +++ b/packages/pros-devices/src/smart/mod.rs @@ -41,6 +41,7 @@ pub use motor::Motor; pub use optical::OpticalSensor; use pros_core::{bail_on, error::PortError}; pub use rotation::RotationSensor; +use vex_sys::{vexDeviceGetByIndex, V5_DeviceT, V5_DeviceType}; pub use vision::VisionSensor; /// Defines common functionality shared by all smart port devices. @@ -139,6 +140,28 @@ impl SmartPort { self.index } + /// Get the raw device handle connected to this port. + pub(crate) fn device_handle(&self) -> V5_DeviceT { + unsafe { + vexDeviceGetByIndex((self.index - 1) as u32) + } + } + + /// Verify that a specific device type is currently plugged into this port. + pub(crate) fn validate(&self, device_type: V5_DeviceType) -> Result<(), PortError> { + let dev = unsafe { *self.device_handle() }; + + if !dev.exists { + // No device is plugged into the port. + return Err(PortError::NoDevice) + } else if dev.device_type != device_type { + // The connected device doesn't match the requested type. + return Err(PortError::IncorrectDevice) + } + + Ok(()) + } + /// Get the type of device currently connected to this port. /// /// # Examples From c3f119b7b8988136164605081f2d6c98eb63f2f7 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 24 Mar 2024 11:11:29 -0500 Subject: [PATCH 02/14] refactor: battery API --- packages/pros-devices/src/battery.rs | 47 +++++++++------------------- 1 file changed, 15 insertions(+), 32 deletions(-) diff --git a/packages/pros-devices/src/battery.rs b/packages/pros-devices/src/battery.rs index ebb02ce4..7e7ee32c 100644 --- a/packages/pros-devices/src/battery.rs +++ b/packages/pros-devices/src/battery.rs @@ -1,46 +1,29 @@ //! Utilites for getting information about the robot's battery. -use pros_core::{bail_on, map_errno}; -use pros_sys::{PROS_ERR, PROS_ERR_F}; -use snafu::Snafu; +use vex_sys::{ + vexBatteryCapacityGet, vexBatteryCurrentGet, vexBatteryTemperatureGet, vexBatteryVoltageGet, +}; /// Get the robot's battery capacity. -pub fn capacity() -> Result { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::misc::battery_get_capacity() - })) +/// TODO: Determine units +pub fn capacity() -> f64 { + unsafe { vexBatteryCapacityGet() } } /// Get the current temperature of the robot's battery. -pub fn temperature() -> Result { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::misc::battery_get_temperature() - })) +/// TODO: Determine units +pub fn temperature() -> f64 { + unsafe { vexBatteryTemperatureGet() } } /// Get the electric current of the robot's battery. -pub fn current() -> Result { - Ok(bail_on!(PROS_ERR, unsafe { - pros_sys::misc::battery_get_current() - })) +/// TODO: Determine units +pub fn current() -> i32 { + unsafe { vexBatteryCurrentGet() } } /// Get the robot's battery voltage. -pub fn voltage() -> Result { - Ok(bail_on!(PROS_ERR, unsafe { - pros_sys::misc::battery_get_voltage() - })) -} - -#[derive(Debug, Snafu)] -/// Errors that can occur when interacting with the robot's battery. -pub enum BatteryError { - /// Another resource is already using the battery. - ConcurrentAccess, -} - -map_errno! { - BatteryError { - EACCES => Self::ConcurrentAccess, - } +/// TODO: Determine units +pub fn voltage() -> i32 { + unsafe { vexBatteryVoltageGet() } } From 1370a73c2842a506f4e2bb44db53c4ae6d69f752 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 24 Mar 2024 11:13:25 -0500 Subject: [PATCH 03/14] refactor: rename pros-devices to vex-devices and remove color api --- CHANGELOG.md | 2 +- packages/pros-devices/src/color.rs | 384 ------------------ packages/pros-panic/Cargo.toml | 4 +- packages/pros/Cargo.toml | 6 +- .../{pros-devices => vex-devices}/Cargo.toml | 2 +- .../{pros-devices => vex-devices}/README.md | 2 +- .../src/adi/analog.rs | 0 .../src/adi/digital.rs | 0 .../src/adi/encoder.rs | 0 .../src/adi/gyro.rs | 0 .../src/adi/linetracker.rs | 0 .../src/adi/mod.rs | 0 .../src/adi/motor.rs | 0 .../src/adi/potentiometer.rs | 0 .../src/adi/pwm.rs | 0 .../src/adi/solenoid.rs | 0 .../src/adi/switch.rs | 0 .../src/adi/ultrasonic.rs | 0 .../src/battery.rs | 0 packages/vex-devices/src/color.rs | 91 +++++ .../src/competition.rs | 0 .../src/controller.rs | 0 .../{pros-devices => vex-devices}/src/lib.rs | 2 +- .../src/peripherals.rs | 0 .../src/position.rs | 0 .../src/screen.rs | 0 .../src/smart/distance.rs | 0 .../src/smart/expander.rs | 0 .../src/smart/gps.rs | 0 .../src/smart/imu.rs | 0 .../src/smart/link.rs | 0 .../src/smart/mod.rs | 0 .../src/smart/motor.rs | 0 .../src/smart/optical.rs | 0 .../src/smart/rotation.rs | 0 .../src/smart/vision.rs | 0 .../{pros-devices => vex-devices}/src/usd.rs | 0 37 files changed, 100 insertions(+), 393 deletions(-) delete mode 100644 packages/pros-devices/src/color.rs rename packages/{pros-devices => vex-devices}/Cargo.toml (97%) rename packages/{pros-devices => vex-devices}/README.md (97%) rename packages/{pros-devices => vex-devices}/src/adi/analog.rs (100%) rename packages/{pros-devices => vex-devices}/src/adi/digital.rs (100%) rename packages/{pros-devices => vex-devices}/src/adi/encoder.rs (100%) rename packages/{pros-devices => vex-devices}/src/adi/gyro.rs (100%) rename packages/{pros-devices => vex-devices}/src/adi/linetracker.rs (100%) rename packages/{pros-devices => vex-devices}/src/adi/mod.rs (100%) rename packages/{pros-devices => vex-devices}/src/adi/motor.rs (100%) rename packages/{pros-devices => vex-devices}/src/adi/potentiometer.rs (100%) rename packages/{pros-devices => vex-devices}/src/adi/pwm.rs (100%) rename packages/{pros-devices => vex-devices}/src/adi/solenoid.rs (100%) rename packages/{pros-devices => vex-devices}/src/adi/switch.rs (100%) rename packages/{pros-devices => vex-devices}/src/adi/ultrasonic.rs (100%) rename packages/{pros-devices => vex-devices}/src/battery.rs (100%) create mode 100644 packages/vex-devices/src/color.rs rename packages/{pros-devices => vex-devices}/src/competition.rs (100%) rename packages/{pros-devices => vex-devices}/src/controller.rs (100%) rename packages/{pros-devices => vex-devices}/src/lib.rs (98%) rename packages/{pros-devices => vex-devices}/src/peripherals.rs (100%) rename packages/{pros-devices => vex-devices}/src/position.rs (100%) rename packages/{pros-devices => vex-devices}/src/screen.rs (100%) rename packages/{pros-devices => vex-devices}/src/smart/distance.rs (100%) rename packages/{pros-devices => vex-devices}/src/smart/expander.rs (100%) rename packages/{pros-devices => vex-devices}/src/smart/gps.rs (100%) rename packages/{pros-devices => vex-devices}/src/smart/imu.rs (100%) rename packages/{pros-devices => vex-devices}/src/smart/link.rs (100%) rename packages/{pros-devices => vex-devices}/src/smart/mod.rs (100%) rename packages/{pros-devices => vex-devices}/src/smart/motor.rs (100%) rename packages/{pros-devices => vex-devices}/src/smart/optical.rs (100%) rename packages/{pros-devices => vex-devices}/src/smart/rotation.rs (100%) rename packages/{pros-devices => vex-devices}/src/smart/vision.rs (100%) rename packages/{pros-devices => vex-devices}/src/usd.rs (100%) diff --git a/CHANGELOG.md b/CHANGELOG.md index ba30391a..c6fe7c70 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -106,7 +106,7 @@ Before releasing: - Repurposed the `pros` crate as a metapackage without any code of its own. (**Breaking Change**) (#86) - Split the pros-rs into several small subcrates. (**Breaking Change**) (#86) - `pros-async` with the async executor and robot trait. - - `pros-devices` for device bindings. + - `vex-devices` for device bindings. - `pros-sync` for the sync robot trait. - `pros-core` with basic abstractions over `pros-sys` needed to compile a program to the brain. - `pros-math` with commonly used controllers and other mathematical models. diff --git a/packages/pros-devices/src/color.rs b/packages/pros-devices/src/color.rs deleted file mode 100644 index 94b3a276..00000000 --- a/packages/pros-devices/src/color.rs +++ /dev/null @@ -1,384 +0,0 @@ -//! Generic RGB8 color type and conversion trait. -//! The [`Rgb`] and [`IntoRgb`] types are used in multiple places in the library to represent colors. - -/// A trait for types that can be converted into an RGB8 color. -pub trait IntoRgb { - /// Consume the value and convert it into an RGB8 color. - fn into_rgb(self) -> Rgb; -} - -impl> IntoRgb for T { - fn into_rgb(self: T) -> Rgb { - Rgb::from_raw(self.into()) - } -} - -/// An RGB8 color. -/// The color space will almost always be assumed as sRGB in this library. -#[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] -pub struct Rgb { - /// Red value of the color. - pub r: u8, - /// Green value of the color. - pub g: u8, - /// Blue value of the color. - pub b: u8, -} - -impl Rgb { - /// #F0F8FF color constant. - pub const ALICE_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_ALICE_BLUE); - /// #FAEBD7 color constant. - pub const ANTIQUE_WHITE: Rgb = Rgb::from_raw(pros_sys::COLOR_ANTIQUE_WHITE); - /// #00FFFF color constant. - pub const AQUA: Rgb = Rgb::from_raw(pros_sys::COLOR_AQUA); - /// #7FFFD4 color constant. - pub const AQUAMARINE: Rgb = Rgb::from_raw(pros_sys::COLOR_AQUAMARINE); - /// #F0FFFF color constant. - pub const AZURE: Rgb = Rgb::from_raw(pros_sys::COLOR_AZURE); - /// #F5F5DC color constant. - pub const BEIGE: Rgb = Rgb::from_raw(pros_sys::COLOR_BEIGE); - /// #FFE4C4 color constant. - pub const BISQUE: Rgb = Rgb::from_raw(pros_sys::COLOR_BISQUE); - /// #000000 color constant. - pub const BLACK: Rgb = Rgb::from_raw(pros_sys::COLOR_BLACK); - /// #FFEBCD color constant. - pub const BLANCHED_ALMOND: Rgb = Rgb::from_raw(pros_sys::COLOR_BLANCHED_ALMOND); - /// #0000FF color constant. - pub const BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_BLUE); - /// #8A2BE2 color constant. - pub const BLUE_VIOLET: Rgb = Rgb::from_raw(pros_sys::COLOR_BLUE_VIOLET); - /// #A52A2A color constant. - pub const BROWN: Rgb = Rgb::from_raw(pros_sys::COLOR_BROWN); - /// #DEB887 color constant. - pub const BURLY_WOOD: Rgb = Rgb::from_raw(pros_sys::COLOR_BURLY_WOOD); - /// #5F9EA0 color constant. - pub const CADET_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_CADET_BLUE); - /// #7FFF00 color constant. - pub const CHARTREUSE: Rgb = Rgb::from_raw(pros_sys::COLOR_CHARTREUSE); - /// #D2691E color constant. - pub const CHOCOLATE: Rgb = Rgb::from_raw(pros_sys::COLOR_CHOCOLATE); - /// #FF7F50 color constant. - pub const CORAL: Rgb = Rgb::from_raw(pros_sys::COLOR_CORAL); - /// #6495ED color constant. - pub const CORNFLOWER_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_CORNFLOWER_BLUE); - /// #FFF8DC color constant. - pub const CORNSILK: Rgb = Rgb::from_raw(pros_sys::COLOR_CORNSILK); - /// #DC143C color constant. - pub const CRIMSON: Rgb = Rgb::from_raw(pros_sys::COLOR_CRIMSON); - /// #00FFFF color constant. - pub const CYAN: Rgb = Rgb::from_raw(pros_sys::COLOR_CYAN); - /// #00008B color constant. - pub const DARK_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_BLUE); - /// #008B8B color constant. - pub const DARK_CYAN: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_CYAN); - /// #B8860B color constant. - pub const DARK_GOLDENROD: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_GOLDENROD); - /// #A9A9A9 color constant. - pub const DARK_GRAY: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_GRAY); - /// #006400 color constant. - pub const DARK_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_GREEN); - /// #BDB76B color constant. - pub const DARK_KHAKI: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_KHAKI); - /// #8B008B color constant. - pub const DARK_MAGENTA: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_MAGENTA); - /// #556B2F color constant. - pub const DARK_OLIVE_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_OLIVE_GREEN); - /// #FF8C00 color constant. - pub const DARK_ORANGE: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_ORANGE); - /// #9932CC color constant. - pub const DARK_ORCHID: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_ORCHID); - /// #8B0000 color constant. - pub const DARK_RED: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_RED); - /// #E9967A color constant. - pub const DARK_SALMON: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_SALMON); - /// #8FBC8F color constant. - pub const DARK_SEA_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_SEA_GREEN); - /// #2F4F4F color constant. - pub const DARK_SLATE_GRAY: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_SLATE_GRAY); - /// #00CED1 color constant. - pub const DARK_TURQUOISE: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_TURQUOISE); - /// #9400D3 color constant. - pub const DARK_VIOLET: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_VIOLET); - /// #FF1493 color constant. - pub const DEEP_PINK: Rgb = Rgb::from_raw(pros_sys::COLOR_DEEP_PINK); - /// #00BFFF color constant. - pub const DEEP_SKY_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_DEEP_SKY_BLUE); - /// #696969 color constant. - pub const DIM_GRAY: Rgb = Rgb::from_raw(pros_sys::COLOR_DIM_GRAY); - /// #1E90FF color constant. - pub const DODGER_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_DODGER_BLUE); - /// #B22222 color constant. - pub const FIRE_BRICK: Rgb = Rgb::from_raw(pros_sys::COLOR_FIRE_BRICK); - /// #FFFAF0 color constant. - pub const FLORAL_WHITE: Rgb = Rgb::from_raw(pros_sys::COLOR_FLORAL_WHITE); - /// #228B22 color constant. - pub const FOREST_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_FOREST_GREEN); - /// #FF00FF color constant. - pub const FUCHSIA: Rgb = Rgb::from_raw(pros_sys::COLOR_FUCHSIA); - /// #DCDCDC color constant. - pub const GAINSBORO: Rgb = Rgb::from_raw(pros_sys::COLOR_GAINSBORO); - /// #F8F8FF color constant. - pub const GHOST_WHITE: Rgb = Rgb::from_raw(pros_sys::COLOR_GHOST_WHITE); - /// #FFD700 color constant. - pub const GOLD: Rgb = Rgb::from_raw(pros_sys::COLOR_GOLD); - /// #DAA520 color constant. - pub const GOLDENROD: Rgb = Rgb::from_raw(pros_sys::COLOR_GOLDENROD); - /// #808080 color constant. - pub const GRAY: Rgb = Rgb::from_raw(pros_sys::COLOR_GRAY); - /// #008000 color constant. - pub const GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_GREEN); - /// #ADFF2F color constant. - pub const GREEN_YELLOW: Rgb = Rgb::from_raw(pros_sys::COLOR_GREEN_YELLOW); - /// #F0FFF0 color constant. - pub const HONEYDEW: Rgb = Rgb::from_raw(pros_sys::COLOR_HONEYDEW); - /// #FF69B4 color constant. - pub const HOT_PINK: Rgb = Rgb::from_raw(pros_sys::COLOR_HOT_PINK); - /// #CD5C5C color constant. - pub const INDIAN_RED: Rgb = Rgb::from_raw(pros_sys::COLOR_INDIAN_RED); - /// #4B0082 color constant. - pub const INDIGO: Rgb = Rgb::from_raw(pros_sys::COLOR_INDIGO); - /// #FFFFF0 color constant. - pub const IVORY: Rgb = Rgb::from_raw(pros_sys::COLOR_IVORY); - /// #F0E68C color constant. - pub const KHAKI: Rgb = Rgb::from_raw(pros_sys::COLOR_KHAKI); - /// #E6E6FA color constant. - pub const LAVENDER: Rgb = Rgb::from_raw(pros_sys::COLOR_LAVENDER); - /// #FFF0F5 color constant. - pub const LAVENDER_BLUSH: Rgb = Rgb::from_raw(pros_sys::COLOR_LAVENDER_BLUSH); - /// #7CFC00 color constant. - pub const LAWN_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_LAWN_GREEN); - /// #FFFACD color constant. - pub const LEMON_CHIFFON: Rgb = Rgb::from_raw(pros_sys::COLOR_LEMON_CHIFFON); - /// #ADD8E6 color constant. - pub const LIGHT_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_BLUE); - /// #F08080 color constant. - pub const LIGHT_CORAL: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_CORAL); - /// #E0FFFF color constant. - pub const LIGHT_CYAN: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_CYAN); - /// #FAFAD2 color constant. - pub const LIGHT_GOLDENROD_YELLOW: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_GOLDENROD_YELLOW); - /// #90EE90 color constant. - pub const LIGHT_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_GREEN); - /// #D3D3D3 color constant. - pub const LIGHT_GRAY: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_GRAY); - /// #FFB6C1 color constant. - pub const LIGHT_PINK: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_PINK); - /// #FFA07A color constant. - pub const LIGHT_SALMON: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_SALMON); - /// #20B2AA color constant. - pub const LIGHT_SEA_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_SEA_GREEN); - /// #87CEFA color constant. - pub const LIGHT_SKY_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_SKY_BLUE); - /// #778899 color constant. - pub const LIGHT_SLATE_GRAY: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_SLATE_GRAY); - /// #B0C4DE color constant. - pub const LIGHT_STEEL_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_STEEL_BLUE); - /// #FFFFE0 color constant. - pub const LIGHT_YELLOW: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_YELLOW); - /// #00FF00 color constant. - pub const LIME: Rgb = Rgb::from_raw(pros_sys::COLOR_LIME); - /// #32CD32 color constant. - pub const LIME_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_LIME_GREEN); - /// #FAF0E6 color constant. - pub const LINEN: Rgb = Rgb::from_raw(pros_sys::COLOR_LINEN); - /// #FF00FF color constant. - pub const MAGENTA: Rgb = Rgb::from_raw(pros_sys::COLOR_MAGENTA); - /// #800000 color constant. - pub const MAROON: Rgb = Rgb::from_raw(pros_sys::COLOR_MAROON); - /// #66CDAA color constant. - pub const MEDIUM_AQUAMARINE: Rgb = Rgb::from_raw(pros_sys::COLOR_MEDIUM_AQUAMARINE); - /// #0000CD color constant. - pub const MEDIUM_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_MEDIUM_BLUE); - /// #BA55D3 color constant. - pub const MEDIUM_ORCHID: Rgb = Rgb::from_raw(pros_sys::COLOR_MEDIUM_ORCHID); - /// #9370DB color constant. - pub const MEDIUM_PURPLE: Rgb = Rgb::from_raw(pros_sys::COLOR_MEDIUM_PURPLE); - /// #3CB371 color constant. - pub const MEDIUM_SEA_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_MEDIUM_SEA_GREEN); - /// #7B68EE color constant. - pub const MEDIUM_SLATE_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_MEDIUM_SLATE_BLUE); - /// #00FA9A color constant. - pub const MEDIUM_SPRING_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_MEDIUM_SPRING_GREEN); - /// #48D1CC color constant. - pub const MEDIUM_TURQUOISE: Rgb = Rgb::from_raw(pros_sys::COLOR_MEDIUM_TURQUOISE); - /// #C71585 color constant. - pub const MEDIUM_VIOLET_RED: Rgb = Rgb::from_raw(pros_sys::COLOR_MEDIUM_VIOLET_RED); - /// #191970 color constant. - pub const MIDNIGHT_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_MIDNIGHT_BLUE); - /// #F5FFFA color constant. - pub const MINT_CREAM: Rgb = Rgb::from_raw(pros_sys::COLOR_MINT_CREAM); - /// #FFE4E1 color constant. - pub const MISTY_ROSE: Rgb = Rgb::from_raw(pros_sys::COLOR_MISTY_ROSE); - /// #FFE4B5 color constant. - pub const MOCCASIN: Rgb = Rgb::from_raw(pros_sys::COLOR_MOCCASIN); - /// #FFDEAD color constant. - pub const NAVAJO_WHITE: Rgb = Rgb::from_raw(pros_sys::COLOR_NAVAJO_WHITE); - /// #000080 color constant. - pub const NAVY: Rgb = Rgb::from_raw(pros_sys::COLOR_NAVY); - /// #FDF5E6 color constant. - pub const OLD_LACE: Rgb = Rgb::from_raw(pros_sys::COLOR_OLD_LACE); - /// #808000 color constant. - pub const OLIVE: Rgb = Rgb::from_raw(pros_sys::COLOR_OLIVE); - /// #6B8E23 color constant. - pub const OLIVE_DRAB: Rgb = Rgb::from_raw(pros_sys::COLOR_OLIVE_DRAB); - /// #FFA500 color constant. - pub const ORANGE: Rgb = Rgb::from_raw(pros_sys::COLOR_ORANGE); - /// #FF4500 color constant. - pub const ORANGE_RED: Rgb = Rgb::from_raw(pros_sys::COLOR_ORANGE_RED); - /// #DA70D6 color constant. - pub const ORCHID: Rgb = Rgb::from_raw(pros_sys::COLOR_ORCHID); - /// #EEE8AA color constant. - pub const PALE_GOLDENROD: Rgb = Rgb::from_raw(pros_sys::COLOR_PALE_GOLDENROD); - /// #98FB98 color constant. - pub const PALE_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_PALE_GREEN); - /// #AFEEEE color constant. - pub const PALE_TURQUOISE: Rgb = Rgb::from_raw(pros_sys::COLOR_PALE_TURQUOISE); - /// #DB7093 color constant. - pub const PALE_VIOLET_RED: Rgb = Rgb::from_raw(pros_sys::COLOR_PALE_VIOLET_RED); - /// #FFEFD5 color constant. - pub const PAPAY_WHIP: Rgb = Rgb::from_raw(pros_sys::COLOR_PAPAY_WHIP); - /// #FFDAB9 color constant. - pub const PEACH_PUFF: Rgb = Rgb::from_raw(pros_sys::COLOR_PEACH_PUFF); - /// #CD853F color constant. - pub const PERU: Rgb = Rgb::from_raw(pros_sys::COLOR_PERU); - /// #FFC0CB color constant. - pub const PINK: Rgb = Rgb::from_raw(pros_sys::COLOR_PINK); - /// #DDA0DD color constant. - pub const PLUM: Rgb = Rgb::from_raw(pros_sys::COLOR_PLUM); - /// #B0E0E6 color constant. - pub const POWDER_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_POWDER_BLUE); - /// #800080 color constant. - pub const PURPLE: Rgb = Rgb::from_raw(pros_sys::COLOR_PURPLE); - /// #FF0000 color constant. - pub const RED: Rgb = Rgb::from_raw(pros_sys::COLOR_RED); - /// #BC8F8F color constant. - pub const ROSY_BROWN: Rgb = Rgb::from_raw(pros_sys::COLOR_ROSY_BROWN); - /// #4169E1 color constant. - pub const ROYAL_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_ROYAL_BLUE); - /// #8B4513 color constant. - pub const SADDLE_BROWN: Rgb = Rgb::from_raw(pros_sys::COLOR_SADDLE_BROWN); - /// #FA8072 color constant. - pub const SALMON: Rgb = Rgb::from_raw(pros_sys::COLOR_SALMON); - /// #F4A460 color constant. - pub const SANDY_BROWN: Rgb = Rgb::from_raw(pros_sys::COLOR_SANDY_BROWN); - /// #2E8B57 color constant. - pub const SEA_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_SEA_GREEN); - /// #FFF5EE color constant. - pub const SEASHELL: Rgb = Rgb::from_raw(pros_sys::COLOR_SEASHELL); - /// #A0522D color constant. - pub const SIENNA: Rgb = Rgb::from_raw(pros_sys::COLOR_SIENNA); - /// #C0C0C0 color constant. - pub const SILVER: Rgb = Rgb::from_raw(pros_sys::COLOR_SILVER); - /// #87CEEB color constant. - pub const SKY_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_SKY_BLUE); - /// #6A5ACD color constant. - pub const SLATE_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_SLATE_BLUE); - /// #708090 color constant. - pub const SLATE_GRAY: Rgb = Rgb::from_raw(pros_sys::COLOR_SLATE_GRAY); - /// #FFFAFA color constant. - pub const SNOW: Rgb = Rgb::from_raw(pros_sys::COLOR_SNOW); - /// #00FF7F color constant. - pub const SPRING_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_SPRING_GREEN); - /// #4682B4 color constant. - pub const STEEL_BLUE: Rgb = Rgb::from_raw(pros_sys::COLOR_STEEL_BLUE); - /// #D2B48C color constant. - pub const TAN: Rgb = Rgb::from_raw(pros_sys::COLOR_TAN); - /// #008080 color constant. - pub const TEAL: Rgb = Rgb::from_raw(pros_sys::COLOR_TEAL); - /// #D8BFD8 color constant. - pub const THISTLE: Rgb = Rgb::from_raw(pros_sys::COLOR_THISTLE); - /// #FF6347 color constant. - pub const TOMATO: Rgb = Rgb::from_raw(pros_sys::COLOR_TOMATO); - /// #40E0D0 color constant. - pub const TURQUOISE: Rgb = Rgb::from_raw(pros_sys::COLOR_TURQUOISE); - /// #EE82EE color constant. - pub const VIOLET: Rgb = Rgb::from_raw(pros_sys::COLOR_VIOLET); - /// #F5DEB3 color constant. - pub const WHEAT: Rgb = Rgb::from_raw(pros_sys::COLOR_WHEAT); - /// #FFFFFF color constant. - pub const WHITE: Rgb = Rgb::from_raw(pros_sys::COLOR_WHITE); - /// #F5F5F5 color constant. - pub const WHITE_SMOKE: Rgb = Rgb::from_raw(pros_sys::COLOR_WHITE_SMOKE); - /// #FFFF00 color constant. - pub const YELLOW: Rgb = Rgb::from_raw(pros_sys::COLOR_YELLOW); - /// #9ACD32 color constant. - pub const YELLOW_GREEN: Rgb = Rgb::from_raw(pros_sys::COLOR_YELLOW_GREEN); - /// Alias to [`Self::SLATE_GRAY`]. - pub const DARK_GREY: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_GREY); - /// Alias to [`Self::DARK_SLATE_GRAY`]. - pub const DARK_SLATE_GREY: Rgb = Rgb::from_raw(pros_sys::COLOR_DARK_SLATE_GREY); - /// Alias to [`Self::DIM_GRAY`]. - pub const DIM_GREY: Rgb = Rgb::from_raw(pros_sys::COLOR_DIM_GREY); - /// Alias to [`Self::GRAY`]. - pub const GREY: Rgb = Rgb::from_raw(pros_sys::COLOR_GREY); - /// Alias to [`Self::LIGHT_GRAY`]. - pub const LIGHT_GREY: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_GREY); - /// Alias to [`Self::LIGHT_SLATE_GRAY`]. - pub const LIGHT_SLATE_GREY: Rgb = Rgb::from_raw(pros_sys::COLOR_LIGHT_SLATE_GREY); - /// Alias to [`Self::SLATE_GREY`]. - pub const SLATE_GREY: Rgb = Rgb::from_raw(pros_sys::COLOR_SLATE_GREY); - - const BITMASK: u32 = 0b11111111; - - /// Create a new RGB8 color. - pub const fn new(red: u8, green: u8, blue: u8) -> Self { - Self { - r: red, - g: green, - b: blue, - } - } - - /// Create a new RGB8 color from a raw u32 value. - pub const fn from_raw(raw: u32) -> Self { - Self { - r: ((raw >> 16) & Self::BITMASK) as _, - g: ((raw >> 8) & Self::BITMASK) as _, - b: (raw & Self::BITMASK) as _, - } - } - - /// Get the red value of the color. - pub const fn red(&self) -> u8 { - self.r - } - - /// Get the green value of the color. - pub const fn green(&self) -> u8 { - self.g - } - - /// Get the blue value of the color. - pub const fn blue(&self) -> u8 { - self.b - } -} - -impl From<(u8, u8, u8)> for Rgb { - fn from(tuple: (u8, u8, u8)) -> Self { - Self { - r: tuple.0, - g: tuple.1, - b: tuple.2, - } - } -} - -impl From for (u8, u8, u8) { - fn from(value: Rgb) -> (u8, u8, u8) { - (value.r, value.g, value.b) - } -} - -impl From for u32 { - fn from(value: Rgb) -> u32 { - ((value.r as u32) << 16) + ((value.g as u32) << 8) + value.b as u32 - } -} - -impl From for Rgb { - fn from(value: u32) -> Self { - Self::from_raw(value) - } -} diff --git a/packages/pros-panic/Cargo.toml b/packages/pros-panic/Cargo.toml index 39237284..95733748 100644 --- a/packages/pros-panic/Cargo.toml +++ b/packages/pros-panic/Cargo.toml @@ -20,13 +20,13 @@ authors = [ [dependencies] pros-core = { version = "0.1.0", path = "../pros-core" } -pros-devices = { version = "0.1.0", path = "../pros-devices", optional = true } +vex-devices = { version = "0.1.0", path = "../vex-devices", optional = true } pros-sys = { version = "0.7.0", path = "../pros-sys" } [features] default = ["display_panics"] -display_panics = ["dep:pros-devices"] +display_panics = ["dep:vex-devices"] [lints] workspace = true diff --git a/packages/pros/Cargo.toml b/packages/pros/Cargo.toml index 97e368fc..f03c9e32 100644 --- a/packages/pros/Cargo.toml +++ b/packages/pros/Cargo.toml @@ -20,7 +20,7 @@ rust-version = "1.75.0" [dependencies] pros-sync = { version = "0.1.0", path = "../pros-sync", optional = true } pros-async = { version = "0.1.0", path = "../pros-async", optional = true } -pros-devices = { version = "0.1.0", path = "../pros-devices", optional = true } +vex-devices = { version = "0.1.0", path = "../vex-devices", optional = true } pros-panic = { version = "0.1.0", path = "../pros-panic", optional = true } pros-core = { version = "0.1.0", path = "../pros-core", optional = true } pros-math = { version = "0.1.0", path = "../pros-math", optional = true } @@ -34,11 +34,11 @@ core = ["dep:pros-core"] async = ["dep:pros-async"] sync = ["dep:pros-sync"] -devices = ["dep:pros-devices"] +devices = ["dep:vex-devices"] math = ["dep:pros-math"] panic = ["dep:pros-panic"] display_panics = ["pros-panic/display_panics"] -dangerous-motor-tuning = ["pros-devices/dangerous_motor_tuning"] +dangerous-motor-tuning = ["vex-devices/dangerous_motor_tuning"] diff --git a/packages/pros-devices/Cargo.toml b/packages/vex-devices/Cargo.toml similarity index 97% rename from packages/pros-devices/Cargo.toml rename to packages/vex-devices/Cargo.toml index 6c47cf22..1d6ad7da 100644 --- a/packages/pros-devices/Cargo.toml +++ b/packages/vex-devices/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "pros-devices" +name = "vex-devices" version = "0.1.0" edition = "2021" license = "MIT" diff --git a/packages/pros-devices/README.md b/packages/vex-devices/README.md similarity index 97% rename from packages/pros-devices/README.md rename to packages/vex-devices/README.md index b5e924ee..0e7d2e8a 100644 --- a/packages/pros-devices/README.md +++ b/packages/vex-devices/README.md @@ -1,4 +1,4 @@ -# pros-devices +# vex-devices Functionality for accessing hardware connected to the V5 brain. diff --git a/packages/pros-devices/src/adi/analog.rs b/packages/vex-devices/src/adi/analog.rs similarity index 100% rename from packages/pros-devices/src/adi/analog.rs rename to packages/vex-devices/src/adi/analog.rs diff --git a/packages/pros-devices/src/adi/digital.rs b/packages/vex-devices/src/adi/digital.rs similarity index 100% rename from packages/pros-devices/src/adi/digital.rs rename to packages/vex-devices/src/adi/digital.rs diff --git a/packages/pros-devices/src/adi/encoder.rs b/packages/vex-devices/src/adi/encoder.rs similarity index 100% rename from packages/pros-devices/src/adi/encoder.rs rename to packages/vex-devices/src/adi/encoder.rs diff --git a/packages/pros-devices/src/adi/gyro.rs b/packages/vex-devices/src/adi/gyro.rs similarity index 100% rename from packages/pros-devices/src/adi/gyro.rs rename to packages/vex-devices/src/adi/gyro.rs diff --git a/packages/pros-devices/src/adi/linetracker.rs b/packages/vex-devices/src/adi/linetracker.rs similarity index 100% rename from packages/pros-devices/src/adi/linetracker.rs rename to packages/vex-devices/src/adi/linetracker.rs diff --git a/packages/pros-devices/src/adi/mod.rs b/packages/vex-devices/src/adi/mod.rs similarity index 100% rename from packages/pros-devices/src/adi/mod.rs rename to packages/vex-devices/src/adi/mod.rs diff --git a/packages/pros-devices/src/adi/motor.rs b/packages/vex-devices/src/adi/motor.rs similarity index 100% rename from packages/pros-devices/src/adi/motor.rs rename to packages/vex-devices/src/adi/motor.rs diff --git a/packages/pros-devices/src/adi/potentiometer.rs b/packages/vex-devices/src/adi/potentiometer.rs similarity index 100% rename from packages/pros-devices/src/adi/potentiometer.rs rename to packages/vex-devices/src/adi/potentiometer.rs diff --git a/packages/pros-devices/src/adi/pwm.rs b/packages/vex-devices/src/adi/pwm.rs similarity index 100% rename from packages/pros-devices/src/adi/pwm.rs rename to packages/vex-devices/src/adi/pwm.rs diff --git a/packages/pros-devices/src/adi/solenoid.rs b/packages/vex-devices/src/adi/solenoid.rs similarity index 100% rename from packages/pros-devices/src/adi/solenoid.rs rename to packages/vex-devices/src/adi/solenoid.rs diff --git a/packages/pros-devices/src/adi/switch.rs b/packages/vex-devices/src/adi/switch.rs similarity index 100% rename from packages/pros-devices/src/adi/switch.rs rename to packages/vex-devices/src/adi/switch.rs diff --git a/packages/pros-devices/src/adi/ultrasonic.rs b/packages/vex-devices/src/adi/ultrasonic.rs similarity index 100% rename from packages/pros-devices/src/adi/ultrasonic.rs rename to packages/vex-devices/src/adi/ultrasonic.rs diff --git a/packages/pros-devices/src/battery.rs b/packages/vex-devices/src/battery.rs similarity index 100% rename from packages/pros-devices/src/battery.rs rename to packages/vex-devices/src/battery.rs diff --git a/packages/vex-devices/src/color.rs b/packages/vex-devices/src/color.rs new file mode 100644 index 00000000..d5c06323 --- /dev/null +++ b/packages/vex-devices/src/color.rs @@ -0,0 +1,91 @@ +//! Generic RGB8 color type and conversion trait. +//! The [`Rgb`] and [`IntoRgb`] types are used in multiple places in the library to represent colors. + +/// A trait for types that can be converted into an RGB8 color. +pub trait IntoRgb { + /// Consume the value and convert it into an RGB8 color. + fn into_rgb(self) -> Rgb; +} + +impl> IntoRgb for T { + fn into_rgb(self: T) -> Rgb { + Rgb::from_raw(self.into()) + } +} + +/// An RGB8 color. +/// The color space will almost always be assumed as sRGB in this library. +#[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] +pub struct Rgb { + /// Red value of the color. + pub r: u8, + /// Green value of the color. + pub g: u8, + /// Blue value of the color. + pub b: u8, +} + +impl Rgb { + const BITMASK: u32 = 0b11111111; + + /// Create a new RGB8 color. + pub const fn new(red: u8, green: u8, blue: u8) -> Self { + Self { + r: red, + g: green, + b: blue, + } + } + + /// Create a new RGB8 color from a raw u32 value. + pub const fn from_raw(raw: u32) -> Self { + Self { + r: ((raw >> 16) & Self::BITMASK) as _, + g: ((raw >> 8) & Self::BITMASK) as _, + b: (raw & Self::BITMASK) as _, + } + } + + /// Get the red value of the color. + pub const fn red(&self) -> u8 { + self.r + } + + /// Get the green value of the color. + pub const fn green(&self) -> u8 { + self.g + } + + /// Get the blue value of the color. + pub const fn blue(&self) -> u8 { + self.b + } +} + +impl From<(u8, u8, u8)> for Rgb { + fn from(tuple: (u8, u8, u8)) -> Self { + Self { + r: tuple.0, + g: tuple.1, + b: tuple.2, + } + } +} + +impl From for (u8, u8, u8) { + fn from(value: Rgb) -> (u8, u8, u8) { + (value.r, value.g, value.b) + } +} + +impl From for u32 { + fn from(value: Rgb) -> u32 { + ((value.r as u32) << 16) + ((value.g as u32) << 8) + value.b as u32 + } +} + +impl From for Rgb { + fn from(value: u32) -> Self { + Self::from_raw(value) + } +} diff --git a/packages/pros-devices/src/competition.rs b/packages/vex-devices/src/competition.rs similarity index 100% rename from packages/pros-devices/src/competition.rs rename to packages/vex-devices/src/competition.rs diff --git a/packages/pros-devices/src/controller.rs b/packages/vex-devices/src/controller.rs similarity index 100% rename from packages/pros-devices/src/controller.rs rename to packages/vex-devices/src/controller.rs diff --git a/packages/pros-devices/src/lib.rs b/packages/vex-devices/src/lib.rs similarity index 98% rename from packages/pros-devices/src/lib.rs rename to packages/vex-devices/src/lib.rs index d3e19f4e..470fd631 100644 --- a/packages/pros-devices/src/lib.rs +++ b/packages/vex-devices/src/lib.rs @@ -1,4 +1,4 @@ -//! # pros-devices +//! # vex-devices //! //! Functionality for accessing hardware connected to the V5 brain. //! diff --git a/packages/pros-devices/src/peripherals.rs b/packages/vex-devices/src/peripherals.rs similarity index 100% rename from packages/pros-devices/src/peripherals.rs rename to packages/vex-devices/src/peripherals.rs diff --git a/packages/pros-devices/src/position.rs b/packages/vex-devices/src/position.rs similarity index 100% rename from packages/pros-devices/src/position.rs rename to packages/vex-devices/src/position.rs diff --git a/packages/pros-devices/src/screen.rs b/packages/vex-devices/src/screen.rs similarity index 100% rename from packages/pros-devices/src/screen.rs rename to packages/vex-devices/src/screen.rs diff --git a/packages/pros-devices/src/smart/distance.rs b/packages/vex-devices/src/smart/distance.rs similarity index 100% rename from packages/pros-devices/src/smart/distance.rs rename to packages/vex-devices/src/smart/distance.rs diff --git a/packages/pros-devices/src/smart/expander.rs b/packages/vex-devices/src/smart/expander.rs similarity index 100% rename from packages/pros-devices/src/smart/expander.rs rename to packages/vex-devices/src/smart/expander.rs diff --git a/packages/pros-devices/src/smart/gps.rs b/packages/vex-devices/src/smart/gps.rs similarity index 100% rename from packages/pros-devices/src/smart/gps.rs rename to packages/vex-devices/src/smart/gps.rs diff --git a/packages/pros-devices/src/smart/imu.rs b/packages/vex-devices/src/smart/imu.rs similarity index 100% rename from packages/pros-devices/src/smart/imu.rs rename to packages/vex-devices/src/smart/imu.rs diff --git a/packages/pros-devices/src/smart/link.rs b/packages/vex-devices/src/smart/link.rs similarity index 100% rename from packages/pros-devices/src/smart/link.rs rename to packages/vex-devices/src/smart/link.rs diff --git a/packages/pros-devices/src/smart/mod.rs b/packages/vex-devices/src/smart/mod.rs similarity index 100% rename from packages/pros-devices/src/smart/mod.rs rename to packages/vex-devices/src/smart/mod.rs diff --git a/packages/pros-devices/src/smart/motor.rs b/packages/vex-devices/src/smart/motor.rs similarity index 100% rename from packages/pros-devices/src/smart/motor.rs rename to packages/vex-devices/src/smart/motor.rs diff --git a/packages/pros-devices/src/smart/optical.rs b/packages/vex-devices/src/smart/optical.rs similarity index 100% rename from packages/pros-devices/src/smart/optical.rs rename to packages/vex-devices/src/smart/optical.rs diff --git a/packages/pros-devices/src/smart/rotation.rs b/packages/vex-devices/src/smart/rotation.rs similarity index 100% rename from packages/pros-devices/src/smart/rotation.rs rename to packages/vex-devices/src/smart/rotation.rs diff --git a/packages/pros-devices/src/smart/vision.rs b/packages/vex-devices/src/smart/vision.rs similarity index 100% rename from packages/pros-devices/src/smart/vision.rs rename to packages/vex-devices/src/smart/vision.rs diff --git a/packages/pros-devices/src/usd.rs b/packages/vex-devices/src/usd.rs similarity index 100% rename from packages/pros-devices/src/usd.rs rename to packages/vex-devices/src/usd.rs From 1aba8db9ae38a02c760e4d00e40e937f99532a21 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 24 Mar 2024 11:20:48 -0500 Subject: [PATCH 04/14] feat: add back HTML 4.01 basic colors --- packages/vex-devices/src/color.rs | 48 +++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/packages/vex-devices/src/color.rs b/packages/vex-devices/src/color.rs index d5c06323..dbcbd877 100644 --- a/packages/vex-devices/src/color.rs +++ b/packages/vex-devices/src/color.rs @@ -26,6 +26,54 @@ pub struct Rgb { } impl Rgb { + /// "White" color as defined in the HTML 4.01 specification. + pub const WHITE: Rgb = Rgb::from_raw(0xFFFFFF); + + /// "Silver" color as defined in the HTML 4.01 specification. + pub const SILVER: Rgb = Rgb::from_raw(0xC0C0C0); + + /// "Gray" color as defined in the HTML 4.01 specification. + pub const GRAY: Rgb = Rgb::from_raw(0x808080); + + /// "Black" color as defined in the HTML 4.01 specification. + pub const BLACK: Rgb = Rgb::from_raw(0x000000); + + /// "Red" color as defined in the HTML 4.01 specification. + pub const RED: Rgb = Rgb::from_raw(0xFF0000); + + /// "Maroon" color as defined in the HTML 4.01 specification. + pub const MAROON: Rgb = Rgb::from_raw(0x800000); + + /// "Yellow" color as defined in the HTML 4.01 specification. + pub const YELLOW: Rgb = Rgb::from_raw(0xFFFF00); + + /// "Olive" color as defined in the HTML 4.01 specification. + pub const OLIVE: Rgb = Rgb::from_raw(0x808000); + + /// "Lime" color as defined in the HTML 4.01 specification. + pub const LIME: Rgb = Rgb::from_raw(0x00FF00); + + /// "Green" color as defined in the HTML 4.01 specification. + pub const GREEN: Rgb = Rgb::from_raw(0x008000); + + /// "Aqua" color as defined in the HTML 4.01 specification. + pub const AQUA: Rgb = Rgb::from_raw(0x00FFFF); + + /// "Teal" color as defined in the HTML 4.01 specification. + pub const TEAL: Rgb = Rgb::from_raw(0x008080); + + /// "Blue" color as defined in the HTML 4.01 specification. + pub const BLUE: Rgb = Rgb::from_raw(0x0000FF); + + /// "Navy" color as defined in the HTML 4.01 specification. + pub const NAVY: Rgb = Rgb::from_raw(0x000080); + + /// "Fuchsia" color as defined in the HTML 4.01 specification. + pub const FUCHSIA: Rgb = Rgb::from_raw(0xFF00FF); + + /// "Purple" color as defined in the HTML 4.01 specification. + pub const PURPLE: Rgb = Rgb::from_raw(0x800080); + const BITMASK: u32 = 0b11111111; /// Create a new RGB8 color. From 66ddcb7b0f28a506ed2d7f78e3ed6e7bca668ff7 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 24 Mar 2024 11:31:44 -0500 Subject: [PATCH 05/14] refactor: competition state --- packages/vex-devices/src/competition.rs | 51 +++++++++++++++++-------- 1 file changed, 35 insertions(+), 16 deletions(-) diff --git a/packages/vex-devices/src/competition.rs b/packages/vex-devices/src/competition.rs index 089b830b..e211a0de 100644 --- a/packages/vex-devices/src/competition.rs +++ b/packages/vex-devices/src/competition.rs @@ -1,9 +1,25 @@ -//! Utilities for getting what state of the competition the robot is in. +//! Utilities for getting competition control state. -use pros_sys::misc::{COMPETITION_AUTONOMOUS, COMPETITION_CONNECTED, COMPETITION_DISABLED}; +use bitflags::bitflags; +use vex_sys::vexCompetitionStatus; -// TODO: change this to use PROS' internal version once we switch to PROS 4. -const COMPETITION_SYSTEM: u8 = 1 << 3; +bitflags! { + /// The status bits returned by [`competition::state`]. + #[derive(Debug, Clone, Copy, Eq, PartialEq)] + pub struct CompetitionStatus: u32 { + /// Robot is connected to field control (NOT competition switch) + const SYSTEM = 1 << 3; + + /// Robot is in autonomous mode. + const AUTONOMOUS = 1 << 0; + + /// Robot is disabled by field control. + const DISABLED = 1 << 1; + + /// Robot is connected to competition control (either competition switch or field control). + const CONNECTED = 1 << 2; + } +} /// Represents a possible mode that robots can be set in during the competition lifecycle. #[derive(Debug, Clone, Copy, PartialEq, Eq)] @@ -27,7 +43,7 @@ pub enum CompetitionMode { /// connecting, but are typically placed into this mode at the start of a match. Autonomous, - /// The Opcontrol competition mode. + /// The drivercontrol competition mode. /// /// When in opcontrol mode, all device access is available including access to /// controller joystick values for reading user-input from drive team members. @@ -35,7 +51,7 @@ pub enum CompetitionMode { /// Robots may be placed into opcontrol mode at any point in the competition after /// connecting, but are typically placed into this mode following the autonomous /// period. - Opcontrol, + Driver, } /// Represents a type of system used to control competition state. @@ -48,33 +64,36 @@ pub enum CompetitionSystem { CompetitionSwitch, } +/// Gets the current competition status flags. +pub fn status() -> CompetitionStatus { + CompetitionStatus::from_bits_retain(unsafe { vexCompetitionStatus() }) +} + /// Gets the current competition mode, or phase. pub fn mode() -> CompetitionMode { - let status = unsafe { pros_sys::misc::competition_get_status() }; + let status = status(); - if status & COMPETITION_DISABLED != 0 { + if status.contains(CompetitionStatus::DISABLED) { CompetitionMode::Disabled - } else if status & COMPETITION_AUTONOMOUS != 0 { + } else if status.contains(CompetitionStatus::AUTONOMOUS) { CompetitionMode::Autonomous } else { - CompetitionMode::Opcontrol + CompetitionMode::Driver } } /// Checks if the robot is connected to a competition control system. pub fn connected() -> bool { - let status = unsafe { pros_sys::misc::competition_get_status() }; - - status & COMPETITION_CONNECTED != 0 + status().contains(CompetitionStatus::CONNECTED) } /// Gets the type of system currently controlling the robot's competition state, or [`None`] if the robot /// is not tethered to a competition controller. pub fn system() -> Option { - let status = unsafe { pros_sys::misc::competition_get_status() }; + let status = status(); - if status & COMPETITION_CONNECTED != 0 { - if status & COMPETITION_SYSTEM == 0 { + if status.contains(CompetitionStatus::CONNECTED) { + if status.contains(CompetitionStatus::SYSTEM) { Some(CompetitionSystem::FieldControl) } else { Some(CompetitionSystem::CompetitionSwitch) From bfa72ca7344c68a502b7f03fc4f7a2b1d6e6e97f Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 24 Mar 2024 11:33:27 -0500 Subject: [PATCH 06/14] refactor: `usd_installed` --- packages/vex-devices/src/usd.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/packages/vex-devices/src/usd.rs b/packages/vex-devices/src/usd.rs index d8abd27f..a3d969ac 100644 --- a/packages/vex-devices/src/usd.rs +++ b/packages/vex-devices/src/usd.rs @@ -2,7 +2,9 @@ //! //! The USD API provides functions for interacting with the SD card slot on the V5 Brain. +use vex_sys::vexFileDriveStatus; + /// Checks if an SD card is installed. pub fn usd_installed() -> bool { - unsafe { pros_sys::misc::usd_is_installed() == 1 } + unsafe { vexFileDriveStatus(0) } } From c2a35c13a6100199ddbbd240c770e8c32a6e5c0c Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Sun, 24 Mar 2024 22:24:04 -0500 Subject: [PATCH 07/14] refactor: optical sensor --- packages/pros-core/src/error.rs | 30 +- packages/pros-panic/src/lib.rs | 28 +- packages/pros/src/lib.rs | 15 +- packages/vex-devices/src/smart/distance.rs | 19 +- packages/vex-devices/src/smart/gps.rs | 130 --------- packages/vex-devices/src/smart/imu.rs | 76 ++--- packages/vex-devices/src/smart/link.rs | 10 - packages/vex-devices/src/smart/mod.rs | 212 ++++++++------ packages/vex-devices/src/smart/optical.rs | 309 ++++++++++----------- 9 files changed, 348 insertions(+), 481 deletions(-) delete mode 100644 packages/vex-devices/src/smart/gps.rs diff --git a/packages/pros-core/src/error.rs b/packages/pros-core/src/error.rs index c3bdbe93..45b911bd 100644 --- a/packages/pros-core/src/error.rs +++ b/packages/pros-core/src/error.rs @@ -86,30 +86,20 @@ macro_rules! bail_on { } use snafu::Snafu; -/// A trait for converting an errno value into an error type. -pub trait FromErrno { - /// Consume the current `errno` and, if it contains a known error, returns Self. - fn from_errno(num: i32) -> Option - where - Self: Sized; -} - #[derive(Debug, Snafu)] /// Generic erros that can take place when using ports on the V5 Brain. pub enum PortError { - /// The specified port is outside of the allowed range! - PortOutOfRange, - /// The specified port couldn't be configured as the specified type. - PortCannotBeConfigured, - /// The specified port is already being used or is mismatched. - AlreadyInUse, + /// No device is plugged into the port. + Disconnected, - NoDevice, + /// The incorrect device type is plugged into the port. IncorrectDevice, } -map_errno!(PortError { - ENXIO => Self::PortOutOfRange, - ENODEV => Self::PortCannotBeConfigured, - EADDRINUSE => Self::AlreadyInUse, -}); +/// A trait for converting an errno value into an error type. +pub trait FromErrno { + /// Consume the current `errno` and, if it contains a known error, returns Self. + fn from_errno(num: i32) -> Option + where + Self: Sized; +} diff --git a/packages/pros-panic/src/lib.rs b/packages/pros-panic/src/lib.rs index 220725db..e1042fb7 100644 --- a/packages/pros-panic/src/lib.rs +++ b/packages/pros-panic/src/lib.rs @@ -10,7 +10,7 @@ use alloc::{format, string::String}; use pros_core::eprintln; #[cfg(feature = "display_panics")] -use pros_devices::Screen; +use vex_devices::Screen; #[cfg(target_arch = "wasm32")] extern "C" { @@ -24,22 +24,22 @@ extern "C" { /// panic messages graphically before exiting. #[cfg(feature = "display_panics")] fn draw_error( - screen: &mut pros_devices::screen::Screen, + screen: &mut vex_devices::screen::Screen, msg: &str, -) -> Result<(), pros_devices::screen::ScreenError> { +) -> Result<(), vex_devices::screen::ScreenError> { const ERROR_BOX_MARGIN: i16 = 16; const ERROR_BOX_PADDING: i16 = 16; const LINE_MAX_WIDTH: usize = 52; - let error_box_rect = pros_devices::screen::Rect::new( + let error_box_rect = vex_devices::screen::Rect::new( ERROR_BOX_MARGIN, ERROR_BOX_MARGIN, Screen::HORIZONTAL_RESOLUTION - ERROR_BOX_MARGIN, Screen::VERTICAL_RESOLUTION - ERROR_BOX_MARGIN, ); - screen.fill(&error_box_rect, pros_devices::color::Rgb::RED)?; - screen.stroke(&error_box_rect, pros_devices::color::Rgb::WHITE)?; + screen.fill(&error_box_rect, vex_devices::color::Rgb::RED)?; + screen.stroke(&error_box_rect, vex_devices::color::Rgb::WHITE)?; let mut buffer = String::new(); let mut line: i16 = 0; @@ -51,15 +51,15 @@ fn draw_error( if character == '\n' || ((buffer.len() % LINE_MAX_WIDTH == 0) && (i > 0)) { screen.fill( - &pros_devices::screen::Text::new( + &vex_devices::screen::Text::new( buffer.as_str(), - pros_devices::screen::TextPosition::Point( + vex_devices::screen::TextPosition::Point( ERROR_BOX_MARGIN + ERROR_BOX_PADDING, ERROR_BOX_MARGIN + ERROR_BOX_PADDING + (line * Screen::LINE_HEIGHT), ), - pros_devices::screen::TextFormat::Small, + vex_devices::screen::TextFormat::Small, ), - pros_devices::color::Rgb::WHITE, + vex_devices::color::Rgb::WHITE, )?; line += 1; @@ -68,15 +68,15 @@ fn draw_error( } screen.fill( - &pros_devices::screen::Text::new( + &vex_devices::screen::Text::new( buffer.as_str(), - pros_devices::screen::TextPosition::Point( + vex_devices::screen::TextPosition::Point( ERROR_BOX_MARGIN + ERROR_BOX_PADDING, ERROR_BOX_MARGIN + ERROR_BOX_PADDING + (line * Screen::LINE_HEIGHT), ), - pros_devices::screen::TextFormat::Small, + vex_devices::screen::TextFormat::Small, ), - pros_devices::color::Rgb::WHITE, + vex_devices::color::Rgb::WHITE, )?; Ok(()) diff --git a/packages/pros/src/lib.rs b/packages/pros/src/lib.rs index 9f10b46f..45f51de4 100644 --- a/packages/pros/src/lib.rs +++ b/packages/pros/src/lib.rs @@ -59,8 +59,6 @@ pub use pros_async as async_runtime; #[cfg(feature = "core")] pub use pros_core as core; -#[cfg(feature = "devices")] -pub use pros_devices as devices; #[cfg(feature = "math")] pub use pros_math as math; #[cfg(feature = "panic")] @@ -68,6 +66,8 @@ pub use pros_panic as panic; #[cfg(feature = "sync")] pub use pros_sync as sync; pub use pros_sys as sys; +#[cfg(feature = "devices")] +pub use vex_devices as devices; /// Commonly used features of pros-rs. /// This module is meant to be glob imported. @@ -82,8 +82,12 @@ pub mod prelude { print, println, task::delay, }; + #[cfg(feature = "math")] + pub use pros_math::{feedforward::MotorFeedforwardController, pid::PidController}; + #[cfg(feature = "sync")] + pub use pros_sync::{sync_robot, SyncRobot}; #[cfg(feature = "devices")] - pub use pros_devices::{ + pub use vex_devices::{ adi::{ analog::AdiAnalogIn, digital::{AdiDigitalIn, AdiDigitalOut}, @@ -104,7 +108,6 @@ pub mod prelude { smart::{ distance::DistanceSensor, expander::AdiExpander, - gps::GpsSensor, imu::InertialSensor, link::{Link, RxLink, TxLink}, motor::{BrakeMode, Direction, Gearset, Motor, MotorControl}, @@ -114,8 +117,4 @@ pub mod prelude { SmartDevice, SmartPort, }, }; - #[cfg(feature = "math")] - pub use pros_math::{feedforward::MotorFeedforwardController, pid::PidController}; - #[cfg(feature = "sync")] - pub use pros_sync::{sync_robot, SyncRobot}; } diff --git a/packages/vex-devices/src/smart/distance.rs b/packages/vex-devices/src/smart/distance.rs index eaf8bf26..2fe3857a 100644 --- a/packages/vex-devices/src/smart/distance.rs +++ b/packages/vex-devices/src/smart/distance.rs @@ -6,10 +6,10 @@ use pros_core::error::PortError; use snafu::Snafu; use vex_sys::{ vexDeviceDistanceConfidenceGet, vexDeviceDistanceDistanceGet, vexDeviceDistanceObjectSizeGet, - vexDeviceDistanceObjectVelocityGet, vexDeviceDistanceStatusGet, V5_DeviceType, + vexDeviceDistanceObjectVelocityGet, vexDeviceDistanceStatusGet, }; -use super::{SmartDevice, SmartDeviceType, SmartPort}; +use super::{SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; /// A physical distance sensor plugged into a port. /// Distance sensors can only keep track of one object at a time. @@ -42,14 +42,14 @@ impl DistanceSensor { pub fn distance(&self) -> Result { self.validate()?; - Ok(unsafe { vexDeviceDistanceDistanceGet(self.port.device_handle()) }) + Ok(unsafe { vexDeviceDistanceDistanceGet(self.device_handle()) }) } /// Returns the velocity of the object the sensor detects in m/s pub fn velocity(&self) -> Result { self.validate()?; - Ok(unsafe { vexDeviceDistanceObjectVelocityGet(self.port.device_handle()) }) + Ok(unsafe { vexDeviceDistanceObjectVelocityGet(self.device_handle()) }) } /// Get the current guess at relative "object size". @@ -66,24 +66,21 @@ impl DistanceSensor { pub fn relative_size(&self) -> Result { self.validate()?; - Ok(unsafe { vexDeviceDistanceObjectSizeGet(self.port.device_handle()) as u32 }) + Ok(unsafe { vexDeviceDistanceObjectSizeGet(self.device_handle()) as u32 }) } /// Returns the confidence in the distance measurement from 0.0 to 1.0. pub fn distance_confidence(&self) -> Result { self.validate()?; - Ok( - unsafe { vexDeviceDistanceConfidenceGet(self.port.device_handle()) as u32 } as f64 - / 63.0, - ) + Ok(unsafe { vexDeviceDistanceConfidenceGet(self.device_handle()) as u32 } as f64 / 63.0) } /// Gets the status code of the distance sensor pub fn status(&self) -> Result { - self.port.validate(V5_DeviceType::DistanceSensor)?; + self.validate_port()?; - Ok(unsafe { vexDeviceDistanceStatusGet(self.port.device_handle()) }) + Ok(unsafe { vexDeviceDistanceStatusGet(self.device_handle()) }) } } diff --git a/packages/vex-devices/src/smart/gps.rs b/packages/vex-devices/src/smart/gps.rs deleted file mode 100644 index 16e561b0..00000000 --- a/packages/vex-devices/src/smart/gps.rs +++ /dev/null @@ -1,130 +0,0 @@ -//! GPS sensor device. -//! -//! A notable differenc between this API and that of PROS -//! is that [`GpsSensor::status`] returns acceleration along with other status data. - -use pros_core::{bail_on, error::PortError, map_errno}; -use pros_sys::{PROS_ERR, PROS_ERR_F}; -use snafu::Snafu; - -use super::{SmartDevice, SmartDeviceType, SmartPort}; - -//TODO: Figure out what all the units are -#[derive(Default, Debug, Clone, Copy, PartialEq)] -/// Represents the data output from a GPS sensor. -pub struct GpsStatus { - /// The x-coordinate of the GPS sensor in meters. - pub x: f64, - /// The y-coordinate of the GPS sensor in meters. - pub y: f64, - /// The pitch of the GPS sensor. - pub pitch: f64, - /// The roll of the GPS sensor. - pub roll: f64, - /// The yaw of the GPS sensor. - pub yaw: f64, - /// The heading of the GPS sensor. - pub heading: f64, - - /// The x-acceleration of the GPS sensor. - pub accel_x: f64, - /// The y-acceleration of the GPS sensor. - pub accel_y: f64, - /// The z-acceleration of the GPS sensor. - pub accel_z: f64, -} - -/// A physical GPS sensor plugged into a port. -#[derive(Debug, Eq, PartialEq)] -pub struct GpsSensor { - port: SmartPort, -} - -impl GpsSensor { - /// Creates a new GPS sensor on the given port. - pub fn new(port: SmartPort) -> Result { - unsafe { - bail_on!( - PROS_ERR, - pros_sys::gps_initialize_full(port.index(), 0.0, 0.0, 0.0, 0.0, 0.0) - ); - } - - Ok(Self { port }) - } - - /// Sets the offset of the GPS sensor, relative to the sensor of turning, in meters. - pub fn set_offset(&mut self, x: f64, y: f64) -> Result<(), GpsError> { - unsafe { - bail_on!(PROS_ERR, pros_sys::gps_set_offset(self.port.index(), x, y)); - } - Ok(()) - } - - /// Gets the possible error of the GPS sensor, in meters. - pub fn rms_error(&self) -> Result { - Ok(unsafe { bail_on!(PROS_ERR_F, pros_sys::gps_get_error(self.port.index())) }) - } - - /// Gets the status of the GPS sensor. - pub fn status(&self) -> Result { - unsafe { - let status = pros_sys::gps_get_status(self.port.index()); - bail_on!(PROS_ERR_F, status.x); - let accel = pros_sys::gps_get_accel(self.port.index()); - bail_on!(PROS_ERR_F, accel.x); - let heading = bail_on!(PROS_ERR_F, pros_sys::gps_get_heading(self.port.index())); - - Ok(GpsStatus { - x: status.x, - y: status.y, - pitch: status.pitch, - roll: status.roll, - yaw: status.yaw, - heading, - - accel_x: accel.x, - accel_y: accel.y, - accel_z: accel.z, - }) - } - } - - /// Zeroes the rotation of the GPS sensor. - pub fn zero_rotation(&mut self) -> Result<(), GpsError> { - unsafe { - bail_on!(PROS_ERR, pros_sys::gps_tare_rotation(self.port.index())); - } - Ok(()) - } -} - -impl SmartDevice for GpsSensor { - fn port_index(&self) -> u8 { - self.port.index() - } - - fn device_type(&self) -> SmartDeviceType { - SmartDeviceType::Gps - } -} - -#[derive(Debug, Snafu)] -/// Errors that can occur when using a GPS sensor. -pub enum GpsError { - /// The GPS sensor is still calibrating. - StillCalibrating, - #[snafu(display("{source}"), context(false))] - /// Generic port related error. - Port { - /// The source of the error. - source: PortError, - }, -} - -map_errno! { - GpsError { - EAGAIN => Self::StillCalibrating, - } - inherit PortError; -} diff --git a/packages/vex-devices/src/smart/imu.rs b/packages/vex-devices/src/smart/imu.rs index d512f8b4..ce6d54bd 100644 --- a/packages/vex-devices/src/smart/imu.rs +++ b/packages/vex-devices/src/smart/imu.rs @@ -9,19 +9,22 @@ use core::{ use bitflags::bitflags; use pros_core::{ bail_on, - error::{take_errno, FromErrno, PortError}, - map_errno, + error::{take_errno, PortError}, time::Instant, }; use pros_sys::{PROS_ERR, PROS_ERR_F}; use snafu::Snafu; +use vex_sys::{vexDeviceImuDegreesGet, vexDeviceImuHeadingGet, vexDeviceImuStatusGet}; -use super::{SmartDevice, SmartDeviceType, SmartPort}; +use super::{SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; /// Represents a smart port configured as a V5 inertial sensor (IMU) -#[derive(Debug, Eq, PartialEq)] +#[derive(Debug, PartialEq)] pub struct InertialSensor { port: SmartPort, + rotation_offset: f64, + heading_offset: f64, + euler_offset: Euler, } impl InertialSensor { @@ -33,7 +36,21 @@ impl InertialSensor { /// Create a new inertial sensor from a smart port index. pub const fn new(port: SmartPort) -> Self { - Self { port } + Self { + port, + rotation_offset: 0.0, + heading_offset: 0.0, + euler_offset: Euler::default(), + } + } + + /// Validates that the sensor is currently connected to its port, and that it isn't currently + /// calibrating. + fn validate(&self) -> Result<(), InertialError> { + if self.is_calibrating()? { + return Err(InertialError::Calibrating); + } + Ok(()) } /// Calibrate IMU asynchronously. @@ -55,9 +72,9 @@ impl InertialSensor { /// 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 { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::imu_get_rotation(self.port.index()) - })) + self.validate()?; + // TODO: Test vexDeviceImuDegreesGet + Ok(unsafe { vexDeviceImuDegreesGet(self.device_handle()) } + self.rotation_offset) } /// Get the Inertial Sensor’s heading relative to the initial direction of its x-axis. @@ -65,39 +82,33 @@ impl InertialSensor { /// 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 { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::imu_get_heading(self.port.index()) - })) + self.validate()?; + // TODO: test vexDeviceImuHeadingGet + Ok(unsafe { vexDeviceImuHeadingGet(self.device_handle()) } + self.heading_offset) + } + + /// Get the Inertial Sensor’s yaw angle bounded by (-180, 180) degrees. + pub fn yaw(&self) -> Result { + Ok(self.euler()?.yaw) } /// Get the Inertial Sensor’s pitch angle bounded by (-180, 180) degrees. pub fn pitch(&self) -> Result { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::imu_get_pitch(self.port.index()) - })) + Ok(self.euler()?.pitch) } /// Get the Inertial Sensor’s roll angle bounded by (-180, 180) degrees. pub fn roll(&self) -> Result { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::imu_get_roll(self.port.index()) - })) - } - - /// Get the Inertial Sensor’s yaw angle bounded by (-180, 180) degrees. - pub fn yaw(&self) -> Result { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::imu_get_yaw(self.port.index()) - })) + Ok(self.euler()?.roll) } /// Read the inertial sensor's status code. pub fn status(&self) -> Result { - let bits = bail_on!(pros_sys::E_IMU_STATUS_ERROR, unsafe { - pros_sys::imu_get_status(self.port.index()) - }); + self.validate_port()?; - Ok(InertialStatus::from_bits_retain(bits)) + Ok(InertialStatus::from_bits_retain(unsafe { + vexDeviceImuStatusGet(self.device_handle()) + })) } /// Get a quaternion representing the Inertial Sensor’s orientation. @@ -434,6 +445,8 @@ impl core::future::Future for InertialCalibrateFuture { pub enum InertialError { /// The inertial sensor spent too long calibrating. CalibrationTimedOut, + /// The inertial is still calibrating. + Calibrating, /// Invalid sensor data rate, expected >= 5 milliseconds. InvalidDataRate, #[snafu(display("{source}"), context(false))] @@ -443,10 +456,3 @@ pub enum InertialError { source: PortError, }, } - -map_errno! { - InertialError { - EAGAIN => Self::CalibrationTimedOut, - } - inherit PortError; -} diff --git a/packages/vex-devices/src/smart/link.rs b/packages/vex-devices/src/smart/link.rs index ddcebdf7..41cf4140 100644 --- a/packages/vex-devices/src/smart/link.rs +++ b/packages/vex-devices/src/smart/link.rs @@ -228,13 +228,3 @@ pub enum LinkError { source: PortError, }, } - -map_errno! { - LinkError { - ENXIO => Self::NoLink, - EBUSY => Self::BufferBusyFull, - EINVAL => Self::NullData, - EBADMSG => Self::Protocol, - } - inherit PortError; -} diff --git a/packages/vex-devices/src/smart/mod.rs b/packages/vex-devices/src/smart/mod.rs index 09d1514a..8d66dc7b 100644 --- a/packages/vex-devices/src/smart/mod.rs +++ b/packages/vex-devices/src/smart/mod.rs @@ -22,7 +22,6 @@ pub mod distance; pub mod expander; -pub mod gps; pub mod imu; pub mod link; pub mod motor; @@ -34,14 +33,13 @@ use core::fmt; pub use distance::DistanceSensor; pub use expander::AdiExpander; -pub use gps::GpsSensor; pub use imu::InertialSensor; pub use link::{Link, RxLink, TxLink}; pub use motor::Motor; pub use optical::OpticalSensor; -use pros_core::{bail_on, error::PortError}; +use pros_core::error::PortError; pub use rotation::RotationSensor; -use vex_sys::{vexDeviceGetByIndex, V5_DeviceT, V5_DeviceType}; +use vex_sys::{vexDeviceGetByIndex, vexDeviceGetTimestamp, V5_DeviceT, V5_DeviceType}; pub use vision::VisionSensor; /// Defines common functionality shared by all smart port devices. @@ -82,18 +80,50 @@ pub trait SmartDevice { /// println!("No IMU connection found."); /// } /// ``` - fn port_connected(&self) -> bool { - let plugged_type_result: Result = - unsafe { pros_sys::apix::registry_get_plugged_type(self.port_index() - 1).try_into() }; - - if let Ok(plugged_type) = plugged_type_result { - plugged_type == self.device_type() - } else { - false + fn is_connected(&self) -> bool { + let connected_type: SmartDeviceType = + unsafe { *vexDeviceGetByIndex((self.port_index() - 1) as u32) } + .device_type + .into(); + + connected_type == self.device_type() + } + + /// Get the timestamp recorded by this device's internal clock. + fn timestamp(&self) -> Result { + Ok(SmartDeviceTimestamp(unsafe { + vexDeviceGetTimestamp(vexDeviceGetByIndex((self.port_index() - 1) as u32)) + })) + } +} + +/// Internal helper functions for port validation, error handling, and interaction with +/// vex-sys on various smart devices. +pub(crate) trait SmartDeviceInternal: SmartDevice { + /// Get the raw device handle connected to this port. + fn device_handle(&self) -> V5_DeviceT { + unsafe { vexDeviceGetByIndex((self.port_index() - 1) as u32) } + } + + /// Verify that the device type is currently plugged into this port. + fn validate_port(&self) -> Result<(), PortError> { + let device = unsafe { *self.device_handle() }; + let plugged_type: SmartDeviceType = device.device_type.into(); + + if !device.exists { + // No device is plugged into the port. + return Err(PortError::Disconnected); + } else if plugged_type != self.device_type() { + // The connected device doesn't match the requested type. + return Err(PortError::IncorrectDevice); } + + Ok(()) } } +impl SmartDeviceInternal for T {} + /// Represents a smart port on a V5 Brain #[derive(Debug, Eq, PartialEq)] pub struct SmartPort { @@ -140,28 +170,6 @@ impl SmartPort { self.index } - /// Get the raw device handle connected to this port. - pub(crate) fn device_handle(&self) -> V5_DeviceT { - unsafe { - vexDeviceGetByIndex((self.index - 1) as u32) - } - } - - /// Verify that a specific device type is currently plugged into this port. - pub(crate) fn validate(&self, device_type: V5_DeviceType) -> Result<(), PortError> { - let dev = unsafe { *self.device_handle() }; - - if !dev.exists { - // No device is plugged into the port. - return Err(PortError::NoDevice) - } else if dev.device_type != device_type { - // The connected device doesn't match the requested type. - return Err(PortError::IncorrectDevice) - } - - Ok(()) - } - /// Get the type of device currently connected to this port. /// /// # Examples @@ -171,96 +179,122 @@ impl SmartPort { /// /// println!("Type of device connected to port 1: {:?}", my_port.connected_type()?); /// ``` - pub fn connected_type(&self) -> Result { - unsafe { pros_sys::apix::registry_get_plugged_type(self.index() - 1).try_into() } - } - - /// Get the type of device this port is configured as. - /// - /// # Examples - /// - /// ``` - /// let my_port = unsafe { SmartPort::new(1) }; - /// let imu = InertialSensor::new(my_port)?; - /// - /// assert_eq!(my_port.configured_type()?, SmartDeviceType::Imu); - /// ``` - pub fn configured_type(&self) -> Result { - unsafe { pros_sys::apix::registry_get_bound_type(self.index() - 1).try_into() } + pub fn device_type(&self) -> Result { + Ok(unsafe { *vexDeviceGetByIndex((self.index() - 1) as u32) } + .device_type + .into()) } } /// Represents a possible type of device that can be registered on a [`SmartPort`]. #[derive(Debug, Clone, Copy, PartialEq, Eq)] -#[repr(u32)] +#[repr(u8)] pub enum SmartDeviceType { /// No device - None = pros_sys::apix::E_DEVICE_NONE, + None, /// Smart Motor - Motor = pros_sys::apix::E_DEVICE_MOTOR, + Motor, /// Rotation Sensor - Rotation = pros_sys::apix::E_DEVICE_ROTATION, + Rotation, /// Inertial Sensor - Imu = pros_sys::apix::E_DEVICE_IMU, + Imu, /// Distance Sensor - Distance = pros_sys::apix::E_DEVICE_DISTANCE, + Distance, /// Vision Sensor - Vision = pros_sys::apix::E_DEVICE_VISION, + Vision, + + /// AI Vision Sensor + AiVision, + + /// Workcell Electromagnet + Magnet, + + /// CTE Workcell Light Tower + LightTower, + + /// CTE Workcell Arm + Arm, /// Optical Sensor - Optical = pros_sys::apix::E_DEVICE_OPTICAL, + Optical, /// GPS Sensor - Gps = pros_sys::apix::E_DEVICE_GPS, + Gps, /// Smart Radio - Radio = pros_sys::apix::E_DEVICE_RADIO, + Radio, /// ADI Expander /// /// This variant is also internally to represent the brain's onboard ADI slots. - Adi = pros_sys::apix::E_DEVICE_ADI, + Adi, /// Generic Serial Port - Serial = pros_sys::apix::E_DEVICE_SERIAL, + GenericSerial, + + /// Other device type code returned by the SDK that is currently unsupported, undocumented, + /// or unknown. + Unknown(V5_DeviceType), } -impl TryFrom for SmartDeviceType { - type Error = PortError; - - /// Convert a raw `pros_sys::apix::v5_device_e_t` from `pros_sys` into a [`SmartDeviceType`]. - fn try_from(value: pros_sys::apix::v5_device_e_t) -> Result { - // PROS returns either -1 (WTF?!?!) or 255 which both cast to E_DEVICE_UNDEFINED - // when setting ERRNO, which can only be ENXIO. - // - // - bail_on!(pros_sys::apix::E_DEVICE_UNDEFINED, value); - - Ok(match value { - pros_sys::apix::E_DEVICE_NONE => Self::None, - pros_sys::apix::E_DEVICE_MOTOR => Self::Motor, - pros_sys::apix::E_DEVICE_ROTATION => Self::Rotation, - pros_sys::apix::E_DEVICE_IMU => Self::Imu, - pros_sys::apix::E_DEVICE_DISTANCE => Self::Distance, - pros_sys::apix::E_DEVICE_VISION => Self::Vision, - pros_sys::apix::E_DEVICE_OPTICAL => Self::Optical, - pros_sys::apix::E_DEVICE_RADIO => Self::Radio, - pros_sys::apix::E_DEVICE_ADI => Self::Adi, - pros_sys::apix::E_DEVICE_SERIAL => Self::Serial, - _ => unreachable!(), - }) +impl From for SmartDeviceType { + fn from(value: V5_DeviceType) -> Self { + match value { + V5_DeviceType::kDeviceTypeNoSensor => Self::None, + V5_DeviceType::kDeviceTypeMotorSensor => Self::Motor, + // TODO: + // I'm not entirely sure that this conversion is correct. + // This behavior is taken from the PROS kernel, which treats + // the rotation sensor as kDeviceTypeAbsEncSensor, however the + // SDK also uses the "absEnc" terminology to represent ADI + // encoders which is an entirely different thing. + // + // Not sure what else this value would be, but it still needs + // hardware testing + V5_DeviceType::kDeviceTypeAbsEncSensor => Self::Rotation, + V5_DeviceType::kDeviceTypeImuSensor => Self::Imu, + V5_DeviceType::kDeviceTypeDistanceSensor => Self::Distance, + V5_DeviceType::kDeviceTypeRadioSensor => Self::Radio, + V5_DeviceType::kDeviceTypeVisionSensor => Self::Vision, + V5_DeviceType::kDeviceTypeAdiSensor => Self::Adi, + V5_DeviceType::kDeviceTypeOpticalSensor => Self::Optical, + V5_DeviceType::kDeviceTypeMagnetSensor => Self::Magnet, + V5_DeviceType::kDeviceTypeGpsSensor => Self::Gps, + V5_DeviceType::kDeviceTypeLightTowerSensor => Self::LightTower, + V5_DeviceType::kDeviceTypeArmDevice => Self::Arm, + V5_DeviceType::kDeviceTypeAiVisionSensor => Self::AiVision, + V5_DeviceType::kDeviceTypeGenericSerial => Self::GenericSerial, + other => Self::Unknown(other), + } } } -impl From for pros_sys::apix::v5_device_e_t { - /// Convert a [`SmartDeviceType`] into a raw `pros_sys::apix::v5_device_e_t`. +impl From for V5_DeviceType { fn from(value: SmartDeviceType) -> Self { - value as _ + match value { + SmartDeviceType::None => V5_DeviceType::kDeviceTypeNoSensor, + SmartDeviceType::Motor => V5_DeviceType::kDeviceTypeMotorSensor, + // TODO: See comment in the conversion impl above this one. Same deal. + SmartDeviceType::Rotation => V5_DeviceType::kDeviceTypeAbsEncSensor, + SmartDeviceType::Imu => V5_DeviceType::kDeviceTypeImuSensor, + SmartDeviceType::Distance => V5_DeviceType::kDeviceTypeDistanceSensor, + SmartDeviceType::Vision => V5_DeviceType::kDeviceTypeVisionSensor, + SmartDeviceType::AiVision => V5_DeviceType::kDeviceTypeAiVisionSensor, + SmartDeviceType::Magnet => V5_DeviceType::kDeviceTypeMagnetSensor, + SmartDeviceType::LightTower => V5_DeviceType::kDeviceTypeLightTowerSensor, + SmartDeviceType::Arm => V5_DeviceType::kDeviceTypeArmDevice, + SmartDeviceType::Optical => V5_DeviceType::kDeviceTypeOpticalSensor, + SmartDeviceType::Gps => V5_DeviceType::kDeviceTypeGpsSensor, + SmartDeviceType::Radio => V5_DeviceType::kDeviceTypeRadioSensor, + SmartDeviceType::Adi => V5_DeviceType::kDeviceTypeAdiSensor, + SmartDeviceType::GenericSerial => V5_DeviceType::kDeviceTypeGenericSerial, + SmartDeviceType::Unknown(raw_type) => raw_type, + } } } diff --git a/packages/vex-devices/src/smart/optical.rs b/packages/vex-devices/src/smart/optical.rs index d529887d..51549100 100644 --- a/packages/vex-devices/src/smart/optical.rs +++ b/packages/vex-devices/src/smart/optical.rs @@ -2,11 +2,14 @@ use core::time::Duration; -use pros_core::{bail_on, error::PortError, map_errno}; -use pros_sys::{OPT_GESTURE_ERR, PROS_ERR, PROS_ERR_F}; +use bitflags::bitflags; +use pros_core::{bail_on, error::PortError}; use snafu::Snafu; +use vex_sys::{ + vexDeviceOpticalBrightnessGet, vexDeviceOpticalGestureDisable, vexDeviceOpticalGestureEnable, vexDeviceOpticalGestureGet, vexDeviceOpticalHueGet, vexDeviceOpticalIntegrationTimeGet, vexDeviceOpticalIntegrationTimeSet, vexDeviceOpticalLedPwmGet, vexDeviceOpticalLedPwmSet, vexDeviceOpticalProximityGet, vexDeviceOpticalProximityThreshold, vexDeviceOpticalRawGet, vexDeviceOpticalRgbGet, vexDeviceOpticalSatGet, vexDeviceOpticalStatusGet, V5_DeviceOpticalGesture, V5_DeviceOpticalRaw, V5_DeviceOpticalRgb +}; -use super::{SmartDevice, SmartDeviceType, SmartPort}; +use super::{SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; /// Represents a smart port configured as a V5 optical sensor #[derive(Debug, Eq, PartialEq)] @@ -17,14 +20,15 @@ pub struct OpticalSensor { impl OpticalSensor { /// The smallest integration time you can set on an optical sensor. + /// + /// Source: pub const MIN_INTEGRATION_TIME: Duration = Duration::from_millis(3); /// The largest integration time you can set on an optical sensor. + /// + /// Source: pub const MAX_INTEGRATION_TIME: Duration = Duration::from_millis(712); - /// The maximum value for the LED PWM. - pub const MAX_LED_PWM: u8 = 100; - /// Creates a new inertial sensor from a smart port index. /// /// Gesture detection features can be optionally enabled, allowing the use of [`Self::last_gesture_direction()`] and [`Self::last_gesture_direction()`]. @@ -43,39 +47,30 @@ impl OpticalSensor { Ok(sensor) } - /// Get the pwm value of the White LED. PWM value ranges from 0 to 100. - pub fn led_pwm(&self) -> Result { - unsafe { - Ok(bail_on!( - PROS_ERR, - pros_sys::optical_get_led_pwm(self.port.index()) - )) - } + /// Get the PWM percentage (intensity/brightness) of the sensor's LED indicator. + pub fn led_brightness(&self) -> Result { + self.validate_port()?; + + Ok(unsafe { vexDeviceOpticalLedPwmGet(self.device_handle()) }) } - /// Sets the pwm value of the White LED. Valid values are in the range `0` `100`. - pub fn set_led_pwm(&mut self, value: u8) -> Result<(), OpticalError> { - if value > Self::MAX_LED_PWM { - return Err(OpticalError::InvalidLedPwm); - } - unsafe { - bail_on!( - PROS_ERR, - pros_sys::optical_set_led_pwm(self.port.index(), value) - ); - } + /// Set the PWM percentage (intensity/brightness) of the sensor's LED indicator. + pub fn set_led_brightness(&mut self, brightness: f64) -> Result<(), OpticalError> { + self.validate_port()?; + + unsafe { vexDeviceOpticalLedPwmSet(self.device_handle(), (brightness * 100.0) as i32) } + Ok(()) } /// Get integration time (update rate) of the optical sensor in milliseconds, with /// minimum time being 3ms and the maximum time being 712ms. pub fn integration_time(&self) -> Result { - unsafe { - Ok(Duration::from_millis(bail_on!( - PROS_ERR_F, - pros_sys::optical_get_integration_time(self.port.index()) - ) as u64)) - } + self.validate_port()?; + + Ok(Duration::from_millis( + unsafe { vexDeviceOpticalIntegrationTimeGet(self.device_handle()) } as u64, + )) } /// Set integration time (update rate) of the optical sensor. @@ -87,16 +82,14 @@ impl OpticalSensor { /// for /// more information. pub fn set_integration_time(&mut self, time: Duration) -> Result<(), OpticalError> { - if time < Self::MIN_INTEGRATION_TIME || time > Self::MAX_INTEGRATION_TIME { - return Err(OpticalError::InvalidIntegrationTime); - } + self.validate_port()?; - unsafe { - bail_on!( - PROS_ERR, - pros_sys::optical_set_integration_time(self.port.index(), time.as_millis() as f64) - ); - } + let time = time.as_millis().clamp( + Self::MIN_INTEGRATION_TIME.as_millis(), + Self::MAX_INTEGRATION_TIME.as_millis(), + ) as f64; + + unsafe { vexDeviceOpticalIntegrationTimeSet(self.device_handle(), time) } Ok(()) } @@ -105,58 +98,68 @@ impl OpticalSensor { /// /// Hue has a range of `0` to `359.999`. pub fn hue(&self) -> Result { - unsafe { - Ok(bail_on!( - PROS_ERR_F, - pros_sys::optical_get_hue(self.port.index()) - )) - } + self.validate_port()?; + + Ok(unsafe { vexDeviceOpticalHueGet(self.device_handle()) }) } /// Gets the detected color saturation. /// /// Saturation has a range `0` to `1.0`. pub fn saturation(&self) -> Result { - unsafe { - Ok(bail_on!( - PROS_ERR_F, - pros_sys::optical_get_saturation(self.port.index()) - )) - } + self.validate_port()?; + + Ok(unsafe { vexDeviceOpticalSatGet(self.device_handle()) }) } /// Get the detected color brightness. /// /// Brightness values range from `0` to `1.0`. pub fn brightness(&self) -> Result { - unsafe { - Ok(bail_on!( - PROS_ERR_F, - pros_sys::optical_get_brightness(self.port.index()) - )) - } + self.validate_port()?; + + Ok(unsafe { vexDeviceOpticalBrightnessGet(self.device_handle()) }) } /// Get the detected proximity value /// /// Proximity has a range of `0` to `255`. pub fn proximity(&self) -> Result { - unsafe { - Ok(bail_on!( - PROS_ERR, - pros_sys::optical_get_proximity(self.port.index()) - )) - } + self.validate_port()?; + + Ok(unsafe { vexDeviceOpticalProximityGet(self.device_handle()) }) + } + + /// Set the sensor's proximity threshold. + /// + /// Range and units are currently undocumented in the SDK. + /// TODO: Test on hardware. + pub fn set_proximity_threshold(&self, proximity: i32) -> Result<(), OpticalError> { + self.validate_port()?; + + unsafe { vexDeviceOpticalProximityThreshold(self.device_handle(), proximity) } + + Ok(()) } - /// Get the processed RGBC data from the sensor - pub fn rgbc(&self) -> Result { - unsafe { pros_sys::optical_get_rgb(self.port.index()).try_into() } + /// Get the processed RGB data from the sensor + pub fn rgb(&self) -> Result { + self.validate_port(); + + let mut data = V5_DeviceOpticalRgb::default(); + unsafe { vexDeviceOpticalRgbGet(self.device_handle(), &mut data) }; + + Ok(data.into()) } /// Get the raw, unprocessed RGBC data from the sensor - pub fn rgbc_raw(&self) -> Result { - unsafe { pros_sys::optical_get_raw(self.port.index()).try_into() } + pub fn raw(&self) -> Result { + self.validate_port(); + + let mut data = V5_DeviceOpticalRaw::default(); + unsafe { vexDeviceOpticalRawGet(self.device_handle(), &mut data) }; + + Ok(data.into()) } /// Enables gesture detection features on the sensor. @@ -164,21 +167,21 @@ impl OpticalSensor { /// This allows [`Self::last_gesture_direction()`] and [`Self::last_gesture_direction()`] to be called without error, if /// gesture detection wasn't already enabled. pub fn enable_gesture_detection(&mut self) -> Result<(), OpticalError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::optical_enable_gesture(self.port.index()) - }); + self.validate_port()?; + unsafe { vexDeviceOpticalGestureEnable(self.device_handle()) } self.gesture_detection_enabled = true; + Ok(()) } /// Disables gesture detection features on the sensor. pub fn disable_gesture_detection(&mut self) -> Result<(), OpticalError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::optical_disable_gesture(self.port.index()) - }); + self.validate_port()?; + + unsafe { vexDeviceOpticalGestureDisable(self.device_handle()) } + self.gesture_detection_enabled = true; - self.gesture_detection_enabled = false; Ok(()) } @@ -191,24 +194,35 @@ impl OpticalSensor { /// /// Will return [`OpticalError::GestureDetectionDisabled`] if the sensor is not /// confgured to detect gestures. - pub fn last_gesture_direction(&self) -> Result { + pub fn last_gesture(&self) -> Result { if !self.gesture_detection_enabled { return Err(OpticalError::GestureDetectionDisabled); } - - unsafe { pros_sys::optical_get_gesture(self.port.index()).try_into() } + self.validate_port()?; + + let mut gesture = V5_DeviceOpticalGesture::default(); + let direction: GestureDirection = + unsafe { vexDeviceOpticalGestureGet(self.device_handle(), &mut gesture) }.into(); + + Ok(Gesture { + direction, + up: gesture.udata, + down: gesture.ddata, + left: gesture.ldata, + right: gesture.rdata, + gesture_type: gesture.gesture_type, + count: gesture.count, + time: gesture.time, + }) } - /// Get the most recent raw gesture data from the sensor. - /// - /// Will return [`OpticalError::GestureDetectionDisabled`] if the sensor is not - /// confgured to detect gestures. - pub fn last_gesture_raw(&self) -> Result { - if !self.gesture_detection_enabled { - return Err(OpticalError::GestureDetectionDisabled); - } + /// Gets the status code of the distance sensor + pub fn status(&self) -> Result { + self.validate_port()?; - unsafe { pros_sys::optical_get_gesture_raw(self.port.index()).try_into() } + Ok(unsafe { + vexDeviceOpticalStatusGet(self.device_handle()) + }) } } @@ -222,44 +236,43 @@ impl SmartDevice for OpticalSensor { } } -#[derive(Default, Debug, Clone, Copy, PartialEq)] /// Represents a gesture and its direction. +#[derive(Default, Debug, Clone, Copy, Eq, PartialEq)] pub enum GestureDirection { + /// No gesture detected. + #[default] + None = 0, /// Up gesture. - Up, + Up = 1, /// Down gesture. - Down, + Down = 2, /// Left gesture. - Left, + Left = 3, /// Right gesture. - Right, - /// Gesture error. - Error, - #[default] - /// No gesture detected. - NoGesture, + Right = 4, } -impl TryFrom for GestureDirection { - type Error = OpticalError; - - fn try_from(value: pros_sys::optical_direction_e_t) -> Result { - bail_on!(pros_sys::E_OPTICAL_DIRECTION_ERROR, value); - - Ok(match value { - pros_sys::E_OPTICAL_DIRECTION_UP => Self::Up, - pros_sys::E_OPTICAL_DIRECTION_DOWN => Self::Down, - pros_sys::E_OPTICAL_DIRECTION_LEFT => Self::Left, - pros_sys::E_OPTICAL_DIRECTION_RIGHT => Self::Right, - pros_sys::E_OPTICAL_DIRECTION_NO_GESTURE => Self::NoGesture, - _ => unreachable!("Encountered unknown gesture direction code."), - }) +impl From for GestureDirection { + fn from(code: u32) -> Self { + // https://github.com/purduesigbots/pros/blob/master/include/pros/optical.h#L37 + match code { + // + 1 => Self::Up, + 2 => Self::Down, + 3 => Self::Left, + 4 => Self::Right, + // Normally this is just 0, but this is `From` so we have to handle + // all values even if they're unreacahable. + _ => Self::None, + } } } +/// Gesture data from an [`OpticalSensor`]. #[derive(Default, Debug, Clone, Copy, Eq, PartialEq)] -/// Raw gesture data from an [`OpticalSensor`]. -pub struct GestureRaw { +pub struct Gesture { + /// Gesture Direction + pub direction: GestureDirection, /// Up value. pub up: u8, /// Down value. @@ -276,25 +289,9 @@ pub struct GestureRaw { pub time: u32, } -impl TryFrom for GestureRaw { - type Error = OpticalError; - - fn try_from(value: pros_sys::optical_gesture_s_t) -> Result { - Ok(Self { - up: bail_on!(OPT_GESTURE_ERR as u8, value.udata), - down: value.ddata, - left: value.ldata, - right: value.rdata, - gesture_type: value.r#type, - count: value.count, - time: value.time, - }) - } -} - +/// RGB data from a [`OpticalSensor`]. #[derive(Default, Debug, Clone, Copy, PartialEq)] -/// RGBC data from a [`OpticalSensor`]. -pub struct Rgbc { +pub struct OpticalRgb { /// The red value from the sensor. pub red: f64, /// The green value from the sensor. @@ -305,56 +302,44 @@ pub struct Rgbc { pub brightness: f64, } -impl TryFrom for Rgbc { - type Error = OpticalError; - - fn try_from(value: pros_sys::optical_rgb_s_t) -> Result { - Ok(Self { - red: bail_on!(PROS_ERR_F, value.red), // Docs incorrectly claim this is PROS_ERR +impl From for OpticalRgb { + fn from(value: V5_DeviceOpticalRgb) -> Self { + Self { + red: value.red, green: value.green, blue: value.blue, brightness: value.brightness, - }) + } } } -#[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] /// Represents the raw RGBC data from the sensor. -pub struct RgbcRaw { +#[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] +pub struct OpticalRaw { /// The red value from the sensor. - pub red: u32, + pub red: u16, /// The green value from the sensor. - pub green: u32, + pub green: u16, /// The blue value from the sensor. - pub blue: u32, + pub blue: u16, /// The clear value from the sensor. - pub clear: u32, + pub clear: u16, } -impl TryFrom for RgbcRaw { - type Error = OpticalError; - - fn try_from(value: pros_sys::optical_raw_s_t) -> Result { - Ok(Self { - clear: bail_on!(PROS_ERR_F as u32, value.clear), +impl From for OpticalRaw { + fn from(value: V5_DeviceOpticalRaw) -> Self { + Self { red: value.red, green: value.green, blue: value.blue, - }) + clear: value.clear, + } } } #[derive(Debug, Snafu)] /// Errors that can occur when interacting with an optical sensor. pub enum OpticalError { - /// Invalid LED PWM value, must be between 0 and 100. - InvalidLedPwm, - - /// Integration time must be between 3 and 712 milliseconds. - /// - /// See for more information. - InvalidIntegrationTime, - /// Gesture detection is not enabled for this sensor. GestureDetectionDisabled, @@ -365,7 +350,3 @@ pub enum OpticalError { source: PortError, }, } - -map_errno! { - OpticalError {} inherit PortError; -} From 8cdb369f0610fbcd44e4038315981556e9805bbf Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Mon, 25 Mar 2024 11:56:39 -0500 Subject: [PATCH 08/14] refactor: optical sensor adjustments --- packages/vex-devices/src/smart/optical.rs | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/packages/vex-devices/src/smart/optical.rs b/packages/vex-devices/src/smart/optical.rs index 51549100..081e3c59 100644 --- a/packages/vex-devices/src/smart/optical.rs +++ b/packages/vex-devices/src/smart/optical.rs @@ -2,11 +2,15 @@ use core::time::Duration; -use bitflags::bitflags; -use pros_core::{bail_on, error::PortError}; +use pros_core::error::PortError; use snafu::Snafu; use vex_sys::{ - vexDeviceOpticalBrightnessGet, vexDeviceOpticalGestureDisable, vexDeviceOpticalGestureEnable, vexDeviceOpticalGestureGet, vexDeviceOpticalHueGet, vexDeviceOpticalIntegrationTimeGet, vexDeviceOpticalIntegrationTimeSet, vexDeviceOpticalLedPwmGet, vexDeviceOpticalLedPwmSet, vexDeviceOpticalProximityGet, vexDeviceOpticalProximityThreshold, vexDeviceOpticalRawGet, vexDeviceOpticalRgbGet, vexDeviceOpticalSatGet, vexDeviceOpticalStatusGet, V5_DeviceOpticalGesture, V5_DeviceOpticalRaw, V5_DeviceOpticalRgb + vexDeviceOpticalBrightnessGet, vexDeviceOpticalGestureDisable, vexDeviceOpticalGestureEnable, + vexDeviceOpticalGestureGet, vexDeviceOpticalHueGet, vexDeviceOpticalIntegrationTimeGet, + vexDeviceOpticalIntegrationTimeSet, vexDeviceOpticalLedPwmGet, vexDeviceOpticalLedPwmSet, + vexDeviceOpticalProximityGet, vexDeviceOpticalProximityThreshold, vexDeviceOpticalRawGet, + vexDeviceOpticalRgbGet, vexDeviceOpticalSatGet, vexDeviceOpticalStatusGet, + V5_DeviceOpticalGesture, V5_DeviceOpticalRaw, V5_DeviceOpticalRgb, }; use super::{SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; @@ -121,13 +125,14 @@ impl OpticalSensor { Ok(unsafe { vexDeviceOpticalBrightnessGet(self.device_handle()) }) } - /// Get the detected proximity value + /// Get the analog proximity value from `0` to `1.0`. /// - /// Proximity has a range of `0` to `255`. - pub fn proximity(&self) -> Result { + /// A reading of 1.0 indicates that the object is close to the sensor, while 0.0 + /// indicates that no object is detected in range of the sensor. + pub fn proximity(&self) -> Result { self.validate_port()?; - Ok(unsafe { vexDeviceOpticalProximityGet(self.device_handle()) }) + Ok(unsafe { vexDeviceOpticalProximityGet(self.device_handle()) } as f64 / 255.0) } /// Set the sensor's proximity threshold. @@ -220,9 +225,7 @@ impl OpticalSensor { pub fn status(&self) -> Result { self.validate_port()?; - Ok(unsafe { - vexDeviceOpticalStatusGet(self.device_handle()) - }) + Ok(unsafe { vexDeviceOpticalStatusGet(self.device_handle()) }) } } From 652dfb6a45644845c68f57909b873eb801078b5b Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Mon, 25 Mar 2024 12:13:52 -0500 Subject: [PATCH 09/14] remove untested vexDeviceOpticalProximityThreshold binding --- packages/vex-devices/src/smart/optical.rs | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/packages/vex-devices/src/smart/optical.rs b/packages/vex-devices/src/smart/optical.rs index 081e3c59..2df02a5b 100644 --- a/packages/vex-devices/src/smart/optical.rs +++ b/packages/vex-devices/src/smart/optical.rs @@ -135,18 +135,6 @@ impl OpticalSensor { Ok(unsafe { vexDeviceOpticalProximityGet(self.device_handle()) } as f64 / 255.0) } - /// Set the sensor's proximity threshold. - /// - /// Range and units are currently undocumented in the SDK. - /// TODO: Test on hardware. - pub fn set_proximity_threshold(&self, proximity: i32) -> Result<(), OpticalError> { - self.validate_port()?; - - unsafe { vexDeviceOpticalProximityThreshold(self.device_handle(), proximity) } - - Ok(()) - } - /// Get the processed RGB data from the sensor pub fn rgb(&self) -> Result { self.validate_port(); From 5b0975cae61de1aaa617f62de45d7d289e2b0752 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Tue, 26 Mar 2024 21:33:16 -0500 Subject: [PATCH 10/14] refactor: rotation sensor support, --- packages/vex-devices/Cargo.toml | 2 +- packages/vex-devices/src/battery.rs | 2 +- packages/vex-devices/src/competition.rs | 2 +- packages/vex-devices/src/smart/distance.rs | 2 +- packages/vex-devices/src/smart/imu.rs | 392 +++++++++------------ packages/vex-devices/src/smart/mod.rs | 39 +- packages/vex-devices/src/smart/optical.rs | 12 +- packages/vex-devices/src/smart/rotation.rs | 115 +++--- packages/vex-devices/src/usd.rs | 2 +- 9 files changed, 268 insertions(+), 300 deletions(-) diff --git a/packages/vex-devices/Cargo.toml b/packages/vex-devices/Cargo.toml index 1d6ad7da..090ebcb0 100644 --- a/packages/vex-devices/Cargo.toml +++ b/packages/vex-devices/Cargo.toml @@ -22,7 +22,7 @@ authors = [ [dependencies] pros-core = { version = "0.1.0", path = "../pros-core" } pros-sys = { path = "../pros-sys", version = "0.7.0", features = ["xapi"] } -vex_sys = { git = "https://github.com/pros-rs/vex-sys/" } +vex-sdk = { version = "0.1.0" } snafu = { version = "0.8.0", default-features = false, features = [ "rust_1_61", "unstable-core-error", diff --git a/packages/vex-devices/src/battery.rs b/packages/vex-devices/src/battery.rs index 7e7ee32c..7a18a40c 100644 --- a/packages/vex-devices/src/battery.rs +++ b/packages/vex-devices/src/battery.rs @@ -1,6 +1,6 @@ //! Utilites for getting information about the robot's battery. -use vex_sys::{ +use vex_sdk::{ vexBatteryCapacityGet, vexBatteryCurrentGet, vexBatteryTemperatureGet, vexBatteryVoltageGet, }; diff --git a/packages/vex-devices/src/competition.rs b/packages/vex-devices/src/competition.rs index e211a0de..f0562b5b 100644 --- a/packages/vex-devices/src/competition.rs +++ b/packages/vex-devices/src/competition.rs @@ -1,7 +1,7 @@ //! Utilities for getting competition control state. use bitflags::bitflags; -use vex_sys::vexCompetitionStatus; +use vex_sdk::vexCompetitionStatus; bitflags! { /// The status bits returned by [`competition::state`]. diff --git a/packages/vex-devices/src/smart/distance.rs b/packages/vex-devices/src/smart/distance.rs index 2fe3857a..613c88d3 100644 --- a/packages/vex-devices/src/smart/distance.rs +++ b/packages/vex-devices/src/smart/distance.rs @@ -4,7 +4,7 @@ use pros_core::error::PortError; use snafu::Snafu; -use vex_sys::{ +use vex_sdk::{ vexDeviceDistanceConfidenceGet, vexDeviceDistanceDistanceGet, vexDeviceDistanceObjectSizeGet, vexDeviceDistanceObjectVelocityGet, vexDeviceDistanceStatusGet, }; diff --git a/packages/vex-devices/src/smart/imu.rs b/packages/vex-devices/src/smart/imu.rs index ce6d54bd..ed635e5a 100644 --- a/packages/vex-devices/src/smart/imu.rs +++ b/packages/vex-devices/src/smart/imu.rs @@ -7,16 +7,16 @@ use core::{ }; use bitflags::bitflags; -use pros_core::{ - bail_on, - error::{take_errno, PortError}, - time::Instant, -}; -use pros_sys::{PROS_ERR, PROS_ERR_F}; +use pros_core::{error::PortError, time::Instant}; use snafu::Snafu; -use vex_sys::{vexDeviceImuDegreesGet, vexDeviceImuHeadingGet, vexDeviceImuStatusGet}; +use vex_sdk::{ + vexDeviceGetByIndex, vexDeviceImuAttitudeGet, vexDeviceImuDataRateSet, vexDeviceImuDegreesGet, + vexDeviceImuHeadingGet, vexDeviceImuQuaternionGet, vexDeviceImuRawAccelGet, + vexDeviceImuRawGyroGet, vexDeviceImuReset, vexDeviceImuStatusGet, V5_DeviceImuAttitude, + V5_DeviceImuQuaternion, V5_DeviceImuRaw, +}; -use super::{SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; +use super::{validate_port, SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; /// Represents a smart port configured as a V5 inertial sensor (IMU) #[derive(Debug, PartialEq)] @@ -28,12 +28,18 @@ pub struct InertialSensor { } impl InertialSensor { - /// The timeout for the IMU to calibrate. + /// The time limit used by the PROS kernel for bailing out of calibration. In theory, this + /// could be as low as 2s, but is kept at 3s for margin-of-error. + /// + /// pub const CALIBRATION_TIMEOUT: Duration = Duration::from_secs(3); /// The minimum data rate that you can set an IMU to. pub const MIN_DATA_RATE: Duration = Duration::from_millis(5); + /// The maximum value that can be returned by [`Self::heading`]. + pub const MAX_HEADING: f64 = 360.0; + /// Create a new inertial sensor from a smart port index. pub const fn new(port: SmartPort) -> Self { Self { @@ -48,11 +54,25 @@ impl InertialSensor { /// calibrating. fn validate(&self) -> Result<(), InertialError> { if self.is_calibrating()? { - return Err(InertialError::Calibrating); + return Err(InertialError::StillCalibrating); } Ok(()) } + /// Read the inertial sensor's status code. + pub fn status(&self) -> Result { + self.validate_port()?; + + Ok(InertialStatus::from_bits_retain(unsafe { + vexDeviceImuStatusGet(self.device_handle()) + })) + } + + /// Check if the Intertial Sensor is currently calibrating. + pub fn is_calibrating(&mut self) -> Result { + Ok(self.status()?.contains(InertialStatus::CALIBRATING)) + } + /// Calibrate IMU asynchronously. /// /// Returns an [`InertialCalibrateFuture`] that is be polled until the IMU status flag reports the sensor as @@ -62,144 +82,123 @@ impl InertialSensor { InertialCalibrateFuture::Calibrate(self.port.index()) } - /// Check if the Intertial Sensor is currently calibrating. - pub fn is_calibrating(&mut self) -> Result { - Ok(self.status()?.contains(InertialStatus::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, /// while counterclockwise rotations are represented with negative ones. pub fn rotation(&self) -> Result { self.validate()?; - // TODO: Test vexDeviceImuDegreesGet - Ok(unsafe { vexDeviceImuDegreesGet(self.device_handle()) } + self.rotation_offset) + Ok(unsafe { vexDeviceImuHeadingGet(self.device_handle()) } - self.rotation_offset) } - /// Get the Inertial Sensor’s heading relative to the initial direction of its x-axis. + /// Get the Inertial Sensor’s yaw angle bounded by [0, 360) degrees. /// - /// This value is bounded by [0, 360) degrees. Clockwise rotations are represented with positive degree values, - /// while counterclockwise rotations are represented with negative ones. + /// Clockwise rotations are represented with positive degree values, while counterclockwise rotations are + /// represented with negative ones. pub fn heading(&self) -> Result { self.validate()?; - // TODO: test vexDeviceImuHeadingGet - Ok(unsafe { vexDeviceImuHeadingGet(self.device_handle()) } + self.heading_offset) + Ok( + (unsafe { vexDeviceImuDegreesGet(self.device_handle()) } - self.heading_offset) + % Self::MAX_HEADING, + ) } - /// Get the Inertial Sensor’s yaw angle bounded by (-180, 180) degrees. - pub fn yaw(&self) -> Result { - Ok(self.euler()?.yaw) - } + /// Get a quaternion representing the Inertial Sensor’s orientation. + pub fn quaternion(&self) -> Result { + self.validate()?; - /// Get the Inertial Sensor’s pitch angle bounded by (-180, 180) degrees. - pub fn pitch(&self) -> Result { - Ok(self.euler()?.pitch) - } + let mut data = V5_DeviceImuQuaternion::default(); + unsafe { + vexDeviceImuQuaternionGet(self.device_handle(), &mut data); + } - /// Get the Inertial Sensor’s roll angle bounded by (-180, 180) degrees. - pub fn roll(&self) -> Result { - Ok(self.euler()?.roll) + Ok(data.into()) } - /// Read the inertial sensor's status code. - pub fn status(&self) -> Result { - self.validate_port()?; - - Ok(InertialStatus::from_bits_retain(unsafe { - vexDeviceImuStatusGet(self.device_handle()) - })) - } + /// Get the Euler angles (yaw, pitch, roll) representing the Inertial Sensor’s orientation. + pub fn euler(&self) -> Result { + self.validate()?; - /// Get a quaternion representing the Inertial Sensor’s orientation. - pub fn quaternion(&self) -> Result { - unsafe { pros_sys::imu_get_quaternion(self.port.index()).try_into() } - } + let mut data = V5_DeviceImuAttitude::default(); + unsafe { + vexDeviceImuAttitudeGet(self.device_handle(), &mut data); + } - /// Get the Euler angles representing the Inertial Sensor’s orientation. - pub fn euler(&self) -> Result { - unsafe { pros_sys::imu_get_euler(self.port.index()).try_into() } + Ok(data.into()) } /// Get the Inertial Sensor’s raw gyroscope values. pub fn gyro_rate(&self) -> Result { - unsafe { pros_sys::imu_get_gyro_rate(self.port.index()).try_into() } + self.validate()?; + + let mut data = V5_DeviceImuRaw::default(); + unsafe { + vexDeviceImuRawGyroGet(self.device_handle(), &mut data); + } + + Ok(data.into()) } /// Get the Inertial Sensor’s raw accelerometer values. pub fn accel(&self) -> Result { - unsafe { pros_sys::imu_get_accel(self.port.index()).try_into() } - } - - /// Resets the current reading of the Inertial Sensor’s heading to zero. - pub fn zero_heading(&mut self) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_tare_heading(self.port.index()) - }); - Ok(()) - } + self.validate()?; - /// Resets the current reading of the Inertial Sensor’s rotation to zero. - pub fn zero_rotation(&mut self) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_tare_rotation(self.port.index()) - }); - Ok(()) - } + let mut data = V5_DeviceImuRaw::default(); + unsafe { + vexDeviceImuRawAccelGet(self.device_handle(), &mut data); + } - /// Resets the current reading of the Inertial Sensor’s pitch to zero. - pub fn zero_pitch(&mut self) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_tare_pitch(self.port.index()) - }); - Ok(()) + Ok(data.into()) } - /// Resets the current reading of the Inertial Sensor’s roll to zero. - pub fn zero_roll(&mut self) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_tare_roll(self.port.index()) - }); - Ok(()) + /// Resets the current reading of the Inertial Sensor’s heading to zero. + pub fn reset_heading(&mut self) -> Result<(), InertialError> { + self.set_heading(Default::default()) } - /// Resets the current reading of the Inertial Sensor’s yaw to zero. - pub fn zero_yaw(&mut self) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_tare_yaw(self.port.index()) - }); - Ok(()) + /// Resets the current reading of the Inertial Sensor’s rotation to zero. + pub fn reset_rotation(&mut self) -> Result<(), InertialError> { + self.set_rotation(Default::default()) } /// Reset all 3 euler values of the Inertial Sensor to 0. - pub fn zero_euler(&mut self) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_tare_euler(self.port.index()) - }); - Ok(()) + pub fn reset_euler(&mut self) -> Result<(), InertialError> { + self.set_euler(Default::default()) } - /// Resets all 5 values of the Inertial Sensor to 0. - pub fn zero(&mut self) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { pros_sys::imu_tare(self.port.index()) }); + /// Resets all values of the Inertial Sensor to 0. + pub fn reset(&mut self) -> Result<(), InertialError> { + self.reset_heading()?; + self.reset_rotation()?; + self.reset_euler()?; + 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(&mut self, euler: Euler) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_set_euler(self.port.index(), euler.into()) - }); + self.validate()?; + + let mut attitude = V5_DeviceImuAttitude::default(); + unsafe { + vexDeviceImuAttitudeGet(self.device_handle(), &mut attitude); + } + + self.euler_offset = Euler { + yaw: euler.yaw - attitude.yaw, + pitch: euler.pitch - attitude.pitch, + roll: euler.roll - attitude.roll, + }; + Ok(()) } /// Sets the current reading of the Inertial Sensor’s rotation to target value. pub fn set_rotation(&mut self, rotation: f64) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_set_rotation(self.port.index(), rotation) - }); + self.validate()?; + + self.rotation_offset = rotation - unsafe { vexDeviceImuHeadingGet(self.device_handle()) }; + Ok(()) } @@ -207,39 +206,10 @@ impl InertialSensor { /// /// Target will default to 360 if above 360 and default to 0 if below 0. pub fn set_heading(&mut self, heading: f64) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_set_heading(self.port.index(), 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(&mut self, pitch: f64) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_set_pitch(self.port.index(), pitch) - }); - Ok(()) - } + self.validate()?; - /// 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(&mut self, roll: f64) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_set_roll(self.port.index(), roll) - }); - Ok(()) - } + self.heading_offset = heading - unsafe { vexDeviceImuDegreesGet(self.device_handle()) }; - /// 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(&mut self, yaw: f64) -> Result<(), InertialError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::imu_set_yaw(self.port.index(), yaw) - }); Ok(()) } @@ -247,22 +217,11 @@ impl InertialSensor { /// /// This duration must be above [`Self::MIN_DATA_RATE`] (5 milliseconds). pub fn set_data_rate(&mut self, data_rate: Duration) -> Result<(), InertialError> { - unsafe { - let rate_ms = if data_rate > Self::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); - }; - - bail_on!( - PROS_ERR, - pros_sys::imu_set_data_rate(self.port.index(), rate_ms) - ); - } + self.validate()?; + + let time_ms = data_rate.as_millis().max(Self::MIN_DATA_RATE.as_millis()) as u32; + unsafe { vexDeviceImuDataRateSet(self.device_handle(), time_ms) } + Ok(()) } } @@ -294,26 +253,13 @@ pub struct Quaternion { pub w: f64, } -impl TryFrom for Quaternion { - type Error = InertialError; - - fn try_from(value: pros_sys::quaternion_s_t) -> Result { - Ok(Self { - x: bail_on!(PROS_ERR_F, value.x), - y: value.y, - z: value.z, - w: value.w, - }) - } -} - -impl From for pros_sys::quaternion_s_t { - fn from(value: Quaternion) -> Self { - pros_sys::quaternion_s_t { - x: value.x, - y: value.y, - z: value.z, - w: value.w, +impl From for Quaternion { + fn from(value: V5_DeviceImuQuaternion) -> Self { + Self { + x: value.a, + y: value.b, + z: value.c, + w: value.d, } } } @@ -331,24 +277,12 @@ pub struct Euler { pub yaw: f64, } -impl TryFrom for Euler { - type Error = InertialError; - - fn try_from(value: pros_sys::euler_s_t) -> Result { - Ok(Self { - pitch: bail_on!(PROS_ERR_F, value.pitch), +impl From for Euler { + fn from(value: V5_DeviceImuAttitude) -> Self { + Self { + pitch: value.pitch, roll: value.roll, yaw: value.yaw, - }) - } -} - -impl From for pros_sys::euler_s_t { - fn from(val: Euler) -> Self { - pros_sys::euler_s_t { - pitch: val.pitch, - roll: val.roll, - yaw: val.yaw, } } } @@ -369,15 +303,13 @@ pub struct InertialRaw { pub z: f64, } -impl TryFrom for InertialRaw { - type Error = InertialError; - - fn try_from(value: pros_sys::imu_raw_s) -> Result { - Ok(Self { - x: bail_on!(PROS_ERR_F, value.x), +impl From for InertialRaw { + fn from(value: V5_DeviceImuRaw) -> Self { + Self { + x: value.x, y: value.y, z: value.z, - }) + } } } @@ -390,14 +322,26 @@ bitflags! { } } -#[derive(Debug, Clone, Copy)] +/// Defines a waiting phase in [`InertialCalibrateFuture`]. +#[derive(Debug, Clone, Copy, Eq, PartialEq)] +pub enum CalibrationPhase { + /// Future is currently waiting for the IMU to report [`InertialStatus::CALIBRATING`], indicating + /// that it has started calibration. + Start, + + /// Waiting for calibration to end. + End, +} + /// Future that calibrates an IMU /// created with [`InertialSensor::calibrate`]. +#[derive(Debug, Clone, Copy)] pub enum InertialCalibrateFuture { /// Calibrate the IMU Calibrate(u8), - /// Wait for the IMU to finish calibrating - Waiting(u8, Instant), + /// Wait for the IMU to either begin calibrating or end calibration, depending on the + /// designated [`CalibrationPhase`]. + Waiting(u8, Instant, CalibrationPhase), } impl core::future::Future for InertialCalibrateFuture { @@ -405,37 +349,49 @@ impl core::future::Future for InertialCalibrateFuture { fn poll(mut self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll { match *self { - Self::Calibrate(port) => match unsafe { pros_sys::imu_reset(port) } { - PROS_ERR => { - let errno = take_errno(); - Poll::Ready(Err(InertialError::from_errno(errno) - .unwrap_or_else(|| panic!("Unknown errno code {errno}")))) - } - _ => { - *self = Self::Waiting(port, Instant::now()); + Self::Calibrate(port) => { + if let Err(err) = validate_port(port, SmartDeviceType::Imu) { + // IMU isn't plugged in, no need to go any further. + Poll::Ready(Err(InertialError::Port { source: err })) + } else { + // Request that vexos calibrate the IMU, and transition to pending state. + unsafe { vexDeviceImuReset(vexDeviceGetByIndex((port - 1) as u32)) } + + // Change to waiting for calibration to start. + *self = Self::Waiting(port, Instant::now(), CalibrationPhase::Start); cx.waker().wake_by_ref(); Poll::Pending } }, - Self::Waiting(port, timestamp) => { - let is_calibrating = match unsafe { pros_sys::imu_get_status(port) } { - pros_sys::E_IMU_STATUS_ERROR => { - let errno = take_errno(); - return Poll::Ready(Err(InertialError::from_errno(take_errno()) - .unwrap_or_else(|| panic!("Unknown errno code {errno}")))); - } - value => (value & pros_sys::E_IMU_STATUS_CALIBRATING) != 0, - }; - - if !is_calibrating { - return Poll::Ready(Ok(())); - } else if timestamp.elapsed() > InertialSensor::CALIBRATION_TIMEOUT { + Self::Waiting(port, timestamp, phase) => { + if timestamp.elapsed() > InertialSensor::CALIBRATION_TIMEOUT { + // Calibration took too long and exceeded timeout. return Poll::Ready(Err(InertialError::CalibrationTimedOut)); } + let status = InertialStatus::from_bits_retain( + if let Err(err) = validate_port(port, SmartDeviceType::Imu) { + // IMU got unplugged, so we'll resolve early. + return Poll::Ready(Err(InertialError::Port { source: err })); + } else { + // Get status flags from vexos. + unsafe { vexDeviceImuStatusGet(vexDeviceGetByIndex((port - 1) as u32)) } + }, + ); + + if status.contains(InertialStatus::CALIBRATING) && phase == CalibrationPhase::Start { + // Calibration has started, so we'll change to waiting for it to end. + *self = Self::Waiting(port, timestamp, CalibrationPhase::End); + cx.waker().wake_by_ref(); + return Poll::Pending; + } else if !status.contains(InertialStatus::CALIBRATING) && phase == CalibrationPhase::End { + // Calibration has finished. + return Poll::Ready(Ok(())); + } + cx.waker().wake_by_ref(); Poll::Pending - } + }, } } } @@ -443,12 +399,10 @@ impl core::future::Future for InertialCalibrateFuture { #[derive(Debug, Snafu)] /// Errors that can occur when interacting with an Inertial Sensor. pub enum InertialError { - /// The inertial sensor spent too long calibrating. + /// The inertial sensor took longer than three seconds to calibrate. CalibrationTimedOut, /// The inertial is still calibrating. - Calibrating, - /// Invalid sensor data rate, expected >= 5 milliseconds. - InvalidDataRate, + StillCalibrating, #[snafu(display("{source}"), context(false))] /// Generic port related error. Port { diff --git a/packages/vex-devices/src/smart/mod.rs b/packages/vex-devices/src/smart/mod.rs index 8d66dc7b..bb368f6a 100644 --- a/packages/vex-devices/src/smart/mod.rs +++ b/packages/vex-devices/src/smart/mod.rs @@ -39,7 +39,7 @@ pub use motor::Motor; pub use optical::OpticalSensor; use pros_core::error::PortError; pub use rotation::RotationSensor; -use vex_sys::{vexDeviceGetByIndex, vexDeviceGetTimestamp, V5_DeviceT, V5_DeviceType}; +use vex_sdk::{vexDeviceGetByIndex, vexDeviceGetTimestamp, V5_DeviceT, V5_DeviceType}; pub use vision::VisionSensor; /// Defines common functionality shared by all smart port devices. @@ -107,19 +107,24 @@ pub(crate) trait SmartDeviceInternal: SmartDevice { /// Verify that the device type is currently plugged into this port. fn validate_port(&self) -> Result<(), PortError> { - let device = unsafe { *self.device_handle() }; - let plugged_type: SmartDeviceType = device.device_type.into(); - - if !device.exists { - // No device is plugged into the port. - return Err(PortError::Disconnected); - } else if plugged_type != self.device_type() { - // The connected device doesn't match the requested type. - return Err(PortError::IncorrectDevice); - } + validate_port(self.port_index(), self.device_type()) + } +} - Ok(()) +/// Verify that the device type is currently plugged into this port. +pub(crate) fn validate_port(index: u8, device_type: SmartDeviceType) -> Result<(), PortError> { + let device = unsafe { *vexDeviceGetByIndex((index - 1) as u32) }; + let plugged_type: SmartDeviceType = device.device_type.into(); + + if !device.exists { + // No device is plugged into the port. + return Err(PortError::Disconnected); + } else if plugged_type != device_type { + // The connected device doesn't match the requested type. + return Err(PortError::IncorrectDevice); } + + Ok(()) } impl SmartDeviceInternal for T {} @@ -247,15 +252,6 @@ impl From for SmartDeviceType { match value { V5_DeviceType::kDeviceTypeNoSensor => Self::None, V5_DeviceType::kDeviceTypeMotorSensor => Self::Motor, - // TODO: - // I'm not entirely sure that this conversion is correct. - // This behavior is taken from the PROS kernel, which treats - // the rotation sensor as kDeviceTypeAbsEncSensor, however the - // SDK also uses the "absEnc" terminology to represent ADI - // encoders which is an entirely different thing. - // - // Not sure what else this value would be, but it still needs - // hardware testing V5_DeviceType::kDeviceTypeAbsEncSensor => Self::Rotation, V5_DeviceType::kDeviceTypeImuSensor => Self::Imu, V5_DeviceType::kDeviceTypeDistanceSensor => Self::Distance, @@ -279,7 +275,6 @@ impl From for V5_DeviceType { match value { SmartDeviceType::None => V5_DeviceType::kDeviceTypeNoSensor, SmartDeviceType::Motor => V5_DeviceType::kDeviceTypeMotorSensor, - // TODO: See comment in the conversion impl above this one. Same deal. SmartDeviceType::Rotation => V5_DeviceType::kDeviceTypeAbsEncSensor, SmartDeviceType::Imu => V5_DeviceType::kDeviceTypeImuSensor, SmartDeviceType::Distance => V5_DeviceType::kDeviceTypeDistanceSensor, diff --git a/packages/vex-devices/src/smart/optical.rs b/packages/vex-devices/src/smart/optical.rs index 2df02a5b..7152fb11 100644 --- a/packages/vex-devices/src/smart/optical.rs +++ b/packages/vex-devices/src/smart/optical.rs @@ -4,13 +4,13 @@ use core::time::Duration; use pros_core::error::PortError; use snafu::Snafu; -use vex_sys::{ +use vex_sdk::{ vexDeviceOpticalBrightnessGet, vexDeviceOpticalGestureDisable, vexDeviceOpticalGestureEnable, vexDeviceOpticalGestureGet, vexDeviceOpticalHueGet, vexDeviceOpticalIntegrationTimeGet, vexDeviceOpticalIntegrationTimeSet, vexDeviceOpticalLedPwmGet, vexDeviceOpticalLedPwmSet, - vexDeviceOpticalProximityGet, vexDeviceOpticalProximityThreshold, vexDeviceOpticalRawGet, - vexDeviceOpticalRgbGet, vexDeviceOpticalSatGet, vexDeviceOpticalStatusGet, - V5_DeviceOpticalGesture, V5_DeviceOpticalRaw, V5_DeviceOpticalRgb, + vexDeviceOpticalProximityGet, vexDeviceOpticalRawGet, vexDeviceOpticalRgbGet, + vexDeviceOpticalSatGet, vexDeviceOpticalStatusGet, V5_DeviceOpticalGesture, + V5_DeviceOpticalRaw, V5_DeviceOpticalRgb, }; use super::{SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; @@ -88,12 +88,12 @@ impl OpticalSensor { pub fn set_integration_time(&mut self, time: Duration) -> Result<(), OpticalError> { self.validate_port()?; - let time = time.as_millis().clamp( + let time_ms = time.as_millis().clamp( Self::MIN_INTEGRATION_TIME.as_millis(), Self::MAX_INTEGRATION_TIME.as_millis(), ) as f64; - unsafe { vexDeviceOpticalIntegrationTimeSet(self.device_handle(), time) } + unsafe { vexDeviceOpticalIntegrationTimeSet(self.device_handle(), time_ms) } Ok(()) } diff --git a/packages/vex-devices/src/smart/rotation.rs b/packages/vex-devices/src/smart/rotation.rs index 2988af2a..d4a4ad09 100644 --- a/packages/vex-devices/src/smart/rotation.rs +++ b/packages/vex-devices/src/smart/rotation.rs @@ -2,88 +2,107 @@ //! //! Rotation sensors operate on the same [`Position`] type as motors to measure rotation. -use pros_core::{bail_on, error::PortError}; -use pros_sys::PROS_ERR; +use pros_core::error::PortError; +use vex_sdk::{ + vexDeviceAbsEncAngleGet, vexDeviceAbsEncPositionGet, vexDeviceAbsEncPositionSet, + vexDeviceAbsEncReset, vexDeviceAbsEncReverseFlagGet, vexDeviceAbsEncReverseFlagSet, + vexDeviceAbsEncStatusGet, vexDeviceAbsEncVelocityGet, +}; -use super::{SmartDevice, SmartDeviceType, SmartPort}; +use super::{motor::Direction, SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; use crate::position::Position; /// A physical rotation sensor plugged into a port. #[derive(Debug, Eq, PartialEq)] pub struct RotationSensor { port: SmartPort, - /// Whether or not the sensor direction is reversed. - pub reversed: bool, } impl RotationSensor { /// Creates a new rotation sensor on the given port. /// Whether or not the sensor should be reversed on creation can be specified. - pub fn new(port: SmartPort, reversed: bool) -> Result { - unsafe { - bail_on!(PROS_ERR, pros_sys::rotation_reset_position(port.index())); - if reversed { - bail_on!( - PROS_ERR, - pros_sys::rotation_set_reversed(port.index(), true) - ); - } - } + pub fn new(port: SmartPort, direction: Direction) -> Result { + let mut sensor = Self { port }; + + sensor.reset()?; + sensor.set_direction(direction)?; - Ok(Self { port, reversed }) + Ok(sensor) } /// Sets the position to zero. - pub fn zero(&mut self) -> Result<(), PortError> { + pub fn reset(&mut self) -> Result<(), PortError> { + self.validate_port()?; + unsafe { - bail_on!( - PROS_ERR, - pros_sys::rotation_reset_position(self.port.index()) - ); + vexDeviceAbsEncReset(self.device_handle()); } + Ok(()) } /// Sets the position. pub fn set_position(&mut self, position: Position) -> Result<(), PortError> { - unsafe { - bail_on!( - PROS_ERR, - pros_sys::rotation_set_position( - self.port.index(), - (position.into_counts() * 100) as _ - ) - ); - } + self.validate_port()?; + + unsafe { vexDeviceAbsEncPositionSet(self.device_handle(), position.into_degrees() as i32) } + Ok(()) } /// Sets whether or not the rotation sensor should be reversed. - pub fn set_reversed(&mut self, reversed: bool) -> Result<(), PortError> { - self.reversed = reversed; + pub fn set_direction(&mut self, direction: Direction) -> Result<(), PortError> { + self.validate_port()?; + + unsafe { vexDeviceAbsEncReverseFlagSet(self.device_handle(), direction.is_reverse()) } - unsafe { - bail_on!( - PROS_ERR, - pros_sys::rotation_set_reversed(self.port.index(), reversed) - ); - } Ok(()) } - /// Reverses the rotation sensor. - pub fn reverse(&mut self) -> Result<(), PortError> { - self.set_reversed(!self.reversed) + /// Sets whether or not the rotation sensor should be reversed. + pub fn direction(&self) -> Result { + self.validate_port()?; + + Ok( + match unsafe { vexDeviceAbsEncReverseFlagGet(self.device_handle()) } { + false => Direction::Forward, + true => Direction::Reverse, + }, + ) } - //TODO: See if this is accurate enough or consider switching to get_position function. - /// Gets the current position of the sensor. + /// Get the total number of degrees rotated by the sensor based on direction. pub fn position(&self) -> Result { - Ok(unsafe { - Position::from_degrees( - bail_on!(PROS_ERR, pros_sys::rotation_get_angle(self.port.index())) as f64 / 100.0, - ) - }) + self.validate_port()?; + + Ok(Position::from_degrees( + unsafe { vexDeviceAbsEncPositionGet(self.device_handle()) } as f64 / 100.0, + )) + } + + /// Get the angle of rotation measured by the sensor. + /// + /// This value is reported from 0-360 degrees. + pub fn angle(&self) -> Result { + self.validate_port()?; + + Ok(Position::from_degrees( + unsafe { vexDeviceAbsEncAngleGet(self.device_handle()) } as f64 / 100.0, + )) + } + + /// Get the sensor's current velocity in degrees per second + pub fn velocity(&self) -> Result { + self.validate_port()?; + + Ok(unsafe { vexDeviceAbsEncVelocityGet(self.device_handle()) as f64 / 1000.0 }) + } + + /// Returns the sensor's status code. + pub fn status(&self) -> Result { + self.validate_port()?; + + Ok(unsafe { vexDeviceAbsEncStatusGet(self.device_handle()) }) } } diff --git a/packages/vex-devices/src/usd.rs b/packages/vex-devices/src/usd.rs index a3d969ac..27cc735a 100644 --- a/packages/vex-devices/src/usd.rs +++ b/packages/vex-devices/src/usd.rs @@ -2,7 +2,7 @@ //! //! The USD API provides functions for interacting with the SD card slot on the V5 Brain. -use vex_sys::vexFileDriveStatus; +use vex_sdk::vexFileDriveStatus; /// Checks if an SD card is installed. pub fn usd_installed() -> bool { From 3572720a922dd82138c37b25600346c77646ac85 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Tue, 26 Mar 2024 22:52:47 -0500 Subject: [PATCH 11/14] refactor: motors --- packages/vex-devices/src/smart/motor.rs | 401 +++++++++++------------- 1 file changed, 185 insertions(+), 216 deletions(-) diff --git a/packages/vex-devices/src/smart/motor.rs b/packages/vex-devices/src/smart/motor.rs index ffcb1e2f..296e7709 100644 --- a/packages/vex-devices/src/smart/motor.rs +++ b/packages/vex-devices/src/smart/motor.rs @@ -3,11 +3,18 @@ use core::time::Duration; use bitflags::bitflags; -use pros_core::{bail_on, error::PortError, map_errno}; -use pros_sys::{PROS_ERR, PROS_ERR_F}; +use pros_core::error::PortError; use snafu::Snafu; +use vex_sdk::{ + vexDeviceMotorAbsoluteTargetSet, vexDeviceMotorBrakeModeSet, vexDeviceMotorCurrentGet, vexDeviceMotorCurrentLimitGet, vexDeviceMotorCurrentLimitSet, vexDeviceMotorEfficiencyGet, vexDeviceMotorEncoderUnitsSet, vexDeviceMotorFaultsGet, vexDeviceMotorFlagsGet, vexDeviceMotorGearingGet, vexDeviceMotorGearingSet, vexDeviceMotorPositionGet, vexDeviceMotorPositionRawGet, vexDeviceMotorPositionReset, vexDeviceMotorPositionSet, vexDeviceMotorPowerGet, vexDeviceMotorReverseFlagGet, vexDeviceMotorReverseFlagSet, vexDeviceMotorTemperatureGet, vexDeviceMotorTorqueGet, vexDeviceMotorVelocityGet, vexDeviceMotorVelocitySet, vexDeviceMotorVelocityUpdate, vexDeviceMotorVoltageGet, vexDeviceMotorVoltageLimitGet, vexDeviceMotorVoltageLimitSet, vexDeviceMotorVoltageSet, V5MotorBrakeMode, V5MotorGearset +}; -use super::{SmartDevice, SmartDeviceTimestamp, SmartDeviceType, SmartPort}; +#[cfg(feature = "dangerous_motor_tuning")] +use vex_sdk::{ + vexDeviceMotorPositionPidSet, vexDeviceMotorVelocityPidSet, V5_DeviceMotorPid +}; + +use super::{SmartDevice, SmartDeviceInternal, SmartDeviceTimestamp, SmartDeviceType, SmartPort}; use crate::Position; /// The basic motor struct. @@ -77,10 +84,6 @@ impl Motor { gearset: Gearset, direction: Direction, ) -> Result { - bail_on!(PROS_ERR, unsafe { - pros_sys::motor_set_encoder_units(port.index() as i8, pros_sys::E_MOTOR_ENCODER_DEGREES) - }); - let mut motor = Self { port, target: MotorControl::Voltage(0.0), @@ -89,6 +92,13 @@ impl Motor { motor.set_gearset(gearset)?; motor.set_direction(direction)?; + unsafe { + vexDeviceMotorEncoderUnitsSet( + motor.device_handle(), + vex_sdk::V5MotorEncoderUnits::kMotorEncoderDegrees, + ); + } + Ok(motor) } @@ -96,52 +106,41 @@ impl Motor { /// /// This could be a voltage, velocity, position, or even brake mode. pub fn set_target(&mut self, target: MotorControl) -> Result<(), MotorError> { + self.validate_port()?; + self.target = target; + match target { MotorControl::Brake(mode) => unsafe { - bail_on!( - PROS_ERR, - pros_sys::motor_set_brake_mode(self.port.index() as i8, mode.into()) - ); - bail_on!(PROS_ERR, pros_sys::motor_brake(self.port.index() as i8)); + vexDeviceMotorBrakeModeSet(self.device_handle(), mode.into()); + vexDeviceMotorVelocitySet(self.device_handle(), 0); }, MotorControl::Velocity(rpm) => unsafe { - bail_on!( - PROS_ERR, - pros_sys::motor_set_brake_mode( - self.port.index() as i8, - pros_sys::E_MOTOR_BRAKE_COAST - ) + vexDeviceMotorBrakeModeSet( + self.device_handle(), + vex_sdk::V5MotorBrakeMode::kV5MotorBrakeModeCoast, ); - bail_on!( - PROS_ERR, - pros_sys::motor_move_velocity(self.port.index() as i8, rpm) + vexDeviceMotorVelocitySet(self.device_handle(), rpm); + }, + MotorControl::Voltage(volts) => unsafe { + vexDeviceMotorBrakeModeSet( + self.device_handle(), + vex_sdk::V5MotorBrakeMode::kV5MotorBrakeModeCoast, ); + vexDeviceMotorVoltageSet(self.device_handle(), (volts * 1000.0) as i32); }, - MotorControl::Voltage(volts) => { - bail_on!(PROS_ERR, unsafe { - pros_sys::motor_move_voltage(self.port.index() as i8, (volts * 1000.0) as i32) - }); - } MotorControl::Position(position, velocity) => unsafe { - bail_on!( - PROS_ERR, - pros_sys::motor_set_brake_mode( - self.port.index() as i8, - pros_sys::E_MOTOR_BRAKE_COAST - ) + vexDeviceMotorBrakeModeSet( + self.device_handle(), + vex_sdk::V5MotorBrakeMode::kV5MotorBrakeModeCoast, ); - bail_on!( - PROS_ERR, - pros_sys::motor_move_absolute( - self.port.index() as i8, - position.into_degrees(), - velocity, - ) + vexDeviceMotorAbsoluteTargetSet( + self.device_handle(), + position.into_degrees(), + velocity, ); }, } - self.target = target; Ok(()) } @@ -180,9 +179,11 @@ impl Motor { /// /// This will have no effect if the motor is not following a profiled movement. pub fn update_profiled_velocity(&mut self, velocity: i32) -> Result<(), MotorError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::motor_modify_profiled_velocity(self.port.index() as i8, velocity) - }); + self.validate_port()?; + + unsafe { + vexDeviceMotorVelocityUpdate(self.device_handle(), velocity); + } match self.target { MotorControl::Position(position, _) => { @@ -195,83 +196,74 @@ impl Motor { } /// Get the current [`MotorControl`] value that the motor is attempting to use. - pub fn target(&self) -> MotorControl { - self.target + pub fn target(&self) -> Result { + self.validate_port()?; + Ok(self.target) } /// Sets the gearset of the motor. pub fn set_gearset(&mut self, gearset: Gearset) -> Result<(), MotorError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::motor_set_gearing(self.port.index() as i8, gearset as i32) - }); + self.validate_port()?; + unsafe { + vexDeviceMotorGearingSet(self.device_handle(), gearset.into()); + } Ok(()) } /// Gets the gearset of the motor. pub fn gearset(&self) -> Result { - unsafe { pros_sys::motor_get_gearing(self.port.index() as i8).try_into() } + self.validate_port()?; + Ok(unsafe { vexDeviceMotorGearingGet(self.device_handle()) }.into()) } /// Gets the estimated angular velocity (RPM) of the motor. - pub fn velocity(&self) -> Result { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::motor_get_actual_velocity(self.port.index() as i8) - })) + pub fn velocity(&self) -> Result { + self.validate_port()?; + Ok(unsafe { vexDeviceMotorVelocityGet(self.device_handle()) }) } /// Returns the power drawn by the motor in Watts. pub fn power(&self) -> Result { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::motor_get_power(self.port.index() as i8) - })) + self.validate_port()?; + Ok(unsafe { vexDeviceMotorPowerGet(self.device_handle()) }) } /// Returns the torque output of the motor in Nm. pub fn torque(&self) -> Result { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::motor_get_torque(self.port.index() as i8) - })) + self.validate_port()?; + Ok(unsafe { vexDeviceMotorTorqueGet(self.device_handle()) }) } /// Returns the voltage the motor is drawing in volts. pub fn voltage(&self) -> Result { - // docs say this function returns PROS_ERR_F but it actually returns PROS_ERR - let millivolts = bail_on!(PROS_ERR, unsafe { - pros_sys::motor_get_voltage(self.port.index() as i8) - }); - Ok(millivolts as f64 / 1000.0) + self.validate_port()?; + Ok(unsafe { vexDeviceMotorVoltageGet(self.device_handle()) } as f64 / 1000.0) } /// Returns the current position of the motor. pub fn position(&self) -> Result { - Ok(Position::from_degrees(bail_on!(PROS_ERR_F, unsafe { - pros_sys::motor_get_position(self.port.index() as i8) - }))) + self.validate_port()?; + Ok(Position::from_degrees(unsafe { + vexDeviceMotorPositionGet(self.device_handle()) + })) } /// Returns the most recently recorded raw encoder tick data from the motor's IME /// along with a timestamp of the internal clock of the motor indicating when the /// data was recorded. pub fn raw_position(&self) -> Result<(i32, SmartDeviceTimestamp), MotorError> { - let timestamp = 0 as *mut u32; + self.validate_port()?; - // PROS docs claim that this function gets the position *at* a recorded timestamp, - // but in reality the "timestamp" paramater is a mutable outvalue. The function - // outputs the most recent recorded posision AND the timestamp it was measured at, - // rather than a position at a requested timestamp. - let ticks = bail_on!(PROS_ERR, unsafe { - pros_sys::motor_get_raw_position(self.port.index() as i8, timestamp) - }); + let mut timestamp = 0 as *mut u32; + let ticks = unsafe { vexDeviceMotorPositionRawGet(self.device_handle(), timestamp) }; Ok((ticks, SmartDeviceTimestamp(unsafe { *timestamp }))) } /// Returns the electrical current draw of the motor in amps. pub fn current(&self) -> Result { - Ok(bail_on!(PROS_ERR, unsafe { - pros_sys::motor_get_current_draw(self.port.index() as i8) - }) as f64 - / 1000.0) + self.validate_port()?; + Ok(unsafe { vexDeviceMotorCurrentGet(self.device_handle()) } as f64 / 1000.0) } /// Gets the efficiency of the motor from a range of [0.0, 1.0]. @@ -280,77 +272,68 @@ impl Motor { /// drawing no electrical power, and an efficiency of 0.0 means that the motor /// is drawing power but not moving. pub fn efficiency(&self) -> Result { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::motor_get_efficiency(self.port.index() as i8) - }) / 100.0) + self.validate_port()?; + + Ok(unsafe { vexDeviceMotorEfficiencyGet(self.device_handle()) } / 100.0) } /// Sets the current encoder position to zero without moving the motor. /// Analogous to taring or resetting the encoder to the current position. - pub fn zero(&mut self) -> Result<(), MotorError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::motor_tare_position(self.port.index() as i8) - }); + pub fn reset_position(&mut self) -> Result<(), MotorError> { + self.validate_port()?; + unsafe { vexDeviceMotorPositionReset(self.device_handle()) } Ok(()) } /// Sets the current encoder position to the given position without moving the motor. /// Analogous to taring or resetting the encoder so that the new position is equal to the given position. pub fn set_position(&mut self, position: Position) -> Result<(), MotorError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::motor_set_zero_position(self.port.index() as i8, position.into_degrees()) - }); + self.validate_port()?; + unsafe { vexDeviceMotorPositionSet(self.device_handle(), position.into_degrees()) } Ok(()) } /// Sets the current limit for the motor in amps. pub fn set_current_limit(&mut self, limit: f64) -> Result<(), MotorError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::motor_set_current_limit(self.port.index() as i8, (limit * 1000.0) as i32) - }); + self.validate_port()?; + unsafe { vexDeviceMotorCurrentLimitSet(self.device_handle(), (limit * 1000.0) as i32) } Ok(()) } /// Sets the voltage limit for the motor in volts. pub fn set_voltage_limit(&mut self, limit: f64) -> Result<(), MotorError> { - bail_on!(PROS_ERR, unsafe { - // Docs claim that this function takes volts, but this is incorrect. It takes millivolts, - // just like all other SDK voltage-related functions. - pros_sys::motor_set_voltage_limit(self.port.index() as i8, (limit * 1000.0) as i32) - }); + self.validate_port()?; + + unsafe { + vexDeviceMotorVoltageLimitSet(self.device_handle(), (limit * 1000.0) as i32); + } Ok(()) } /// Gets the current limit for the motor in amps. pub fn current_limit(&self) -> Result { - Ok(bail_on!(PROS_ERR, unsafe { - pros_sys::motor_get_current_limit(self.port.index() as i8) - }) as f64 - / 1000.0) - } - - // /// Gets the voltage limit for the motor if one has been explicitly set. - // /// NOTE: Broken until next kernel version due to mutex release bug. - // pub fn voltage_limit(&self) -> Result { - // // NOTE: PROS docs claim that this function will return zero if voltage is uncapped. - // // - // // From testing this does not appear to be true, so we don't need to perform any - // // special checks for a zero return value. - // Ok(bail_on!(PROS_ERR, unsafe { - // pros_sys::motor_get_voltage_limit(self.port.index() as i8) - // }) as f64 - // / 1000.0) - // } + self.validate_port()?; + Ok(unsafe { vexDeviceMotorCurrentLimitGet(self.device_handle()) } as f64 / 1000.0) + } + + /// Gets the voltage limit for the motor if one has been explicitly set. + pub fn voltage_limit(&self) -> Result { + self.validate_port()?; + Ok(unsafe { vexDeviceMotorVoltageLimitGet(self.device_handle()) } as f64 / 1000.0) + } + + pub fn temperature(&self) -> Result { + self.validate_port()?; + Ok(unsafe { vexDeviceMotorTemperatureGet(self.device_handle()) }) + } /// Get the status flags of a motor. pub fn status(&self) -> Result { - let bits = bail_on!(PROS_ERR as u32, unsafe { - pros_sys::motor_get_flags(self.port.index() as i8) - }); + self.validate_port()?; + + let bits = unsafe { vexDeviceMotorFlagsGet(self.device_handle()) }; - // For some reason, PROS doesn't set errno if this flag is returned, - // even though it is by-definition an error (failing to retrieve flags). if (bits & pros_sys::E_MOTOR_FLAGS_BUSY) != 0 { return Err(MotorError::Busy); } @@ -360,11 +343,11 @@ impl Motor { /// Get the fault flags of the motor. pub fn faults(&self) -> Result { - let bits = bail_on!(PROS_ERR as u32, unsafe { - pros_sys::motor_get_faults(self.port.index() as i8) - }); + self.validate_port(); - Ok(MotorFaults::from_bits_retain(bits)) + Ok(MotorFaults::from_bits_retain(unsafe { + vexDeviceMotorFaultsGet(self.device_handle()) + })) } /// Check if the motor's over temperature flag is set. @@ -389,22 +372,25 @@ impl Motor { /// Set the [`Direction`] of this motor. pub fn set_direction(&mut self, direction: Direction) -> Result<(), MotorError> { - bail_on!(PROS_ERR, unsafe { - pros_sys::motor_set_reversed(self.port.index() as i8, direction.is_reverse()) - }); + self.validate_port()?; + + unsafe { + vexDeviceMotorReverseFlagSet(self.device_handle(), direction.is_reverse()); + } + Ok(()) } /// Get the [`Direction`] of this motor. pub fn direction(&self) -> Result { - let reversed = bail_on!(PROS_ERR, unsafe { - pros_sys::motor_is_reversed(self.port.index() as i8) - }) == 1; + self.validate_port()?; - Ok(match reversed { - false => Direction::Forward, - true => Direction::Reverse, - }) + Ok( + match unsafe { vexDeviceMotorReverseFlagGet(self.device_handle()) } { + false => Direction::Forward, + true => Direction::Reverse, + }, + ) } /// Adjusts the internal tuning constants of the motor when using velocity control. @@ -423,10 +409,10 @@ impl Motor { &mut self, constants: MotorTuningConstants, ) -> Result<(), MotorError> { - bail_on!(PROS_ERR, unsafe { - #[allow(deprecated)] - pros_sys::motor_set_pos_pid_full(self.port.index() as i8, constants.into()) - }); + self.validate_port()?; + + unsafe { vexDeviceMotorVelocityPidSet(self.device_handle(), constants.into()) } + Ok(()) } @@ -446,10 +432,10 @@ impl Motor { &mut self, constants: MotorTuningConstants, ) -> Result<(), MotorError> { - bail_on!(PROS_ERR, unsafe { - #[allow(deprecated)] - pros_sys::motor_set_vel_pid_full(self.port.index() as i8, constants.into()) - }); + self.validate_port()?; + + unsafe { vexDeviceMotorPositionPidSet(self.device_handle(), constants.into()) } + Ok(()) } } @@ -466,34 +452,34 @@ impl SmartDevice for Motor { /// Determines how a motor should act when braking. #[derive(Debug, Clone, Copy, Eq, PartialEq)] -#[repr(i32)] pub enum BrakeMode { /// Motor never brakes. - None = pros_sys::E_MOTOR_BRAKE_COAST, + Coast, + /// Motor uses regenerative braking to slow down faster. - Brake = pros_sys::E_MOTOR_BRAKE_BRAKE, + Brake, + /// Motor exerts force to hold the same position. - Hold = pros_sys::E_MOTOR_BRAKE_HOLD, + Hold, } -impl TryFrom for BrakeMode { - type Error = MotorError; - - fn try_from(value: pros_sys::motor_brake_mode_e_t) -> Result { - bail_on!(PROS_ERR, value); - - Ok(match value { - pros_sys::E_MOTOR_BRAKE_COAST => Self::None, - pros_sys::E_MOTOR_BRAKE_BRAKE => Self::Brake, - pros_sys::E_MOTOR_BRAKE_HOLD => Self::Hold, - _ => unreachable!(), - }) +impl From for BrakeMode { + fn from(value: V5MotorBrakeMode) -> Self { + match value { + V5MotorBrakeMode::kV5MotorBrakeModeBrake => Self::Brake, + V5MotorBrakeMode::kV5MotorBrakeModeCoast => Self::Coast, + V5MotorBrakeMode::kV5MotorBrakeModeHold => Self::Hold, + } } } -impl From for pros_sys::motor_brake_mode_e_t { - fn from(value: BrakeMode) -> pros_sys::motor_brake_mode_e_t { - value as _ +impl From for V5MotorBrakeMode { + fn from(value: BrakeMode) -> Self { + match value { + BrakeMode::Brake => Self::kV5MotorBrakeModeBrake, + BrakeMode::Coast => Self::kV5MotorBrakeModeCoast, + BrakeMode::Hold => Self::kV5MotorBrakeModeHold, + } } } @@ -502,16 +488,16 @@ bitflags! { #[derive(Debug, Clone, Copy, Eq, PartialEq)] pub struct MotorFaults: u32 { /// The motor's temperature is above its limit. - const OVER_TEMPERATURE = pros_sys::E_MOTOR_FAULT_MOTOR_OVER_TEMP; + const OVER_TEMPERATURE = 0x01; /// The motor is over current. - const OVER_CURRENT = pros_sys::E_MOTOR_FAULT_OVER_CURRENT; + const OVER_CURRENT = 0x04; /// The motor's H-bridge has encountered a fault. - const DRIVER_FAULT = pros_sys::E_MOTOR_FAULT_DRIVER_FAULT; + const DRIVER_FAULT = 0x02; /// The motor's H-bridge is over current. - const DRIVER_OVER_CURRENT = pros_sys::E_MOTOR_FAULT_DRV_OVER_CURRENT; + const DRIVER_OVER_CURRENT = 0x08 ; } } @@ -519,35 +505,34 @@ bitflags! { /// The status bits returned by a [`Motor`]. #[derive(Debug, Clone, Copy, Eq, PartialEq)] pub struct MotorStatus: u32 { + /// Failed communicate with the motor + const BUSY = 0x01; + /// The motor is currently near zero velocity. #[deprecated( since = "0.9.0", note = "This flag will never be set by the hardware, even though it exists in the SDK. This may change in the future." )] - const ZERO_VELOCITY = pros_sys::E_MOTOR_FLAGS_ZERO_VELOCITY; + const ZERO_VELOCITY = 0x02; /// The motor is at its zero position. #[deprecated( since = "0.9.0", note = "This flag will never be set by the hardware, even though it exists in the SDK. This may change in the future." )] - const ZERO_POSITION = pros_sys::E_MOTOR_FLAGS_ZERO_POSITION; - - /// Cannot currently communicate to the motor - const BUSY = pros_sys::E_MOTOR_FLAGS_BUSY; + const ZERO_POSITION = 0x04; } } /// Internal gearset used by VEX smart motors. #[derive(Debug, Clone, Copy, PartialEq, Eq)] -#[repr(i32)] pub enum Gearset { /// 36:1 gear ratio - Red = pros_sys::E_MOTOR_GEAR_RED, + Red, /// 18:1 gear ratio - Green = pros_sys::E_MOTOR_GEAR_GREEN, + Green, /// 6:1 gear ratio - Blue = pros_sys::E_MOTOR_GEAR_BLUE, + Blue, } impl Gearset { @@ -598,24 +583,23 @@ impl Gearset { } } -impl From for pros_sys::motor_gearset_e_t { - fn from(value: Gearset) -> Self { - value as _ +impl From for Gearset { + fn from(value: V5MotorGearset) -> Self { + match value { + V5MotorGearset::kMotorGearSet_06 => Self::Blue, + V5MotorGearset::kMotorGearSet_18 => Self::Green, + V5MotorGearset::kMotorGearSet_36 => Self::Red, + } } } -impl TryFrom for Gearset { - type Error = MotorError; - - fn try_from(value: pros_sys::motor_gearset_e_t) -> Result { - bail_on!(PROS_ERR, value); - - Ok(match value { - pros_sys::E_MOTOR_GEAR_RED => Self::Red, - pros_sys::E_MOTOR_GEAR_GREEN => Self::Green, - pros_sys::E_MOTOR_GEAR_BLUE => Self::Blue, - _ => unreachable!(), - }) +impl From for V5MotorGearset { + fn from(value: Gearset) -> Self { + match value { + Gearset::Blue => Self::kMotorGearSet_06, + Gearset::Green => Self::kMotorGearSet_18, + Gearset::Red => Self::kMotorGearSet_36, + } } } @@ -630,7 +614,7 @@ impl TryFrom for Gearset { /// has no plans to do so. As such, the units and finer details of [`MotorTuningConstants`] are not /// well-known or understood, as we have no reference for what these constants should look /// like. -#[cfg(feature = "dangerous_motor_tuning")] +// #[cfg(feature = "dangerous_motor_tuning")] #[derive(Debug, Clone, Copy, PartialEq)] pub struct MotorTuningConstants { /// The feedforward constant. @@ -663,22 +647,18 @@ pub struct MotorTuningConstants { } #[cfg(feature = "dangerous_motor_tuning")] -impl From for pros_sys::motor_pid_full_s_t { +impl From for V5_DeviceMotorPid { fn from(value: MotorTuningConstants) -> Self { - unsafe { - // Docs incorrectly claim that this function can set errno. - // It can't. . - #[allow(deprecated)] - pros_sys::motor_convert_pid_full( - value.kf, - value.kp, - value.ki, - value.kd, - value.filter, - value.limit, - value.tolerance, - value.sample_rate.as_millis() as f64, - ) + Self { + kf: (value.kf * 16.0) as u8, + kp: (value.kp * 16.0) as u8, + ki: (value.ki * 16.0) as u8, + kd: (value.kd * 16.0) as u8, + filter: (value.filter * 16.0) as u8, + limit: (value.integral_limit * 16.0) as u16, + threshold: (value.tolerance * 16.0) as u8, + loopspeed: (value.sample_rate.as_millis() * 16) as u8, + ..Default::default() } } } @@ -689,10 +669,6 @@ pub enum MotorError { /// Failed to communicate with the motor while attempting to read flags. Busy, - /// This functionality is not currently implemented in hardware, even - /// though the SDK may support it. - NotImplemented, - /// Generic port related error. #[snafu(display("{source}"), context(false))] Port { @@ -700,10 +676,3 @@ pub enum MotorError { source: PortError, }, } - -map_errno! { - MotorError { - ENOSYS => Self::NotImplemented, - } - inherit PortError; -} From 817158a5290b95f9e11a34680ec4da8c478a8cfd Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Tue, 26 Mar 2024 23:17:09 -0500 Subject: [PATCH 12/14] docs: document `Motor::temperature` --- packages/vex-devices/src/smart/imu.rs | 11 +++++++---- packages/vex-devices/src/smart/motor.rs | 20 +++++++++++++++----- 2 files changed, 22 insertions(+), 9 deletions(-) diff --git a/packages/vex-devices/src/smart/imu.rs b/packages/vex-devices/src/smart/imu.rs index ed635e5a..e06189ca 100644 --- a/packages/vex-devices/src/smart/imu.rs +++ b/packages/vex-devices/src/smart/imu.rs @@ -362,7 +362,7 @@ impl core::future::Future for InertialCalibrateFuture { cx.waker().wake_by_ref(); Poll::Pending } - }, + } Self::Waiting(port, timestamp, phase) => { if timestamp.elapsed() > InertialSensor::CALIBRATION_TIMEOUT { // Calibration took too long and exceeded timeout. @@ -379,19 +379,22 @@ impl core::future::Future for InertialCalibrateFuture { }, ); - if status.contains(InertialStatus::CALIBRATING) && phase == CalibrationPhase::Start { + if status.contains(InertialStatus::CALIBRATING) && phase == CalibrationPhase::Start + { // Calibration has started, so we'll change to waiting for it to end. *self = Self::Waiting(port, timestamp, CalibrationPhase::End); cx.waker().wake_by_ref(); return Poll::Pending; - } else if !status.contains(InertialStatus::CALIBRATING) && phase == CalibrationPhase::End { + } else if !status.contains(InertialStatus::CALIBRATING) + && phase == CalibrationPhase::End + { // Calibration has finished. return Poll::Ready(Ok(())); } cx.waker().wake_by_ref(); Poll::Pending - }, + } } } } diff --git a/packages/vex-devices/src/smart/motor.rs b/packages/vex-devices/src/smart/motor.rs index 296e7709..5851103c 100644 --- a/packages/vex-devices/src/smart/motor.rs +++ b/packages/vex-devices/src/smart/motor.rs @@ -6,13 +6,19 @@ use bitflags::bitflags; use pros_core::error::PortError; use snafu::Snafu; use vex_sdk::{ - vexDeviceMotorAbsoluteTargetSet, vexDeviceMotorBrakeModeSet, vexDeviceMotorCurrentGet, vexDeviceMotorCurrentLimitGet, vexDeviceMotorCurrentLimitSet, vexDeviceMotorEfficiencyGet, vexDeviceMotorEncoderUnitsSet, vexDeviceMotorFaultsGet, vexDeviceMotorFlagsGet, vexDeviceMotorGearingGet, vexDeviceMotorGearingSet, vexDeviceMotorPositionGet, vexDeviceMotorPositionRawGet, vexDeviceMotorPositionReset, vexDeviceMotorPositionSet, vexDeviceMotorPowerGet, vexDeviceMotorReverseFlagGet, vexDeviceMotorReverseFlagSet, vexDeviceMotorTemperatureGet, vexDeviceMotorTorqueGet, vexDeviceMotorVelocityGet, vexDeviceMotorVelocitySet, vexDeviceMotorVelocityUpdate, vexDeviceMotorVoltageGet, vexDeviceMotorVoltageLimitGet, vexDeviceMotorVoltageLimitSet, vexDeviceMotorVoltageSet, V5MotorBrakeMode, V5MotorGearset + vexDeviceMotorAbsoluteTargetSet, vexDeviceMotorBrakeModeSet, vexDeviceMotorCurrentGet, + vexDeviceMotorCurrentLimitGet, vexDeviceMotorCurrentLimitSet, vexDeviceMotorEfficiencyGet, + vexDeviceMotorEncoderUnitsSet, vexDeviceMotorFaultsGet, vexDeviceMotorFlagsGet, + vexDeviceMotorGearingGet, vexDeviceMotorGearingSet, vexDeviceMotorPositionGet, + vexDeviceMotorPositionRawGet, vexDeviceMotorPositionReset, vexDeviceMotorPositionSet, + vexDeviceMotorPowerGet, vexDeviceMotorReverseFlagGet, vexDeviceMotorReverseFlagSet, + vexDeviceMotorTemperatureGet, vexDeviceMotorTorqueGet, vexDeviceMotorVelocityGet, + vexDeviceMotorVelocitySet, vexDeviceMotorVelocityUpdate, vexDeviceMotorVoltageGet, + vexDeviceMotorVoltageLimitGet, vexDeviceMotorVoltageLimitSet, vexDeviceMotorVoltageSet, + V5MotorBrakeMode, V5MotorGearset, }; - #[cfg(feature = "dangerous_motor_tuning")] -use vex_sdk::{ - vexDeviceMotorPositionPidSet, vexDeviceMotorVelocityPidSet, V5_DeviceMotorPid -}; +use vex_sdk::{vexDeviceMotorPositionPidSet, vexDeviceMotorVelocityPidSet, V5_DeviceMotorPid}; use super::{SmartDevice, SmartDeviceInternal, SmartDeviceTimestamp, SmartDeviceType, SmartPort}; use crate::Position; @@ -112,6 +118,7 @@ impl Motor { match target { MotorControl::Brake(mode) => unsafe { vexDeviceMotorBrakeModeSet(self.device_handle(), mode.into()); + // Force motor into braking by putting it into velocity control with a 0rpm setpoint. vexDeviceMotorVelocitySet(self.device_handle(), 0); }, MotorControl::Velocity(rpm) => unsafe { @@ -323,6 +330,7 @@ impl Motor { Ok(unsafe { vexDeviceMotorVoltageLimitGet(self.device_handle()) } as f64 / 1000.0) } + /// Returns the internal teperature recorded by the motor in increments of 5°C. pub fn temperature(&self) -> Result { self.validate_port()?; Ok(unsafe { vexDeviceMotorTemperatureGet(self.device_handle()) }) @@ -334,6 +342,8 @@ impl Motor { let bits = unsafe { vexDeviceMotorFlagsGet(self.device_handle()) }; + // This is technically just a flag, but it indicates that an error occurred when trying + // to get the flags, so we return early here. if (bits & pros_sys::E_MOTOR_FLAGS_BUSY) != 0 { return Err(MotorError::Busy); } From c2682cd39af6f07047403c07537941f4b49f17ed Mon Sep 17 00:00:00 2001 From: Gavin-Niederman Date: Tue, 26 Mar 2024 22:56:01 -0700 Subject: [PATCH 13/14] refactor: implement controllers in terms of vex-sdk --- packages/vex-devices/src/controller.rs | 649 +++++++++++++------------ 1 file changed, 349 insertions(+), 300 deletions(-) diff --git a/packages/vex-devices/src/controller.rs b/packages/vex-devices/src/controller.rs index ba06b0b8..268eea42 100644 --- a/packages/vex-devices/src/controller.rs +++ b/packages/vex-devices/src/controller.rs @@ -2,349 +2,398 @@ //! //! Controllers are identified by their id, which is either 0 (master) or 1 (partner). //! State of a controller can be checked by calling [`Controller::state`] which will return a struct with all of the buttons' and joysticks' state. +use alloc::ffi::CString; -use alloc::{ffi::CString, vec::Vec}; - -use pros_core::{bail_on, map_errno}; -use pros_sys::{controller_id_e_t, PROS_ERR}; use snafu::Snafu; +use vex_sdk::{vexControllerConnectionStatusGet, vexControllerGet, vexControllerTextSet, V5_ControllerId, V5_ControllerIndex}; + +use crate::{ + adi::digital::LogicLevel, + competition::{self, CompetitionMode}, +}; + +fn controller_connected(id: ControllerId) -> bool { + unsafe { vexControllerConnectionStatusGet(id.into()) as u32 != 0 } +} + +/// Digital Controller Button +#[derive(Debug, Eq, PartialEq)] +pub struct Button { + id: ControllerId, + channel: V5_ControllerIndex, + last_level: LogicLevel, +} + +impl Button { + fn validate(&self) -> Result<(), ControllerError> { + if !controller_connected(self.id) { + return Err(ControllerError::NotConnected); + } + + Ok(()) + } + + /// Gets the current logic level of a digital input pin. + pub fn level(&self) -> Result { + self.validate()?; + if competition::mode() != CompetitionMode::Driver { + return Err(ControllerError::CompetitionControl); + } -/// Holds whether or not the buttons on the controller are pressed or not -#[derive(Default, Debug, Clone, Copy, Eq, PartialEq)] -pub struct Buttons { - /// The 'A' button on the right button pad of the controller. - pub a: bool, - /// The 'B' button on the right button pad of the controller. - pub b: bool, - /// The 'X' button on the right button pad of the controller. - pub x: bool, - /// The 'Y' button on the right button pad of the controller. - pub y: bool, - - /// The up arrow on the left arrow pad of the controller. - pub up: bool, - /// The down arrow on the left arrow pad of the controller. - pub down: bool, - /// The left arrow on the left arrow pad of the controller. - pub left: bool, - /// The right arrow on the left arrow pad of the controller. - pub right: bool, - /// The first trigger on the left side of the controller. - pub left_trigger_1: bool, - /// The second trigger on the left side of the controller. - pub left_trigger_2: bool, - /// The first trigger on the right side of the controller. - pub right_trigger_1: bool, - /// The second trigger on the right side of the controller. - pub right_trigger_2: bool, + let value = + unsafe { vexControllerGet(self.id.into(), self.channel.try_into().unwrap()) != 0 }; + + let level = match value { + true => LogicLevel::High, + false => LogicLevel::Low, + }; + self.last_level = level; + + Ok(level) + } + + /// Returrns `true` if the button is currently being pressed. + /// + /// This is equivalent shorthand to calling `Self::level().is_high()`. + pub fn is_pressed(&self) -> Result { + self.validate()?; + Ok(self.level()?.is_high()) + } + + /// Returns `true` if the button has been pressed again since the last time this + /// function was called. + /// + /// # Thread Safety + /// + /// This function is not thread-safe. + /// + /// Multiple tasks polling a single button may return different results under the + /// same circumstances, so only one task should call this function for any given + /// switch. E.g., Task A calls this function for buttons 1 and 2. Task B may call + /// this function for button 3, but should not for buttons 1 or 2. A typical + /// use-case for this function is to call inside opcontrol to detect new button + /// presses, and not in any other tasks. + pub fn was_pressed(&mut self) -> Result { + self.validate()?; + if competition::mode() != CompetitionMode::Driver { + return Err(ControllerError::CompetitionControl); + } + let current_level = self.level()?; + Ok(self.last_level.is_low() && current_level.is_high()) + } } /// Stores how far the joystick is away from the center (at *(0, 0)*) from -1 to 1. /// On the x axis left is negative, and right is positive. /// On the y axis down is negative, and up is positive. -#[derive(Default, Debug, Clone, Copy, PartialEq)] +#[derive(Debug, Eq, PartialEq)] pub struct Joystick { - /// Left and right x value of the joystick - pub x: f32, - /// Up and down y value of the joystick - pub y: f32, + id: ControllerId, + x_channel: V5_ControllerIndex, + y_channel: V5_ControllerIndex, } -/// Stores both joysticks on the controller. -#[derive(Debug, Clone, Copy, PartialEq)] -pub struct Joysticks { - /// Left joystick - pub left: Joystick, - /// Right joystick - pub right: Joystick, +impl Joystick { + fn validate(&self) -> Result<(), ControllerError> { + if !controller_connected(self.id) { + return Err(ControllerError::NotConnected); + } + + Ok(()) + } + + /// Gets the value of the joystick position on its x-axis from [-1, 1]. + pub fn x(&self) -> Result { + self.validate()?; + Ok(self.x_raw()? as f32 / 127.0) + } + + /// Gets the value of the joystick position on its y-axis from [-1, 1]. + pub fn y(&self) -> Result { + self.validate()?; + Ok(self.y_raw()? as f32 / 127.0) + } + + /// Gets the raw value of the joystick position on its x-axis from [-128, 127]. + pub fn x_raw(&self) -> Result { + self.validate()?; + if competition::mode() != CompetitionMode::Driver { + return Err(ControllerError::CompetitionControl); + } + + Ok(unsafe { vexControllerGet(self.id.into(), self.x_channel) } as _) + } + + /// Gets the raw value of the joystick position on its x-axis from [-128, 127]. + pub fn y_raw(&self) -> Result { + self.validate()?; + if competition::mode() != CompetitionMode::Driver { + return Err(ControllerError::CompetitionControl); + } + + Ok(unsafe { vexControllerGet(self.id.into(), self.y_channel) } as _) + } } -/// Stores the current state of the controller; the joysticks and buttons. -#[derive(Debug, Clone, Copy, PartialEq)] -pub struct ControllerState { - /// Analog joysticks state - pub joysticks: Joysticks, - /// Digital buttons state - pub buttons: Buttons, +/// The basic type for a controller. +/// Used to get the state of its joysticks and controllers. +#[derive(Debug, Eq, PartialEq)] +pub struct Controller { + id: ControllerId, + + /// Controller Screen + pub screen: ControllerScreen, + + /// Left Joystick + pub left_stick: Joystick, + /// Right Joystick + pub right_stick: Joystick, + + /// Button A + pub button_a: Button, + /// Button B + pub button_b: Button, + /// Button X + pub button_x: Button, + /// Button Y + pub button_y: Button, + + /// Button Up + pub button_up: Button, + /// Button Down + pub button_down: Button, + /// Button Left + pub button_left: Button, + /// Button Right + pub button_right: Button, + + /// Top Left Trigger + pub left_trigger_1: Button, + /// Bottom Left Trigger + pub left_trigger_2: Button, + /// Top Right Trigger + pub right_trigger_1: Button, + /// Bottom Right Trigger + pub right_trigger_2: Button, } -/// Represents one line on the controller console. -#[derive(Debug, Clone, Copy)] -pub struct ControllerLine { - controller: Controller, - line: u8, +/// Controller LCD Console +#[derive(Debug, Eq, PartialEq)] +pub struct ControllerScreen { + id: ControllerId, } -impl ControllerLine { - /// The maximum length that can fit in one line on the controllers display. - pub const MAX_TEXT_LEN: usize = 14; - /// The maximum line number that can be used on the controller display. - pub const MAX_LINE_NUM: u8 = 2; - - /// Attempts to print text to the controller display. - /// Returns an error if the text is too long to fit on the display or if an internal PROS error occured. - pub fn try_print(&self, text: impl Into>) -> Result<(), ControllerError> { - let text = text.into(); - let text_len = text.len(); - assert!( - text_len > ControllerLine::MAX_TEXT_LEN, - "Printed text is too long to fit on controller display ({text_len} > {})", - Self::MAX_TEXT_LEN - ); - let c_text = CString::new(text).expect("parameter `text` should not contain null bytes"); - bail_on!(PROS_ERR, unsafe { - pros_sys::controller_set_text(self.controller.id(), self.line, 0, c_text.as_ptr()) - }); +impl ControllerScreen { + /// Maximum number of characters that can be drawn to a text line. + pub const MAX_LINE_LENGTH: usize = 14; + + /// Number of available text lines on the controller before clearing the screen. + pub const MAX_LINES: usize = 2; + + fn validate(&self) -> Result<(), ControllerError> { + if !controller_connected(self.id) { + return Err(ControllerError::NotConnected); + } + Ok(()) } - /// Prints text to the controller display. - /// # Panics - /// Unlike [`ControllerLine::try_print`], - /// this function will panic if the text is too long to fit on the display - /// or if an internal PROS error occured. - pub fn print(&self, text: impl Into>) { - self.try_print(text).unwrap(); + + /// Clear the contents of a specific text line. + pub fn clear_line(&mut self, line: u8) -> Result<(), ControllerError> { + //TODO: Older versions of VexOS clear the controller by setting the line to " ". + //TODO: We should check the version and change behavior based on it. + self.set_text("", line, 0)?; + + Ok(()) } -} -/// A digital channel (button) on the VEX controller. -#[repr(u32)] -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub enum ControllerButton { - /// The 'A' button on the right button pad of the controller. - A = pros_sys::E_CONTROLLER_DIGITAL_A, - /// The 'B' button on the right button pad of the controller. - B = pros_sys::E_CONTROLLER_DIGITAL_B, - /// The 'X' button on the right button pad of the controller. - X = pros_sys::E_CONTROLLER_DIGITAL_X, - /// The 'Y' button on the right button pad of the controller. - Y = pros_sys::E_CONTROLLER_DIGITAL_Y, - /// The up arrow on the left arrow pad of the controller. - Up = pros_sys::E_CONTROLLER_DIGITAL_UP, - /// The down arrow on the left arrow pad of the controller. - Down = pros_sys::E_CONTROLLER_DIGITAL_DOWN, - /// The left arrow on the left arrow pad of the controller. - Left = pros_sys::E_CONTROLLER_DIGITAL_LEFT, - /// The right arrow on the left arrow pad of the controller. - Right = pros_sys::E_CONTROLLER_DIGITAL_RIGHT, - /// The first trigger on the left side of the controller. - LeftTrigger1 = pros_sys::E_CONTROLLER_DIGITAL_L1, - /// The second trigger on the left side of the controller. - LeftTrigger2 = pros_sys::E_CONTROLLER_DIGITAL_L2, - /// The first trigger on the right side of the controller. - RightTrigger1 = pros_sys::E_CONTROLLER_DIGITAL_R1, - /// The second trigger on the right side of the controller. - RightTrigger2 = pros_sys::E_CONTROLLER_DIGITAL_R2, + /// Clear the whole screen. + pub fn clear_screen(&mut self) -> Result<(), ControllerError> { + for line in 0..Self::MAX_LINES as u8 { + self.clear_line(line)?; + } + + Ok(()) + } + + /// Set the text contents at a specific row/column offset. + pub fn set_text(&mut self, text: &str, line: u8, col: u8) -> Result<(), ControllerError> { + self.validate()?; + if col >= Self::MAX_LINE_LENGTH as u8 { + return Err(ControllerError::InvalidLine); + } + + let id: V5_ControllerId = self.id.into(); + let text = CString::new(text).map_err(|_| ControllerError::NonTerminatingNull)?.into_raw(); + + unsafe { vexControllerTextSet(id as u32, (line + 1) as _, (col + 1) as _, text as *const _); } + + // stop rust from leaking the CString + drop(unsafe { CString::from_raw(text) }); + + Ok(()) + } } -/// An analog channel (joystick axis) on the VEX controller. -#[repr(u32)] +/// Represents an identifier for one of the two possible controllers +/// connected to the V5 brain. #[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub enum JoystickAxis { - /// Left (-1.0) and right (1.0) x axis of the left joystick - LeftX = pros_sys::E_CONTROLLER_ANALOG_LEFT_X, - /// Down (-1.0) and up (1.0) y axis of the left joystick - LeftY = pros_sys::E_CONTROLLER_ANALOG_LEFT_Y, - /// Left (-1.0) and right (1.0) x axis of the right joystick - RightX = pros_sys::E_CONTROLLER_ANALOG_RIGHT_X, - /// Down (-1.0) and up (1.0) y axis of the right joystick - RightY = pros_sys::E_CONTROLLER_ANALOG_RIGHT_Y, -} +pub enum ControllerId { + /// Primary ("Master") Controller + Primary, -/// The basic type for a controller. -/// Used to get the state of its joysticks and controllers. -#[repr(u32)] -#[derive(Debug, Clone, Copy, Default)] -pub enum Controller { - /// The master controller. Controllers default to this value. - #[default] - Master = pros_sys::E_CONTROLLER_MASTER, - /// The partner controller. - Partner = pros_sys::E_CONTROLLER_PARTNER, + /// Partner Controller + Partner, } -impl Controller { - const fn id(&self) -> controller_id_e_t { - *self as controller_id_e_t +impl From for V5_ControllerId { + fn from(id: ControllerId) -> Self { + match id { + ControllerId::Primary => V5_ControllerId::kControllerMaster, + ControllerId::Partner => V5_ControllerId::kControllerPartner, + } } +} - /// Returns a line on the controller display that can be used to print to the controller. - pub fn line(&self, line_num: u8) -> ControllerLine { - assert!( - line_num > ControllerLine::MAX_LINE_NUM, - "Line number is too large for controller display ({line_num} > {})", - ControllerLine::MAX_LINE_NUM - ); - - ControllerLine { - controller: *self, - line: line_num, +impl Controller { + fn validate(&self) -> Result<(), ControllerError> { + if !controller_connected(self.id) { + return Err(ControllerError::NotConnected); } + + Ok(()) } - /// Gets the current state of the controller in its entirety. - pub fn state(&self) -> Result { - Ok(ControllerState { - joysticks: unsafe { - Joysticks { - left: Joystick { - x: bail_on!( - PROS_ERR, - pros_sys::controller_get_analog( - self.id(), - pros_sys::E_CONTROLLER_ANALOG_LEFT_X, - ) - ) as f32 - / 127.0, - y: bail_on!( - PROS_ERR, - pros_sys::controller_get_analog( - self.id(), - pros_sys::E_CONTROLLER_ANALOG_LEFT_Y, - ) - ) as f32 - / 127.0, - }, - right: Joystick { - x: bail_on!( - PROS_ERR, - pros_sys::controller_get_analog( - self.id(), - pros_sys::E_CONTROLLER_ANALOG_RIGHT_X, - ) - ) as f32 - / 127.0, - y: bail_on!( - PROS_ERR, - pros_sys::controller_get_analog( - self.id(), - pros_sys::E_CONTROLLER_ANALOG_RIGHT_Y, - ) - ) as f32 - / 127.0, - }, - } + /// Create a new controller. + /// + /// # Safety + /// + /// Creating new `Controller`s is inherently unsafe due to the possibility of constructing + /// more than one screen at once allowing multiple mutable references to the same + /// hardware device. Prefer using [`Peripherals`](crate::peripherals::Peripherals) to register devices if possible. + pub const unsafe fn new(id: ControllerId) -> Self { + Self { + id, + screen: ControllerScreen { id }, + left_stick: Joystick { + id, + x_channel: V5_ControllerIndex::Axis1, + y_channel: V5_ControllerIndex::Axis2, + }, + right_stick: Joystick { + id, + x_channel: V5_ControllerIndex::Axis3, + y_channel: V5_ControllerIndex::Axis4, + }, + button_a: Button { + id, + channel: V5_ControllerIndex::ButtonA, + last_level: LogicLevel::Low, }, - buttons: unsafe { - Buttons { - a: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_A, - ) - ) == 1, - b: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_B, - ) - ) == 1, - x: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_X, - ) - ) == 1, - y: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_Y, - ) - ) == 1, - up: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_UP, - ) - ) == 1, - down: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_DOWN, - ) - ) == 1, - left: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_LEFT, - ) - ) == 1, - right: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_RIGHT, - ) - ) == 1, - left_trigger_1: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_L1, - ) - ) == 1, - left_trigger_2: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_L2, - ) - ) == 1, - right_trigger_1: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_R1, - ) - ) == 1, - right_trigger_2: bail_on!( - PROS_ERR, - pros_sys::controller_get_digital( - self.id(), - pros_sys::E_CONTROLLER_DIGITAL_R2, - ) - ) == 1, - } + button_b: Button { + id, + channel: V5_ControllerIndex::ButtonB, + last_level: LogicLevel::Low, }, + button_x: Button { + id, + channel: V5_ControllerIndex::ButtonX, + last_level: LogicLevel::Low, + }, + button_y: Button { + id, + channel: V5_ControllerIndex::ButtonY, + last_level: LogicLevel::Low, + }, + button_up: Button { + id, + channel: V5_ControllerIndex::ButtonUp, + last_level: LogicLevel::Low, + }, + button_down: Button { + id, + channel: V5_ControllerIndex::ButtonDown, + last_level: LogicLevel::Low, + }, + button_left: Button { + id, + channel: V5_ControllerIndex::ButtonLeft, + last_level: LogicLevel::Low, + }, + button_right: Button { + id, + channel: V5_ControllerIndex::ButtonRight, + last_level: LogicLevel::Low, + }, + left_trigger_1: Button { + id, + channel: V5_ControllerIndex::ButtonL1, + last_level: LogicLevel::Low, + }, + left_trigger_2: Button { + id, + channel: V5_ControllerIndex::ButtonL2, + last_level: LogicLevel::Low, + }, + right_trigger_1: Button { + id, + channel: V5_ControllerIndex::ButtonR1, + last_level: LogicLevel::Low, + }, + right_trigger_2: Button { + id, + channel: V5_ControllerIndex::ButtonR2, + last_level: LogicLevel::Low, + }, + } + } + + /// Returns `true` if the controller is connected to the brain. + pub fn is_connected(&self) -> bool { + controller_connected(self.id) + } + + /// Gets the controller's battery capacity. + pub fn battery_capacity(&self) -> Result { + self.validate()?; + + Ok(unsafe { + vexControllerGet(self.id.into(), V5_ControllerIndex::BatteryCapacity) }) } - /// Gets the state of a specific button on the controller. - pub fn button(&self, button: ControllerButton) -> Result { - Ok(bail_on!(PROS_ERR, unsafe { - pros_sys::controller_get_digital(self.id(), button as pros_sys::controller_digital_e_t) - }) == 1) + /// Gets the controller's battery level. + pub fn battery_level(&self) -> Result { + self.validate()?; + + Ok(unsafe { + vexControllerGet(self.id.into(), V5_ControllerIndex::BatteryLevel) + }) } - /// Gets the state of a specific joystick axis on the controller. - pub fn joystick_axis(&self, axis: JoystickAxis) -> Result { - Ok(bail_on!(PROS_ERR, unsafe { - pros_sys::controller_get_analog(self.id(), axis as pros_sys::controller_analog_e_t) - }) as f32 - / 127.0) + /// Send a rumble pattern to the controller's vibration motor. + /// + /// This function takes a string consisting of the characters '.', '-', and ' ', where + /// dots are short rumbles, dashes are long rumbles, and spaces are pauses. Maximum + /// supported length is 8 characters. + pub fn rumble(&mut self, pattern: &str) -> Result<(), ControllerError> { + self.validate()?; + + self.screen.set_text(pattern, 3, 0); + + Ok(()) } } #[derive(Debug, Snafu)] /// Errors that can occur when interacting with the controller. pub enum ControllerError { - /// The controller ID given was invalid, expected E_CONTROLLER_MASTER or E_CONTROLLER_PARTNER. - InvalidControllerId, - - /// Another resource is already using the controller. - ConcurrentAccess, -} - -map_errno! { - ControllerError { - EACCES => Self::ConcurrentAccess, - EINVAL => Self::InvalidControllerId, - } + /// The controller is not connected to the brain. + NotConnected, + /// CString::new encountered NULL (U+0000) byte in non-terminating position. + NonTerminatingNull, + /// Access to controller data is restricted by competition control. + CompetitionControl, + /// An invalid line number was given. + InvalidLine, } From dcea0cb68c703a49df2c7d00b4ed3e08e9b785c1 Mon Sep 17 00:00:00 2001 From: Tropical <42101043+Tropix126@users.noreply.github.com> Date: Wed, 27 Mar 2024 20:18:07 -0500 Subject: [PATCH 14/14] feat: IMU orientation getter --- packages/vex-devices/src/smart/imu.rs | 70 +++++++++++++++++++++++++-- 1 file changed, 65 insertions(+), 5 deletions(-) diff --git a/packages/vex-devices/src/smart/imu.rs b/packages/vex-devices/src/smart/imu.rs index e06189ca..e0a421c6 100644 --- a/packages/vex-devices/src/smart/imu.rs +++ b/packages/vex-devices/src/smart/imu.rs @@ -10,10 +10,7 @@ use bitflags::bitflags; use pros_core::{error::PortError, time::Instant}; use snafu::Snafu; use vex_sdk::{ - vexDeviceGetByIndex, vexDeviceImuAttitudeGet, vexDeviceImuDataRateSet, vexDeviceImuDegreesGet, - vexDeviceImuHeadingGet, vexDeviceImuQuaternionGet, vexDeviceImuRawAccelGet, - vexDeviceImuRawGyroGet, vexDeviceImuReset, vexDeviceImuStatusGet, V5_DeviceImuAttitude, - V5_DeviceImuQuaternion, V5_DeviceImuRaw, + vexDeviceGetByIndex, vexDeviceImuAttitudeGet, vexDeviceImuDataRateSet, vexDeviceImuDegreesGet, vexDeviceImuHeadingGet, vexDeviceImuQuaternionGet, vexDeviceImuRawAccelGet, vexDeviceImuRawGyroGet, vexDeviceImuReset, vexDeviceImuStatusGet, V5ImuOrientationMode, V5_DeviceImuAttitude, V5_DeviceImuQuaternion, V5_DeviceImuRaw }; use super::{validate_port, SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; @@ -73,6 +70,16 @@ impl InertialSensor { Ok(self.status()?.contains(InertialStatus::CALIBRATING)) } + /// Check if the Intertial Sensor was calibrated using auto-calibration. + pub fn is_auto_calibrated(&mut self) -> Result { + Ok(self.status()?.contains(InertialStatus::AUTO_CALIBRTED)) + } + + /// Check if the Intertial Sensor was calibrated using auto-calibration. + pub fn physical_orientation(&mut self) -> Result { + Ok(self.status()?.physical_orientation()) + } + /// Calibrate IMU asynchronously. /// /// Returns an [`InertialCalibrateFuture`] that is be polled until the IMU status flag reports the sensor as @@ -313,12 +320,65 @@ impl From for InertialRaw { } } +/// Represents one of six possible physical IMU orientations relative +/// to the earth's center of gravity. +#[derive(Debug, Clone, Copy, Eq, PartialEq)] +pub enum InertialOrientation { + /// Z-Axis facing up (VEX logo facing DOWN). + ZUp, + + /// Z-Axis facing down (VEX logo facing UP). + ZDown, + + /// X-axis facing up. + XUp, + + /// X-axis facing down. + XDown, + + /// Y-axis facing up. + YUp, + + /// Y-axis facing down. + YDown, +} + +impl From for V5ImuOrientationMode { + fn from(value: InertialOrientation) -> Self { + match value { + InertialOrientation::ZUp => Self::kImuOrientationZUp, + InertialOrientation::ZDown => Self::kImuOrientationZDown, + InertialOrientation::XUp => Self::kImuOrientationXUp, + InertialOrientation::XDown => Self::kImuOrientationXDown, + InertialOrientation::YUp => Self::kImuOrientationYUp, + InertialOrientation::YDown => Self::kImuOrientationYDown, + } + } +} + bitflags! { /// The status bits returned by an [`InertialSensor`]. #[derive(Debug, Clone, Copy, Eq, PartialEq)] pub struct InertialStatus: u32 { /// The sensor is currently calibrating. - const CALIBRATING = pros_sys::E_IMU_STATUS_CALIBRATING; + const CALIBRATING = 0b00001; + + /// The sensor is calibrated using auto-calibration. + const AUTO_CALIBRTED = 0b10000; + } +} + +impl InertialStatus { + /// Returns the physical orientation of the sensor measured at calibration. + pub fn physical_orientation(&self) -> InertialOrientation { + match (self.bits() >> 1) & 0b111 { + 0 => InertialOrientation::ZUp, + 1 => InertialOrientation::ZDown, + 2 => InertialOrientation::XUp, + 3 => InertialOrientation::XDown, + 4 => InertialOrientation::YUp, + 5 => InertialOrientation::YDown, + } } }