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-core/src/error.rs b/packages/pros-core/src/error.rs index f129cede..45b911bd 100644 --- a/packages/pros-core/src/error.rs +++ b/packages/pros-core/src/error.rs @@ -86,6 +86,16 @@ macro_rules! bail_on { } use snafu::Snafu; +#[derive(Debug, Snafu)] +/// Generic erros that can take place when using ports on the V5 Brain. +pub enum PortError { + /// No device is plugged into the port. + Disconnected, + + /// The incorrect device type is plugged into the port. + IncorrectDevice, +} + /// 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. @@ -93,20 +103,3 @@ pub trait FromErrno { 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, -} - -map_errno!(PortError { - ENXIO => Self::PortOutOfRange, - ENODEV => Self::PortCannotBeConfigured, - EADDRINUSE => Self::AlreadyInUse, -}); diff --git a/packages/pros-devices/src/battery.rs b/packages/pros-devices/src/battery.rs deleted file mode 100644 index ebb02ce4..00000000 --- a/packages/pros-devices/src/battery.rs +++ /dev/null @@ -1,46 +0,0 @@ -//! 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; - -/// Get the robot's battery capacity. -pub fn capacity() -> Result { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::misc::battery_get_capacity() - })) -} - -/// 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() - })) -} - -/// 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() - })) -} - -/// 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, - } -} 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-devices/src/controller.rs b/packages/pros-devices/src/controller.rs deleted file mode 100644 index ba06b0b8..00000000 --- a/packages/pros-devices/src/controller.rs +++ /dev/null @@ -1,350 +0,0 @@ -//! Read from the buttons and joysticks on the controller and write to the controller's display. -//! -//! 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, vec::Vec}; - -use pros_core::{bail_on, map_errno}; -use pros_sys::{controller_id_e_t, PROS_ERR}; -use snafu::Snafu; - -/// 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, -} - -/// 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)] -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, -} - -/// Stores both joysticks on the controller. -#[derive(Debug, Clone, Copy, PartialEq)] -pub struct Joysticks { - /// Left joystick - pub left: Joystick, - /// Right joystick - pub right: Joystick, -} - -/// 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, -} - -/// Represents one line on the controller console. -#[derive(Debug, Clone, Copy)] -pub struct ControllerLine { - controller: Controller, - line: u8, -} - -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()) - }); - 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(); - } -} - -/// 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, -} - -/// An analog channel (joystick axis) on the VEX controller. -#[repr(u32)] -#[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, -} - -/// 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, -} - -impl Controller { - const fn id(&self) -> controller_id_e_t { - *self as controller_id_e_t - } - - /// 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, - } - } - - /// 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, - }, - } - }, - 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, - } - }, - }) - } - - /// 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 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) - } -} - -#[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, - } -} diff --git a/packages/pros-devices/src/smart/distance.rs b/packages/pros-devices/src/smart/distance.rs deleted file mode 100644 index 53fa0849..00000000 --- a/packages/pros-devices/src/smart/distance.rs +++ /dev/null @@ -1,77 +0,0 @@ -//! Distance sensor device. -//! -//! 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 super::{SmartDevice, SmartDeviceType, SmartPort}; - -/// A physical distance sensor plugged into a port. -/// Distance sensors can only keep track of one object at a time. -#[derive(Debug, Eq, PartialEq)] -pub struct DistanceSensor { - port: SmartPort, -} - -impl DistanceSensor { - /// Create a new distance sensor from a smart port index. - pub const fn new(port: SmartPort) -> Self { - Self { port } - } - - /// 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) - } - - /// 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()) - })) - } - - /// Get the current guess at relative "object size". - /// - /// This is a value that has a range of 0 to 400. A 18" x 30" grey card will return - /// a value of approximately 75 in typical room lighting. - /// - /// This sensor reading is unusual, as it is entirely unitless with the seemingly arbitrary - /// range of 0-400 existing due to VEXCode's [`vex::sizeType`] enum having four variants. It's - /// unknown what the sensor is *actually* measuring here either, so use this data with a grain - /// 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) - } - - /// 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; - - Ok(confidence / 63.0) - } -} - -impl SmartDevice for DistanceSensor { - fn port_index(&self) -> u8 { - self.port.index() - } - - fn device_type(&self) -> SmartDeviceType { - SmartDeviceType::Distance - } -} diff --git a/packages/pros-devices/src/smart/gps.rs b/packages/pros-devices/src/smart/gps.rs deleted file mode 100644 index 16e561b0..00000000 --- a/packages/pros-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/pros-devices/src/smart/imu.rs b/packages/pros-devices/src/smart/imu.rs deleted file mode 100644 index b76ef393..00000000 --- a/packages/pros-devices/src/smart/imu.rs +++ /dev/null @@ -1,463 +0,0 @@ -//! Inertial sensor (IMU) device. - -use core::{ - pin::Pin, - task::{Context, Poll}, - time::Duration, -}; - -use bitflags::bitflags; -use pros_core::{ - bail_on, - error::{take_errno, FromErrno, PortError}, - map_errno, - time::Instant, -}; -use pros_sys::{PROS_ERR, PROS_ERR_F}; -use snafu::Snafu; - -use super::{SmartDevice, SmartDeviceType, SmartPort}; - -/// Represents a smart port configured as a V5 inertial sensor (IMU) -#[derive(Debug, Eq, PartialEq)] -pub struct InertialSensor { - port: SmartPort, -} - -impl InertialSensor { - /// The timeout for the IMU to calibrate. - 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); - - /// Create a new inertial sensor from a smart port index. - pub const fn new(port: SmartPort) -> Self { - 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 - /// no longer calibrating. - /// There a 3 second timeout that will return [`InertialError::CalibrationTimedOut`] if the timeout is exceeded. - pub fn calibrate(&mut self) -> InertialCalibrateFuture { - 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 { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::imu_get_rotation(self.port.index()) - })) - } - - /// Get the Inertial Sensor’s heading relative to the initial direction of its x-axis. - /// - /// This value is bounded by [0, 360) degrees. Clockwise rotations are represented with positive degree values, - /// while counterclockwise rotations are represented with negative ones. - pub fn heading(&self) -> Result { - Ok(bail_on!(PROS_ERR_F, unsafe { - pros_sys::imu_get_heading(self.port.index()) - })) - } - - /// 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()) - })) - } - - /// 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()) - })) - } - - /// 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()) - }); - - Ok(InertialStatus::from_bits_retain(bits)) - } - - /// 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() } - } - - /// 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() } - } - - /// 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() } - } - - /// 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(()) - } - - /// 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(()) - } - - /// 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(()) - } - - /// 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 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(()) - } - - /// 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(()) - } - - /// 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()) }); - 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()) - }); - 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) - }); - Ok(()) - } - - /// Sets the current reading of the Inertial Sensor’s heading to target value. - /// - /// Target will default to 360 if above 360 and default to 0 if below 0. - pub fn set_heading(&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(()) - } - - /// 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(()) - } - - /// 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(()) - } - - /// Sets the update rate of the IMU. - /// - /// 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) - ); - } - Ok(()) - } -} - -impl SmartDevice for InertialSensor { - fn port_index(&self) -> u8 { - self.port.index() - } - - fn device_type(&self) -> SmartDeviceType { - SmartDeviceType::Imu - } -} - -/// Standard quaternion consisting of a vector defining an axis of rotation -/// and a rotation value about the axis. -#[derive(Default, Debug, Clone, Copy, PartialEq)] -pub struct Quaternion { - /// The x-component of the axis of rotation. - pub x: f64, - - /// The y-component of the axis of rotation. - pub y: f64, - - /// The z-component of the axis of rotation. - pub z: f64, - - /// The magnitude of rotation about the axis. - pub w: f64, -} - -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, - } - } -} - -/// A 3-axis set of euler angles. -#[derive(Default, Debug, Clone, Copy, PartialEq)] -pub struct Euler { - /// The angle measured along the pitch axis. - pub pitch: f64, - - /// The angle measured along the roll axis. - pub roll: f64, - - /// The angle measured along the yaw axis. - pub yaw: f64, -} - -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), - 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, - } - } -} - -/// Represents raw data reported by the IMU. -/// -/// This is effectively a 3D vector containing either angular velocity or -/// acceleration values depending on the type of data requested.. -#[derive(Default, Debug, Clone, Copy, PartialEq)] -pub struct InertialRaw { - /// The x component of the raw data. - pub x: f64, - - /// The y component of the raw data. - pub y: f64, - - /// The z component of the raw data. - pub z: f64, -} - -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), - y: value.y, - z: value.z, - }) - } -} - -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; - } -} - -#[derive(Debug, Clone, Copy)] -/// Future that calibrates an IMU -/// created with [`InertialSensor::calibrate`]. -pub enum InertialCalibrateFuture { - /// Calibrate the IMU - Calibrate(u8), - /// Wait for the IMU to finish calibrating - Waiting(u8, Instant), -} - -impl core::future::Future for InertialCalibrateFuture { - type Output = Result<(), InertialError>; - - 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()); - 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 { - return Poll::Ready(Err(InertialError::CalibrationTimedOut)); - } - - cx.waker().wake_by_ref(); - Poll::Pending - } - } - } -} - -#[derive(Debug, Snafu)] -/// Errors that can occur when interacting with an Inertial Sensor. -pub enum InertialError { - /// The inertial sensor spent too long calibrating. - CalibrationTimedOut, - /// Invalid sensor data rate, expected >= 5 milliseconds. - InvalidDataRate, - #[snafu(display("{source}"), context(false))] - /// Generic port related error. - Port { - /// The source of the error. - source: PortError, - }, -} - -map_errno! { - InertialError { - EAGAIN => Self::CalibrationTimedOut, - } - inherit PortError; -} diff --git a/packages/pros-devices/src/smart/optical.rs b/packages/pros-devices/src/smart/optical.rs deleted file mode 100644 index d529887d..00000000 --- a/packages/pros-devices/src/smart/optical.rs +++ /dev/null @@ -1,371 +0,0 @@ -//! Optical sensor device - -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 snafu::Snafu; - -use super::{SmartDevice, SmartDeviceType, SmartPort}; - -/// Represents a smart port configured as a V5 optical sensor -#[derive(Debug, Eq, PartialEq)] -pub struct OpticalSensor { - port: SmartPort, - gesture_detection_enabled: bool, -} - -impl OpticalSensor { - /// The smallest integration time you can set on an optical sensor. - pub const MIN_INTEGRATION_TIME: Duration = Duration::from_millis(3); - - /// The largest integration time you can set on an optical sensor. - 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()`]. - pub fn new(port: SmartPort, gesture_detection_enabled: bool) -> Result { - let mut sensor = Self { - port, - gesture_detection_enabled, - }; - - if gesture_detection_enabled { - sensor.enable_gesture_detection()?; - } else { - sensor.disable_gesture_detection()?; - } - - 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()) - )) - } - } - - /// 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) - ); - } - 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)) - } - } - - /// Set integration time (update rate) of the optical sensor. - /// - /// Lower integration time results in faster update rates with lower accuracy - /// due to less available light being read by the sensor. - /// - /// Time value must be a [`Duration`] between 3 and 712 milliseconds. See - /// 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); - } - - unsafe { - bail_on!( - PROS_ERR, - pros_sys::optical_set_integration_time(self.port.index(), time.as_millis() as f64) - ); - } - - Ok(()) - } - - /// Get the detected color hue. - /// - /// 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()) - )) - } - } - - /// 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()) - )) - } - } - - /// 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()) - )) - } - } - - /// 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()) - )) - } - } - - /// 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 raw, unprocessed RGBC data from the sensor - pub fn rgbc_raw(&self) -> Result { - unsafe { pros_sys::optical_get_raw(self.port.index()).try_into() } - } - - /// Enables gesture detection features on the sensor. - /// - /// 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.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.gesture_detection_enabled = false; - Ok(()) - } - - /// Determine if gesture detection is enabled or not on the sensor. - pub const fn gesture_detection_enabled(&self) -> bool { - self.gesture_detection_enabled - } - - /// Get the most recent gesture data from the sensor. Gestures will be cleared after 500mS. - /// - /// Will return [`OpticalError::GestureDetectionDisabled`] if the sensor is not - /// confgured to detect gestures. - pub fn last_gesture_direction(&self) -> Result { - if !self.gesture_detection_enabled { - return Err(OpticalError::GestureDetectionDisabled); - } - - unsafe { pros_sys::optical_get_gesture(self.port.index()).try_into() } - } - - /// 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); - } - - unsafe { pros_sys::optical_get_gesture_raw(self.port.index()).try_into() } - } -} - -impl SmartDevice for OpticalSensor { - fn port_index(&self) -> u8 { - self.port.index() - } - - fn device_type(&self) -> SmartDeviceType { - SmartDeviceType::Optical - } -} - -#[derive(Default, Debug, Clone, Copy, PartialEq)] -/// Represents a gesture and its direction. -pub enum GestureDirection { - /// Up gesture. - Up, - /// Down gesture. - Down, - /// Left gesture. - Left, - /// Right gesture. - Right, - /// Gesture error. - Error, - #[default] - /// No gesture detected. - NoGesture, -} - -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."), - }) - } -} - -#[derive(Default, Debug, Clone, Copy, Eq, PartialEq)] -/// Raw gesture data from an [`OpticalSensor`]. -pub struct GestureRaw { - /// Up value. - pub up: u8, - /// Down value. - pub down: u8, - /// Left value. - pub left: u8, - /// Right value. - pub right: u8, - /// Gesture type. - pub gesture_type: u8, - /// The count of the gesture. - pub count: u16, - /// The time of the gesture. - 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, - }) - } -} - -#[derive(Default, Debug, Clone, Copy, PartialEq)] -/// RGBC data from a [`OpticalSensor`]. -pub struct Rgbc { - /// The red value from the sensor. - pub red: f64, - /// The green value from the sensor. - pub green: f64, - /// The blue value from the sensor. - pub blue: f64, - /// The brightness value from the sensor. - 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 - 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 { - /// The red value from the sensor. - pub red: u32, - /// The green value from the sensor. - pub green: u32, - /// The blue value from the sensor. - pub blue: u32, - /// The clear value from the sensor. - pub clear: u32, -} - -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), - red: value.red, - green: value.green, - blue: value.blue, - }) - } -} - -#[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, - - #[snafu(display("{source}"), context(false))] - /// Generic port related error. - Port { - /// The source of the error - source: PortError, - }, -} - -map_errno! { - OpticalError {} inherit PortError; -} diff --git a/packages/pros-devices/src/smart/rotation.rs b/packages/pros-devices/src/smart/rotation.rs deleted file mode 100644 index 2988af2a..00000000 --- a/packages/pros-devices/src/smart/rotation.rs +++ /dev/null @@ -1,98 +0,0 @@ -//! Rotation sensor device. -//! -//! 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 super::{SmartDevice, 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) - ); - } - } - - Ok(Self { port, reversed }) - } - - /// Sets the position to zero. - pub fn zero(&mut self) -> Result<(), PortError> { - unsafe { - bail_on!( - PROS_ERR, - pros_sys::rotation_reset_position(self.port.index()) - ); - } - 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 _ - ) - ); - } - Ok(()) - } - - /// Sets whether or not the rotation sensor should be reversed. - pub fn set_reversed(&mut self, reversed: bool) -> Result<(), PortError> { - self.reversed = reversed; - - 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) - } - - //TODO: See if this is accurate enough or consider switching to get_position function. - /// Gets the current position of the sensor. - 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, - ) - }) - } -} - -impl SmartDevice for RotationSensor { - fn port_index(&self) -> u8 { - self.port.index() - } - - fn device_type(&self) -> SmartDeviceType { - SmartDeviceType::Rotation - } -} diff --git a/packages/pros-devices/src/smart/vision.rs b/packages/pros-devices/src/smart/vision.rs deleted file mode 100644 index ad27ce5e..00000000 --- a/packages/pros-devices/src/smart/vision.rs +++ /dev/null @@ -1,229 +0,0 @@ -//! Vision sensor device. -//! -//! Vision sensors take in a zero point at creation. - -extern crate alloc; -use alloc::vec::Vec; - -use pros_core::{bail_errno, bail_on, error::PortError, map_errno}; -use pros_sys::{PROS_ERR, VISION_OBJECT_ERR_SIG}; -use snafu::Snafu; - -use super::{SmartDevice, SmartDeviceType, SmartPort}; -use crate::color::Rgb; - -/// Represents a vision sensor plugged into the vex. -#[derive(Debug, Eq, PartialEq)] -pub struct VisionSensor { - port: SmartPort, -} - -impl VisionSensor { - /// Creates a new vision sensor. - pub fn new(port: SmartPort, zero: VisionZeroPoint) -> Result { - unsafe { - bail_on!( - PROS_ERR, - pros_sys::vision_set_zero_point(port.index(), zero as _) - ); - } - - Ok(Self { port }) - } - - /// Returns the nth largest object seen by the camera. - pub fn nth_largest_object(&self, n: u32) -> Result { - unsafe { pros_sys::vision_get_by_size(self.port.index(), n).try_into() } - } - - /// Returns a list of all objects in order of size (largest to smallest). - pub fn objects(&self) -> Result, VisionError> { - let obj_count = self.num_objects()?; - let mut objects_buf = Vec::with_capacity(obj_count); - - unsafe { - pros_sys::vision_read_by_size( - self.port.index(), - 0, - obj_count as _, - objects_buf.as_mut_ptr(), - ); - } - - bail_errno!(); - - Ok(objects_buf - .into_iter() - .filter_map(|object| object.try_into().ok()) - .collect()) - } - - /// Returns the number of objects seen by the camera. - pub fn num_objects(&self) -> Result { - unsafe { - Ok(bail_on!( - PROS_ERR, - pros_sys::vision_get_object_count(self.port.index()) - ) - .try_into() - .unwrap()) - } - } - - /// Get the current exposure percentage of the vision sensor. The returned result should be within 0.0 to 1.5. - pub fn exposure(&self) -> f32 { - unsafe { (pros_sys::vision_get_exposure(self.port.index()) as f32) * 1.5 / 150.0 } - } - - /// Get the current white balance of the vision sensor. - pub fn current_white_balance(&self) -> Rgb { - unsafe { (pros_sys::vision_get_white_balance(self.port.index()) as u32).into() } - } - - /// Sets the exposure percentage of the vision sensor. Should be between 0.0 and 1.5. - pub fn set_exposure(&mut self, exposure: f32) { - unsafe { - pros_sys::vision_set_exposure(self.port.index(), (exposure * 150.0 / 1.5) as u8); - } - } - - /// Sets the white balance of the vision sensor. - pub fn set_white_balance(&mut self, white_balance: WhiteBalance) { - unsafe { - match white_balance { - WhiteBalance::Auto => pros_sys::vision_set_auto_white_balance(self.port.index(), 1), - WhiteBalance::Rgb(rgb) => { - // Turn off automatic white balance - pros_sys::vision_set_auto_white_balance(self.port.index(), 0); - pros_sys::vision_set_white_balance( - self.port.index(), - >::into(rgb) as i32, - ) - } - }; - } - } - - /// Sets the point that object positions are relative to, in other words where (0, 0) is or the zero point. - pub fn set_zero_point(&mut self, zero: VisionZeroPoint) { - unsafe { - pros_sys::vision_set_zero_point(self.port.index(), zero as _); - } - } - - /// Sets the color of the led. - pub fn set_led(&mut self, mode: LedMode) { - unsafe { - match mode { - LedMode::Off => pros_sys::vision_clear_led(self.port.index()), - LedMode::On(rgb) => pros_sys::vision_set_led( - self.port.index(), - >::into(rgb) as i32, - ), - }; - } - } -} - -impl SmartDevice for VisionSensor { - fn port_index(&self) -> u8 { - self.port.index() - } - - fn device_type(&self) -> SmartDeviceType { - SmartDeviceType::Vision - } -} - -//TODO: figure out how coordinates are done. -#[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] -/// An object detected by the vision sensor -pub struct VisionObject { - /// The offset from the top of the object to the vision center. - pub top: i16, - /// The offset from the left of the object to the vision center. - pub left: i16, - /// The x-coordinate of the middle of the object relative to the vision center. - pub middle_x: i16, - /// The y-coordinate of the middle of the object relative to the vision center. - pub middle_y: i16, - - /// The width of the object. - pub width: i16, - /// The height of the object. - pub height: i16, -} - -impl TryFrom for VisionObject { - type Error = VisionError; - fn try_from(value: pros_sys::vision_object_s_t) -> Result { - if value.signature == VISION_OBJECT_ERR_SIG { - bail_errno!(); - unreachable!("Errno should be non-zero") - } - - Ok(Self { - top: value.top_coord, - left: value.left_coord, - middle_x: value.x_middle_coord, - middle_y: value.y_middle_coord, - width: value.width, - height: value.height, - }) - } -} - -#[repr(u32)] -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -/// The zero point of the vision sensor. -/// Vision object coordinates are relative to this point. -pub enum VisionZeroPoint { - /// The zero point will be the top left corner of the vision sensor. - TopLeft, - /// The zero point will be the top right corner of the vision sensor. - Center, -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -/// The white balance of the vision sensor. -pub enum WhiteBalance { - /// Provide a specific color to balance the white balance. - Rgb(Rgb), - /// Automatically balance the white balance. - Auto, -} - -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -/// The mode of the vision sensor led. -pub enum LedMode { - /// Turn on the led with a certain color. - On(Rgb), - /// Turn off the led. - Off, -} - -#[derive(Debug, Snafu)] -/// Errors that can occur when using a vision sensor. -pub enum VisionError { - /// The camera could not be read. - ReadingFailed, - /// The index specified was higher than the total number of objects seen by the camera. - IndexTooHigh, - /// Port already taken. - PortTaken, - #[snafu(display("{source}"), context(false))] - /// Generic port related error. - Port { - /// The source of the error. - source: PortError, - }, -} - -map_errno! { - VisionError { - EHOSTDOWN => Self::ReadingFailed, - EDOM => Self::IndexTooHigh, - EACCES => Self::PortTaken, - } - inherit PortError; -} 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-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/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/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/pros-devices/Cargo.toml b/packages/vex-devices/Cargo.toml similarity index 93% rename from packages/pros-devices/Cargo.toml rename to packages/vex-devices/Cargo.toml index 52bbf80d..d919a490 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" @@ -22,10 +22,12 @@ authors = [ [dependencies] pros-core = { version = "0.1.0", path = "../pros-core" } pros-sys = { path = "../pros-sys", version = "0.7.0", features = ["xapi"] } +vex-sdk = { version = "0.1.0" } snafu = { version = "0.8.0", default-features = false, features = [ "rust_1_61", "unstable-core-error", ] } +mint = "0.5.9" no_std_io = { version = "0.6.0", features = ["alloc"] } bitflags = "2.4.2" 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/vex-devices/src/battery.rs b/packages/vex-devices/src/battery.rs new file mode 100644 index 00000000..7a18a40c --- /dev/null +++ b/packages/vex-devices/src/battery.rs @@ -0,0 +1,29 @@ +//! Utilites for getting information about the robot's battery. + +use vex_sdk::{ + vexBatteryCapacityGet, vexBatteryCurrentGet, vexBatteryTemperatureGet, vexBatteryVoltageGet, +}; + +/// Get the robot's battery capacity. +/// TODO: Determine units +pub fn capacity() -> f64 { + unsafe { vexBatteryCapacityGet() } +} + +/// Get the current temperature of the robot's battery. +/// TODO: Determine units +pub fn temperature() -> f64 { + unsafe { vexBatteryTemperatureGet() } +} + +/// Get the electric current of the robot's battery. +/// TODO: Determine units +pub fn current() -> i32 { + unsafe { vexBatteryCurrentGet() } +} + +/// Get the robot's battery voltage. +/// TODO: Determine units +pub fn voltage() -> i32 { + unsafe { vexBatteryVoltageGet() } +} diff --git a/packages/vex-devices/src/color.rs b/packages/vex-devices/src/color.rs new file mode 100644 index 00000000..dbcbd877 --- /dev/null +++ b/packages/vex-devices/src/color.rs @@ -0,0 +1,139 @@ +//! 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 { + /// "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. + 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 65% rename from packages/pros-devices/src/competition.rs rename to packages/vex-devices/src/competition.rs index 089b830b..f0562b5b 100644 --- a/packages/pros-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_sdk::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) diff --git a/packages/vex-devices/src/controller.rs b/packages/vex-devices/src/controller.rs new file mode 100644 index 00000000..268eea42 --- /dev/null +++ b/packages/vex-devices/src/controller.rs @@ -0,0 +1,399 @@ +//! Read from the buttons and joysticks on the controller and write to the controller's display. +//! +//! 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 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); + } + + 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(Debug, Eq, PartialEq)] +pub struct Joystick { + id: ControllerId, + x_channel: V5_ControllerIndex, + y_channel: V5_ControllerIndex, +} + +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 _) + } +} + +/// 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, +} + +/// Controller LCD Console +#[derive(Debug, Eq, PartialEq)] +pub struct ControllerScreen { + id: ControllerId, +} + +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(()) + } + + /// 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(()) + } + + /// 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(()) + } +} + +/// Represents an identifier for one of the two possible controllers +/// connected to the V5 brain. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ControllerId { + /// Primary ("Master") Controller + Primary, + + /// Partner Controller + Partner, +} + +impl From for V5_ControllerId { + fn from(id: ControllerId) -> Self { + match id { + ControllerId::Primary => V5_ControllerId::kControllerMaster, + ControllerId::Partner => V5_ControllerId::kControllerPartner, + } + } +} + +impl Controller { + fn validate(&self) -> Result<(), ControllerError> { + if !controller_connected(self.id) { + return Err(ControllerError::NotConnected); + } + + Ok(()) + } + + /// 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, + }, + 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 controller's battery level. + pub fn battery_level(&self) -> Result { + self.validate()?; + + Ok(unsafe { + vexControllerGet(self.id.into(), V5_ControllerIndex::BatteryLevel) + }) + } + + /// 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 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, +} 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/vex-devices/src/smart/distance.rs b/packages/vex-devices/src/smart/distance.rs new file mode 100644 index 00000000..613c88d3 --- /dev/null +++ b/packages/vex-devices/src/smart/distance.rs @@ -0,0 +1,109 @@ +//! Distance sensor device. +//! +//! Pretty much one to one with the PROS C and CPP API, except Result is used instead of ERRNO values. + +use pros_core::error::PortError; +use snafu::Snafu; +use vex_sdk::{ + vexDeviceDistanceConfidenceGet, vexDeviceDistanceDistanceGet, vexDeviceDistanceObjectSizeGet, + vexDeviceDistanceObjectVelocityGet, vexDeviceDistanceStatusGet, +}; + +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. +#[derive(Debug, Eq, PartialEq)] +pub struct DistanceSensor { + port: SmartPort, +} + +impl DistanceSensor { + /// Create a new distance sensor from a smart port index. + pub const fn new(port: SmartPort) -> Self { + 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 { + self.validate()?; + + 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.device_handle()) }) + } + + /// Get the current guess at relative "object size". + /// + /// This is a value that has a range of 0 to 400. A 18" x 30" grey card will return + /// a value of approximately 75 in typical room lighting. + /// + /// This sensor reading is unusual, as it is entirely unitless with the seemingly arbitrary + /// range of 0-400 existing due to VEXCode's [`vex::sizeType`] enum having four variants. It's + /// unknown what the sensor is *actually* measuring here either, so use this data with a grain + /// of salt. + /// + /// [`vex::sizeType`]: https://api.vexcode.cloud/v5/search/sizeType/sizeType/enum + pub fn relative_size(&self) -> Result { + self.validate()?; + + 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.device_handle()) as u32 } as f64 / 63.0) + } + + /// Gets the status code of the distance sensor + pub fn status(&self) -> Result { + self.validate_port()?; + + Ok(unsafe { vexDeviceDistanceStatusGet(self.device_handle()) }) + } +} + +impl SmartDevice for DistanceSensor { + fn port_index(&self) -> u8 { + self.port.index() + } + + fn device_type(&self) -> SmartDeviceType { + 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/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/vex-devices/src/smart/imu.rs b/packages/vex-devices/src/smart/imu.rs new file mode 100644 index 00000000..640add52 --- /dev/null +++ b/packages/vex-devices/src/smart/imu.rs @@ -0,0 +1,404 @@ +//! Inertial sensor (IMU) device. + +use core::{ + marker::PhantomData, pin::Pin, task::{Context, Poll}, time::Duration +}; + +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, V5ImuOrientationMode, + V5_DeviceImuAttitude, V5_DeviceImuQuaternion, V5_DeviceImuRaw, +}; + +use super::{validate_port, SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; + +/// Represents a smart port configured as a V5 inertial sensor (IMU) +#[derive(Debug, PartialEq)] +pub struct InertialSensor { + port: SmartPort, + rotation_offset: f64, + heading_offset: f64, +} + +impl InertialSensor { + /// 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 { + port, + rotation_offset: 0.0, + heading_offset: 0.0, + } + } + + /// 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::StillCalibrating); + } + Ok(()) + } + + /// Read the inertial sensor's status code. + pub fn status(&self) -> Result { + self.validate_port()?; + + let bits = unsafe { vexDeviceImuStatusGet(self.device_handle()) }; + + if bits == InertialStatus::STATUS_ERROR { + return Err(InertialError::BadStatus); + } + + Ok(InertialStatus::from_bits_retain(bits)) + } + + /// Check if the Intertial Sensor is currently calibrating. + pub fn is_calibrating(&mut self) -> Result { + 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 + /// no longer calibrating. + /// There a 3 second timeout that will return [`InertialError::CalibrationTimedOut`] if the timeout is exceeded. + pub fn calibrate(&mut self) -> InertialCalibrateFuture { + InertialCalibrateFuture::Calibrate(self.port.index()) + } + + /// 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()?; + Ok(unsafe { vexDeviceImuHeadingGet(self.device_handle()) } - self.rotation_offset) + } + + /// Get the Inertial Sensor’s yaw angle 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 { + self.validate()?; + Ok( + (unsafe { vexDeviceImuDegreesGet(self.device_handle()) } - self.heading_offset) + % Self::MAX_HEADING, + ) + } + + /// Get a quaternion representing the Inertial Sensor’s orientation. + pub fn quaternion(&self) -> Result, InertialError> { + self.validate()?; + + let mut data = V5_DeviceImuQuaternion::default(); + unsafe { + vexDeviceImuQuaternionGet(self.device_handle(), &mut data); + } + + Ok(mint::Quaternion { + v: mint::Vector3 { x: data.a, y: data.b, z: data.c }, + s: data.d, + }) + } + + /// Get the Euler angles (pitch, yaw, roll) representing the Inertial Sensor’s orientation. + pub fn euler(&self) -> Result, InertialError> { + self.validate()?; + + let mut data = V5_DeviceImuAttitude::default(); + unsafe { + vexDeviceImuAttitudeGet(self.device_handle(), &mut data); + } + + Ok(mint::EulerAngles { + a: data.pitch.to_radians(), + b: data.yaw.to_radians(), + c: data.roll.to_radians(), + marker: PhantomData + }) + } + + /// Get the Inertial Sensor’s raw gyroscope values. + pub fn gyro_rate(&self) -> Result, InertialError> { + self.validate()?; + + let mut data = V5_DeviceImuRaw::default(); + unsafe { + vexDeviceImuRawGyroGet(self.device_handle(), &mut data); + } + + Ok(mint::Vector3 { + x: data.x, + y: data.y, + z: data.z, + // NOTE: data.w is unused in the SDK. + // See: + }) + } + + /// Get the Inertial Sensor’s raw accelerometer values. + pub fn accel(&self) -> Result, InertialError> { + self.validate()?; + + let mut data = V5_DeviceImuRaw::default(); + unsafe { + vexDeviceImuRawAccelGet(self.device_handle(), &mut data); + } + + Ok(mint::Vector3 { + x: data.x, + y: data.y, + z: data.z, + // NOTE: data.w is unused in the SDK. + // See: + }) + } + + /// 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 rotation to zero. + pub fn reset_rotation(&mut self) -> Result<(), InertialError> { + self.set_rotation(Default::default()) + } + + /// Sets the current reading of the Inertial Sensor’s rotation to target value. + pub fn set_rotation(&mut self, rotation: f64) -> Result<(), InertialError> { + self.validate()?; + + self.rotation_offset = rotation - unsafe { vexDeviceImuHeadingGet(self.device_handle()) }; + + Ok(()) + } + + /// Sets the current reading of the Inertial Sensor’s heading to target value. + /// + /// Target will default to 360 if above 360 and default to 0 if below 0. + pub fn set_heading(&mut self, heading: f64) -> Result<(), InertialError> { + self.validate()?; + + self.heading_offset = heading - unsafe { vexDeviceImuDegreesGet(self.device_handle()) }; + + Ok(()) + } + + /// Sets the update rate of the IMU. + /// + /// This duration should be above [`Self::MIN_DATA_RATE`] (5 milliseconds). + pub fn set_data_rate(&mut self, data_rate: Duration) -> Result<(), InertialError> { + 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(()) + } +} + +impl SmartDevice for InertialSensor { + fn port_index(&self) -> u8 { + self.port.index() + } + + fn device_type(&self) -> SmartDeviceType { + SmartDeviceType::Imu + } +} + +/// 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 = 0b00001; + + /// The sensor is calibrated using auto-calibration. + const AUTO_CALIBRTED = 0b10000; + } +} + +impl InertialStatus { + /// The return value of [`vexDeviceImuStatusGet`] when the device fails to report its + /// status bits. + pub const STATUS_ERROR: u32 = 0xFF; + + /// 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, + } + } +} + +/// 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 either begin calibrating or end calibration, depending on the + /// designated [`CalibrationPhase`]. + Waiting(u8, Instant, CalibrationPhase), +} + +impl core::future::Future for InertialCalibrateFuture { + type Output = Result<(), InertialError>; + + fn poll(mut self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll { + match *self { + 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, 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. + let flags = unsafe { + vexDeviceImuStatusGet(vexDeviceGetByIndex((port - 1) as u32)) + }; + + // 0xFF is returned when the sensor fails to report flags. + if flags == InertialStatus::STATUS_ERROR { + return Poll::Ready(Err(InertialError::BadStatus)); + } + + flags + }, + ); + + 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 + } + } + } +} + +#[derive(Debug, Snafu)] +/// Errors that can occur when interacting with an Inertial Sensor. +pub enum InertialError { + /// The sensor took longer than three seconds to calibrate. + CalibrationTimedOut, + /// The sensor is still calibrating. + StillCalibrating, + /// The sensor failed to report its status flags (returned 0xFF). + BadStatus, + #[snafu(display("{source}"), context(false))] + /// Generic port related error. + Port { + /// The source of the error. + source: PortError, + }, +} diff --git a/packages/pros-devices/src/smart/link.rs b/packages/vex-devices/src/smart/link.rs similarity index 97% rename from packages/pros-devices/src/smart/link.rs rename to packages/vex-devices/src/smart/link.rs index ddcebdf7..41cf4140 100644 --- a/packages/pros-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/pros-devices/src/smart/mod.rs b/packages/vex-devices/src/smart/mod.rs similarity index 53% rename from packages/pros-devices/src/smart/mod.rs rename to packages/vex-devices/src/smart/mod.rs index f528aa90..bb368f6a 100644 --- a/packages/pros-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,13 +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_sdk::{vexDeviceGetByIndex, vexDeviceGetTimestamp, V5_DeviceT, V5_DeviceType}; pub use vision::VisionSensor; /// Defines common functionality shared by all smart port devices. @@ -81,18 +80,55 @@ 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> { + validate_port(self.port_index(), self.device_type()) + } +} + +/// 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 {} + /// Represents a smart port on a V5 Brain #[derive(Debug, Eq, PartialEq)] pub struct SmartPort { @@ -148,96 +184,112 @@ 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, + 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, + 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/pros-devices/src/smart/motor.rs b/packages/vex-devices/src/smart/motor.rs similarity index 63% rename from packages/pros-devices/src/smart/motor.rs rename to packages/vex-devices/src/smart/motor.rs index ffcb1e2f..5851103c 100644 --- a/packages/pros-devices/src/smart/motor.rs +++ b/packages/vex-devices/src/smart/motor.rs @@ -3,11 +3,24 @@ 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, +}; +#[cfg(feature = "dangerous_motor_tuning")] +use vex_sdk::{vexDeviceMotorPositionPidSet, vexDeviceMotorVelocityPidSet, V5_DeviceMotorPid}; -use super::{SmartDevice, SmartDeviceTimestamp, SmartDeviceType, SmartPort}; +use super::{SmartDevice, SmartDeviceInternal, SmartDeviceTimestamp, SmartDeviceType, SmartPort}; use crate::Position; /// The basic motor struct. @@ -77,10 +90,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 +98,13 @@ impl Motor { motor.set_gearset(gearset)?; motor.set_direction(direction)?; + unsafe { + vexDeviceMotorEncoderUnitsSet( + motor.device_handle(), + vex_sdk::V5MotorEncoderUnits::kMotorEncoderDegrees, + ); + } + Ok(motor) } @@ -96,52 +112,42 @@ 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()); + // Force motor into braking by putting it into velocity control with a 0rpm setpoint. + 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 +186,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 +203,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 +279,71 @@ 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) + } + + /// 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()) }) + } /// 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). + // 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); } @@ -360,11 +353,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 +382,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 +419,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 +442,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 +462,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 +498,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 +515,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 +593,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 +624,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 +657,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 +679,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 +686,3 @@ pub enum MotorError { source: PortError, }, } - -map_errno! { - MotorError { - ENOSYS => Self::NotImplemented, - } - inherit PortError; -} diff --git a/packages/vex-devices/src/smart/optical.rs b/packages/vex-devices/src/smart/optical.rs new file mode 100644 index 00000000..7152fb11 --- /dev/null +++ b/packages/vex-devices/src/smart/optical.rs @@ -0,0 +1,343 @@ +//! Optical sensor device + +use core::time::Duration; + +use pros_core::error::PortError; +use snafu::Snafu; +use vex_sdk::{ + vexDeviceOpticalBrightnessGet, vexDeviceOpticalGestureDisable, vexDeviceOpticalGestureEnable, + vexDeviceOpticalGestureGet, vexDeviceOpticalHueGet, vexDeviceOpticalIntegrationTimeGet, + vexDeviceOpticalIntegrationTimeSet, vexDeviceOpticalLedPwmGet, vexDeviceOpticalLedPwmSet, + vexDeviceOpticalProximityGet, vexDeviceOpticalRawGet, vexDeviceOpticalRgbGet, + vexDeviceOpticalSatGet, vexDeviceOpticalStatusGet, V5_DeviceOpticalGesture, + V5_DeviceOpticalRaw, V5_DeviceOpticalRgb, +}; + +use super::{SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; + +/// Represents a smart port configured as a V5 optical sensor +#[derive(Debug, Eq, PartialEq)] +pub struct OpticalSensor { + port: SmartPort, + gesture_detection_enabled: bool, +} + +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); + + /// 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()`]. + pub fn new(port: SmartPort, gesture_detection_enabled: bool) -> Result { + let mut sensor = Self { + port, + gesture_detection_enabled, + }; + + if gesture_detection_enabled { + sensor.enable_gesture_detection()?; + } else { + sensor.disable_gesture_detection()?; + } + + Ok(sensor) + } + + /// 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()) }) + } + + /// 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 { + self.validate_port()?; + + Ok(Duration::from_millis( + unsafe { vexDeviceOpticalIntegrationTimeGet(self.device_handle()) } as u64, + )) + } + + /// Set integration time (update rate) of the optical sensor. + /// + /// Lower integration time results in faster update rates with lower accuracy + /// due to less available light being read by the sensor. + /// + /// Time value must be a [`Duration`] between 3 and 712 milliseconds. See + /// for + /// more information. + pub fn set_integration_time(&mut self, time: Duration) -> Result<(), OpticalError> { + self.validate_port()?; + + 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_ms) } + + Ok(()) + } + + /// Get the detected color hue. + /// + /// Hue has a range of `0` to `359.999`. + pub fn hue(&self) -> Result { + 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 { + 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 { + self.validate_port()?; + + Ok(unsafe { vexDeviceOpticalBrightnessGet(self.device_handle()) }) + } + + /// Get the analog proximity value from `0` to `1.0`. + /// + /// 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()) } as f64 / 255.0) + } + + /// 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 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. + /// + /// 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> { + 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> { + self.validate_port()?; + + unsafe { vexDeviceOpticalGestureDisable(self.device_handle()) } + self.gesture_detection_enabled = true; + + Ok(()) + } + + /// Determine if gesture detection is enabled or not on the sensor. + pub const fn gesture_detection_enabled(&self) -> bool { + self.gesture_detection_enabled + } + + /// Get the most recent gesture data from the sensor. Gestures will be cleared after 500mS. + /// + /// Will return [`OpticalError::GestureDetectionDisabled`] if the sensor is not + /// confgured to detect gestures. + pub fn last_gesture(&self) -> Result { + if !self.gesture_detection_enabled { + return Err(OpticalError::GestureDetectionDisabled); + } + 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, + }) + } + + /// Gets the status code of the distance sensor + pub fn status(&self) -> Result { + self.validate_port()?; + + Ok(unsafe { vexDeviceOpticalStatusGet(self.device_handle()) }) + } +} + +impl SmartDevice for OpticalSensor { + fn port_index(&self) -> u8 { + self.port.index() + } + + fn device_type(&self) -> SmartDeviceType { + SmartDeviceType::Optical + } +} + +/// 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 = 1, + /// Down gesture. + Down = 2, + /// Left gesture. + Left = 3, + /// Right gesture. + Right = 4, +} + +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)] +pub struct Gesture { + /// Gesture Direction + pub direction: GestureDirection, + /// Up value. + pub up: u8, + /// Down value. + pub down: u8, + /// Left value. + pub left: u8, + /// Right value. + pub right: u8, + /// Gesture type. + pub gesture_type: u8, + /// The count of the gesture. + pub count: u16, + /// The time of the gesture. + pub time: u32, +} + +/// RGB data from a [`OpticalSensor`]. +#[derive(Default, Debug, Clone, Copy, PartialEq)] +pub struct OpticalRgb { + /// The red value from the sensor. + pub red: f64, + /// The green value from the sensor. + pub green: f64, + /// The blue value from the sensor. + pub blue: f64, + /// The brightness value from the sensor. + pub brightness: f64, +} + +impl From for OpticalRgb { + fn from(value: V5_DeviceOpticalRgb) -> Self { + Self { + red: value.red, + green: value.green, + blue: value.blue, + brightness: value.brightness, + } + } +} + +/// Represents the raw RGBC data from the sensor. +#[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] +pub struct OpticalRaw { + /// The red value from the sensor. + pub red: u16, + /// The green value from the sensor. + pub green: u16, + /// The blue value from the sensor. + pub blue: u16, + /// The clear value from the sensor. + pub clear: u16, +} + +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 { + /// Gesture detection is not enabled for this sensor. + GestureDetectionDisabled, + + #[snafu(display("{source}"), context(false))] + /// Generic port related error. + Port { + /// The source of the error + source: PortError, + }, +} diff --git a/packages/vex-devices/src/smart/rotation.rs b/packages/vex-devices/src/smart/rotation.rs new file mode 100644 index 00000000..d4a4ad09 --- /dev/null +++ b/packages/vex-devices/src/smart/rotation.rs @@ -0,0 +1,117 @@ +//! Rotation sensor device. +//! +//! Rotation sensors operate on the same [`Position`] type as motors to measure rotation. + +use pros_core::error::PortError; +use vex_sdk::{ + vexDeviceAbsEncAngleGet, vexDeviceAbsEncPositionGet, vexDeviceAbsEncPositionSet, + vexDeviceAbsEncReset, vexDeviceAbsEncReverseFlagGet, vexDeviceAbsEncReverseFlagSet, + vexDeviceAbsEncStatusGet, vexDeviceAbsEncVelocityGet, +}; + +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, +} + +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, direction: Direction) -> Result { + let mut sensor = Self { port }; + + sensor.reset()?; + sensor.set_direction(direction)?; + + Ok(sensor) + } + + /// Sets the position to zero. + pub fn reset(&mut self) -> Result<(), PortError> { + self.validate_port()?; + + unsafe { + vexDeviceAbsEncReset(self.device_handle()); + } + + Ok(()) + } + + /// Sets the position. + pub fn set_position(&mut self, position: Position) -> Result<(), PortError> { + 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_direction(&mut self, direction: Direction) -> Result<(), PortError> { + self.validate_port()?; + + unsafe { vexDeviceAbsEncReverseFlagSet(self.device_handle(), direction.is_reverse()) } + + Ok(()) + } + + /// 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, + }, + ) + } + + /// Get the total number of degrees rotated by the sensor based on direction. + pub fn position(&self) -> Result { + 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()) }) + } +} + +impl SmartDevice for RotationSensor { + fn port_index(&self) -> u8 { + self.port.index() + } + + fn device_type(&self) -> SmartDeviceType { + SmartDeviceType::Rotation + } +} diff --git a/packages/vex-devices/src/smart/vision.rs b/packages/vex-devices/src/smart/vision.rs new file mode 100644 index 00000000..5861e6c8 --- /dev/null +++ b/packages/vex-devices/src/smart/vision.rs @@ -0,0 +1,860 @@ +//! Vision sensor device module. +//! +//! This module provides an interface for interacting with the VEX Vision Sensor. +//! +//! # Hardware Overview +//! +//! The VEX Vision Sensor is a device powered by an ARM Cortex M4 and Cortex M0 coprocessor +//! with a color camera for the purpose of performing object recognition. The sensor can be +//! trained to locate objects by color. The camera module itself is very similar internally +//! to the Pixy2 camera, and performs its own onboard image processing. Manually processing +//! raw image data from the sensor is not currently possible. +//! +//! Every 200 milliseconds, the camera provides a list of the objects found matching up +//! to seven unique [`VisionSignature`]s. The object’s height, width, and location is provided. +//! Multi-colored objects may also be programmed through the use of [`VisionCode`]s. +//! +//! The Vision Sensor has USB for a direct connection to a computer, where it can be configured +//! using VEX's proprietary vision utility tool to generate color signatures. The Vision Sensor +//! also has WiFi Direct and can act as web server, allowing a live video feed of the camera +//! from any computer equipped with a browser and WiFi. + +extern crate alloc; + +use alloc::vec::Vec; +use core::time::Duration; + +use pros_core::error::PortError; +use snafu::Snafu; +use vex_sdk::{ + vexDeviceVisionBrightnessGet, vexDeviceVisionBrightnessSet, vexDeviceVisionLedColorGet, + vexDeviceVisionLedColorSet, vexDeviceVisionLedModeGet, vexDeviceVisionLedModeSet, + vexDeviceVisionModeGet, vexDeviceVisionModeSet, vexDeviceVisionObjectCountGet, + vexDeviceVisionObjectGet, vexDeviceVisionSignatureGet, vexDeviceVisionSignatureSet, + vexDeviceVisionWhiteBalanceGet, vexDeviceVisionWhiteBalanceModeGet, + vexDeviceVisionWhiteBalanceModeSet, vexDeviceVisionWhiteBalanceSet, vexDeviceVisionWifiModeGet, + vexDeviceVisionWifiModeSet, V5VisionBlockType, V5VisionLedMode, V5VisionMode, V5VisionWBMode, + V5VisionWifiMode, V5_DeviceVisionObject, V5_DeviceVisionRgb, V5_DeviceVisionSignature, +}; + +use super::{SmartDevice, SmartDeviceInternal, SmartDeviceType, SmartPort}; +use crate::color::Rgb; + +/// VEX Vision Sensor +/// +/// This struct represents a vision sensor plugged into a smart port. +#[derive(Debug, Eq, PartialEq)] +pub struct VisionSensor { + port: SmartPort, + codes: Vec, +} + +impl VisionSensor { + /// The horizontal resolution of the vision sensor. + /// + /// This value is based on the `VISION_FOV_WIDTH` macro constant in PROS. + pub const RESOLUTION_WIDTH: u16 = 316; + + /// The vertical resolution of the vision sensor. + /// + /// This value is based on the `VISION_FOV_HEIGHT` msacro constant in PROS. + pub const RESOLUTION_HEIGHT: u16 = 212; + + /// The update rate of the vision sensor. + pub const UPDATE_RATE: Duration = Duration::from_millis(50); + + /// Creates a new vision sensor on a smart port. + /// + /// # Examples + /// + /// ``` + /// // Register a vision sensor on port 1. + /// let mut sensor = VisionSensor::new(peripherals.port_1); + /// ``` + pub fn new(port: SmartPort, mode: VisionMode) -> Result { + let mut sensor = Self { + port, + codes: Vec::new(), + }; + + sensor.set_mode(mode)?; + + Ok(sensor) + } + + /// Adds a detection signature to the sensor's onboard memory. This signature will be used to + /// identify objects when using [`VisionSensor::objects`]. + /// + /// The sensor can store up to 7 unique signatures, with each signature slot denoted by the + /// [`VisionSignature::id`] field. If a signature with an ID matching an existing signature + /// on the sensor is added, then the existing signature will be overwritten with the new one. + /// + /// # Volatile Memory + /// + /// The memory on the Vision Sensor is *volatile* and will therefore be wiped when the sensor + /// loses power. As a result, this function should be called every time the sensor is used on + /// program start. + pub fn set_signature(&mut self, id: u8, signature: VisionSignature) -> Result<(), VisionError> { + if !(1..7).contains(&id) { + return Err(VisionError::InvalidId); + } + + self.validate_port()?; + + let mut signature = V5_DeviceVisionSignature { + id, + uMin: signature.u_threshold.0, + uMean: signature.u_threshold.1, + uMax: signature.u_threshold.2, + vMin: signature.v_threshold.0, + vMean: signature.v_threshold.1, + vMax: signature.v_threshold.2, + range: signature.range, + mType: if self + .codes + .into_iter() + .any(|code| code.contains_signature(id)) + { + V5VisionBlockType::kVisionTypeColorCode + } else { + V5VisionBlockType::kVisionTypeNormal + } as _, + ..Default::default() + }; + + unsafe { vexDeviceVisionSignatureSet(self.device_handle(), &mut signature) } + + Ok(()) + } + + fn raw_signature(&self, id: u8) -> Result { + if !(1..7).contains(&id) { + return Err(VisionError::InvalidId); + } + + let mut raw_signature = V5_DeviceVisionSignature::default(); + let read_operation = unsafe { + vexDeviceVisionSignatureGet(self.device_handle(), id as u32, &mut raw_signature) + }; + + // pad[0] is actually an undocumented flags field on V5_DeviceVisionSignature. If the sensor returns + // no flags, then it has failed to send data back. + // + // TODO: Make sure this is correct and not the PROS docs being wrong here. + // + // We also check that the read operation succeeded from the return of vexDeviceVisionSignatureGet. + if !read_operation || raw_signature.pad[0] == 0 { + return Err(VisionError::ReadingFailed); + } + + Ok(raw_signature) + } + + fn set_signature_type(&mut self, id: u8, sig_type: u32) -> Result<(), VisionError> { + let mut raw_sig = self.raw_signature(id)?; + + raw_sig.mType = sig_type; + + unsafe { vexDeviceVisionSignatureSet(self.device_handle(), &mut raw_sig) } + + Ok(()) + } + + /// Get a signature from the sensor's onboard volatile memory. + pub fn signature(&self, id: u8) -> Result { + self.validate_port()?; + + Ok(self.raw_signature(id)?.into()) + } + + /// Registers a color code to the sensor's onboard memory. This code will be used to identify objects + /// when using [`VisionSensor::objects`]. + /// + /// Color codes are effectively "signature groups" that the sensor will use to identify objects + /// containing the color of their signatures next to each other. + /// + /// # Volatile Memory + /// + /// The onboard memory of the Vision Sensor is *volatile* and will therefore be wiped when the + /// sensor loses its power source. As a result, this function should be called every time the + /// sensor is used on program start. + pub fn add_code(&mut self, code: impl Into) -> Result<(), VisionError> { + self.validate_port()?; + + let device = self.device_handle(); + + let code = code.into(); + + unsafe { + self.set_signature_type(code.0, V5VisionBlockType::kVisionTypeColorCode as _)?; + self.set_signature_type(code.1, V5VisionBlockType::kVisionTypeColorCode as _)?; + if let Some(sig_3) = code.2 { + self.set_signature_type(sig_3, V5VisionBlockType::kVisionTypeColorCode as _)?; + } + if let Some(sig_4) = code.3 { + self.set_signature_type(sig_4, V5VisionBlockType::kVisionTypeColorCode as _)?; + } + if let Some(sig_5) = code.4 { + self.set_signature_type(sig_5, V5VisionBlockType::kVisionTypeColorCode as _)?; + } + } + + self.codes.push(code); + + Ok(()) + } + + /// Get the current brightness setting of the vision sensor as a percentage. + /// + /// The returned result should be from `0.0` (0%) to `1.0` (100%). + pub fn brightness(&self) -> Result { + self.validate_port()?; + + // SDK function gives us brightness percentage 0-100. + Ok(unsafe { vexDeviceVisionBrightnessGet(self.device_handle()) } as f64 / 100.0) + } + + /// Get the current white balance of the vision sensor as an RGB color. + pub fn white_balance(&self) -> Result { + self.validate_port()?; + + let handle = self.device_handle(); + + Ok( + match unsafe { vexDeviceVisionWhiteBalanceModeGet(handle) } { + V5VisionWBMode::kVisionWBNormal => WhiteBalance::Auto, + V5VisionWBMode::kVisionWBStart => WhiteBalance::StartupAuto, + V5VisionWBMode::kVisionWBManual => { + WhiteBalance::Manual(unsafe { vexDeviceVisionWhiteBalanceGet(handle) }.into()) + } + }, + ) + } + + /// Sets the brightness percentage of the vision sensor. Should be between 0.0 and 1.0. + pub fn set_brightness(&mut self, brightness: f64) -> Result<(), VisionError> { + self.validate_port()?; + + unsafe { vexDeviceVisionBrightnessSet(self.device_handle(), (brightness * 100.0) as u8) } + + Ok(()) + } + + /// Sets the white balance of the vision sensor. + /// + /// White balance can be either automatically set or manually set through an RGB color. + pub fn set_white_balance(&mut self, white_balance: WhiteBalance) -> Result<(), VisionError> { + self.validate_port()?; + + unsafe { vexDeviceVisionWhiteBalanceModeSet(self.device_handle(), white_balance.into()) } + + if let WhiteBalance::Manual(rgb) = white_balance { + unsafe { + vexDeviceVisionWhiteBalanceSet( + self.device_handle(), + V5_DeviceVisionRgb { + red: rgb.red(), + green: rgb.green(), + blue: rgb.blue(), + + // Pretty sure this field does nothing, but PROS sets it to this. + // + // TODO: Run some hardware tests to see if this value actually influences + // white balance. Based on the Pixy2 API, I doubt it and bet this is just + // here for the LED setter, which uses the same type. + brightness: 255, + }, + ) + } + } + + Ok(()) + } + + /// Configure the behavior of the LED indicator on the sensor. + /// + /// The default behavior is represented by [`LedMode::Auto`], which will display the color of the most prominent + /// detected object's signature color. Alternatively, the LED can be configured to display a single RGB color. + pub fn set_led_mode(&mut self, mode: LedMode) -> Result<(), VisionError> { + self.validate_port()?; + + unsafe { vexDeviceVisionLedModeSet(self.device_handle(), mode.into()) } + + if let LedMode::Manual(rgb, brightness) = mode { + unsafe { + vexDeviceVisionLedColorSet( + self.device_handle(), + V5_DeviceVisionRgb { + red: rgb.red(), + green: rgb.green(), + blue: rgb.blue(), + brightness: (brightness * 100.0) as u8, + }, + ) + } + } + + Ok(()) + } + + /// Get the user-set behavior of the LED indicator on the sensor. + pub fn led_mode(&self) -> Result { + self.validate_port()?; + + Ok( + match unsafe { vexDeviceVisionLedModeGet(self.device_handle()) } { + V5VisionLedMode::kVisionLedModeAuto => LedMode::Auto, + V5VisionLedMode::kVisionLedModeManual => { + let led_color = unsafe { vexDeviceVisionLedColorGet(self.device_handle()) }; + + LedMode::Manual( + Rgb::new(led_color.red, led_color.green, led_color.blue), + led_color.brightness as f64 / 100.0, + ) + } + }, + ) + } + + /// Returns a [`Vec`] of objects detected by the sensor. + pub fn objects(&self) -> Result, VisionError> { + if self.mode()? == VisionMode::Wifi { + return Err(VisionError::WifiMode); + } + + let device = self.device_handle(); + + let object_count = unsafe { vexDeviceVisionObjectCountGet(device) } as usize; + let mut objects = Vec::with_capacity(object_count); + + for i in 0..object_count { + let mut object = V5_DeviceVisionObject::default(); + + if unsafe { vexDeviceVisionObjectGet(device, i as u32, &mut object) } == 0 { + return Err(VisionError::ReadingFailed); + } + + let object: VisionObject = object.into(); + + match object.source { + DetectionSource::Signature(_) | DetectionSource::Line => { + objects.push(object); + } + DetectionSource::Code(code) => { + if self.codes.contains(&code) { + objects.push(object); + } + } + } + } + + Ok(objects) + } + + /// Returns the number of objects detected by the sensor. + pub fn object_count(&self) -> Result { + // NOTE: We actually can't rely on [`vexDeviceVisionObjectCountGet`], due to the way that + // vision codes are registered. + // + // When a code is registered, all this really does is set a bunch of normal signatures with + // an additional flag set (see: [`Self::set_code_signature`]). This means that if the user + // has multiple vision codes, we can't distinguish between which objects were detected by + // a certain code until AFTER we get the full objects list (where we can then distinguish) + // by [`VisionObject::source`]. + Ok(self.objects()?.len()) + } + + /// Sets the vision sensor's detection mode. See [`VisionMode`] for more information on what + /// each mode does. + pub fn set_mode(&self, mode: VisionMode) -> Result<(), VisionError> { + self.validate_port()?; + + let device = self.device_handle(); + + unsafe { + vexDeviceVisionWifiModeSet( + device, + match mode { + VisionMode::Wifi => V5VisionWifiMode::kVisionWifiModeOn, + _ => V5VisionWifiMode::kVisionWifiModeOff, + }, + ); + + vexDeviceVisionModeSet( + device, + match mode { + VisionMode::ColorDetection => V5VisionMode::kVisionModeNormal, + VisionMode::LineDetection => V5VisionMode::kVisionModeLineDetect, + VisionMode::MixedDetection => V5VisionMode::kVisionModeMixed, + // If the user requested WiFi mode, then we already set + // it around 14 lines ago, so there's nothing to do here. + VisionMode::Wifi => return Ok(()), + VisionMode::Test => V5VisionMode::kVisionTypeTest, + }, + ); + } + + Ok(()) + } + + /// Gets the current detection mode that the sensor is in. + pub fn mode(&self) -> Result { + self.validate_port()?; + + let device = self.device_handle(); + + if unsafe { vexDeviceVisionWifiModeGet(device) } == V5VisionWifiMode::kVisionWifiModeOn { + return Ok(VisionMode::Wifi); + } + + Ok(unsafe { vexDeviceVisionModeGet(device) }.into()) + } +} + +impl SmartDevice for VisionSensor { + fn port_index(&self) -> u8 { + self.port.index() + } + + fn device_type(&self) -> SmartDeviceType { + SmartDeviceType::Vision + } +} + +/// A vision detection color signature. +/// +/// Vision signatures contain information used by the vision sensor to detect objects of a certain +/// color. These signatures are typically generated through VEX's vision utility tool rather than +/// written by hand. For creating signatures using the utility, see [`from_utility`]. +/// +/// [`from_utility`]: VisionSignature::from_utility +/// +/// # Format & Detection Overview +/// +/// Vision signatures operate in a version of the Y'UV color space, specifically using the "U" and "V" +/// chroma components for edge detection purposes. This can be seen in the `u_threshold` and +/// `v_threshold` fields of this struct. These fields place three "threshold" (min, max, mean) +/// values on the u and v chroma values detected by the sensor. The values are then transformed to a +/// 3D lookup table to detect actual colors. +/// +/// There is additionally a `range` field, which works as a scale factor or threshold for how lenient +/// edge detection should be. +/// +/// Signatures can additionally be grouped together into [`VisionCode`]s, which narrow the filter for +/// object detection by requiring two colors. +#[derive(Debug, Copy, Clone, PartialEq)] +pub struct VisionSignature { + /// The (min, max, mean) values on the "U" axis. + /// + /// This defines a threshold of values for the sensor to match against a certain chroma in the + /// Y'UV color space - speciailly on the U component. + pub u_threshold: (i32, i32, i32), + + /// The (min, max, mean) values on the V axis. + /// + /// This defines a threshold of values for the sensor to match against a certain chroma in the + /// Y'UV color space - speciailly on the "V" component. + pub v_threshold: (i32, i32, i32), + + /// The signature range scale factor. + /// + /// This value effectively serves as a threshold for how lenient the sensor should be + /// when detecting the edges of colors. This value ranges from 0-11 in Vision Utility. + /// + /// Higher values of `range` will increase the range of brightness that the sensor will + /// consider to be part of the signature. Lighter/Darker shades of the signature's color + /// will be detected more often. + pub range: f32, + + /// The signature's flags. + pub flags: u8, +} + +impl VisionSignature { + /// Create a [`VisionSignature`]. + /// + /// # Examples + pub const fn new( + u_threshold: (i32, i32, i32), + v_threshold: (i32, i32, i32), + range: f32, + ) -> Self { + Self { + flags: 0, + u_threshold, + v_threshold, + range, + } + } + + /// Create a [`VisionSignature`] using the same format as VEX's Vision Utility tool. + /// + /// # Panics + /// + /// Panics if the provided `id` is equal to 0. Signature IDs are internally stored as + /// [`NonZeroU8`], and the IDs given by Vision Utility should always be from 1-7. + /// + /// # Examples + /// + /// ```` + /// // Register a signature for detecting red objects. + /// // This numbers in this signature was generated using VEX's vision utility app. + /// let my_signature = + /// VisionSignature::from_utility(1, 10049, 11513, 10781, -425, 1, -212, 4.1, 0); + /// ```` + pub const fn from_utility( + _id: u8, // We don't store IDs in our vision signatures. + u_min: i32, + u_max: i32, + u_mean: i32, + v_min: i32, + v_max: i32, + v_mean: i32, + range: f32, + _signature_type: u32, // This is handled automatically by [`VisionSensor::add_code`]. + ) -> Self { + Self { + u_threshold: (u_min, u_max, u_mean), + v_threshold: (v_min, v_max, v_mean), + range, + flags: Default::default(), + } + } +} + +impl From for VisionSignature { + fn from(value: V5_DeviceVisionSignature) -> Self { + Self { + u_threshold: (value.uMin, value.uMax, value.uMean), + v_threshold: (value.vMin, value.vMax, value.vMean), + range: value.range, + flags: value.flags, + } + } +} + +/// A vision detection code. +/// +/// Codes are a special type of detection signature that group multiple [`VisionSignature`]s +/// together. A [`VisionCode`] can associate 2-5 color signatures together, detecting the resulting object +/// when its color signatures are present close to each other. +/// +/// These codes work very similarly to [Pixy2 Color Codes](https://docs.pixycam.com/wiki/doku.php?id=wiki:v2:using_color_codes). +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +pub struct VisionCode( + pub u8, + pub u8, + pub Option, + pub Option, + pub Option, +); + +impl VisionCode { + /// Creates a new vision code. + /// + /// Two signatures are required to create a vision code, with an additional three + /// optional signatures. + pub const fn new( + sig_1: u8, + sig_2: u8, + sig_3: Option, + sig_4: Option, + sig_5: Option, + ) -> Self { + Self(sig_1, sig_2, sig_3, sig_4, sig_4) + } + + /// Creates a [`VisionCode`] from a bit representation of its signature IDs. + pub const fn from_id(id: u16) -> Self { + const MASK: u16 = (1 << 3) - 1; + + Self( + ((id >> 12) & MASK) as u8, + ((id >> 9) & MASK) as u8, + match ((id >> 6) & MASK) as u8 { + 0 => None, + sig => Some(sig), + }, + match ((id >> 3) & MASK) as u8 { + 0 => None, + sig => Some(sig), + }, + match (id & MASK) as u8 { + 0 => None, + sig => Some(sig), + }, + ) + } + + /// Returns `true` if a given signature ID is stored in this code. + pub const fn contains_signature(&self, id: u8) -> bool { + if self.0 == id || self.1 == id { + return true; + } + + if let Some(sig_3) = self.2 { + if sig_3 == id { + return true; + } + } + if let Some(sig_4) = self.3 { + if sig_4 == id { + return true; + } + } + if let Some(sig_5) = self.4 { + if sig_5 == id { + return true; + } + } + + return false; + } + + /// Returns the internal ID used by the sensor to determine which signatures + /// belong to which code. + pub const fn id(&self) -> u16 { + let mut id: u16 = 0; + + id = (id << 3) | self.0 as u16; + id = (id << 3) | self.1 as u16; + id = (id << 3) | self.2.unwrap_or_default() as u16; + id = (id << 3) | self.3.unwrap_or_default() as u16; + id = (id << 3) | self.4.unwrap_or_default() as u16; + + id + } +} + +impl From<(u8, u8)> for VisionCode { + /// Convert a tuple of two [`VisionSignatures`] into a [`VisionCode`]. + fn from(signatures: (u8, u8)) -> Self { + Self(signatures.0, signatures.1, None, None, None) + } +} + +impl From<(u8, u8, u8)> for VisionCode { + /// Convert a tuple of three [`VisionSignatures`] into a [`VisionCode`]. + fn from(signatures: (u8, u8, u8)) -> Self { + Self(signatures.0, signatures.1, Some(signatures.2), None, None) + } +} + +impl From<(u8, u8, u8, u8)> for VisionCode { + /// Convert a tuple of four [`VisionSignatures`] into a [`VisionCode`]. + fn from(signatures: (u8, u8, u8, u8)) -> Self { + Self( + signatures.0, + signatures.1, + Some(signatures.2), + Some(signatures.3), + None, + ) + } +} + +impl From<(u8, u8, u8, u8, u8)> for VisionCode { + /// Convert a tuple of five [`VisionSignatures`] into a [`VisionCode`]. + fn from(signatures: (u8, u8, u8, u8, u8)) -> Self { + Self( + signatures.0, + signatures.1, + Some(signatures.2), + Some(signatures.3), + Some(signatures.4), + ) + } +} + +#[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] +pub enum VisionMode { + /// Uses color signatures and codes to identify objects in blocks. + #[default] + ColorDetection, + + /// Uses line tracking to identify lines. + LineDetection, + + /// Both color signatures and lines will be detected as objects. + MixedDetection, + + /// Sets the sensor into "wifi mode", which disables all forms of object detection and + /// enables the sensor's onboard Wi-Fi hotspot for streaming camera data over a webserver. + /// + /// Once enabled, the sensor will create a wireless network with an SSID + /// in the format of of VISION_XXXX. The sensor's camera feed is available + /// at `192.168.1.1`. + /// + /// This mode will be automatically disabled when connected to field control. + Wifi, + + /// Unknown use. + Test, +} + +impl From for VisionMode { + fn from(value: V5VisionMode) -> Self { + match value { + V5VisionMode::kVisionModeNormal => Self::ColorDetection, + V5VisionMode::kVisionModeLineDetect => Self::LineDetection, + V5VisionMode::kVisionModeMixed => Self::MixedDetection, + V5VisionMode::kVisionTypeTest => Self::Test, + } + } +} + +/// Defines a source for what method was used to detect a [`VisionObject`]. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum DetectionSource { + /// A normal vision signature not associated with a color code was used to detect this object. + Signature(u8), + + /// Multiple signatures joined in a color code were used to detect this object. + Code(VisionCode), + + /// Line detection was used to find this object. + Line, +} + +/// A detected vision object. +/// +/// This struct contains metadata about objects detected by the vision sensor. Objects are +/// detected by calling [`VisionSensor::objects`] after adding signatures and color codes +/// to the sensor. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub struct VisionObject { + /// The ID of the signature or color code used to detect this object. + pub source: DetectionSource, + + /// The width of the detected object's bounding box in pixels. + pub width: u16, + + /// The height of the detected object's bounding box in pixels. + pub height: u16, + + /// The top-left coordinate of the detected object relative to the top-left + /// of the camera's field of view. + pub offset: mint::Point2, + + /// The center coordinate of the detected object relative to the top-left + /// of the camera's field of view. + pub center: mint::Point2, + + /// The approximate degrees of rotation of the detected object's bounding box. + pub angle: u16, +} + +impl From for VisionObject { + fn from(value: V5_DeviceVisionObject) -> Self { + Self { + source: match value.r#type { + V5VisionBlockType::kVisionTypeColorCode => { + DetectionSource::Code(VisionCode::from_id(value.signature)) + } + V5VisionBlockType::kVisionTypeNormal => { + DetectionSource::Signature(value.signature as u8) + } + V5VisionBlockType::kVisionTypeLineDetect => DetectionSource::Line, + }, + width: value.width, + height: value.height, + offset: mint::Point2 { + x: value.xoffset, + y: value.yoffset, + }, + center: mint::Point2 { + x: value.xoffset + (value.width / 2), + y: value.yoffset + (value.height / 2), + }, + angle: value.angle * 10, + } + } +} + +/// Vision Sensor white balance mode. +/// +/// Represents a white balance configuration for the vision sensor's camera. +#[derive(Default, Debug, Clone, Copy, PartialEq, Eq)] +pub enum WhiteBalance { + /// Automatic Mode + /// + /// The sensor will automatically adjust the camera's white balance, using the brightest + /// part of the image as a white point. + #[default] + Auto, + + /// "Startup" Automatic Mode + /// + /// The sensor will automatically adjust the camera's white balance, but will only perform + /// this adjustment once on power-on. + StartupAuto, + + /// Manual Mode + /// + /// Allows for manual control over white balance using an RGB color. + Manual(Rgb), +} + +impl From for V5VisionWBMode { + fn from(value: WhiteBalance) -> Self { + match value { + WhiteBalance::Auto => Self::kVisionWBNormal, + WhiteBalance::StartupAuto => Self::kVisionWBStart, + WhiteBalance::Manual(_) => Self::kVisionWBManual, + } + } +} + +/// Vision Sensor LED mode. +/// +/// Represents the states that the integrated LED indicator on a vision sensor can be in. +#[derive(Default, Debug, Clone, Copy, PartialEq)] +pub enum LedMode { + /// Automatic Mode + /// + /// When in automatic mode, the integrated LED will display the color of the most prominent + /// detected object's signature color. + #[default] + Auto, + + /// Manual Mode + /// + /// When in manual mode, the integrated LED will display a user-set RGB color code and brightness + /// percentage from 0.0-1.0. + Manual(Rgb, f64), +} + +impl From for V5VisionLedMode { + fn from(value: LedMode) -> Self { + match value { + LedMode::Auto => Self::kVisionLedModeAuto, + LedMode::Manual(_, _) => Self::kVisionLedModeManual, + } + } +} + +impl From for Rgb { + fn from(value: V5_DeviceVisionRgb) -> Self { + Self::new(value.red, value.green, value.blue) + } +} + +#[derive(Debug, Snafu)] +/// Errors that can occur when using a vision sensor. +pub enum VisionError { + /// Objects cannot be read while wifi mode is enabled. + WifiMode, + + /// The given signature ID or argument is out of range. + InvalidId, + + /// The camera could not be read. + ReadingFailed, + + /// Generic port related error. + #[snafu(display("{source}"), context(false))] + Port { + /// The source of the error. + source: PortError, + }, +} diff --git a/packages/pros-devices/src/usd.rs b/packages/vex-devices/src/usd.rs similarity index 71% rename from packages/pros-devices/src/usd.rs rename to packages/vex-devices/src/usd.rs index d8abd27f..27cc735a 100644 --- a/packages/pros-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_sdk::vexFileDriveStatus; + /// Checks if an SD card is installed. pub fn usd_installed() -> bool { - unsafe { pros_sys::misc::usd_is_installed() == 1 } + unsafe { vexFileDriveStatus(0) } }