From 29fd5b7c70ca8e7318a57851b75d4bf9563b1feb Mon Sep 17 00:00:00 2001 From: AtomicGamer9523 Date: Mon, 18 Dec 2023 08:01:29 -0800 Subject: [PATCH] Added Pos2D --- arc/Cargo.toml | 6 +- arc/__init__.pyi | 22 +- arc/__init__.rs | 129 ++++++---- arc/hardware/__init__.rs | 32 +-- arc/hardware/gamepad.pyi | 53 ++++ arc/hardware/gamepad.rs | 350 +++++++++++++++++++++++--- arc/lib.rs | 42 +++- arc/macros.rs | 108 ++++++++ arc/math/__init__.rs | 21 ++ arc/math/angle.rs | 165 ++++++++++++ arc/math/pos2d.pyi | 11 +- arc/math/pos2d.rs | 102 ++++++++ arc/math/vec2d.pyi | 26 +- arc/math/vec2d.rs | 168 +++++++++++++ arc/pyproject.toml | 14 ++ arc/threadsafe.rs | 217 +++++++++------- examples/auto.py | 8 +- examples/util/drive.py | 48 ++++ examples/util/mecanum_drive.py | 14 ++ libs/helper/libtrig/angle.rs | 38 +++ libs/helper/libtrig/coords/coord2d.rs | 34 ++- libs/helper/libtrig/lib.rs | 3 + libs/helper/libtrig/pos2d.rs | 265 +++++++++++++++++++ libs/helper/macros-core/lib.rs | 3 +- libs/helper/macros/lib.rs | 3 +- libs/robot/README.md | 0 libs/robot/hardware/gamepad/impls.rs | 139 ++++++---- libs/robot/hardware/gamepad/mod.rs | 118 ++++++--- libs/robot/hardware/lib.rs | 79 +++++- libs/robot/main.rs | 60 +++-- pyproject.toml | 4 + 31 files changed, 1965 insertions(+), 317 deletions(-) create mode 100644 arc/macros.rs create mode 100644 arc/math/angle.rs create mode 100644 arc/math/pos2d.rs create mode 100644 arc/math/vec2d.rs create mode 100644 arc/pyproject.toml create mode 100644 examples/util/drive.py create mode 100644 examples/util/mecanum_drive.py create mode 100644 libs/helper/libtrig/pos2d.rs create mode 100644 libs/robot/README.md diff --git a/arc/Cargo.toml b/arc/Cargo.toml index 8b5297d..b211989 100644 --- a/arc/Cargo.toml +++ b/arc/Cargo.toml @@ -11,10 +11,14 @@ license.workspace = true path = "lib.rs" crate-type = ["cdylib", "rlib"] -[dependencies.hardware] +[dependencies.arc_robot_hardware] package = "arc-robot-hardware" path = "../libs/robot/hardware" +[dependencies.libtrig] +package = "libtrig" +path = "../libs/helper/libtrig" + [dependencies.pyo3] #!!! Important - DO NOT ENABLE extension-module FEATURE HERE!!! version = "0.20.0" diff --git a/arc/__init__.pyi b/arc/__init__.pyi index 9122eb7..a7b3cd7 100644 --- a/arc/__init__.pyi +++ b/arc/__init__.pyi @@ -8,9 +8,20 @@ Additionally, it is easy to extend, so that you can add your own functionality t """ from .hardware.gamepad import Gamepad as _Gamepad -from typing import Callable as _Callable +import typing as _typing -type RunResult = bool | int | str | None +RunResult = _typing.Union[bool, int, str, None] +""" +This type is used to indicate whether or not an `Op` completed successfully. + +If the `Op` completed successfully, then it should return `OK` (or `None`). + +If the `Op` did not complete successfully, then it should return an object that +provides information about why it did not complete successfully: +- If it's a `bool`, then it should be False. +- If it's an `int`, then it should be the error code. +- If it's a `str`, then it should be the error message. +""" class Op(object): """Represents an operation that can be run on the robot.""" @@ -25,11 +36,11 @@ class Op(object): """Whether or not the operation is running.""" ... -def Auto(func: _Callable[[Op], RunResult]) -> _Callable[[Op], RunResult]: +def Auto(name: str) -> _typing.Callable[[_typing.Callable[[Op], RunResult]], _typing.Callable[[Op], RunResult]]: """Decorator for an autonomous operation.""" ... -def Teleop(func: _Callable[[Op], RunResult]) -> _Callable[[Op], RunResult]: +def Teleop(name: str) -> _typing.Callable[[_typing.Callable[[Op], RunResult]], _typing.Callable[[Op], RunResult]]: """Decorator for a teleop operation.""" ... @@ -38,3 +49,6 @@ def sleep(seconds: float) -> None: ... OK: RunResult = True +""" +A built-in `RunResult` that indicates that the `Op` completed successfully. +""" diff --git a/arc/__init__.rs b/arc/__init__.rs index 4d9045e..cf272fd 100644 --- a/arc/__init__.rs +++ b/arc/__init__.rs @@ -1,14 +1,20 @@ +//! The rust bindings for the root of the arc library +//! +//! This module contains the [`Op`] struct, which is the main struct that +//! is used to access the robot's hardware. +//! +//! [`Op`]: struct.Op.html + use crate::threadsafe::{self, ThreadSafe}; use pyo3::prelude::*; /// Hardware submodule #[path = "hardware/__init__.rs"] -pub mod _hardware; +pub mod hardware; -#[doc(hidden)] -fn make_err(e: &'static str) -> PyErr { - PyErr::new::(e) -} +/// Math submodule +#[path = "math/__init__.rs"] +pub mod math; /// The struct that actually contains the necessary data for the op mode /// to run. @@ -18,7 +24,8 @@ fn make_err(e: &'static str) -> PyErr { #[derive(Debug)] pub struct OpHolder { running: threadsafe::ThreadSafeBool, - gamepad: _hardware::gamepad::Gamepad, + gamepad: hardware::gamepad::Gamepad, + start_time: std::time::Instant, } impl OpHolder { @@ -34,19 +41,26 @@ impl OpHolder { /// Returns a reference to the gamepad /// /// This call aquires a lock on the data - pub fn gamepad(&self) -> &_hardware::gamepad::Gamepad { + pub fn gamepad(&self) -> &hardware::gamepad::Gamepad { &self.gamepad } /// Stops the op mode /// - /// DO NOT CALL THIS FROM THE OP MODE THREAD - pub fn stop(&self) { - self.running.get_mut().unwrap().set(false); + /// DO NOT CALL THIS FROM THE PYTHON THREAD + pub fn stop(&self) -> core::result::Result<(), &'static str> { + self.running.get_mut()?.set(false); + Ok(()) + } + /// Returns the running time of the op mode + /// + /// This call does not aquire a lock on the data, + /// nor does it need to. + pub fn running_time(&self) -> core::time::Duration { + std::time::Instant::now() - self.start_time } } -unsafe impl Send for OpHolder {} -unsafe impl Sync for OpHolder {} +threadsafe::thread_safe!(OpHolder); /// The struct that is used to access the data in the op mode /// @@ -59,43 +73,60 @@ unsafe impl Sync for OpHolder {} /// /// # Example /// -/// ```rust,no_run,ignore -/// let op = pylib::Op::new(gamepad_wrapper); -/// let op_wrapper = pylib::Op::wrap(&op); +/// ```rust +/// # use pyo3::prelude::*; +/// # use arc_pylib as pylib; +/// # use arc_pylib::arc_robot_hardware as hardware; +/// # use hardware::IO_OK; +/// # use pylib::PyWrappedComponent as _; +/// # use pylib::__init__::Op; +/// # use pylib::setup_wrapped_component; +/// # fn main() -> hardware::Result { +/// # let (gamepad, gamepad_wrapper) = setup_wrapped_component!( +/// # pylib::arc_robot_hardware::gamepad::impls::LogitechF310::default(); +/// # pylib::__init__::hardware::gamepad::Gamepad +/// # ); +/// let (op, op_wrapper) = setup_wrapped_component!(gamepad_wrapper; Op); /// /// // IO Thread -/// op.get_mut()? +/// let time = op.get()?.running_time(); +/// if time.as_secs() >= 30 { // If we want to stop after 30 seconds +/// op.get_mut()?.stop()?; +/// } +/// # IO_OK +/// # } /// ``` #[pyclass] #[derive(Debug, Clone)] pub struct Op(ThreadSafe); -impl Op { - /// This creates a new `ThreadSafe` struct. NOT a `Op` struct. - /// - /// You then need to wrap it in a `Op` struct using the [`Op::wrap()`] method. - pub fn new(gamepad: _hardware::gamepad::Gamepad) -> ThreadSafe { +impl crate::PyWrappedComponent for Op { + type Holder = OpHolder; + fn new(gamepad: hardware::gamepad::Gamepad) -> ThreadSafe { ThreadSafe::new(OpHolder { + start_time: std::time::Instant::now(), running: true.into(), gamepad, }) } - /// Wraps a `ThreadSafe` in a `Op` struct. - pub fn wrap(op: &ThreadSafe) -> Self { - Self(op.clone()) + fn wrap(gamepad: &ThreadSafe) -> Self { + Self(gamepad.clone()) } +} + +impl Op { /// Returns a new [`Gamepad`] struct that is a clone of the one in the op mode. /// /// (It's an `Arc` so it's cheap to clone) /// /// [`Gamepad`]: _hardware/gamepad/struct.Gamepad.html - pub fn get_gamepad(&self) -> threadsafe::TSResult<_hardware::gamepad::Gamepad> { + pub fn get_gamepad(&self) -> core::result::Result { self.0.get().map(|g| g.gamepad().clone()) } /// Returns whether the op mode is running /// /// This call aquires a lock on the data - pub fn is_running(&self) -> threadsafe::TSResult { + pub fn is_running(&self) -> core::result::Result { self.0.get().map(|g| g.running()) } } @@ -106,12 +137,12 @@ impl Op { #[getter] #[doc(hidden)] fn running(&self) -> PyResult { - self.is_running().map_err(make_err) + self.is_running().map_err(crate::make_err) } #[getter] #[doc(hidden)] - fn gamepad(&self) -> PyResult<_hardware::gamepad::Gamepad> { - self.get_gamepad().map_err(make_err) + fn gamepad(&self) -> PyResult { + self.get_gamepad().map_err(crate::make_err) } } @@ -130,23 +161,24 @@ fn sleep(seconds: f64) -> PyResult<()> { /// This annotation is used to mark a function as an autonomous function. #[pyclass] #[doc(hidden)] -struct Auto(Py); +struct Auto(String); #[pymethods] impl Auto { #[new] #[doc(hidden)] - fn __new__(wraps: Py) -> Self { - Self(wraps) + fn __new__(name: &str) -> Self { + Self(name.to_string()) } #[doc(hidden)] - #[pyo3(signature = (*args, **kwargs))] + #[pyo3(signature = (*args, **_kwargs))] fn __call__( &self, - py: Python<'_>, + _py: Python<'_>, args: &pyo3::types::PyTuple, - kwargs: Option<&pyo3::types::PyDict>, + _kwargs: Option<&pyo3::types::PyDict>, ) -> PyResult> { - self.0.call(py, args, kwargs) + let func = args.get_item(0)?; + func.extract::>() } } @@ -155,23 +187,24 @@ impl Auto { /// This annotation is used to mark a function as a teleop function. #[pyclass] #[doc(hidden)] -struct Teleop(Py); +struct Teleop(String); #[pymethods] impl Teleop { #[new] #[doc(hidden)] - fn __new__(wraps: Py) -> Self { - Self(wraps) + fn __new__(name: &str) -> Self { + Self(name.to_string()) } #[doc(hidden)] - #[pyo3(signature = (*args, **kwargs))] + #[pyo3(signature = (*args, **_kwargs))] fn __call__( &self, - py: Python<'_>, + _py: Python<'_>, args: &pyo3::types::PyTuple, - kwargs: Option<&pyo3::types::PyDict>, + _kwargs: Option<&pyo3::types::PyDict>, ) -> PyResult> { - self.0.call(py, args, kwargs) + let func = args.get_item(0)?; + func.extract::>() } } @@ -180,15 +213,19 @@ impl Teleop { /// This function is called by the Python interpreter when the module is imported. #[pymodule] #[doc(hidden)] -pub fn arc(_py: Python<'_>, m: &PyModule) -> PyResult<()> { +pub fn arc(py: Python<'_>, m: &PyModule) -> PyResult<()> { m.add_function(wrap_pyfunction!(sleep, m)?)?; m.add_class::()?; m.add_class::()?; m.add_class::()?; m.add("OK", true)?; - // Modules - m.add_wrapped(pyo3::wrap_pymodule!(_hardware::hardware))?; + // Submodules + // + // Currently there is no better way to do this. + // See https://github.com/DranikiRobotics/arc/issues/3 + crate::pymod!(hardware -> hardware::hardware_submodule, "arc.hardware", py, m); + crate::pymod!(math -> math::math_submodule, "arc.math", py, m); Ok(()) } diff --git a/arc/hardware/__init__.rs b/arc/hardware/__init__.rs index 6e47e46..9992579 100644 --- a/arc/hardware/__init__.rs +++ b/arc/hardware/__init__.rs @@ -1,14 +1,18 @@ -use pyo3::prelude::*; - -pub mod gamepad; - -#[pymodule] -pub fn hardware(_py: Python<'_>, m: &PyModule) -> PyResult<()> { - // Modules - m.add_wrapped(pyo3::wrap_pymodule!(gamepad::gamepad))?; - - // Their classes - m.add_class::()?; - - Ok(()) -} +//! Hardware module for the arc crate +//! +//! Python identifier: `arc.hardware` + +use pyo3::prelude::*; + +pub mod gamepad; + +/// The hardware module. +pub(crate) fn hardware_submodule(py: Python<'_>, m: &PyModule) -> PyResult<()> { + crate::pymod!(gamepad -> gamepad::gamepad_submodule, "arc.hardware.gamepad", py, m); + + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + + Ok(()) +} diff --git a/arc/hardware/gamepad.pyi b/arc/hardware/gamepad.pyi index 355d0d6..e0b1e8d 100644 --- a/arc/hardware/gamepad.pyi +++ b/arc/hardware/gamepad.pyi @@ -8,6 +8,31 @@ This module contains the Gamepad class, and related classes. class Gamepad(object): """Represents a gamepad with buttons x, y, a, and b.""" + @property + def dpad(self) -> GamepadDpad: + """The state of the dpad.""" + ... + + @property + def left_stick(self) -> GamepadStick: + """The state of the left stick.""" + ... + + @property + def right_stick(self) -> GamepadStick: + """The state of the right stick.""" + ... + + @property + def left_trigger(self) -> float: + """The state of the left trigger.""" + ... + + @property + def right_trigger(self) -> float: + """The state of the right trigger.""" + ... + @property def x(self) -> bool: """Whether or not the x button is pressed.""" @@ -24,6 +49,34 @@ class Gamepad(object): def b(self) -> bool: """Whether or not the b button is pressed.""" ... + + @property + def left_bumper(self) -> bool: + """Whether or not the left bumper is pressed.""" + ... + + @property + def right_bumper(self) -> bool: + """Whether or not the right bumper is pressed.""" + ... + + @property + def back(self) -> bool: + """ + Whether or not the 'back' button is pressed. + + This is a non-standard button. Use with caution. + """ + ... + + @property + def start(self) -> bool: + """ + Whether or not the 'start' button is pressed. + + This is a non-standard button. Use with caution. + """ + ... class GamepadDpad(object): """Represents a dpad on a gamepad.""" diff --git a/arc/hardware/gamepad.rs b/arc/hardware/gamepad.rs index 954e67c..162ca45 100644 --- a/arc/hardware/gamepad.rs +++ b/arc/hardware/gamepad.rs @@ -1,79 +1,363 @@ +//! Gamepad module for the arc crate +//! +//! Python identifier: `arc.hardware.gamepad` + use crate::threadsafe::ThreadSafe; -use hardware::gamepad::MutableGamepad; +use arc_robot_hardware::gamepad::{self as gp, Gamepad as _}; use pyo3::prelude::*; +/// The struct that actually contains the necessary data for the gamepad +/// to function. +/// +/// This struct should only be used for mutating the data outside of the +/// gamepad thread. For reading the up to date data, use the `Gamepad` struct. #[derive(Debug)] pub struct GamepadHolder { - gamepad: Box, + gamepad: Box, +} + +macro_rules! i { + (g $n: ident $ty:ty) => { + #[inline] + fn $n(&self) -> $crate::arc_robot_hardware::Result<$ty> { + self.gamepad.$n() + } + }; + (s $n: ident $ty:ty) => { + #[inline] + fn $n(&mut self, value: $ty) -> $crate::arc_robot_hardware::Result { + self.gamepad.$n(value) + } + }; +} + +impl gp::Gamepad for GamepadHolder { + i!(g a bool); + i!(g b bool); + i!(g x bool); + i!(g y bool); + i!(g left_bumper bool); + i!(g right_bumper bool); + i!(g back bool); + i!(g start bool); + i!(g left_trigger f64); + i!(g right_trigger f64); + i!(g left_stick gp::GamepadStick); + i!(g right_stick gp::GamepadStick); + i!(g dpad gp::GamepadDpad); +} + +impl gp::MutableGamepad for GamepadHolder { + i!(s set_a bool); + i!(s set_b bool); + i!(s set_x bool); + i!(s set_y bool); + i!(s set_left_bumper bool); + i!(s set_right_bumper bool); + i!(s set_back bool); + i!(s set_start bool); + i!(s set_left_trigger f64); + i!(s set_right_trigger f64); + i!(s set_left_stick gp::GamepadStick); + i!(s set_right_stick gp::GamepadStick); + i!(s set_dpad gp::GamepadDpad); +} + +crate::threadsafe::thread_safe!(GamepadHolder); + +/// A struct that holds the state of a gamepad stick +#[pyclass] +#[derive(Debug, Clone)] +pub struct GamepadStick(gp::GamepadStick); + +impl GamepadStick { + /// Returns the x value of the stick. + pub fn get_x(&self) -> f64 { + self.0.x + } + /// Returns the y value of the stick. + pub fn get_y(&self) -> f64 { + self.0.y + } + /// Returns whether or not the stick is pressed. + pub fn get_pressed(&self) -> bool { + self.0.pressed + } + /// Converts the stick into an angle. + pub fn into_angle(&self) -> libtrig::Angle2D { + libtrig::Angle2D::from((self.get_x(), self.get_y())) + } + /// Converts the stick into a vector. + pub fn into_vector(&self) -> libtrig::Vec2D { + libtrig::Vec2D::from((self.get_x(), self.get_y())) + } +} + +impl From for libtrig::Angle2D { + fn from(stick: GamepadStick) -> Self { + stick.into_angle() + } +} + +impl From for GamepadStick { + fn from(angle: libtrig::Angle2D) -> Self { + libtrig::prelude!(); + Self(gp::GamepadStick { + x: angle.cos(), + y: angle.sin(), + pressed: false, + }) + } } -impl core::ops::Deref for GamepadHolder { - type Target = dyn MutableGamepad; - fn deref(&self) -> &Self::Target { - &*self.gamepad +impl From for libtrig::Vec2D { + fn from(stick: GamepadStick) -> Self { + stick.into_vector() } } -impl core::ops::DerefMut for GamepadHolder { - fn deref_mut(&mut self) -> &mut Self::Target { - &mut *self.gamepad +impl From for GamepadStick { + fn from(vector: libtrig::Vec2D) -> Self { + let vector = vector.normalize(); + Self(gp::GamepadStick { + x: vector.x(), + y: vector.y(), + pressed: false, + }) } } -unsafe impl Send for GamepadHolder {} -unsafe impl Sync for GamepadHolder {} +#[pymethods] +impl GamepadStick { + #[getter] + fn x(&self) -> PyResult { + Ok(self.get_x()) + } + #[getter] + fn y(&self) -> PyResult { + Ok(self.get_y()) + } + #[getter] + fn pressed(&self) -> PyResult { + Ok(self.get_pressed()) + } + fn as_angle(&self) -> PyResult { + Ok(self.into_angle().into()) + } + fn as_vec2d(&self) -> PyResult { + Ok(self.into_vector().into()) + } +} +/// A struct that holds the state of a gamepad dpad #[pyclass] -#[derive(Debug)] +#[derive(Debug, Clone)] +pub struct GamepadDpad(gp::GamepadDpad); + +impl From for GamepadDpad { + fn from(dpad: gp::GamepadDpad) -> Self { + Self(dpad) + } +} + +impl From for gp::GamepadDpad { + fn from(dpad: GamepadDpad) -> Self { + dpad.0 + } +} + +#[pymethods] +impl GamepadDpad { + #[getter] + fn up(&self) -> PyResult { + Ok(self.0.up) + } + #[getter] + fn down(&self) -> PyResult { + Ok(self.0.down) + } + #[getter] + fn left(&self) -> PyResult { + Ok(self.0.left) + } + #[getter] + fn right(&self) -> PyResult { + Ok(self.0.right) + } +} + +/// The struct that is used to access the gamepad data from python. +/// +/// This struct is thread safe, and can be used to read the gamepad data +/// from any thread. +/// +/// This struct should not be used to modify the gamepad data. For that, +/// use the `GamepadHolder` struct. +#[pyclass] +#[derive(Debug, Clone)] pub struct Gamepad(ThreadSafe); +impl crate::PyWrappedComponent for Gamepad { + type Holder = GamepadHolder; + fn new(gamepad: G) -> ThreadSafe { + ThreadSafe::new(GamepadHolder { + gamepad: Box::new(gamepad), + }) + } + fn wrap(gamepad: &ThreadSafe) -> Self { + Self(gamepad.clone()) + } +} + impl Gamepad { - pub fn new(gamepad: G) -> ThreadSafe { + /// This creates a new `ThreadSafe` struct. NOT a `Gamepad` struct. + /// + /// You then need to wrap it in a `Gamepad` struct using the [`Gamepad::wrap()`] method. + pub fn new(gamepad: G) -> ThreadSafe { ThreadSafe::new(GamepadHolder { gamepad: Box::new(gamepad), }) } + /// Wraps a `ThreadSafe` in a `Gamepad` struct. pub fn wrap(gamepad: &ThreadSafe) -> Self { Self(gamepad.clone()) } - fn make_err(e: &'static str) -> PyErr { - PyErr::new::(e) + /// Returns the state of the dpad + /// + /// Includes up, down, left, and right + pub fn get_dpad(&self) -> core::result::Result { + self.0 + .get()? + .dpad() + .map(|d| GamepadDpad(d)) + .map_err(|e| e.into()) + } + /// Returns the state of the left stick + /// + /// Includes x, y, and pressed + pub fn get_left_stick(&self) -> core::result::Result { + self.0 + .get()? + .left_stick() + .map(|d| GamepadStick(d)) + .map_err(|e| e.into()) + } + /// Returns the state of the right stick + /// + /// Includes x, y, and pressed + pub fn get_right_stick(&self) -> core::result::Result { + self.0 + .get()? + .right_stick() + .map(|d| GamepadStick(d)) + .map_err(|e| e.into()) + } + /// Returns the state of the left trigger + pub fn get_left_trigger(&self) -> core::result::Result { + self.0.get()?.left_trigger().map_err(|e| e.into()) + } + /// Returns the state of the right trigger + pub fn get_right_trigger(&self) -> core::result::Result { + self.0.get()?.right_trigger().map_err(|e| e.into()) + } + /// Is the 'x' button pressed? + pub fn get_x(&self) -> core::result::Result { + self.0.get()?.x().map_err(|e| e.into()) + } + /// Is the 'y' button pressed? + pub fn get_y(&self) -> core::result::Result { + self.0.get()?.y().map_err(|e| e.into()) + } + /// Is the 'a' button pressed? + pub fn get_a(&self) -> core::result::Result { + self.0.get()?.a().map_err(|e| e.into()) + } + /// Is the 'b' button pressed? + pub fn get_b(&self) -> core::result::Result { + self.0.get()?.b().map_err(|e| e.into()) + } + /// Is the left bumper pressed? + pub fn get_left_bumper(&self) -> core::result::Result { + self.0.get()?.left_bumper().map_err(|e| e.into()) + } + /// Is the right bumper pressed? + pub fn get_right_bumper(&self) -> core::result::Result { + self.0.get()?.right_bumper().map_err(|e| e.into()) } -} -impl Clone for Gamepad { - fn clone(&self) -> Self { - Self(self.0.clone()) + /// A non-standard 'back' button + pub fn get_back(&self) -> core::result::Result { + self.0.get()?.back().map_err(|e| e.into()) + } + /// A non-standard 'start' button + pub fn get_start(&self) -> core::result::Result { + self.0.get()?.start().map_err(|e| e.into()) } } #[pymethods] impl Gamepad { #[getter] - pub fn x(&self) -> PyResult { - self.0.get().map(|g| g.x()).map_err(Self::make_err) + fn dpad(&self) -> PyResult { + self.get_dpad().map_err(crate::make_err) + } + #[getter] + fn left_stick(&self) -> PyResult { + self.get_left_stick().map_err(crate::make_err) + } + #[getter] + fn right_stick(&self) -> PyResult { + self.get_right_stick().map_err(crate::make_err) + } + #[getter] + fn left_trigger(&self) -> PyResult { + self.get_left_trigger().map_err(crate::make_err) + } + #[getter] + fn right_trigger(&self) -> PyResult { + self.get_right_trigger().map_err(crate::make_err) + } + #[getter] + fn x(&self) -> PyResult { + self.get_x().map_err(crate::make_err) + } + #[getter] + fn y(&self) -> PyResult { + self.get_y().map_err(crate::make_err) + } + #[getter] + fn a(&self) -> PyResult { + self.get_a().map_err(crate::make_err) + } + #[getter] + fn b(&self) -> PyResult { + self.get_b().map_err(crate::make_err) + } + #[getter] + fn left_bumper(&self) -> PyResult { + self.get_left_bumper().map_err(crate::make_err) } #[getter] - pub fn y(&self) -> PyResult { - self.0.get().map(|g| g.y()).map_err(Self::make_err) + fn right_bumper(&self) -> PyResult { + self.get_right_bumper().map_err(crate::make_err) } + #[getter] - pub fn a(&self) -> PyResult { - self.0.get().map(|g| g.a()).map_err(Self::make_err) + fn back(&self) -> PyResult { + self.get_back().map_err(crate::make_err) } #[getter] - pub fn b(&self) -> PyResult { - self.0.get().map(|g| g.b()).map_err(Self::make_err) + fn start(&self) -> PyResult { + self.get_start().map_err(crate::make_err) } } -#[pymodule] -pub fn gamepad(_py: Python<'_>, m: &PyModule) -> PyResult<()> { +/// The gamepad module +#[doc(hidden)] +pub(crate) fn gamepad_submodule(_py: Python<'_>, m: &PyModule) -> PyResult<()> { m.add_class::()?; - - // todo!("add GamepadStick and GamepadDpad"); - - eprintln!("GamepadStick and GamepadDpad are not implemented yet"); + m.add_class::()?; + m.add_class::()?; Ok(()) } diff --git a/arc/lib.rs b/arc/lib.rs index 0b4850e..9ceca74 100644 --- a/arc/lib.rs +++ b/arc/lib.rs @@ -1,10 +1,40 @@ #![doc = include_str!("./README.md")] -#![warn(missing_docs, unused, clippy::all)] +#![warn(missing_docs, unused, clippy::all, unsafe_code)] +#![deny(missing_debug_implementations)] -pub use hardware; +#[doc(hidden)] +pub use arc_robot_hardware; -mod __init__; -mod threadsafe; +pub mod __init__; +mod macros; +pub mod threadsafe; -pub use __init__::*; -pub use threadsafe::ThreadSafe; +pub use threadsafe::{ThreadSafe, ThreadSafeError}; + +/// A trait for hardware components that can be used in the robot. +pub trait PyWrappedComponent { + /// The type that holds the hardware component. + /// + /// This type isn't required to be `Send` or `Sync`. + /// + /// The holder is what will be written to by the python thread. + type Holder; + /// Creates a new hardware component. + /// + /// This function should be called before the python main thread is started. + fn new(hardware: Input) -> crate::ThreadSafe; + /// Wraps the hardware component in a `ThreadSafe` type. + /// + /// The wrapper is what will be read from by the python thread. + fn wrap(hardware_component: &crate::ThreadSafe) -> Self; +} + +/// Internal function to translate a static string into a PyIOError. +#[doc(hidden)] +fn make_err(e: &'static str) -> pyo3::PyErr { + pyo3::PyErr::new::(e) +} + +// pub fn run_op() -> pyo3::PyResult<()> { +// +// } diff --git a/arc/macros.rs b/arc/macros.rs new file mode 100644 index 0000000..08cfa15 --- /dev/null +++ b/arc/macros.rs @@ -0,0 +1,108 @@ +/// A macro for simplifying the creation of a python module. +/// +/// This exists because the `pyo3`'s `#[pymodule]` macro doesn't allow for the creation of submodules. +/// +/// See [this](https://github.com/DranikiRobotics/arc/issues/3) for more information. +/// +/// # Example +/// +/// ```rust +/// # use pyo3::prelude::*; +/// fn my_submodule(py: Python<'_>, m: &PyModule) -> PyResult<()> { +/// m.add("true", true)?; +/// Ok(()) +/// } +/// +/// #[pymodule] +/// fn my_module(py: Python<'_>, m: &PyModule) -> PyResult<()> { +/// arc_pylib::pymod!(submodule -> my_submodule, "my_module.submodule", py, m); +/// Ok(()) +/// } +/// ``` +#[macro_export] +macro_rules! pymod { + ( + $module_name: ident -> $submodule_func: path, $module_path: literal, $py:ident, $m: ident + ) => { + let $module_name = PyModule::new($py, ::core::stringify!($module_name))?; + $submodule_func($py, $module_name)?; + $m.add_submodule($module_name)?; + pyo3::py_run!( + $py, + $module_name, + ::core::concat!( + "import sys; sys.modules['", + $module_path, + "'] = ", + ::core::stringify!($module_name), + ) + ); + }; +} + +/// A macro for simplifying the creation of a wrapped component. +/// +/// This works for any component that implements the [`PyWrappedComponent`] trait. +/// +/// # Example +/// +/// ```rust +/// # use pyo3::prelude::*; +/// # use arc_pylib as pylib; +/// # use arc_pylib::arc_robot_hardware as hardware; +/// # use pylib::PyWrappedComponent as _; +/// # use pylib::__init__::hardware::gamepad::Gamepad; +/// # use pylib::__init__::Op; +/// +/// // Instead of this: +/// let gamepad = Gamepad::new(hardware::gamepad::impls::LogitechF310::default()); +/// let gamepad_wrapper = Gamepad::wrap(&gamepad); +/// let op = Op::new(gamepad_wrapper); +/// let op_wrapper = Op::wrap(&op); +/// +/// // You can do this: +/// let (gamepad, gamepad_wrapper) = pylib::setup_wrapped_component!( +/// hardware::gamepad::impls::LogitechF310::default(); Gamepad +/// ); +/// let (op, op_wrapper) = pylib::setup_wrapped_component!(gamepad_wrapper; Op); +/// ``` +/// +/// [`PyWrappedComponent`]: trait.PyWrappedComponent.html +#[macro_export] +macro_rules! setup_wrapped_component { + ( + $value: expr; $component: ty + ) => {{ + fn __setup_wrapped_component_internal< + // Wrapped component constructor input + Input, + // Holder type (this is what will be read from by the python thread) + Holder, + // Hardware component (wrapped) + Component, + >( + // The hardware component constructor + input: Input, + // We return a tuple + ) -> ( + // A thread safe holder + $crate::ThreadSafe, + // The wrapped component + Component, + // Validation + ) + where + // Component must implement PyWrappedComponent + Component: $crate::PyWrappedComponent, + { + // Create the holder + let holder = Component::new(input); + // Wrap the holder in a thread safe type + let wrapper = Component::wrap(&holder); + // Return the holder and the wrapped component + (holder, wrapper) + } + // Call the internal function + __setup_wrapped_component_internal::<_, _, $component>($value) + }}; +} diff --git a/arc/math/__init__.rs b/arc/math/__init__.rs index e69de29..5454930 100644 --- a/arc/math/__init__.rs +++ b/arc/math/__init__.rs @@ -0,0 +1,21 @@ +//! Math module for the arc crate +//! +//! Python identifier: `arc.math` + +use pyo3::prelude::*; + +pub mod angle; +pub mod pos2d; +pub mod vec2d; + +/// The math module. +pub(crate) fn math_submodule(py: Python<'_>, m: &PyModule) -> PyResult<()> { + crate::pymod!(angle -> angle::angle_submodule, "arc.math.angle", py, m); + crate::pymod!(pos2d -> pos2d::pos2d_submodule, "arc.math.pos2d", py, m); + crate::pymod!(vec2d -> vec2d::vec2d_submodule, "arc.math.vec2d", py, m); + + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + Ok(()) +} diff --git a/arc/math/angle.rs b/arc/math/angle.rs new file mode 100644 index 0000000..b1584a4 --- /dev/null +++ b/arc/math/angle.rs @@ -0,0 +1,165 @@ +//! Angle module for the arc crate +//! +//! Python identifier: `arc.math.angle` + +use pyo3::prelude::*; + +/// A Python class for 2D angles +/// +/// This class is a wrapper around the [`libtrig::Angle2D`] type. +/// +/// All methods and operators are implemented for this class. +/// +/// If you want to use this class in your own code, you can use the [`libtrig::Angle2D`] type directly. +/// +/// # Examples +/// +/// ```py,no_run,ignore +/// from arc.math.angle import Angle +/// +/// angle = Angle.from_degrees(90) +/// print(angle.radians()) +/// ``` +/// +/// [`libtrig::Angle2D`]: ../../../libtrig/struct.Angle2D.html +#[pyclass] +#[repr(transparent)] +#[derive(Debug, Clone)] +pub struct Angle(pub(crate) libtrig::Angle2D); + +#[pymethods] +impl Angle { + #[inline] + #[staticmethod] + const fn from_degrees(degrees: libtrig::Degree64) -> Self { + Self(libtrig::Angle2D::from_degrees(degrees)) + } + #[inline] + #[staticmethod] + const fn from_radians(radians: libtrig::Radian64) -> Self { + Self(libtrig::Angle2D::from_radians(radians)) + } + #[inline] + #[staticmethod] + fn from_vec2d(vec: super::vec2d::Vec2D) -> Self { + Self(libtrig::Angle2D::from(vec)) + } + #[inline] + #[staticmethod] + const fn zero() -> Self { + Self(libtrig::Angle2D::zero()) + } + #[inline] + fn degrees(&self) -> libtrig::Degree64 { + self.0.to_degrees() + } + #[inline] + fn radians(&self) -> libtrig::Radian64 { + self.0.to_radians() + } + #[inline] + fn sin(&self) -> libtrig::Float64 { + libtrig::prelude!(); + self.0.sin() + } + #[inline] + fn cos(&self) -> libtrig::Float64 { + libtrig::prelude!(); + self.0.cos() + } + #[inline] + fn sqrt(&self) -> libtrig::Float64 { + libtrig::prelude!(); + self.0.sqrt() + } + #[inline] + fn __add__(&self, other: &Self) -> Self { + Self(self.0 + other.0) + } + #[inline] + fn __sub__(&self, other: &Self) -> Self { + Self(self.0 - other.0) + } + #[inline] + fn __mul__(&self, other: libtrig::Float64) -> Self { + Self(self.0 * other) + } + #[inline] + fn __truediv__(&self, other: libtrig::Float64) -> Self { + Self(self.0 / other) + } + #[inline] + fn __neg__(&self) -> Self { + Self(-self.0) + } + #[inline] + fn __eq__(&self, other: &Self) -> bool { + self.0 == other.0 + } + #[inline] + fn __ne__(&self, other: &Self) -> bool { + self.0 != other.0 + } + #[inline] + fn __lt__(&self, other: &Self) -> bool { + self.0 < other.0 + } + #[inline] + fn __le__(&self, other: &Self) -> bool { + self.0 <= other.0 + } + #[inline] + fn __gt__(&self, other: &Self) -> bool { + self.0 > other.0 + } + #[inline] + fn __ge__(&self, other: &Self) -> bool { + self.0 >= other.0 + } + #[inline] + fn __str__(&self) -> String { + format!("{}", self.0) + } + #[inline] + fn __repr__(&self) -> String { + format!("Angle({:?})", self.0) + } +} + +impl From for Angle { + fn from(angle: libtrig::Angle2D) -> Self { + Self(angle) + } +} + +impl From for libtrig::Angle2D { + fn from(angle: Angle) -> Self { + angle.0 + } +} + +impl From for libtrig::Vec2D { + fn from(angle: Angle) -> Self { + angle.0.into() + } +} + +impl core::ops::Deref for Angle { + type Target = libtrig::Angle2D; + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +impl core::ops::DerefMut for Angle { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.0 + } +} + +/// The angle module +pub(crate) fn angle_submodule(_py: Python<'_>, m: &PyModule) -> PyResult<()> { + m.add_class::()?; + + Ok(()) +} diff --git a/arc/math/pos2d.pyi b/arc/math/pos2d.pyi index 0030e7d..6bf8e3b 100644 --- a/arc/math/pos2d.pyi +++ b/arc/math/pos2d.pyi @@ -20,12 +20,17 @@ class Pos2D(object): @staticmethod def from_xy(x: float, y: float) -> Pos2D: - """Creates a position from x and y.""" + """Creates a position from x and y. facing 0 degrees.""" ... @staticmethod def from_vec2d(vec: _Vec2D) -> Pos2D: - """Creates a position from a vector.""" + """Creates a position from a vector. facing 0 degrees.""" + ... + + @staticmethod + def from_angle(angle: _Angle) -> Pos2D: + """Creates a position from an angle. at the origin.""" ... @staticmethod @@ -50,6 +55,8 @@ class Pos2D(object): def move(self, delta: Pos2D) -> None: """ Moves the current position by the specified delta. + + This is an impure method, because it mutates the current position. """ ... diff --git a/arc/math/pos2d.rs b/arc/math/pos2d.rs new file mode 100644 index 0000000..41a0eb4 --- /dev/null +++ b/arc/math/pos2d.rs @@ -0,0 +1,102 @@ +//! Pos2D module for the arc crate +//! +//! Python identifier: `arc.math.pos2d` + +use libtrig::{Float64, Radian64}; +use pyo3::prelude::*; + +/// The Pos2D class. +#[pyclass] +#[repr(transparent)] +#[derive(Debug, Clone)] +pub struct Pos2D(libtrig::Pos2D); + +#[pymethods] +impl Pos2D { + #[inline] + #[staticmethod] + fn from_angle_and_pos(rot: super::angle::Angle, pos: super::vec2d::Vec2D) -> Self { + Self(libtrig::Pos2D::new(pos.into(), rot.into())) + } + #[inline] + #[staticmethod] + fn from_xyr(x: Float64, y: Float64, radians: Radian64) -> Self { + Self(libtrig::Pos2D::new( + libtrig::Coord2D::new(x, y), + libtrig::Angle2D::from_radians(radians), + )) + } + #[inline] + #[staticmethod] + fn from_xy(x: Float64, y: Float64) -> Self { + Self(libtrig::Pos2D::new( + libtrig::Coord2D::new(x, y), + libtrig::Angle2D::zero(), + )) + } + #[inline] + #[staticmethod] + fn from_vec2d(vec: super::vec2d::Vec2D) -> Self { + Self(libtrig::Pos2D::new( + vec.into(), libtrig::Angle2D::zero() + )) + } + #[inline] + #[staticmethod] + fn from_angle(angle: super::angle::Angle) -> Self { + Self(libtrig::Pos2D::new( + libtrig::Coord2D::origin(), + libtrig::Angle2D::from(angle), + )) + } + #[inline] + #[staticmethod] + fn zero() -> Self { + Self(libtrig::Pos2D::zero()) + } + #[inline] + #[getter] + fn x(&self) -> Float64 { + self.0.x() + } + #[inline] + #[getter] + fn y(&self) -> Float64 { + self.0.y() + } + #[inline] + #[getter] + fn angle(&self) -> super::angle::Angle { + super::angle::Angle(self.0.angle()) + } + #[inline] + fn r#move(&mut self, delta: Self) { + self.0.translate(delta.0) + } + #[inline] + fn __add__(&self, other: &Self) -> Self { + Self(self.0 + other.0) + } + #[inline] + fn __sub__(&self, other: &Self) -> Self { + Self(self.0 - other.0) + } + #[inline] + fn __neg__(&self) -> Self { + Self(-self.0) + } + #[inline] + fn __eq__(&self, other: &Self) -> bool { + self.0 == other.0 + } + #[inline] + fn __ne__(&self, other: &Self) -> bool { + self.0 != other.0 + } +} + +pub(crate) fn pos2d_submodule(_py: Python<'_>, m: &PyModule) -> PyResult<()> { + m.add_class::()?; + + Ok(()) +} diff --git a/arc/math/vec2d.pyi b/arc/math/vec2d.pyi index 0dc7a23..1a18632 100644 --- a/arc/math/vec2d.pyi +++ b/arc/math/vec2d.pyi @@ -19,16 +19,26 @@ class Vec2D(object): """Creates a vector from x and y components.""" ... + @property + def x(self) -> float: + """Returns the x component of the vector.""" + ... + + @property + def y(self) -> float: + """Returns the y component of the vector.""" + ... + def angle(self) -> Angle: """Returns the angle of the vector.""" ... - def length(self) -> float: - """Returns the length of the vector.""" + def magnitude(self) -> float: + """Returns the magnitude of the vector.""" ... def normalize(self) -> Vec2D: - """Returns a normalized version of the vector.""" + """Returns a new normalized version of this vector.""" ... def dot(self, other: Vec2D) -> float: @@ -84,5 +94,13 @@ class Vec2D(object): ... def __repr__(self) -> str: - """Returns a string representation of the vector.""" + """Returns a string representation of the vector. (for debugging)""" + ... + + def __len__(self) -> int: + """ + Returns the magnitude of the vector. + + This is the same as calling `magnitude`. + """ ... diff --git a/arc/math/vec2d.rs b/arc/math/vec2d.rs new file mode 100644 index 0000000..9a74018 --- /dev/null +++ b/arc/math/vec2d.rs @@ -0,0 +1,168 @@ +//! Vec2D module for the arc crate +//! +//! Python identifier: `arc.math.vec2d` + +use pyo3::prelude::*; + +/// A Python class for 2D vectors +/// +/// This class is a wrapper around the [`libtrig::Vec2D`] type. +/// +/// All methods and operators are implemented for this class. +/// +/// If you want to use this class in your own code, you can use the [`libtrig::Vec2D`] type directly. +/// +/// # Examples +/// +/// ```py,no_run,ignore +/// from arc.math.vec2d import Vec2D +/// +/// vec = Vec2D.from_xy(3, 4) +/// print(vec.magnitude()) +/// ``` +/// +/// [`libtrig::Vec2D`]: ../../../libtrig/struct.Vec2D.html +#[pyclass] +#[derive(Debug, Clone)] +pub struct Vec2D(libtrig::Vec2D); + +#[pymethods] +impl Vec2D { + #[inline] + #[staticmethod] + fn from_angle(angle: super::angle::Angle) -> Self { + Self(libtrig::Vec2D::from(angle)) + } + #[inline] + #[staticmethod] + const fn from_xy(x: libtrig::Float64, y: libtrig::Float64) -> Self { + Self(libtrig::Vec2D::new(x, y)) + } + #[inline] + #[getter] + fn x(&self) -> libtrig::Float64 { + self.0.x() + } + #[inline] + #[getter] + fn y(&self) -> libtrig::Float64 { + self.0.y() + } + #[inline] + fn angle(&self) -> super::angle::Angle { + super::angle::Angle(self.0.into()) + } + #[inline] + fn magnitude(&self) -> libtrig::Float64 { + self.0.magnitude() + } + #[inline] + fn normalize(&self) -> Self { + Self(self.0.normalize()) + } + #[inline] + fn dot(&self, other: Self) -> libtrig::Float64 { + libtrig::prelude!(); + self.0.dot(other.0) + } + #[inline] + fn __add__(&self, other: Self) -> Self { + Self(self.0 + other.0) + } + #[inline] + fn __sub__(&self, other: Self) -> Self { + Self(self.0 - other.0) + } + #[inline] + fn __mul__(&self, other: libtrig::Float64) -> Self { + Self(self.0 * other) + } + #[inline] + fn __truediv__(&self, other: libtrig::Float64) -> Self { + Self(self.0 / other) + } + #[inline] + fn __neg__(&self) -> Self { + Self(-self.0) + } + #[inline] + fn __eq__(&self, other: Self) -> bool { + self.0 == other.0 + } + #[inline] + fn __ne__(&self, other: Self) -> bool { + self.0 != other.0 + } + #[inline] + fn __lt__(&self, other: Self) -> bool { + self.0 < other.0 + } + #[inline] + fn __le__(&self, other: Self) -> bool { + self.0 <= other.0 + } + #[inline] + fn __gt__(&self, other: Self) -> bool { + self.0 > other.0 + } + #[inline] + fn __ge__(&self, other: Self) -> bool { + self.0 >= other.0 + } + #[inline] + fn __str__(&self) -> String { + format!("{}", self.0) + } + #[inline] + fn __repr__(&self) -> String { + format!("Vec2D({:?})", self.0) + } + #[inline] + fn __len__(&self) -> usize { + self.magnitude().round() as usize + } +} + +impl From for Vec2D { + fn from(vec: libtrig::Vec2D) -> Self { + Self(vec) + } +} + +impl From for libtrig::Vec2D { + fn from(vec: Vec2D) -> Self { + vec.0 + } +} + +impl From for libtrig::Angle2D { + fn from(vec: Vec2D) -> Self { + vec.0.into() + } +} + +impl From for libtrig::Coord2D { + fn from(vec: Vec2D) -> Self { + vec.0.into() + } +} + +impl core::ops::Deref for Vec2D { + type Target = libtrig::Vec2D; + fn deref(&self) -> &Self::Target { + &self.0 + } +} + +impl core::ops::DerefMut for Vec2D { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.0 + } +} + +/// The Vec2D module +pub(crate) fn vec2d_submodule(_py: Python<'_>, m: &PyModule) -> PyResult<()> { + m.add_class::()?; + + Ok(()) +} diff --git a/arc/pyproject.toml b/arc/pyproject.toml new file mode 100644 index 0000000..abe9ddf --- /dev/null +++ b/arc/pyproject.toml @@ -0,0 +1,14 @@ +[build-system] +requires = ["maturin>=1,<2"] +build-backend = "maturin" + +[project] +name = "arc" +version = "0.0.1-dev" +description = "Advanced Robot Controller" +requires-python = ">=3.11" +classifiers = [ + "Programming Language :: Rust", + "Programming Language :: Python :: Implementation :: CPython", + "Programming Language :: Python :: Implementation :: PyPy", +] diff --git a/arc/threadsafe.rs b/arc/threadsafe.rs index ab14cc6..7f7cf25 100644 --- a/arc/threadsafe.rs +++ b/arc/threadsafe.rs @@ -1,7 +1,25 @@ -pub type TSResult = core::result::Result; +//! Thread-safe values. +//! +//! This module contains thread-safe values that can be used in a +//! multi-threaded environment. + +macro_rules! thread_safe { + ($struct: ident < $($generics: ident),* >) => { + #[allow(unsafe_code)] + unsafe impl<$($generics),*> Send for $struct<$($generics),*> {} + #[allow(unsafe_code)] + unsafe impl<$($generics),*> Sync for $struct<$($generics),*> {} + }; + ($struct: ident) => { + #[allow(unsafe_code)] + unsafe impl Send for $struct {} + #[allow(unsafe_code)] + unsafe impl Sync for $struct {} + }; +} +pub(crate) use thread_safe; mod holders { - use super::TSResult; use std::sync::{Arc, Mutex, MutexGuard}; const POISON_MESSAGE: &str = "Poisoned mutex"; @@ -9,12 +27,14 @@ mod holders { #[derive(Debug, Default)] pub struct ThreadSafeHolder(Arc>); - pub type GetResult<'a, T> = TSResult>; - pub type GetResultMut<'a, T> = TSResult>; + pub type GetResult<'a, T> = core::result::Result, &'static str>; + pub type GetResultMut<'a, T> = core::result::Result, &'static str>; + #[derive(Debug)] #[repr(transparent)] pub struct SafeHeld<'a, T>(MutexGuard<'a, T>); + #[derive(Debug)] #[repr(transparent)] pub struct SafeHeldMut<'a, T>(MutexGuard<'a, T>); @@ -56,8 +76,7 @@ mod holders { } } - unsafe impl Send for ThreadSafeHolder {} - unsafe impl Sync for ThreadSafeHolder {} + super::thread_safe!(ThreadSafeHolder); } /// A thread-safe value. @@ -65,121 +84,125 @@ mod holders { /// This is a wrapper around a value that by default is not thread-safe. pub type ThreadSafe = holders::ThreadSafeHolder; +/// A thread-safe error. +pub trait ThreadSafeError: std::error::Error + Send + Sync {} + +impl ThreadSafeError for T {} + macro_rules! thread_safe_primitive { ( $(#[$outer:meta])* pub type $name:ident = $primitive:ty; { $thread_safe_mod_name:ident; $default_value:expr } $($rest:tt)* ) => ( +$(#[$outer])* +pub type $name = $thread_safe_mod_name::$name; +mod $thread_safe_mod_name { + use super::*; - $(#[$outer])* - pub type $name = $thread_safe_mod_name::$name; - mod $thread_safe_mod_name { - use super::*; - - #[repr(transparent)] - #[derive(Default, Debug, Clone)] - pub struct $name(ThreadSafe); - - impl PartialEq for $name { - fn eq(&self, other: &Self) -> bool { - match (self.get(), other.get() ) { - (Ok(a), Ok(b)) => a.get() == b.get(), - _ => false, - } - } + #[repr(transparent)] + #[derive(Default, Debug, Clone)] + pub struct $name(ThreadSafe); + + impl PartialEq for $name { + fn eq(&self, other: &Self) -> bool { + match (self.get(), other.get() ) { + (Ok(a), Ok(b)) => a.get() == b.get(), + _ => false, } + } + } - impl Eq for $name {} + impl Eq for $name {} - impl PartialOrd for $name { - fn partial_cmp(&self, other: &Self) -> Option { - match (self.get(), other.get() ) { - (Ok(a), Ok(b)) => a.get().partial_cmp(&b.get()), - _ => None, - } - } + impl PartialOrd for $name { + fn partial_cmp(&self, other: &Self) -> Option { + match (self.get(), other.get() ) { + (Ok(a), Ok(b)) => a.get().partial_cmp(&b.get()), + _ => None, } + } + } - impl Ord for $name { - fn cmp(&self, other: &Self) -> std::cmp::Ordering { - match (self.get(), other.get() ) { - (Ok(a), Ok(b)) => a.get().cmp(&b.get()), - _ => std::cmp::Ordering::Equal, - } - } + impl Ord for $name { + fn cmp(&self, other: &Self) -> std::cmp::Ordering { + match (self.get(), other.get() ) { + (Ok(a), Ok(b)) => a.get().cmp(&b.get()), + _ => std::cmp::Ordering::Equal, } + } + } - impl $name { - pub fn new(value: $primitive) -> Self { - Self(ThreadSafe::new(Primitive::new(value))) - } - pub fn get(&self) -> holders::GetResult<'_, Primitive> { - self.0.get() - } - pub fn get_mut(&self) -> holders::GetResultMut<'_, Primitive> { - self.0.get_mut() - } - } + impl $name { + pub fn new(value: $primitive) -> Self { + Self(ThreadSafe::new(Primitive::new(value))) + } + pub fn get(&self) -> holders::GetResult<'_, Primitive> { + self.0.get() + } + pub fn get_mut(&self) -> holders::GetResultMut<'_, Primitive> { + self.0.get_mut() + } + } - #[repr(transparent)] - #[derive(Default, Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)] - pub struct Primitive($primitive); - - impl Primitive { - pub fn new(value: $primitive) -> Self { - Self(value) - } - pub fn get(&self) -> $primitive { - self.0 - } - pub fn set(&mut self, value: $primitive) { - self.0 = value; - } - } + #[repr(transparent)] + #[derive(Default, Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)] + pub struct Primitive($primitive); - impl core::ops::Deref for Primitive { - type Target = $primitive; - fn deref(&self) -> &Self::Target { - &self.0 - } - } + impl Primitive { + pub fn new(value: $primitive) -> Self { + Self(value) + } + pub fn get(&self) -> $primitive { + self.0 + } + pub fn set(&mut self, value: $primitive) { + self.0 = value; + } + } - impl core::ops::DerefMut for Primitive { - fn deref_mut(&mut self) -> &mut Self::Target { - &mut self.0 - } - } + impl core::ops::Deref for Primitive { + type Target = $primitive; + fn deref(&self) -> &Self::Target { + &self.0 + } + } - impl From<$primitive> for Primitive { - fn from(value: $primitive) -> Self { - Self(value) - } - } + impl core::ops::DerefMut for Primitive { + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.0 + } + } - impl From for $primitive { - fn from(value: Primitive) -> Self { - value.0 - } - } + impl From<$primitive> for Primitive { + fn from(value: $primitive) -> Self { + Self(value) + } + } - impl From<$primitive> for $name { - fn from(value: $primitive) -> Self { - Self::new(value) - } - } + impl From for $primitive { + fn from(value: Primitive) -> Self { + value.0 + } + } - impl From<$name> for $primitive { - fn from(value: $name) -> Self { - match value.get() { - Ok(b) => b.get(), - Err(_) => $default_value, - } - } + impl From<$primitive> for $name { + fn from(value: $primitive) -> Self { + Self::new(value) + } + } + + impl From<$name> for $primitive { + fn from(value: $name) -> Self { + match value.get() { + Ok(b) => b.get(), + Err(_) => $default_value, } } + } +} - thread_safe_primitive!($($rest)*); +thread_safe_primitive!($($rest)*); ); () => (); } diff --git a/examples/auto.py b/examples/auto.py index ba9730e..c8a97d5 100644 --- a/examples/auto.py +++ b/examples/auto.py @@ -1,10 +1,10 @@ -# from arc.hardware import * -# from arc.math import * +from arc.hardware import * +from arc.math import * from arc import * # You are REQUIRED to have a main() function in your program. # and you MUST NOT call it yourself. -@Auto +@Auto("My Auto") def main(op: Op): print("Starting...") @@ -19,4 +19,6 @@ def main(op: Op): print("A released!") holding_a = False + print("Done!") + return OK diff --git a/examples/util/drive.py b/examples/util/drive.py new file mode 100644 index 0000000..2f9695d --- /dev/null +++ b/examples/util/drive.py @@ -0,0 +1,48 @@ +from arc.math import Pos2D as _Pos2D +import abc as _abc + +class Drive(_abc.ABC): + """Represents a drive system.""" + @_abc.abstractmethod + def forward(self, distance: float) -> None: + """Drives forward the specified distance.""" + pass + + @_abc.abstractmethod + def backward(self, distance: float) -> None: + """Drives backward the specified distance.""" + pass + + @_abc.abstractmethod + def turn_left(self, angle: float) -> None: + """Turns left the specified angle.""" + pass + + @_abc.abstractmethod + def turn_right(self, angle: float) -> None: + """Turns right the specified angle.""" + pass + +class StrafingDrive(Drive): + """Represents a drive system that can also strafe.""" + @_abc.abstractmethod + def left(self, distance: float) -> None: + """Strafes left the specified distance.""" + + @_abc.abstractmethod + def right(self, distance: float) -> None: + """Strafes right the specified distance.""" + +class AdvancedDrive(StrafingDrive): + """Represents a drive system that can also move to a position.""" + @_abc.abstractmethod + def moveto(self, pos: _Pos2D) -> None: + """Moves to the specified position.""" + pass + +class PositionAwareDrive(_abc.ABC): + """Represents a drive system that can also return its position.""" + @_abc.abstractproperty + def position(self) -> _Pos2D: + """The current position of the robot.""" + return None # type: ignore diff --git a/examples/util/mecanum_drive.py b/examples/util/mecanum_drive.py new file mode 100644 index 0000000..9b81c1e --- /dev/null +++ b/examples/util/mecanum_drive.py @@ -0,0 +1,14 @@ +from .drive import AdvancedDrive, PositionAwareDrive + +class MecanumDrive(AdvancedDrive, PositionAwareDrive): + def forward(self, distance: float) -> None: + pass + + def backward(self, distance: float) -> None: + pass + + def turn_left(self, angle: float) -> None: + pass + + def turn_right(self, angle: float) -> None: + pass \ No newline at end of file diff --git a/libs/helper/libtrig/angle.rs b/libs/helper/libtrig/angle.rs index 0bdfe52..db4b809 100644 --- a/libs/helper/libtrig/angle.rs +++ b/libs/helper/libtrig/angle.rs @@ -117,6 +117,44 @@ impl Angle2D { } } +impl From for Angle2D { + /// Creates a new `Angle2D` from the given `Vec2D`. + /// + /// This is equivalent to `Angle2D::from_radians(v.y().atan2(v.x()))`. + #[inline] + #[must_use] + fn from(v: Vec2D) -> Self { + crate::prelude!(); + Self::from_radians(v.y().atan2(v.x())) + } +} + +impl From<(Float64, Float64)> for Angle2D { + /// Creates a new `Angle2D` from the given `(x, y)` pair. + /// + /// This is equivalent to `Angle2D::from_radians(y.atan2(x))`. + #[inline] + #[must_use] + fn from((x, y): (Float64, Float64)) -> Self { + crate::prelude!(); + Self::from_radians(y.atan2(x)) + } +} + +impl From for Vec2D { + /// Creates a new `Vec2D` from the given `Angle2D`. + /// + /// This is equivalent to `Vec2D::new(a.cos(), a.sin())`. + /// + /// THE VECTOR MAY OR MAY NOT BE NORMALIZED. + #[inline] + #[must_use] + fn from(a: Angle2D) -> Self { + crate::prelude!(); + Self::new(a.cos(), a.sin()) + } +} + #[macros::mass_impl( $THIS = @ORM Angle2D, $OTHER = @ORM Angle2D diff --git a/libs/helper/libtrig/coords/coord2d.rs b/libs/helper/libtrig/coords/coord2d.rs index 81bac40..0396ac8 100644 --- a/libs/helper/libtrig/coords/coord2d.rs +++ b/libs/helper/libtrig/coords/coord2d.rs @@ -4,10 +4,8 @@ use crate::*; #[repr(C)] #[derive(Debug, Clone, Copy, PartialEq, PartialOrd)] pub struct Coord2D { - /// The x coordinate. - pub x: Float64, - /// The y coordinate. - pub y: Float64, + x: Float64, + y: Float64, } impl Coord2D { @@ -49,6 +47,18 @@ impl Coord2D { pub fn inverse(&self) -> Self { -self } + /// Returns the x coordinate. + #[inline] + #[must_use] + pub const fn x(&self) -> Float64 { + self.x + } + /// Returns the y coordinate. + #[inline] + #[must_use] + pub const fn y(&self) -> Float64 { + self.y + } } #[macros::mass_impl( @@ -291,3 +301,19 @@ impl From for (Float64, Float64) { (x, y) } } + +impl From for Coord2D { + #[inline] + #[must_use] + fn from(vec: crate::Vec2D) -> Self { + Self::new(vec.x(), vec.y()) + } +} + +impl From for crate::Vec2D { + #[inline] + #[must_use] + fn from(vec: Coord2D) -> Self { + Self::new(vec.x, vec.y) + } +} diff --git a/libs/helper/libtrig/lib.rs b/libs/helper/libtrig/lib.rs index ced61b4..b2930b5 100644 --- a/libs/helper/libtrig/lib.rs +++ b/libs/helper/libtrig/lib.rs @@ -3,11 +3,13 @@ #![cfg_attr(feature = "unstable", feature(const_mut_refs))] #![cfg_attr(feature = "unstable", feature(const_mut_refs))] #![warn(missing_docs, unused, clippy::all, unsafe_code)] +#![deny(missing_debug_implementations)] #![doc = include_str!("./README.md")] pub(crate) mod angle; pub(crate) mod coords; pub(crate) mod morenums; +pub(crate) mod pos2d; pub(crate) mod traits; pub(crate) mod types; pub(crate) mod vectors; @@ -20,6 +22,7 @@ pub mod prelude { pub use angle::Angle2D; pub use coords::Coord2D; pub use morenums::{u2, u3}; +pub use pos2d::Pos2D; pub use types::*; pub use vectors::{Vec2D, Vec3D}; diff --git a/libs/helper/libtrig/pos2d.rs b/libs/helper/libtrig/pos2d.rs new file mode 100644 index 0000000..e396f1a --- /dev/null +++ b/libs/helper/libtrig/pos2d.rs @@ -0,0 +1,265 @@ +use l2math::{Float64, Radian64}; + +use crate::{Angle2D, Coord2D, Vec2D}; + +/// A 2D position with rotation. +/// +/// This type is a wrapper around the [`Coord2D`] and [`Angle2D`] types. +/// +/// All logical operations are implemented for this type. +#[repr(C)] +#[derive(Debug, Clone, Copy)] +pub struct Pos2D { + pos: Coord2D, + rot: Angle2D, +} + +impl core::fmt::Display for Pos2D { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + write!(f, "{};{} {}", self.x(), self.y(), self.angle()) + } +} + +impl Default for Pos2D { + #[inline] + #[must_use] + fn default() -> Self { + Self::zero() + } +} + +impl Pos2D { + /// Create a new `Pos2D` from a `Coord2D` and an `Angle2D`. + #[inline] + #[must_use] + pub const fn new(pos: Coord2D, rot: Angle2D) -> Self { + Self { pos, rot } + } + /// Create a new `Pos2D` positioned at the origin with no rotation. + #[inline] + #[must_use] + pub const fn zero() -> Self { + Self::new(Coord2D::origin(), Angle2D::zero()) + } + /// Returns the x coordinate of the `Pos2D`. + #[inline] + #[must_use] + pub const fn x(&self) -> Float64 { + self.pos.x() + } + /// Returns the y coordinate of the `Pos2D`. + #[inline] + #[must_use] + pub const fn y(&self) -> Float64 { + self.pos.y() + } + /// Returns the angle of rotation of the `Pos2D`. + #[inline] + #[must_use] + pub const fn angle(&self) -> Angle2D { + self.rot + } + /// Moves the `Pos2D` by the given delta `Pos2D` + #[inline] + pub fn translate(&mut self, delta: Pos2D) { + self.pos += delta.pos; + self.rot += delta.rot; + } +} + +impl From for Pos2D { + fn from(pos: Coord2D) -> Self { + Self::new(pos, Angle2D::zero()) + } +} + +impl From for Pos2D { + fn from(vec: Vec2D) -> Self { + Self::new(vec.into(), Angle2D::zero()) + } +} + +impl From for Pos2D { + fn from(rot: Angle2D) -> Self { + Self::new(Coord2D::origin(), rot) + } +} + +impl From<(Float64, Float64, Radian64)> for Pos2D { + fn from((x, y, rot): (Float64, Float64, Radian64)) -> Self { + Self::new(Coord2D::new(x, y), Angle2D::from_radians(rot)) + } +} + +impl From<(Float64, Float64)> for Pos2D { + fn from((x, y): (Float64, Float64)) -> Self { + Self::new(Coord2D::new(x, y), Angle2D::zero()) + } +} + +#[macros::mass_impl( + $THIS = @ORM Pos2D, + $OTHER = @ORM Vec2D +)] +impl core::ops::Add for THIS { + type Output = Coord2D; + #[inline] + #[must_use] + fn add(self, rhs: OTHER) -> Self::Output { + self.pos + rhs + } +} + +#[macros::mass_impl( + $THIS = @ORM Pos2D, + $OTHER = @ORM Angle2D +)] +impl core::ops::Add for THIS { + type Output = Angle2D; + #[inline] + #[must_use] + fn add(self, rhs: OTHER) -> Self::Output { + self.rot + rhs + } +} + +#[macros::mass_impl( + $THIS = @OM Pos2D, + $OTHER = @ORM Pos2D +)] +impl core::ops::Add for THIS { + type Output = Pos2D; + #[inline] + #[must_use] + fn add(self, rhs: OTHER) -> Self::Output { + Pos2D::new(self.pos + rhs.pos, self.rot + rhs.rot) + } +} + +#[macros::mass_impl( + $THIS = @OM Pos2D, + $OTHER = @ORM Vec2D +)] +impl core::ops::AddAssign for THIS { + #[inline] + fn add_assign(&mut self, rhs: OTHER) { + self.pos += rhs; + } +} + +#[macros::mass_impl( + $THIS = @OM Pos2D, + $OTHER = @ORM Angle2D +)] +impl core::ops::AddAssign for THIS { + #[inline] + fn add_assign(&mut self, rhs: OTHER) { + self.rot += rhs; + } +} + +#[macros::mass_impl( + $THIS = @OM Pos2D, + $OTHER = @ORM Pos2D +)] +impl core::ops::AddAssign for THIS { + #[inline] + fn add_assign(&mut self, rhs: OTHER) { + self.pos += rhs.pos; + self.rot += rhs.rot; + } +} + +#[macros::mass_impl( + $THIS = @ORM Pos2D, + $OTHER = @ORM Vec2D +)] +impl core::ops::Sub for THIS { + type Output = Coord2D; + #[inline] + #[must_use] + fn sub(self, rhs: OTHER) -> Self::Output { + self.pos - rhs + } +} + +#[macros::mass_impl( + $THIS = @ORM Pos2D, + $OTHER = @ORM Angle2D +)] +impl core::ops::Sub for THIS { + type Output = Angle2D; + #[inline] + #[must_use] + fn sub(self, rhs: OTHER) -> Self::Output { + self.rot - rhs + } +} + +#[macros::mass_impl( + $THIS = @OM Pos2D, + $OTHER = @ORM Pos2D +)] +impl core::ops::Sub for THIS { + type Output = Pos2D; + #[inline] + #[must_use] + fn sub(self, rhs: OTHER) -> Self::Output { + Pos2D::new(self.pos - rhs.pos, self.rot - rhs.rot) + } +} + +#[macros::mass_impl( + $THIS = @OM Pos2D, + $OTHER = @ORM Vec2D +)] +impl core::ops::SubAssign for THIS { + #[inline] + fn sub_assign(&mut self, rhs: OTHER) { + self.pos -= rhs; + } +} + +#[macros::mass_impl( + $THIS = @OM Pos2D, + $OTHER = @OR Angle2D +)] +impl core::ops::SubAssign for THIS { + #[inline] + fn sub_assign(&mut self, rhs: OTHER) { + self.rot -= rhs; + } +} + +#[macros::mass_impl( + $THIS = @OM Pos2D, + $OTHER = @ORM Pos2D +)] +impl core::ops::SubAssign for THIS { + #[inline] + fn sub_assign(&mut self, rhs: OTHER) { + self.pos -= rhs.pos; + self.rot -= rhs.rot; + } +} + +#[macros::mass_impl( + $THIS = @ORM Pos2D +)] +impl core::ops::Neg for THIS { + type Output = Pos2D; + #[inline] + #[must_use] + fn neg(self) -> Self::Output { + Pos2D::new(-self.pos, -self.rot) + } +} + +impl PartialEq for Pos2D { + #[inline] + fn eq(&self, other: &Self) -> bool { + self.pos == other.pos && self.rot == other.rot + } +} + +impl Eq for Pos2D {} diff --git a/libs/helper/macros-core/lib.rs b/libs/helper/macros-core/lib.rs index 5c81041..200f527 100644 --- a/libs/helper/macros-core/lib.rs +++ b/libs/helper/macros-core/lib.rs @@ -1,6 +1,7 @@ //! A collection of core functionality for the macros. -#![warn(missing_docs, unused, clippy::all)] +#![warn(missing_docs, unused, clippy::all, unsafe_code)] +#![deny(missing_debug_implementations)] use proc_macro2::TokenStream; diff --git a/libs/helper/macros/lib.rs b/libs/helper/macros/lib.rs index 97583cb..8710131 100644 --- a/libs/helper/macros/lib.rs +++ b/libs/helper/macros/lib.rs @@ -1,5 +1,6 @@ #![doc = include_str!("./README.md")] -#![warn(missing_docs, unused, clippy::all)] +#![warn(missing_docs, unused, clippy::all, unsafe_code)] +#![deny(missing_debug_implementations)] use proc_macro::TokenStream; diff --git a/libs/robot/README.md b/libs/robot/README.md new file mode 100644 index 0000000..e69de29 diff --git a/libs/robot/hardware/gamepad/impls.rs b/libs/robot/hardware/gamepad/impls.rs index c23093b..f4fe64a 100644 --- a/libs/robot/hardware/gamepad/impls.rs +++ b/libs/robot/hardware/gamepad/impls.rs @@ -1,6 +1,9 @@ +//! Implementations of the `Gamepad` trait for various gamepads. + use super::*; -// #[repr(C)] +/// A gamepad with no inputs. +#[repr(C)] #[derive(Default, Debug, Clone, Copy, PartialEq)] pub struct LogitechF310 { // Dpad @@ -36,128 +39,180 @@ pub struct LogitechF310 { right_stick_button: bool, } +impl LogitechF310 { + /// Creates a new `LogitechF310` with all values set to their defaults. + #[inline] + #[must_use = "This returns a new LogitechF310"] + pub const fn new() -> Self { + Self { + dpad_up: false, + dpad_down: false, + dpad_left: false, + dpad_right: false, + + left_stick_x: 0.0, + left_stick_y: 0.0, + right_stick_x: 0.0, + right_stick_y: 0.0, + + left_trigger: 0.0, + right_trigger: 0.0, + + a: false, + b: false, + x: false, + y: false, + + left_bumper: false, + right_bumper: false, + + back: false, + start: false, + left_stick_button: false, + right_stick_button: false, + } + } +} + impl Gamepad for LogitechF310 { #[inline] - fn dpad(&self) -> GamepadDpad { - GamepadDpad::new( + fn dpad(&self) -> crate::Result { + Ok(GamepadDpad::new( self.dpad_up, self.dpad_down, self.dpad_left, self.dpad_right, - ) + )) } #[inline] - fn left_stick(&self) -> GamepadStick { - GamepadStick::new(self.left_stick_x, self.left_stick_y, self.left_stick_button) + fn left_stick(&self) -> crate::Result { + Ok(GamepadStick::new( + self.left_stick_x, + self.left_stick_y, + self.left_stick_button, + )) } #[inline] - fn right_stick(&self) -> GamepadStick { - GamepadStick::new( + fn right_stick(&self) -> crate::Result { + Ok(GamepadStick::new( self.right_stick_x, self.right_stick_y, self.right_stick_button, - ) + )) } #[inline] - fn left_trigger(&self) -> f64 { - self.left_trigger + fn left_trigger(&self) -> crate::Result { + Ok(self.left_trigger) } #[inline] - fn right_trigger(&self) -> f64 { - self.right_trigger + fn right_trigger(&self) -> crate::Result { + Ok(self.right_trigger) } #[inline] - fn a(&self) -> bool { - self.a + fn a(&self) -> crate::Result { + Ok(self.a) } #[inline] - fn b(&self) -> bool { - self.b + fn b(&self) -> crate::Result { + Ok(self.b) } #[inline] - fn x(&self) -> bool { - self.x + fn x(&self) -> crate::Result { + Ok(self.x) } #[inline] - fn y(&self) -> bool { - self.y + fn y(&self) -> crate::Result { + Ok(self.y) } #[inline] - fn left_bumper(&self) -> bool { - self.left_bumper + fn left_bumper(&self) -> crate::Result { + Ok(self.left_bumper) } #[inline] - fn right_bumper(&self) -> bool { - self.right_bumper + fn right_bumper(&self) -> crate::Result { + Ok(self.right_bumper) } #[inline] - fn back(&self) -> bool { - self.back + fn back(&self) -> crate::Result { + Ok(self.back) } #[inline] - fn start(&self) -> bool { - self.start + fn start(&self) -> crate::Result { + Ok(self.start) } } impl MutableGamepad for LogitechF310 { #[inline] - fn set_dpap(&mut self, dpad: GamepadDpad) { + fn set_dpad(&mut self, dpad: GamepadDpad) -> crate::Result { self.dpad_up = dpad.up; self.dpad_down = dpad.down; self.dpad_left = dpad.left; self.dpad_right = dpad.right; + Ok(()) } #[inline] - fn set_left_stick(&mut self, stick: GamepadStick) { + fn set_left_stick(&mut self, stick: GamepadStick) -> crate::Result { self.left_stick_x = stick.x; self.left_stick_y = stick.y; self.left_stick_button = stick.pressed; + Ok(()) } #[inline] - fn set_right_stick(&mut self, stick: GamepadStick) { + fn set_right_stick(&mut self, stick: GamepadStick) -> crate::Result { self.right_stick_x = stick.x; self.right_stick_y = stick.y; self.right_stick_button = stick.pressed; + Ok(()) } #[inline] - fn set_left_trigger(&mut self, trigger: f64) { + fn set_left_trigger(&mut self, trigger: f64) -> crate::Result { self.left_trigger = trigger; + Ok(()) } #[inline] - fn set_right_trigger(&mut self, trigger: f64) { + fn set_right_trigger(&mut self, trigger: f64) -> crate::Result { self.right_trigger = trigger; + Ok(()) } #[inline] - fn set_a(&mut self, value: bool) { + fn set_a(&mut self, value: bool) -> crate::Result { self.a = value; + Ok(()) } #[inline] - fn set_b(&mut self, value: bool) { + fn set_b(&mut self, value: bool) -> crate::Result { self.b = value; + Ok(()) } #[inline] - fn set_x(&mut self, value: bool) { + fn set_x(&mut self, value: bool) -> crate::Result { self.x = value; + Ok(()) } #[inline] - fn set_y(&mut self, value: bool) { + fn set_y(&mut self, value: bool) -> crate::Result { self.y = value; + Ok(()) } #[inline] - fn set_left_bumper(&mut self, value: bool) { + fn set_left_bumper(&mut self, value: bool) -> crate::Result { self.left_bumper = value; + Ok(()) } #[inline] - fn set_right_bumper(&mut self, value: bool) { + fn set_right_bumper(&mut self, value: bool) -> crate::Result { self.right_bumper = value; + Ok(()) } #[inline] - fn set_back(&mut self, value: bool) { + fn set_back(&mut self, value: bool) -> crate::Result { self.back = value; + Ok(()) } #[inline] - fn set_start(&mut self, value: bool) { + fn set_start(&mut self, value: bool) -> crate::Result { self.start = value; + Ok(()) } } diff --git a/libs/robot/hardware/gamepad/mod.rs b/libs/robot/hardware/gamepad/mod.rs index 69eb529..f729b87 100644 --- a/libs/robot/hardware/gamepad/mod.rs +++ b/libs/robot/hardware/gamepad/mod.rs @@ -1,85 +1,128 @@ +//! + +use l2math::Float64; use libtrig::{Angle2D, Vec2D}; pub mod impls; +/// A trait that allows for reading from a gamepad +/// +/// This is a trait so that it can be implemented for any gamepad pub trait Gamepad: core::fmt::Debug { /// Returns the state of the dpad /// /// Includes up, down, left, and right - fn dpad(&self) -> GamepadDpad; + fn dpad(&self) -> crate::Result; /// Returns the state of the left stick /// /// Includes x, y, and pressed - fn left_stick(&self) -> GamepadStick; + fn left_stick(&self) -> crate::Result; /// Returns the state of the right stick /// /// Includes x, y, and pressed - fn right_stick(&self) -> GamepadStick; + fn right_stick(&self) -> crate::Result; /// Returns the state of the left trigger - fn left_trigger(&self) -> f64; + fn left_trigger(&self) -> crate::Result; /// Returns the state of the right trigger - fn right_trigger(&self) -> f64; + fn right_trigger(&self) -> crate::Result; /// Is the A button pressed? - fn a(&self) -> bool; + fn a(&self) -> crate::Result; /// Is the B button pressed? - fn b(&self) -> bool; + fn b(&self) -> crate::Result; /// Is the X button pressed? - fn x(&self) -> bool; + fn x(&self) -> crate::Result; /// Is the Y button pressed? - fn y(&self) -> bool; + fn y(&self) -> crate::Result; /// Is the left bumper pressed? - fn left_bumper(&self) -> bool; + fn left_bumper(&self) -> crate::Result; /// Is the right bumper pressed? - fn right_bumper(&self) -> bool; + fn right_bumper(&self) -> crate::Result; /// A non-standard 'back' button #[inline] - fn back(&self) -> bool { - false + fn back(&self) -> crate::Result { + Err(crate::HardwareError::Other { + message: "This gamepad does not have a 'back' button", + }) } /// A non-standard 'start' button #[inline] - fn start(&self) -> bool { - false + fn start(&self) -> crate::Result { + Err(crate::HardwareError::Other { + message: "This gamepad does not have a 'start' button", + }) } } /// Allows for the gamepad to be modified +/// +/// PLEASE DO NOT MODIFY THE GAMEPAD ON THE MAIN THREAD pub trait MutableGamepad: Gamepad { - fn set_dpap(&mut self, dpad: GamepadDpad); - fn set_left_stick(&mut self, stick: GamepadStick); - fn set_right_stick(&mut self, stick: GamepadStick); - fn set_left_trigger(&mut self, trigger: f64); - fn set_right_trigger(&mut self, trigger: f64); - fn set_a(&mut self, value: bool); - fn set_b(&mut self, value: bool); - fn set_x(&mut self, value: bool); - fn set_y(&mut self, value: bool); - fn set_left_bumper(&mut self, value: bool); - fn set_right_bumper(&mut self, value: bool); - fn set_back(&mut self, _value: bool) {} - fn set_start(&mut self, _value: bool) {} + /// Sets the state of the dpad + fn set_dpad(&mut self, dpad: GamepadDpad) -> crate::Result; + /// Sets the state of the left stick + fn set_left_stick(&mut self, stick: GamepadStick) -> crate::Result; + /// Sets the state of the right stick + fn set_right_stick(&mut self, stick: GamepadStick) -> crate::Result; + /// Sets the state of the left trigger + fn set_left_trigger(&mut self, trigger: f64) -> crate::Result; + /// Sets the state of the right trigger + fn set_right_trigger(&mut self, trigger: f64) -> crate::Result; + /// Sets the state of the A button + fn set_a(&mut self, value: bool) -> crate::Result; + /// Sets the state of the B button + fn set_b(&mut self, value: bool) -> crate::Result; + /// Sets the state of the X button + fn set_x(&mut self, value: bool) -> crate::Result; + /// Sets the state of the Y button + fn set_y(&mut self, value: bool) -> crate::Result; + /// Sets the state of the left bumper + fn set_left_bumper(&mut self, value: bool) -> crate::Result; + /// Sets the state of the right bumper + fn set_right_bumper(&mut self, value: bool) -> crate::Result; + /// Sets the state of the 'back' button + fn set_back(&mut self, _value: bool) -> crate::Result { + Err(crate::HardwareError::Other { + message: "This gamepad does not have a 'back' button", + }) + } + /// Sets the state of the 'start' button + fn set_start(&mut self, _value: bool) -> crate::Result { + Err(crate::HardwareError::Other { + message: "This gamepad does not have a 'start' button", + }) + } } +/// A struct that holds the state of a gamepad +/// stick #[repr(C)] #[derive(Default, Debug, Clone, Copy, PartialEq)] pub struct GamepadStick { + /// How far the stick is pushed in the x direction (left/right) pub x: f64, + /// How far the stick is pushed in the y direction (up/down) pub y: f64, + /// Is the stick pressed down? pub pressed: bool, } impl GamepadStick { + /// Creates a new `GamepadStick` with the given values #[inline] #[must_use = "This returns a new GamepadStick"] pub const fn new(x: f64, y: f64, pressed: bool) -> Self { Self { x, y, pressed } } + /// Creates a new `GamepadStick` with all values set to their defaults #[inline] #[must_use = "This returns a new GamepadStick"] pub const fn default() -> Self { Self::new(0.0, 0.0, false) } + /// Turns the x and y values of the stick into an angle + /// + /// This is useful for things like precision driving #[inline] pub fn angle(&self) -> libtrig::Angle2D { libtrig::prelude!(); @@ -138,16 +181,26 @@ impl From<(f64, f64)> for GamepadStick { impl Eq for GamepadStick {} +/// A struct that holds the state of a gamepad's Dpad +/// +/// Includes up, down, left, and right +/// +/// (Why is this not called GamepadDpad if it's more like a `+` symbol?) #[repr(C)] #[derive(Default, Debug, Clone, Copy, PartialEq, Eq, Hash)] pub struct GamepadDpad { - up: bool, - down: bool, - left: bool, - right: bool, + /// Is the up button pressed? + pub up: bool, + /// Is the down button pressed? + pub down: bool, + /// Is the left button pressed? + pub left: bool, + /// Is the right button pressed? + pub right: bool, } impl GamepadDpad { + /// Creates a new `GamepadDpad` with the given values #[inline] #[must_use = "This returns a new GamepadDpad"] pub const fn new(up: bool, down: bool, left: bool, right: bool) -> Self { @@ -158,6 +211,7 @@ impl GamepadDpad { right, } } + /// Creates a new `GamepadDpad` with all values set to their defaults #[inline] #[must_use = "This returns a new GamepadDpad"] pub const fn default() -> Self { diff --git a/libs/robot/hardware/lib.rs b/libs/robot/hardware/lib.rs index 71c2927..72e91d3 100644 --- a/libs/robot/hardware/lib.rs +++ b/libs/robot/hardware/lib.rs @@ -1,3 +1,76 @@ -#![no_std] - -pub mod gamepad; +#![doc = include_str!("../README.md")] +#![warn(missing_docs, unused, clippy::all, unsafe_code)] +#![deny(missing_debug_implementations)] + +pub mod gamepad; + +/// ### O_O +/// +/// A type that can be used to represent a result that is always `Ok` +/// +/// It is a shorthand for `Result<(), HardwareError>`. +pub const IO_OK: Result = Ok(()); + +/// An error that occurs when reading from or writing to a hardware component +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub enum HardwareError { + /// An IO error occurred + /// + /// This error is returned when reading from or writing to a hardware component fails. + IO, + /// Also an IO error, but this one is returned when the hardware component is disconnected. + /// + /// That means that the hardware component was connected when the program started, but it was + /// later disconnected. + Disconnected, + /// Some other error occurred + Other { + /// The error message + message: &'static str, + }, +} + +/// A result that occurs when reading or writing to a hardware component +pub type Result = core::result::Result; + +impl core::fmt::Display for HardwareError { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + Self::IO => write!(f, "IO error"), + Self::Disconnected => write!(f, "Gamepad disconnected"), + Self::Other { message } => write!(f, "{}", message), + } + } +} + +impl From<&'static str> for HardwareError { + #[inline] + fn from(message: &'static str) -> Self { + Self::Other { message } + } +} + +impl From for String { + #[inline] + fn from(error: HardwareError) -> Self { + format!("{}", error) + } +} + +impl From for &'static str { + #[inline] + fn from(error: HardwareError) -> Self { + match error { + HardwareError::IO => "IO error", + HardwareError::Disconnected => "Gamepad disconnected", + HardwareError::Other { message } => message, + } + } +} + +#[allow(unsafe_code)] +unsafe impl Send for HardwareError {} +#[allow(unsafe_code)] +unsafe impl Sync for HardwareError {} + +impl std::error::Error for HardwareError {} diff --git a/libs/robot/main.rs b/libs/robot/main.rs index 8722dfe..28c73af 100644 --- a/libs/robot/main.rs +++ b/libs/robot/main.rs @@ -1,39 +1,51 @@ -use pylib::arc as arc_py_module; +use pylib::__init__::arc as arc_py_module; +use pylib::arc_robot_hardware::IO_OK; use pyo3::prelude::*; use std::io::Read; fn main() -> PyResult<()> { + // Initialize Python pyo3::append_to_inittab!(arc_py_module); - pyo3::prepare_freethreaded_python(); + // Setup hardware components + let (gamepad, gamepad_wrapper) = pylib::setup_wrapped_component!( + pylib::arc_robot_hardware::gamepad::impls::LogitechF310::default(); + pylib::__init__::hardware::gamepad::Gamepad + ); + let (op, op_wrapper) = pylib::setup_wrapped_component!(gamepad_wrapper; pylib::__init__::Op); + + // IO Thread + std::thread::spawn(move || { + use hardware::gamepad::MutableGamepad as _; + + std::thread::sleep(std::time::Duration::from_secs(1)); + gamepad.get_mut()?.set_a(true)?; + + std::thread::sleep(std::time::Duration::from_secs(1)); + gamepad.get_mut()?.set_a(false)?; + + std::thread::sleep(std::time::Duration::from_secs(1)); + gamepad.get_mut()?.set_a(true)?; + + std::thread::sleep(std::time::Duration::from_secs(1)); + op.get_mut()?.stop()?; + + op.get_mut()?.running_time(); + + IO_OK + }); + + // Main Thread let mut code = String::new(); std::fs::File::open("./examples/auto.py")?.read_to_string(&mut code)?; - Python::with_gil(|py| { - let fun: Py = PyModule::from_code(py, &code, "main.py", "main")? + // Python (Main Thread) + Python::with_gil(move |py| { + let main_func: pyo3::Py = PyModule::from_code(py, &code, "auto.py", "")? .getattr("main")? .into(); - - let gamepad = pylib::_hardware::gamepad::Gamepad::new( - pylib::hardware::gamepad::impls::LogitechF310::default(), - ); - let gamepad_wrapper = pylib::_hardware::gamepad::Gamepad::wrap(&gamepad); - let op = pylib::Op::new(gamepad_wrapper); - let op_wrapper = pylib::Op::wrap(&op); - - std::thread::spawn(move || { - std::thread::sleep(std::time::Duration::from_secs(1)); - gamepad.get_mut().unwrap().set_a(true); - std::thread::sleep(std::time::Duration::from_secs(1)); - gamepad.get_mut().unwrap().set_a(false); - std::thread::sleep(std::time::Duration::from_secs(1)); - gamepad.get_mut().unwrap().set_a(true); - std::thread::sleep(std::time::Duration::from_secs(1)); - op.get_mut().unwrap().stop(); - }); - - fun.call1(py, (op_wrapper,))?; + main_func.call1(py, (op_wrapper,))?; Ok(()) }) } diff --git a/pyproject.toml b/pyproject.toml index f11280c..adf6e02 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -3,3 +3,7 @@ name = "arc" version = "0.0.1-dev" description = "Advanced Robot Controller" requires-python = ">=3.11" + +[build-system] +requires = ["maturin>=1,<2"] +build-backend = "maturin"