diff --git a/.github/funding.yml b/.github/funding.yml new file mode 100644 index 0000000..a5330d2 --- /dev/null +++ b/.github/funding.yml @@ -0,0 +1 @@ +github: Atomicgamer9523 \ No newline at end of file diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..93005a9 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,119 @@ +name: CI + +env: + RUST_BACKTRACE: 1 + CARGO_INCREMENTAL: 0 + +on: + push: + branches: [ main ] + pull_request: + workflow_dispatch: + +jobs: + # If anything fails, the whole workflow fails. + ci-pass: + name: CI Pass + needs: [style, android, linux, tests] + runs-on: ubuntu-latest + steps: + - run: exit 0 + + # Check style. + style: + name: Style + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Set up Rust + uses: dtolnay/rust-toolchain@stable + with: + components: rustfmt + - name: Install toolchain + run: rustup component add rustfmt + - name: Set up Python + uses: actions/setup-python@v4 + with: + python-version: 3.11 + - name: Give permission to run epearl + run: chmod +x ./epearl + - name: Check style + run: ./epearl fmt --check + + # Check if the Android build works. + android: + name: Android + needs: [style] + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Set up Rust + uses: dtolnay/rust-toolchain@stable + with: + target: aarch64-linux-android + - name: Install rustup target + run: rustup target add aarch64-linux-android + - name: Set up Python + uses: actions/setup-python@v4 + with: + python-version: 3.11 + # We don't need to install the NDK, but this is nice to keep for reference. + # - name: Install Android NDK + # id: setup-ndk + # uses: nttld/setup-ndk@v1 + # with: + # ndk-version: r26b + # local-cache: true + # add-to-path: true + - name: Give permission to run epearl + run: chmod +x ./epearl + - name: Build Android + run: ./epearl build --android + # env: + # ANDROID_NDK_HOME: ${{ steps.setup-ndk.outputs.ndk-path }} + + # Check if the Linux build works. + linux: + name: Linux + needs: [style] + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Set up Rust + uses: dtolnay/rust-toolchain@stable + - name: Set up Python + uses: actions/setup-python@v4 + with: + python-version: 3.11 + - name: Install Python C headers + run: sudo apt-get install -y python3-dev + - name: Give permission to run epearl + run: chmod +x ./epearl + - name: Build + run: ./epearl build + + # Check if the tests pass. + tests: + name: Tests + needs: [style, linux] + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v3 + - name: Set up Rust + uses: dtolnay/rust-toolchain@stable + - name: Set up Binstall + uses: cargo-bins/cargo-binstall@main + - name: Set up Python + uses: actions/setup-python@v4 + with: + python-version: 3.11 + - name: Install Python C headers + run: sudo apt-get install -y python3-dev + - name: Give permission to run epearl + run: chmod +x ./epearl + - name: Run tests + run: ./epearl test all diff --git a/.gitignore b/.gitignore index c4c79e0..8198ebd 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,28 @@ -/*.lock -target \ No newline at end of file +# IDE Files +*.code-workspace +.vscode +.idea + +# Bazel files +bazel-* + +# Lock files +*.lock + +# Python files +__pycache__ + +# Java / Kotlin files +.gradle +local.properties + +# Compiled source +target +build +out + +# Compiled Item +*.dylib +*.dll +*.lib +*.so diff --git a/Cargo.toml b/Cargo.toml index bf28be9..3fbbe34 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,11 +1,25 @@ -[package] -name = "libtrig" -version = "0.1.0" -edition = "2021" +[workspace] +resolver = "2" # Resolver version (keep it 2) +members = [ + # Python interface implementation + "arc", + # ARC client (used to control the robot) + "client", + # The robot code (hardware abstractions, python runtime, http server) + "robot", +] -[lib] -path = "./libtrig/main.rs" +[workspace.package] +# Contributors of team Draniki may add their name bellow +authors = [ + "Матвей Т AtomicGamer9523@github" +] +# Github Repository +repository = "https://github.com/DranikiRobotics/arc" +# Rust edition (keep it 2021) +edition = "2021" +# License of the entire workspace +license = "BSD-2-Clause" -[dependencies.macros] -package = "libtrig-macros" -path = "./macros" +# Version of the entire workspace +version = "0.0.1-dev" diff --git a/LICENSE b/LICENSE index 5f619ee..eda5deb 100644 --- a/LICENSE +++ b/LICENSE @@ -1,21 +1,24 @@ -MIT License +BSD 2-Clause License -Copyright (c) 2023 AtomicGamer9523 +Copyright (c) 2023 AtomicGamer9523 and members of the Draniki Team -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..b92b140 --- /dev/null +++ b/README.md @@ -0,0 +1,25 @@ +# ARC: A complete framework for controlling robots + +ARC is a feature complete framework for writing robot code. It is designed to be easy to use, and easy to understand. It is also designed to be modular, so that you can use only the parts you need. ARC is also designed to be easy to extend, so that you can add your own functionality to it. ARC is an acronym, and it stands for: **A**dvanced **R**obot **C**ontroller. + +## Features + +- **[`l2math`](./libs/l2math/)** - Low Level Math - Fundamental math functions and types. +- **[`libtrig`](./libs/libtrig/)** - Trigonometry functionality (angles, vectors, etc.). + +## Documentation + +For Developers who are planning on using this [read here](./docs/GUIDE.md). + +For Developers who are going to be contributing to this [read here](./docs/CONTRIBUTING.md). + + diff --git a/arc/Cargo.toml b/arc/Cargo.toml new file mode 100644 index 0000000..3a61182 --- /dev/null +++ b/arc/Cargo.toml @@ -0,0 +1,36 @@ +[package] +name = "arc-pylib" +description = "ARC python interface" +repository.workspace = true +version.workspace = true +edition.workspace = true +authors.workspace = true +license.workspace = true + +[lib] +path = "lib.rs" +crate-type = ["cdylib", "rlib"] + +[dependencies.dranikcore] +package = "dranik-core" +path = "../robot/core" +features = [ + "reveal_modules", # More items + "internals", # Internal modules +] + +[dependencies.libtrig] +git = "https://github.com/DranikiRobotics/librobomath" +package = "libtrig" +optional = true + +[dependencies.pyo3] +version = "0.20" +features = [ + "extension-module", # for standalone extension module + "abi3-py311" # for python 3.11 +] + +[features] +dranik-only-builtins = ["dranikcore/only_builtins"] +math = ["dep:libtrig"] diff --git a/macros/README.md b/arc/README.md similarity index 100% rename from macros/README.md rename to arc/README.md diff --git a/arc/__init__.pyi b/arc/__init__.pyi new file mode 100644 index 0000000..7dffd2f --- /dev/null +++ b/arc/__init__.pyi @@ -0,0 +1,111 @@ +""" +### ARC: A complete framework for controlling robots + +ARC is a feature complete framework for writing robot code. +It is designed to be easy to use, and easy to understand. +It is also designed to be modular, so that you can use only the parts you need. +Additionally, it is easy to extend, so that you can add your own functionality to it. +""" + +from .hardware import HardwareMap as _HardwareMap +from .hardware import Telemetry as _Telemetry +from .hardware import Gamepad as _Gamepad +import typing as _typing + +RunResult = _typing.Union[bool, int, str, None] +""" +This type is used to indicate whether or not an `Op` completed successfully. + +NOTE: This does NOT exist at runtime. It is only used for type checking. + +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.""" + + @property + def gamepad(self) -> _Gamepad: + """The gamepad that you can use to control the robot.""" + ... + + @property + def running(self) -> bool: + """Whether or not the operation is running.""" + ... + + @property + def hardwareMap(self) -> _HardwareMap: + """The hardware map for the robot.""" + ... + + @property + def telemetry(self) -> _Telemetry: + """The telemetry system for the robot.""" + ... + + def debug(self, *message: object) -> None: + """ + Sends a debug message using the telemetry system. + + See `op.telemetry.debug` for more information. + """ + self.telemetry.debug(*message) + + def log(self, *message: object) -> None: + """ + Logs message using the telemetry system. + + See `op.telemetry.log` for more information. + """ + self.telemetry.log(*message) + +__OpFunc = _typing.Callable[[Op], RunResult] +__OpAnnotation = _typing.Callable[[__OpFunc], __OpFunc] + +def Auto(name: str, config: str) -> __OpAnnotation: + """ + Decorator for an autonomous operation. + + ### Example: + ```python + @Auto("My Auto") + def main(op: Op): + op.log("Hello, world!") + ``` + """ + ... + +def Teleop(name: str, config: str) -> __OpAnnotation: + """ + Decorator for a teleop operation. + + ### Example: + ```python + @Teleop("My Teleop") + def main(op: Op): + op.log("Hello, world!") + ``` + """ + ... + +def sleep(seconds: float) -> None: + """ + Sleeps for the specified number of seconds. + + This is a blocking operation, which means NOTHING will happen until the specified time has passed. + """ + ... + +OK: RunResult = True +""" +A built-in `RunResult` that indicates that the `Op` completed successfully. + +See `RunResult` for more information. +""" diff --git a/arc/__init__.rs b/arc/__init__.rs new file mode 100644 index 0000000..9ea5f53 --- /dev/null +++ b/arc/__init__.rs @@ -0,0 +1,198 @@ +//! 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 dranikcore::RuntimeOp; +use pyo3::prelude::*; + +/// Hardware submodule +#[path = "hardware/__init__.rs"] +pub mod hardware; + +/// Math submodule +#[path = "math/__init__.rs"] +pub mod math; + +#[cfg(feature = "dranik-only-builtins")] +type OpImpl = RuntimeOp; + +/// The struct that is used to access the data in the op mode +/// +/// This struct internally uses a `ThreadSafe` to access the data. +/// So feel free to clone it. It is also `Send` and `Sync`. +/// +/// This struct should be used for reading the data in the op mode thread. +/// For mutating the data outside of the op mode thread, use the `OpHolder` +/// struct. +/// +/// # Example +/// +/// ```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 +/// 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(Default, Debug, Clone)] +pub struct Op(OpImpl); + +impl From for Op { + #[inline(always)] + #[cfg(feature = "dranik-only-builtins")] + fn from(op: OpImpl) -> Self { + Self(op) + } +} + +impl From for OpImpl { + #[inline(always)] + #[cfg(feature = "dranik-only-builtins")] + fn from(op: Op) -> Self { + op.0 + } +} + +impl pyo3::IntoPy> for Op { + fn into_py(self, py: Python<'_>) -> pyo3::Py { + (self, ).into_py(py) + } +} + +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) -> core::result::Result { + self.0.get().map(|g| g.get_gamepad().into()) + } + /// Returns whether the op mode is running + /// + /// This call aquires a lock on the data + pub fn is_running(&self) -> core::result::Result { + self.0.get().map(|g| g.running()) + } +} + +#[pymethods] +#[doc(hidden)] +impl Op { + #[getter] + #[doc(hidden)] + fn running(&self) -> PyResult { + self.is_running().map_err(crate::make_err) + } + #[getter] + #[doc(hidden)] + fn gamepad(&self) -> PyResult { + self.get_gamepad().map_err(crate::make_err) + } +} + +/// Sleeps for a certain amount of seconds +/// +/// THIS BLOCKS THE CURRENT THREAD +#[pyfunction] +#[doc(hidden)] +fn sleep(seconds: f64) -> PyResult<()> { + std::thread::sleep(std::time::Duration::from_secs_f64(seconds)); + Ok(()) +} + +/// An Autonomous Annotation (Decorator) for Python +/// +/// This annotation is used to mark a function as an autonomous function. +#[pyclass] +#[doc(hidden)] +struct Auto(String); +#[pymethods] +impl Auto { + #[new] + #[doc(hidden)] + #[pyo3(signature = (name, **_kwargs))] + fn __new__(name: &str, _kwargs: Option<&pyo3::types::PyDict>) -> Self { + Self(name.to_string()) + } + #[doc(hidden)] + #[pyo3(signature = (*args, **_kwargs))] + fn __call__( + &self, + _py: Python<'_>, + args: &pyo3::types::PyTuple, + _kwargs: Option<&pyo3::types::PyDict>, + ) -> PyResult> { + let func = args.get_item(0)?; + func.extract::>() + } +} + +/// A Teleop Annotation (Decorator) for Python +/// +/// This annotation is used to mark a function as a teleop function. +#[pyclass] +#[doc(hidden)] +struct Teleop(String); +#[pymethods] +impl Teleop { + #[new] + #[doc(hidden)] + #[pyo3(signature = (name, **_kwargs))] + fn __new__(name: &str, _kwargs: Option<&pyo3::types::PyDict>) -> Self { + Self(name.to_string()) + } + #[doc(hidden)] + #[pyo3(signature = (*args, **_kwargs))] + fn __call__( + &self, + _py: Python<'_>, + args: &pyo3::types::PyTuple, + _kwargs: Option<&pyo3::types::PyDict>, + ) -> PyResult> { + let func = args.get_item(0)?; + func.extract::>() + } +} + +/// Constructs the Python module +/// +/// This function is called by the Python interpreter when the module is imported. +#[pymodule] +#[doc(hidden)] +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)?; + + // 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__.pyi b/arc/hardware/__init__.pyi new file mode 100644 index 0000000..fbb2712 --- /dev/null +++ b/arc/hardware/__init__.pyi @@ -0,0 +1,41 @@ +""" +ARC Hardware module + +This module contains the hardware classes for ARC. +This module includes: +- Gamepads +- Motors +- Sensors +- Servos +""" + +from .telemetry import * +from .gamepad import * +from .dcmotor import * +import typing as _typing +import abc as _abc + +class HardwareComponent(_abc.ABC): + """Represents a basic hardware component.""" + + @_abc.abstractproperty + def uuid(self) -> str: + """The UUID of this hardware component.""" + ... + +class HardwareMap(object): + """ + Represents a map of hardware components. + + This class is used to get hardware components by name. + """ + + HC = _typing.TypeVar("HC", bound = HardwareComponent) + def __getitem__(self, type: type[HC]) -> _typing.Callable[[str], HC]: + """ + Get a hardware component by name. + + This method will return a function that takes a name and returns the hardware component with the specified name. + If no hardware component with the specified name exists, then this method will raise a KeyError. + """ + ... diff --git a/arc/hardware/__init__.rs b/arc/hardware/__init__.rs new file mode 100644 index 0000000..9992579 --- /dev/null +++ b/arc/hardware/__init__.rs @@ -0,0 +1,18 @@ +//! 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/dcmotor.pyi b/arc/hardware/dcmotor.pyi new file mode 100644 index 0000000..6a29b18 --- /dev/null +++ b/arc/hardware/dcmotor.pyi @@ -0,0 +1,11 @@ +from arc.hardware import HardwareComponent as _HardwareComponent + +class DcMotor(_HardwareComponent): + @property + def uuid(self) -> str: ... + def power(self, power: float) -> None: + """ + Set the power of the motor. + + If the power is negative, the motor will spin backwards. + """ \ No newline at end of file diff --git a/arc/hardware/dcmotor.rs b/arc/hardware/dcmotor.rs new file mode 100644 index 0000000..e69de29 diff --git a/arc/hardware/gamepad.pyi b/arc/hardware/gamepad.pyi new file mode 100644 index 0000000..c65b64c --- /dev/null +++ b/arc/hardware/gamepad.pyi @@ -0,0 +1,131 @@ +""" +Gamepad module. + +This module contains the Gamepad class, and related classes. +99% of the time, you will only need the Gamepad class. +""" + +from arc.hardware import HardwareComponent as _HardwareComponent + +class Gamepad(_HardwareComponent): + """Represents a gamepad with buttons x, y, a, and b.""" + + @property + def uuid(self) -> str: ... + + @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.""" + ... + @property + def y(self) -> bool: + """Whether or not the y button is pressed.""" + ... + @property + def a(self) -> bool: + """Whether or not the a button is pressed.""" + ... + @property + 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.""" + + @property + def up(self) -> bool: + """Whether or not the up button is pressed.""" + ... + @property + def down(self) -> bool: + """Whether or not the down button is pressed.""" + ... + @property + def left(self) -> bool: + """Whether or not the left button is pressed.""" + ... + @property + def right(self) -> bool: + """Whether or not the right button is pressed.""" + ... + +class GamepadStick(object): + """Represents a stick on a gamepad.""" + + @property + def x(self) -> float: + """The x value of the stick.""" + ... + @property + def y(self) -> float: + """The y value of the stick.""" + ... + @property + def pressed(self) -> bool: + """Whether or not the stick is pressed.""" + ... + + from ..math import Angle as _Angle + from ..math import Vec2D as _Vec2D + + def as_angle(self) -> _Angle: + """The angle of the stick, in degrees.""" + ... + + def as_vec2d(self) -> _Vec2D: + """The stick as a Vec2D.""" + ... diff --git a/arc/hardware/gamepad.rs b/arc/hardware/gamepad.rs new file mode 100644 index 0000000..b77f9ff --- /dev/null +++ b/arc/hardware/gamepad.rs @@ -0,0 +1,315 @@ +//! Gamepad module for the arc crate +//! +//! Python identifier: `arc.hardware.gamepad` + +use dranikcore::{gamepad as gp, threadsafe::ThreadSafe}; +use gp::Gamepad as _; +use pyo3::prelude::*; + +/// 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. + #[inline(always)] + pub fn get_x(&self) -> f64 { + self.0.x + } + /// Returns the y value of the stick. + #[inline(always)] + pub fn get_y(&self) -> f64 { + self.0.y + } + /// Returns whether or not the stick is pressed. + #[inline(always)] + pub fn get_pressed(&self) -> bool { + self.0.pressed + } + /// Converts the stick into an angle. + #[inline] + #[must_use = "This returns a new angle"] + pub fn into_angle(&self) -> libtrig::Angle2D { + libtrig::Angle2D::from((self.get_x(), self.get_y())) + } + /// Converts the stick into a vector. + #[inline] + #[must_use = "This returns a new 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 From for libtrig::Vec2D { + fn from(stick: GamepadStick) -> Self { + stick.into_vector() + } +} + +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, + }) + } +} + +#[pymethods] +impl GamepadStick { + #[getter] + #[inline(always)] + fn x(&self) -> PyResult { + Ok(self.get_x()) + } + #[getter] + #[inline(always)] + fn y(&self) -> PyResult { + Ok(self.get_y()) + } + #[getter] + #[inline(always)] + fn pressed(&self) -> PyResult { + Ok(self.get_pressed()) + } + #[inline(always)] + fn as_angle(&self) -> PyResult { + Ok(self.into_angle().into()) + } + #[inline(always)] + fn as_vec2d(&self) -> PyResult { + Ok(self.into_vector().into()) + } +} + +/// A struct that holds the state of a gamepad dpad +#[pyclass] +#[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) + } +} + +#[cfg(feature = "dranik-only-builtins")] +type GImpl = ThreadSafe; + +/// 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. +#[pyclass] +#[derive(Debug, Clone)] +pub struct Gamepad(GImpl); + +impl From for Gamepad { + #[inline(always)] + #[cfg(feature = "dranik-only-builtins")] + fn from(gamepad: GImpl) -> Self { + Self(gamepad) + } +} + +impl Gamepad { + /// 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()) + } + + /// 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] + #[inline(always)] + fn dpad(&self) -> PyResult { + self.get_dpad().map_err(crate::make_err) + } + #[getter] + #[inline(always)] + fn left_stick(&self) -> PyResult { + self.get_left_stick().map_err(crate::make_err) + } + #[getter] + #[inline(always)] + fn right_stick(&self) -> PyResult { + self.get_right_stick().map_err(crate::make_err) + } + #[getter] + #[inline(always)] + fn left_trigger(&self) -> PyResult { + self.get_left_trigger().map_err(crate::make_err) + } + #[getter] + #[inline(always)] + fn right_trigger(&self) -> PyResult { + self.get_right_trigger().map_err(crate::make_err) + } + #[getter] + #[inline(always)] + fn x(&self) -> PyResult { + self.get_x().map_err(crate::make_err) + } + #[getter] + #[inline(always)] + fn y(&self) -> PyResult { + self.get_y().map_err(crate::make_err) + } + #[getter] + #[inline(always)] + fn a(&self) -> PyResult { + self.get_a().map_err(crate::make_err) + } + #[getter] + #[inline(always)] + fn b(&self) -> PyResult { + self.get_b().map_err(crate::make_err) + } + #[getter] + #[inline(always)] + fn left_bumper(&self) -> PyResult { + self.get_left_bumper().map_err(crate::make_err) + } + #[getter] + #[inline(always)] + fn right_bumper(&self) -> PyResult { + self.get_right_bumper().map_err(crate::make_err) + } + + #[getter] + #[inline(always)] + fn back(&self) -> PyResult { + self.get_back().map_err(crate::make_err) + } + #[getter] + #[inline(always)] + fn start(&self) -> PyResult { + self.get_start().map_err(crate::make_err) + } +} + +/// The gamepad module +#[doc(hidden)] +pub(crate) fn gamepad_submodule(_py: Python<'_>, m: &PyModule) -> PyResult<()> { + m.add_class::()?; + m.add_class::()?; + m.add_class::()?; + + Ok(()) +} diff --git a/arc/hardware/telemetry.pyi b/arc/hardware/telemetry.pyi new file mode 100644 index 0000000..124cbdc --- /dev/null +++ b/arc/hardware/telemetry.pyi @@ -0,0 +1,22 @@ +class Telemetry(object): + """ + The telemetry system. + + This is used to send messages to the driver station. + """ + + def debug(self, *message: object) -> None: + """ + Sends a debug message to the telemetry system. + + This always sends the message, regardless whether or not the client is going to log it. + """ + ... + + def log(self, *message: object) -> None: + """ + Sends a log message to the telemetry system. + + This always sends the message, and the client will also always log it. + """ + ... diff --git a/arc/hardware/telemetry.rs b/arc/hardware/telemetry.rs new file mode 100644 index 0000000..e69de29 diff --git a/arc/lib.rs b/arc/lib.rs new file mode 100644 index 0000000..ffbcee4 --- /dev/null +++ b/arc/lib.rs @@ -0,0 +1,62 @@ +#![doc = include_str!("./README.md")] +#![warn(missing_docs, unused, clippy::all, unsafe_code)] +#![deny(missing_debug_implementations)] + +#[cfg(any( + not(feature = "dranik-only-builtins"), + not(feature = "math") +))] +compile_error!("The `dranik-only-builtins` feature is required to use this crate, as well as the `math` feature."); + +pub mod __init__; +#[doc(hidden)] +pub mod macros; + +/// Internal function to translate a static string into a PyIOError. +#[doc(hidden)] +fn make_err(e: &'static str) -> pyo3::PyErr { + pyo3::PyErr::new::(e) +} + +/// A type that represents a python function. +/// +/// This type is used to call python functions from rust. +/// +/// # Example +/// +/// ```rust +/// # use pyo3::prelude::*; +/// # use arc_pylib as pylib; +/// ``` +#[repr(transparent)] +#[derive(Debug, Clone)] +pub struct PyFunction(pyo3::Py); + +impl PyFunction { + /// Calls the python function with the given arguments. + pub fn call(&self, + py: pyo3::Python<'_>, + args: impl pyo3::IntoPy> + ) -> pyo3::PyResult { + self.0.call1(py, args) + } +} + +#[doc(hidden)] +#[allow(non_camel_case_types)] +#[derive(Default, Debug, Clone, Copy)] +pub struct __dranik_config; + +impl dranikcore::prelude::RobotConfig for __dranik_config { + fn python_preload() { + use crate::__init__::arc as __arc_pylib; + pyo3::append_to_inittab!(__arc_pylib); + } + type Args = __init__::Op; + fn build_python_main_function_args<'a>( + _py: &pyo3::Python<'_>, + op: &dranikcore::RuntimeOp + ) -> (Self::Args, Option<&'a pyo3::types::PyDict>) { + (__init__::Op::from(op.clone()), None) + } +} diff --git a/arc/macros.rs b/arc/macros.rs new file mode 100644 index 0000000..0925005 --- /dev/null +++ b/arc/macros.rs @@ -0,0 +1,124 @@ +/// 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 { + // Explanation: + // + // 1. We create the submodule via PyModule::new + // 2. Call that the function that represents the submodule (providing py, so this module) + // 3. Then we add the submodule to the parent module. (m.add_submodule(module_name)) + // 4. Finally, run a python script to add the submodule to sys.modules. + ($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) + }}; +} + +#[macro_export(local_inner_macros)] +macro_rules! pyimpl { + ($strcut: ident { $($body: tt)* }) => ( + impl $strcut { + $crate::pyimpl!(@INNER $($body)*); + } + ); + (@INNER) => ( + + ) +} diff --git a/arc/math/__init__.pyi b/arc/math/__init__.pyi new file mode 100644 index 0000000..ec420e3 --- /dev/null +++ b/arc/math/__init__.pyi @@ -0,0 +1,130 @@ +""" +Mathematical utilities for arc. + +Includes: +- Angle +- Vector2D +- Vector4D +- Position2D +""" + +Float64 = float +"""A floating point number with 64 bits of precision.""" +Radian64 = float +"""An angle in radians with 64 bits of precision.""" +Degree64 = float +"""An angle in degrees with 64 bits of precision.""" + +EGAMMA: Float64 +"""The Euler-Mascheroni constant (γ)""" +FRAC_1_SQRT_3: Float64 +"""1/sqrt(3)""" +FRAC_1_SQRT_PI: Float64 +"""1/sqrt(π)""" +PHI: Float64 +"""The golden ratio (φ)""" +SQRT_3: Float64 +"""sqrt(3)""" +E: Float64 +"""Euler’s number (e)""" +FRAC_1_PI: Float64 +"""1/π""" +FRAC_1_SQRT_2: Float64 +"""1/sqrt(2)""" +FRAC_2_PI: Float64 +"""2/π""" +FRAC_2_SQRT_PI: Float64 +"""2/sqrt(π)""" +FRAC_PI_2: Float64 +"""π/2""" +FRAC_PI_3: Float64 +"""π/3""" +FRAC_PI_4: Float64 +"""π/4""" +FRAC_PI_6: Float64 +"""π/6""" +FRAC_PI_8: Float64 +"""π/8""" +LN_2: Float64 +"""ln(2)""" +LN_10: Float64 +"""ln(10)""" +LOG2_10: Float64 +"""log2(10)""" +LOG2_E: Float64 +"""log2(e)""" +LOG10_2: Float64 +"""log10(2)""" +LOG10_E: Float64 +"""log10(e)""" +PI: Float64 +"""Archimedes’ constant (π)""" +SQRT_2: Float64 +"""sqrt(2)""" +TAU: Float64 +"""The full circle constant (τ)""" + +def acos(x: Float64) -> Radian64: + """Arccosine""" + ... +def asin(x: Float64) -> Radian64: + """Arcsine""" + ... +def atan(x: Float64) -> Radian64: + """Arctangent""" + ... +def atan2(y: Float64, x: Float64) -> Radian64: + """Arctangent of y/x""" + ... +def ceil(x: Float64) -> Float64: + """Ceiling""" + ... +def cos(x: Radian64) -> Float64: + """Cosine""" + ... +def dif(x: Float64, y: Float64) -> Float64: + """Positive difference""" + ... +def floor(x: Float64) -> Float64: + """Floor""" + ... +def hypot(x: Float64, y: Float64) -> Float64: + """Calculate the length of the hypotenuse of a right-angle triangle given legs of length x and y.""" + ... +def ln(x: Float64) -> Float64: + """Natural logarithm""" + ... +def ln1p(x: Float64) -> Float64: + """Natural logarithm of 1 plus x""" + ... +def log(x: Float64) -> Float64: + """Natural logarithm""" + ... +def log2(x: Float64) -> Float64: + """Base 2 logarithm""" + ... +def log10(x: Float64) -> Float64: + """Base 10 logarithm""" + ... +def sin(x: Radian64) -> Float64: + """Sine""" + ... +def sqrt(x: Float64) -> Float64: + """Square root""" + ... +def tan(x: Radian64) -> Float64: + """Tangent""" + ... + +from .angle import * +from .vec2d import * +from .vec4d import * +from .pos2d import * + +NonStandardFloat = float +"""A floating point number that can be NaN or infinity.""" + +INFINITY: NonStandardFloat +"""The python representation of infinity.""" +NAN: NonStandardFloat +"""The python representation of NaN.""" diff --git a/arc/math/__init__.rs b/arc/math/__init__.rs new file mode 100644 index 0000000..5454930 --- /dev/null +++ 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.pyi b/arc/math/angle.pyi new file mode 100644 index 0000000..251d616 --- /dev/null +++ b/arc/math/angle.pyi @@ -0,0 +1,103 @@ +from . import Degree64 as _Degree64, Float64 as _Float64, Radian64 as _Radian64 +from .vec2d import Vec2D as _Vec2D + +class Angle(object): + """ + A type representing an angle. + + It is usually used in odometry, but can be used for other things as well. + If you intend to use it to save the position of a robot, you should use + the `Pos2D` type instead. + """ + + @staticmethod + def from_degrees(degrees: _Degree64) -> Angle: + """Creates an angle from degrees.""" + ... + + @staticmethod + def from_radians(radians: _Radian64) -> Angle: + """Creates an angle from radians.""" + ... + + @staticmethod + def from_vec2d(vec: _Vec2D) -> Angle: + """Creates an angle from a vector.""" + ... + + @staticmethod + def zero() -> Angle: + """Creates an angle of zero degrees.""" + ... + + def degrees(self) -> _Degree64: + """Returns the angle in degrees.""" + ... + + def radians(self) -> _Radian64: + """Returns the angle in radians.""" + ... + + def sin(self) -> _Radian64: + """Returns the sine of the angle.""" + ... + + def cos(self) -> _Radian64: + """Returns the cosine of the angle.""" + ... + + def sqrt(self) -> _Float64: + """Returns the square root of the angle.""" + ... + + def __add__(self, other: Angle) -> Angle: + """Adds two angles.""" + ... + + def __sub__(self, other: Angle) -> Angle: + """Subtracts two angles.""" + ... + + def __mul__(self, other: _Float64) -> Angle: + """Multiplies an angle by a scalar.""" + ... + + def __truediv__(self, other: _Float64) -> Angle: + """Divides an angle by a scalar.""" + ... + + def __neg__(self) -> Angle: + """Negates an angle.""" + ... + + def __eq__(self, other: Angle) -> bool: + """Checks if two angles are equal.""" + ... + + def __ne__(self, other: Angle) -> bool: + """Checks if two angles are not equal.""" + ... + + def __lt__(self, other: Angle) -> bool: + """Checks if one angle is less than another.""" + ... + + def __le__(self, other: Angle) -> bool: + """Checks if one angle is less than or equal to another.""" + ... + + def __gt__(self, other: Angle) -> bool: + """Checks if one angle is greater than another.""" + ... + + def __ge__(self, other: Angle) -> bool: + """Checks if one angle is greater than or equal to another.""" + ... + + def __str__(self) -> str: + """Returns a string representation of the angle.""" + ... + + def __repr__(self) -> str: + """Returns a string representation of the angle.""" + ... \ No newline at end of file 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 new file mode 100644 index 0000000..39be423 --- /dev/null +++ b/arc/math/pos2d.pyi @@ -0,0 +1,97 @@ +from . import Float64 as _Float64, Radian64 as _Radian64 +from .vec2d import Vec2D as _Vec2D +from .angle import Angle as _Angle + +class Pos2D(object): + """ + A type that represents a position in 2D space. + + It is usually used to save the position of a robot. + """ + + @staticmethod + def from_angle_and_pos(angle: _Angle, pos: _Vec2D) -> Pos2D: + """Creates a position from an angle and a vector.""" + ... + + @staticmethod + def from_xyr(x: _Float64, y: _Float64, radians: _Radian64) -> Pos2D: + """Creates a position from x, y, and radians.""" + ... + + @staticmethod + def from_xy(x: _Float64, y: _Float64) -> Pos2D: + """Creates a position from x and y. facing 0 degrees.""" + ... + + @staticmethod + def from_vec2d(vec: _Vec2D) -> Pos2D: + """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 + def zero() -> Pos2D: + """Creates a position positioned at the origin. facing 0 degrees.""" + + @property + def x(self) -> _Float64: + """The x component of the position.""" + ... + + @property + def y(self) -> _Float64: + """The y component of the position.""" + ... + + @property + def angle(self) -> _Angle: + """The angle of the position.""" + ... + + 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. + """ + ... + + def __add__(self, other: Pos2D) -> Pos2D: + """ + Adds two positions. + + The angle of the resulting position is the sum of the angles of the two positions. + """ + ... + + def __sub__(self, other: Pos2D) -> Pos2D: + """ + Subtracts two positions. + + The angle of the resulting position is the difference of the angles of the two positions. + + This is what you should use to find the delta between two positions. + """ + ... + + def __neg__(self) -> Pos2D: + """ + Negates a position. + + The angle also gets negated (rotated 180 degrees). + """ + ... + + def __eq__(self, other: Pos2D) -> bool: + """Checks if two positions are equal.""" + ... + + def __ne__(self, other: Pos2D) -> bool: + """Checks if two positions are not equal.""" + ... + \ No newline at end of file diff --git a/arc/math/pos2d.rs b/arc/math/pos2d.rs new file mode 100644 index 0000000..bd06695 --- /dev/null +++ b/arc/math/pos2d.rs @@ -0,0 +1,100 @@ +//! 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 new file mode 100644 index 0000000..8aa587f --- /dev/null +++ b/arc/math/vec2d.pyi @@ -0,0 +1,107 @@ +from .angle import Angle as _Angle +from . import Float64 as _Float64 + +class Vec2D(object): + """ + A type representing a 2D vector. + + It is usually used in odometry, but can be used for other things as well. + If you intend to use it to save the position of a robot, you should use + the `Pos2D` type instead. + """ + + @staticmethod + def from_angle(angle: _Angle, length: _Float64) -> Vec2D: + """Creates a vector from an angle and a length.""" + ... + + @staticmethod + def new(x: _Float64, y: _Float64) -> Vec2D: + """Creates a vector from x and y components.""" + ... + + @property + def x(self) -> _Float64: + """Returns the x component of the vector.""" + ... + + @property + def y(self) -> _Float64: + """Returns the y component of the vector.""" + ... + + def angle(self) -> _Angle: + """Returns the angle of the vector.""" + ... + + def magnitude(self) -> _Float64: + """Returns the magnitude of the vector.""" + ... + + def normalize(self) -> Vec2D: + """Returns a new normalized version of this vector.""" + ... + + def dot(self, other: Vec2D) -> _Float64: + """Returns the dot product of two vectors.""" + ... + + def __add__(self, other: Vec2D) -> Vec2D: + """Adds two vectors.""" + ... + + def __sub__(self, other: Vec2D) -> Vec2D: + """Subtracts two vectors.""" + ... + + def __mul__(self, other: _Float64) -> Vec2D: + """Multiplies a vector by a scalar.""" + ... + + def __truediv__(self, other: _Float64) -> Vec2D: + """Divides a vector by a scalar.""" + ... + + def __neg__(self) -> Vec2D: + """Negates a vector.""" + ... + + def __eq__(self, other: Vec2D) -> bool: + """Checks if two vectors are equal.""" + ... + + def __ne__(self, other: Vec2D) -> bool: + """Checks if two vectors are not equal.""" + ... + + def __lt__(self, other: Vec2D) -> bool: + """Checks if the magnitude of one vector is less than another.""" + ... + + def __gt__(self, other: Vec2D) -> bool: + """Checks if the magnitude of one vector is greater than another.""" + ... + + def __le__(self, other: Vec2D) -> bool: + """Checks if the magnitude of one vector is less than or equal to another.""" + ... + + def __ge__(self, other: Vec2D) -> bool: + """Checks if the magnitude of one vector is greater than or equal to another.""" + ... + + def __str__(self) -> str: + """Returns a string representation of the vector.""" + ... + + def __repr__(self) -> str: + """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/math/vec4d.pyi b/arc/math/vec4d.pyi new file mode 100644 index 0000000..690cff2 --- /dev/null +++ b/arc/math/vec4d.pyi @@ -0,0 +1,105 @@ +from . import Float64 as _Float64 + +class Vec4D(object): + """ + A type representing a 4D vector. + + It is usually used to transmit motion information to the motors. + """ + + @staticmethod + def new(x: _Float64, y: _Float64, z: _Float64, w: _Float64) -> Vec4D: + """Creates a vector from x, y, z, and w components.""" + ... + + @property + def x(self) -> _Float64: + """Returns the x component of the vector.""" + ... + + @property + def y(self) -> _Float64: + """Returns the y component of the vector.""" + ... + + @property + def z(self) -> _Float64: + """Returns the z component of the vector.""" + ... + + @property + def w(self) -> _Float64: + """Returns the w component of the vector.""" + ... + + def magnitude(self) -> _Float64: + """Returns the magnitude of the vector.""" + ... + + def normalize(self) -> Vec4D: + """Returns a new normalized version of this vector.""" + ... + + def dot(self, other: Vec4D) -> float: + """Returns the dot product of two vectors.""" + ... + + def __add__(self, other: Vec4D) -> Vec4D: + """Adds two vectors.""" + ... + + def __sub__(self, other: Vec4D) -> Vec4D: + """Subtracts two vectors.""" + ... + + def __mul__(self, other: _Float64) -> Vec4D: + """Multiplies a vector by a scalar.""" + ... + + def __truediv__(self, other: _Float64) -> Vec4D: + """Divides a vector by a scalar.""" + ... + + def __neg__(self) -> Vec4D: + """Negates a vector.""" + ... + + def __eq__(self, other: Vec4D) -> bool: + """Checks if two vectors are equal.""" + ... + + def __ne__(self, other: Vec4D) -> bool: + """Checks if two vectors are not equal.""" + ... + + def __lt__(self, other: Vec4D) -> bool: + """Checks if the magnitude of one vector is less than another.""" + ... + + def __gt__(self, other: Vec4D) -> bool: + """Checks if the magnitude of one vector is greater than another.""" + ... + + def __le__(self, other: Vec4D) -> bool: + """Checks if the magnitude of one vector is less than or equal to another.""" + ... + + def __ge__(self, other: Vec4D) -> bool: + """Checks if the magnitude of one vector is greater than or equal to another.""" + ... + + def __str__(self) -> str: + """Returns a string representation of the vector.""" + ... + + def __repr__(self) -> str: + """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/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/client/Cargo.toml b/client/Cargo.toml new file mode 100644 index 0000000..6684e8a --- /dev/null +++ b/client/Cargo.toml @@ -0,0 +1,12 @@ +[package] +name = "arc" +description = "ARC Software" +repository.workspace = true +version.workspace = true +edition.workspace = true +authors.workspace = true +license.workspace = true + +[[bin]] +name = "arc" +path = "main.rs" diff --git a/client/main.rs b/client/main.rs new file mode 100644 index 0000000..6655fd6 --- /dev/null +++ b/client/main.rs @@ -0,0 +1,3 @@ +fn main() { + println!("ARC") +} diff --git a/docs/.gitignore b/docs/.gitignore new file mode 100644 index 0000000..46be8dd --- /dev/null +++ b/docs/.gitignore @@ -0,0 +1,2 @@ +# Private files +/*.xcf diff --git a/docs/CONTRIBUTING.md b/docs/CONTRIBUTING.md new file mode 100644 index 0000000..35498f1 --- /dev/null +++ b/docs/CONTRIBUTING.md @@ -0,0 +1,9 @@ +# Development Guide + +## TODO + +### Testing + +```bash +cargo tarpaulin --help +``` diff --git a/docs/GUIDE.md b/docs/GUIDE.md new file mode 100644 index 0000000..e670d8f --- /dev/null +++ b/docs/GUIDE.md @@ -0,0 +1,3 @@ +# Usage Guide + +## TODO diff --git a/docs/GUIDE_MODIFYING_ROBOT_CONFIG.md b/docs/GUIDE_MODIFYING_ROBOT_CONFIG.md new file mode 100644 index 0000000..e69de29 diff --git a/docs/logo.png b/docs/logo.png new file mode 100644 index 0000000..8a14d8c Binary files /dev/null and b/docs/logo.png differ diff --git a/docs/logo.svg b/docs/logo.svg new file mode 100644 index 0000000..6ff0791 --- /dev/null +++ b/docs/logo.svg @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/docs/logo192.ico b/docs/logo192.ico new file mode 100644 index 0000000..d1ed52f Binary files /dev/null and b/docs/logo192.ico differ diff --git a/docs/logo256.ico b/docs/logo256.ico new file mode 100644 index 0000000..13f1822 Binary files /dev/null and b/docs/logo256.ico differ diff --git a/docs/logo64.ico b/docs/logo64.ico new file mode 100644 index 0000000..a7c964b Binary files /dev/null and b/docs/logo64.ico differ diff --git a/docs/logo_white.png b/docs/logo_white.png new file mode 100644 index 0000000..821a899 Binary files /dev/null and b/docs/logo_white.png differ diff --git a/docs/logo_white_a.png b/docs/logo_white_a.png new file mode 100644 index 0000000..52270de Binary files /dev/null and b/docs/logo_white_a.png differ diff --git a/docs/logo_white_gear.png b/docs/logo_white_gear.png new file mode 100644 index 0000000..f75d501 Binary files /dev/null and b/docs/logo_white_gear.png differ diff --git a/dranik.rs b/dranik.rs new file mode 100644 index 0000000..41614c4 --- /dev/null +++ b/dranik.rs @@ -0,0 +1,2 @@ +dranik::use_config!(arc); +dranik::main!(); \ No newline at end of file diff --git a/examples/auto.py b/examples/auto.py new file mode 100644 index 0000000..a1d6428 --- /dev/null +++ b/examples/auto.py @@ -0,0 +1,26 @@ +from arc.hardware import * +from arc import * + +# You are REQUIRED to have a main() function in your program. +# and you MUST NOT call it yourself. +@Teleop("Tank Drive Example", config = "tank_drive") +def main(op: Op): + op.log("Starting...") + + # Create a tank drive with the left and right motors. + leftMotor = op.hardwareMap[DcMotor]("motor0") + rightMotor = op.hardwareMap[DcMotor]("motor1") + + # While the op is running... + while op.running: + # Get the left and right stick values. + l = op.gamepad.left_stick.y + r = op.gamepad.right_stick.y + op.debug("Left: ", l, "Right: ", r) + + # Drive the robot with the left and right sticks. + leftMotor.power(l) + rightMotor.power(r) + + op.log("Done!") + return OK \ No newline at end of file diff --git a/examples/mecanum_auto.py b/examples/mecanum_auto.py new file mode 100644 index 0000000..ac00cec --- /dev/null +++ b/examples/mecanum_auto.py @@ -0,0 +1,35 @@ +from .util.drive import MechanumDrive +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("Mechanum Drive Example Autonomous") +def main(op: Op): + op.log("Starting...") + + # Create a MechanumDrive object. + fl = op.hardwareMap[DcMotor]("motor0") + fr = op.hardwareMap[DcMotor]("motor1") + bl = op.hardwareMap[DcMotor]("motor2") + br = op.hardwareMap[DcMotor]("motor3") + drive = MechanumDrive(fl, fr, bl, br) + + # Drive forward for 2 seconds. + movement = drive.calc(Angle.from_degrees(90), 1, 0) + drive.move(movement) + + sleep(2) + + # Drive right for 2 seconds. + movement = drive.calc(Angle.from_degrees(0), 1, 0) + drive.move(movement) + + sleep(2) + + # Stop + drive.stop() + + op.log("Done!") + return OK diff --git a/examples/mecanum_basic_teleop.py b/examples/mecanum_basic_teleop.py new file mode 100644 index 0000000..54b47fe --- /dev/null +++ b/examples/mecanum_basic_teleop.py @@ -0,0 +1,24 @@ +from .util.drive import MechanumDrive +from arc.hardware import * +from arc import * + +# You are REQUIRED to have a main() function in your program. +# and you MUST NOT call it yourself. +@Teleop("Basic Mechanum Drive Example") +def main(op: Op): + op.log("Starting...") + + # Create a MechanumDrive object. + fl = op.hardwareMap[DcMotor]("motor0") + fr = op.hardwareMap[DcMotor]("motor1") + bl = op.hardwareMap[DcMotor]("motor2") + br = op.hardwareMap[DcMotor]("motor3") + drive = MechanumDrive(fl, fr, bl, br) + + # While the op is running... + while op.running: + # Drive the robot using the gamepad. + drive.moveUsingDefaultSheme(op.gamepad) + + op.log("Done!") + return OK diff --git a/examples/tank_drive_teleop.py b/examples/tank_drive_teleop.py new file mode 100644 index 0000000..a1d6428 --- /dev/null +++ b/examples/tank_drive_teleop.py @@ -0,0 +1,26 @@ +from arc.hardware import * +from arc import * + +# You are REQUIRED to have a main() function in your program. +# and you MUST NOT call it yourself. +@Teleop("Tank Drive Example", config = "tank_drive") +def main(op: Op): + op.log("Starting...") + + # Create a tank drive with the left and right motors. + leftMotor = op.hardwareMap[DcMotor]("motor0") + rightMotor = op.hardwareMap[DcMotor]("motor1") + + # While the op is running... + while op.running: + # Get the left and right stick values. + l = op.gamepad.left_stick.y + r = op.gamepad.right_stick.y + op.debug("Left: ", l, "Right: ", r) + + # Drive the robot with the left and right sticks. + leftMotor.power(l) + rightMotor.power(r) + + op.log("Done!") + return OK \ No newline at end of file diff --git a/examples/util/drive.py b/examples/util/drive.py new file mode 100644 index 0000000..9c9bd60 --- /dev/null +++ b/examples/util/drive.py @@ -0,0 +1,95 @@ +from arc.math import Vec2D, Vec4D, Radian64, Float64, Angle +from arc.hardware import DcMotor, Gamepad +import arc.math as l2math + +from abc import ABC, abstractmethod, abstractstaticmethod +from typing import Generic, TypeVar + +MovementVector = TypeVar("MovementVector") +"""A type variable for movement vectors.""" + +class DriveBase(ABC, Generic[MovementVector]): + """A base class for drive bases.""" + @abstractmethod + def move(self, movement: MovementVector) -> None: + """Drive the robot using a movement vector.""" + raise NotImplementedError("drive() is not implemented for this drive base.") + + @abstractmethod + def stop(self) -> None: + """Stop the robot.""" + raise NotImplementedError("stop() is not implemented for this drive base.") + + @abstractstaticmethod + def calc(*args) -> MovementVector: + """Calculate the movement vector for a drive base.""" + raise NotImplementedError("calc() is not implemented for this drive base.") + +class TankDrive(DriveBase[Vec2D]): + """A class for tank drive.""" + def __init__(self, left: DcMotor, right: DcMotor): + """Create a new TankDrive object.""" + self.left = left + self.right = right + + def move(self, movement: Vec2D): + self.left.power(movement.x) + self.right.power(movement.y) + + def stop(self): + self.move(Vec2D.new(0, 0)) + + @staticmethod + def calc(left: Float64, right: Float64) -> Vec2D: + return Vec2D.new(left, right) + +class MechanumDrive(DriveBase[Vec4D]): + """A class for mechanum drive.""" + def __init__(self, fl: DcMotor, fr: DcMotor, bl: DcMotor, br: DcMotor): + """Create a new MechanumDrive object.""" + self.fl = fl + self.fr = fr + self.bl = bl + self.br = br + + def move(self, movement: Vec4D): + self.fl.power(movement.x) + self.fr.power(movement.y) + self.bl.power(movement.z) + self.br.power(movement.w) + + def stop(self): + self.move(Vec4D.new(0, 0, 0, 0)) + + def moveUsingDefaultSheme(self, gamepad: Gamepad): + """ + Move the robot using the default scheme. + The left stick controls movement, and the right stick controls rotation. + """ + theta = gamepad.left_stick.as_angle() + power = gamepad.left_stick.as_vec2d().magnitude() + rotation = gamepad.right_stick.x + movement = MechanumDrive.calc(theta, power, rotation) + self.move(movement) + + @staticmethod + def calc(angle: Angle, speed: Float64, turn: Radian64) -> Vec4D: + sin = l2math.sin(angle.radians() - l2math.FRAC_PI_4) + cos = l2math.cos(angle.radians() - l2math.FRAC_PI_4) + mx = max(abs(sin), abs(cos)) + + # Calculate the powers for each motor. + fl = speed * (cos / mx) + turn + fr = speed * (sin / mx) - turn + bl = speed * (sin / mx) + turn + br = speed * (cos / mx) - turn + + # Pump up the power if we can + if (speed + abs(turn)) > 1: + fl /= (speed + abs(turn)) + fr /= (speed + abs(turn)) + bl /= (speed + abs(turn)) + br /= (speed + abs(turn)) + + # Calculate the final vector. + return Vec4D.new(fl, fr, bl, br) diff --git a/libtrig/angle.rs b/libtrig/angle.rs deleted file mode 100644 index 8d42879..0000000 --- a/libtrig/angle.rs +++ /dev/null @@ -1,55 +0,0 @@ -use crate::*; - -/// A wrapper around a `Float` value that represents an angle. -/// -/// It can be created from either radians or degrees, and can be converted to either radians or degrees. -#[repr(C)] -#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)] -pub struct Angle(Float, bool); - -impl Angle { - /// Creates a new `Angle` from the given `value` and `is_radians` flag. - #[inline] - #[must_use] - pub const fn new(value: Float, is_radians: bool) -> Self { - Self(value, is_radians) - } - /// Creates a new `Angle` from the given `value` in radians. - #[inline] - #[must_use] - pub const fn from_radians(value: Float) -> Self { - Self::new(value, true) - } - /// Creates a new `Angle` from the given `value` in degrees. - #[inline] - #[must_use] - pub const fn from_degrees(value: Float) -> Self { - Self::new(value, false) - } - /// Returns the value of the angle in radians. - #[inline] - #[must_use] - pub fn to_radians(&self) -> Float { - if self.1 { - self.0 - } else { - self.0.to_radians() - } - } - /// Returns the value of the angle in degrees. - #[inline] - #[must_use] - pub fn to_degrees(&self) -> Float { - if self.1 { - self.0.to_degrees() - } else { - self.0 - } - } -} - -impl std::fmt::Display for Angle { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "{}°", self.to_degrees()) - } -} diff --git a/libtrig/main.rs b/libtrig/main.rs deleted file mode 100644 index e4ceb3f..0000000 --- a/libtrig/main.rs +++ /dev/null @@ -1,8 +0,0 @@ -pub(crate) mod vectors; -pub(crate) mod angle; - -pub use vectors::*; -pub use angle::*; - -pub type Float = f64; -pub type Int = i32; diff --git a/libtrig/vectors.rs b/libtrig/vectors.rs deleted file mode 100644 index 2e7dbee..0000000 --- a/libtrig/vectors.rs +++ /dev/null @@ -1,216 +0,0 @@ -use crate::{Angle, Float}; - -#[repr(C)] -#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)] -pub struct Vec2D(Float, Float); - -impl Vec2D { - /// Creates a new `Vec2D` from the given `x` and `y` values. - #[inline] - #[must_use] - pub const fn new(x: Float, y: Float) -> Self { - Self(x, y) - } - /// Creates a new `Vec2D` from the given `x` and `y` values. - #[inline] - #[must_use] - pub const fn origin() -> Self { - Self(0.0, 0.0) - } - /// Returns the x component of the vector. - #[inline] - #[must_use] - pub const fn x(&self) -> Float { - self.0 - } - /// Returns the y component of the vector. - #[inline] - #[must_use] - pub const fn y(&self) -> Float { - self.1 - } - /// Rotates the vector by the given angle in radians. - #[must_use] - pub fn rotate_by(&self, angle: Angle) -> Self { - let angle = angle.to_radians(); - let (sin, cos) = angle.sin_cos(); - Self( - self.x() * cos - self.y() * sin, - self.x() * sin + self.y() * cos, - ) - } - /// Returns the angle of the vector in radians. - #[inline] - #[must_use] - pub fn angle(&self) -> Angle { - Angle::from_radians(self.y().atan2(self.x())) - } - /// Returns the inverted vector. - /// - /// Same as rotating the vector by 180 degrees. - #[inline] - #[must_use] - pub fn inverse(&self) -> Self { - -*self - } - /// Returns the dot product of the vector and the given vector. - #[inline] - #[must_use] - pub fn dot(&self, other: Self) -> Float { - self.x() * other.x() + self.y() * other.y() - } - /// Returns the magnitude of the vector. - #[inline] - #[must_use] - pub fn magnitude(&self) -> Float { - self.dot(*self).sqrt() - } - /// Returns the normalized vector. - #[inline] - #[must_use] - pub fn normalize(&mut self) { - // let magnitude = self.magnitude(); - // self /= magnitude; - } -} - -#[macros::mass_impl( - $THIS = @ORM Vec2D, - $OTHER = @ORM Vec2D -)] -impl std::ops::Add for THIS { - type Output = Vec2D; - #[inline] - #[must_use] - fn add(self, rhs: OTHER) -> Self::Output { - Vec2D(self.x() + rhs.x(), self.y() + rhs.y()) - } -} - -#[macros::mass_impl( - $THIS = @OM Vec2D, - $OTHER = @ORM Vec2D -)] -impl std::ops::AddAssign for THIS { - #[inline] - fn add_assign(&mut self, rhs: OTHER) { - self.0 += rhs.x(); - self.1 += rhs.y(); - } -} - -#[macros::mass_impl( - $THIS = @ORM Vec2D, - $OTHER = @ORM Vec2D -)] -impl std::ops::Sub for THIS { - type Output = Vec2D; - #[inline] - #[must_use] - fn sub(self, rhs: OTHER) -> Self::Output { - Vec2D(self.x() - rhs.x(), self.y() - rhs.y()) - } -} - -#[macros::mass_impl( - $THIS = @OM Vec2D, - $OTHER = @ORM Vec2D -)] -impl std::ops::SubAssign for THIS { - #[inline] - fn sub_assign(&mut self, rhs: OTHER) { - self.0 -= rhs.x(); - self.1 -= rhs.y(); - } -} - -#[macros::mass_impl( - $THIS = @ORM Vec2D, - $OTHER = @OR Float -)] -impl std::ops::Mul for THIS { - type Output = Vec2D; - #[inline] - #[must_use] - fn mul(self, rhs: OTHER) -> Self::Output { - Vec2D(self.x() * rhs, self.y() * rhs) - } -} - -#[macros::mass_impl( - $THIS = @OM Vec2D, - $OTHER = @OR Float -)] -impl std::ops::MulAssign for THIS { - #[inline] - fn mul_assign(&mut self, rhs: OTHER) { - self.0 *= rhs; - self.1 *= rhs; - } -} - -#[macros::mass_impl( - $THIS = @ORM Vec2D, - $OTHER = @OR Float -)] -impl std::ops::Div for THIS { - type Output = Vec2D; - #[inline] - #[must_use] - fn div(self, rhs: OTHER) -> Self::Output { - Vec2D(self.x() / rhs, self.y() / rhs) - } -} - -#[macros::mass_impl( - $THIS = @OM Vec2D, - $OTHER = @OR Float -)] -impl std::ops::DivAssign for THIS { - #[inline] - fn div_assign(&mut self, rhs: OTHER) { - self.0 /= rhs; - self.1 /= rhs; - } -} - -#[macros::mass_impl($THIS = @ORM Vec2D)] -impl std::ops::Neg for THIS { - type Output = Vec2D; - #[inline] - #[must_use] - fn neg(self) -> Self::Output { - Vec2D(-self.x(), -self.y()) - } -} - -impl std::fmt::Display for Vec2D { - #[inline] - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - write!(f, "[{}, {}]", self.x(), self.y()) - } -} - -impl Default for Vec2D { - #[inline] - #[must_use] - fn default() -> Self { - Self::origin() - } -} - -impl From<(Float, Float)> for Vec2D { - #[inline] - #[must_use] - fn from((x, y): (Float, Float)) -> Self { - Self::new(x, y) - } -} - -impl From for (Float, Float) { - #[inline] - #[must_use] - fn from(Vec2D(x, y): Vec2D) -> Self { - (x, y) - } -} diff --git a/macros/Cargo.toml b/macros/Cargo.toml deleted file mode 100644 index ea674f4..0000000 --- a/macros/Cargo.toml +++ /dev/null @@ -1,13 +0,0 @@ -[package] -name = "libtrig-macros" -version = "0.1.0" -edition = "2021" - -[lib] -name = "libtrig_macros" -proc-macro = true -path = "lib.rs" - -[dependencies.mc] -package = "libtrig-macros-core" -path = "./libtrig-macros-core" diff --git a/macros/lib.rs b/macros/lib.rs deleted file mode 100644 index 886ee0c..0000000 --- a/macros/lib.rs +++ /dev/null @@ -1,17 +0,0 @@ -#![doc = include_str!("./README.md")] - -#![warn(missing_docs, unused, clippy::all)] - -use proc_macro::TokenStream; - -/// A macro for implementing a trait for a list of types. -#[proc_macro_attribute] -pub fn mass_impl(cfg: TokenStream, input: TokenStream) -> TokenStream { - mc::mass_impl(cfg, input).into() -} - -/// A macro for generating FFI functions. -#[proc_macro_attribute] -pub fn ffi(cfg: TokenStream, input: TokenStream) -> TokenStream { - mc::ffi(cfg, input).into() -} diff --git a/macros/libtrig-macros-core/Cargo.toml b/macros/libtrig-macros-core/Cargo.toml deleted file mode 100644 index db0398b..0000000 --- a/macros/libtrig-macros-core/Cargo.toml +++ /dev/null @@ -1,12 +0,0 @@ -[package] -name = "libtrig-macros-core" -version = "0.1.0" -edition = "2021" - -[lib] -path = "lib.rs" - -[dependencies] -syn = { version = "2.0", features = ["full"] } -proc-macro2 = "1.0" -quote = "1.0" diff --git a/macros/libtrig-macros-core/ffi/mod.rs b/macros/libtrig-macros-core/ffi/mod.rs deleted file mode 100644 index 1127c50..0000000 --- a/macros/libtrig-macros-core/ffi/mod.rs +++ /dev/null @@ -1,78 +0,0 @@ -use crate::TokenStream; - -/// Simplifies the creation of FFI functions -/// -/// # Example -/// ``` -/// #[utils::ffi(type = "system")] -/// pub fn Java_Main_greet<'a>( -/// mut env: JNIEnv<'a>, _class: JClass<'a>, input: JString<'a> -/// ) -> jstring { -/// // First, we have to get the string out of Java. Check out the `strings` -/// // module for more info on how this works. -/// let input: String = env.get_string(&input).expect("Couldn't get java string!").into(); -/// -/// // Then we have to create a new Java string to return. Again, more info -/// // in the `strings` module. -/// let output = env.new_string( -/// format!("Hello, {}!", input) -/// ).expect("Couldn't create java string!"); -/// -/// // Finally, extract the raw pointer to return. -/// output.into_raw() -/// } -/// ``` -pub fn ffi>(cfg: T, input: T) -> TokenStream { - let res = parse_func(input.into()).to_string(); - let settings = parse_settings(cfg.into()); - let res = res.replace("__FFI_RAW_MODIFIERS__", &settings.0); - let res = res.replace("__FFI_EXTERN_MODIFIER__", &settings.1); - match syn::parse_str::(&res) { - Ok(res) => res, - Err(err) => err.to_compile_error(), - } -} - -/// parses the function it should be attached to -fn parse_func(item: TokenStream) -> TokenStream { - let input = match syn::parse2::(item) { - Ok(input) => input, - Err(err) => { - return err.to_compile_error(); - } - }; - let ret = &input.sig.output; - let inputs = &input.sig.inputs; - let name = &input.sig.ident; - let generics = &input.sig.generics; - let body = &input.block; - let attrs = &input.attrs; - let vis = &input.vis; - let result = quote::quote! { - #(#attrs)* - #[no_mangle] - #vis __FFI_RAW_MODIFIERS__ extern __FFI_EXTERN_MODIFIER__ fn #name #generics(#inputs) #ret { - #body - } - }; - result.into() -} - -/// parses the settings (can be none, const, unsafe or both) -fn parse_settings<'a>(attr: TokenStream) -> (String, String) { - let modifiers_str = attr.to_string(); - let mut res = String::new(); - if modifiers_str.contains("const") { - res.push_str("const "); - } - if modifiers_str.contains("unsafe") { - res.push_str("unsafe "); - } - let t = if modifiers_str.contains("type=") { - let type_str = modifiers_str.split("type=").collect::>()[1]; - type_str.split(" ").collect::>()[0] - } else { - "C" - }; - (res, format!("\"{}\"",t)) -} diff --git a/macros/libtrig-macros-core/lib.rs b/macros/libtrig-macros-core/lib.rs deleted file mode 100644 index 2f66cc6..0000000 --- a/macros/libtrig-macros-core/lib.rs +++ /dev/null @@ -1,11 +0,0 @@ -//! A collection of core functionality for the macros. - -#![warn(missing_docs, unused, clippy::all)] - -use proc_macro2::TokenStream; - -mod mass_impl; -mod ffi; - -pub use mass_impl::mass_impl; -pub use ffi::ffi; diff --git a/macros/libtrig-macros-core/mass_impl/args.rs b/macros/libtrig-macros-core/mass_impl/args.rs deleted file mode 100644 index c41e1d5..0000000 --- a/macros/libtrig-macros-core/mass_impl/args.rs +++ /dev/null @@ -1,47 +0,0 @@ -//! Argument parsing for the `mass_impl` macro. - -pub use super::variants::TypeVariant; - -/// The configuration for the `mass_impl` macro. -pub struct MassImplMacroConfig { - /// The type variants to implement the trait for. - pub type_variants: Vec, -} - -impl std::fmt::Debug for MassImplMacroConfig { - /// Formats a `TypeVariant` - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - f.debug_struct("MassImplMacroConfig") - .field("type_variants", &self.type_variants) - .field("number_of_passthroughs", &self.number_of_passthroughs()) - .finish() - } -} - -impl MassImplMacroConfig { - /// Returns the number of passthroughs for this type variant. - pub fn number_of_passthroughs(&self) -> i32 { - let mut i = 1; - for variant in &self.type_variants { - i *= variant.number_of_passthroughs(); - } - i - } -} - -// parses this: -// $SELF = @R @M Vec2D, -// $OTHER = @R @M Vec2D -// -impl syn::parse::Parse for MassImplMacroConfig { - fn parse(input: syn::parse::ParseStream) -> syn::Result { - let mut type_variants = Vec::new(); - let vars = syn::punctuated::Punctuated::::parse_terminated(input)?; - for var in vars { - type_variants.push(var); - } - Ok(MassImplMacroConfig { - type_variants, - }) - } -} \ No newline at end of file diff --git a/macros/libtrig-macros-core/mass_impl/mod.rs b/macros/libtrig-macros-core/mass_impl/mod.rs deleted file mode 100644 index cfc830c..0000000 --- a/macros/libtrig-macros-core/mass_impl/mod.rs +++ /dev/null @@ -1,45 +0,0 @@ -use crate::TokenStream; - -mod variants; -mod args; - -/// A macro for implementing a trait for a list of types. -pub fn mass_impl>(cfg: T, input: T) -> TokenStream { - let cfg: TokenStream = cfg.into(); - let input: TokenStream = input.into(); - let config = match syn::parse2::(cfg) { - Ok(config) => config, - Err(err) => { - return err.to_compile_error(); - } - }; - let input = input.to_string(); - let mut results = Vec::new(); - results.push(input); - - for tv in &config.type_variants { - let mut temp_results = Vec::new(); - if tv.allow_owned { - for r in &results { - let new = r.replace(&tv.alias.to_string(), &tv.ty.to_string()); - temp_results.push(new); - } - } - if tv.allow_ref { - for r in &results { - let new = r.replace(&tv.alias.to_string(), &format!("&{}", tv.ty)); - temp_results.push(new); - } - } - if tv.allow_mut { - for r in &results { - let new = r.replace(&tv.alias.to_string(), &format!("&mut {}", tv.ty)); - temp_results.push(new); - } - } - results = temp_results; - } - println!("{:#?}", &results); - let single_str = results.join("\n"); - syn::parse_str::(&single_str).unwrap() -} \ No newline at end of file diff --git a/macros/libtrig-macros-core/mass_impl/variants.rs b/macros/libtrig-macros-core/mass_impl/variants.rs deleted file mode 100644 index 1af0dd3..0000000 --- a/macros/libtrig-macros-core/mass_impl/variants.rs +++ /dev/null @@ -1,107 +0,0 @@ -/// A type variant, used in the `#[mass_impl(...)]` macro. -/// -/// Input generally looks like this: -/// -/// ```rust,no_run,ignore -/// $NAME = @RM SomeStruct -/// ``` -/// -/// Which essentially means: -/// - `$NAME` is the alias for `SomeStruct` -/// - `@R` means that `SomeStruct` can be passed by reference -/// - `@RM` means that `SomeStruct` can be passed by mutable reference -/// -/// So it will create 3 bodies for the macro: -/// - `impl SomeTrait for SomeStruct` -/// - `impl SomeTrait for &SomeStruct` -/// - `impl SomeTrait for &mut SomeStruct` -pub struct TypeVariant { - /// The alias for the type (e.g. `$NAME`) - pub alias: syn::Ident, - /// The type (e.g. `SomeStruct`) - pub ty: syn::Ident, - /// Whether or not the type can be passed by value - pub allow_owned: bool, - /// Whether or not the type can be passed by reference - pub allow_ref: bool, - /// Whether or not the type can be passed by mutable reference - pub allow_mut: bool, -} - -impl TypeVariant { - /// Returns the number of passthroughs for this type variant. - pub fn number_of_passthroughs(&self) -> i32 { - let mut i =if self.allow_owned { 1 } else { 0 }; - if self.allow_ref { i += 1 }; - if self.allow_mut { i += 1 }; - i - } -} - -impl std::fmt::Debug for TypeVariant { - /// Formats a `TypeVariant` - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - f.debug_struct("TypeVariant") - .field("alias", &self.alias) - .field("ty", &self.ty) - .field("allow_owned", &self.allow_owned) - .field("allow_ref", &self.allow_ref) - .field("allow_mut", &self.allow_mut) - .finish() - } -} - -impl syn::parse::Parse for TypeVariant { - /// Turns this: - /// - /// ``` - /// $NAME = @RM SomeStruct, - /// ``` - /// - /// Into this: - /// - /// ```rust,ignore,no_run - /// TypeVariant { - /// alias: "NAME" - /// ty: SomeStruct, - /// allow_owned: true, - /// allow_ref: true, - /// allow_mut: true, - /// } - /// ``` - fn parse(input: syn::parse::ParseStream) -> syn::Result { - use syn::*; - let _ = input.parse::()?; - let alias = input.parse::()?; - input.parse::()?; - let mut allow_owned = false; - let mut allow_ref = false; - let mut allow_mut = false; - loop { - if input.peek(Token![@]) { - let _ = input.parse::()?; - let modifier = input.parse::()?; - let modifier = modifier.to_string(); - if modifier.contains('R') { - allow_ref = true; - } - if modifier.contains('M') { - allow_mut = true; - } - if modifier.contains('O') { - allow_owned = true; - } - } else { - break; - } - }; - let ty = input.parse::()?; - Ok(TypeVariant { - alias, - ty, - allow_owned, - allow_ref, - allow_mut, - }) - } -} diff --git a/robot/Cargo.toml b/robot/Cargo.toml new file mode 100644 index 0000000..3386e75 --- /dev/null +++ b/robot/Cargo.toml @@ -0,0 +1,42 @@ +[package] +name = "dranik" +description = "Dranik Robot" +repository.workspace = true +version.workspace = true +edition.workspace = true +authors.workspace = true +license.workspace = true + +[lib] +name = "dranik" +path = "lib.rs" + +[[bin]] +name = "dranik" +path = "../dranik.rs" + +[dependencies] +# Core of the robot (hardware abstraction layer) +dranik-core = { path = "./core" } +# Robot HTTP client, used to control the robot via HTTP +dranik-web = { path = "./web" } +# Python runtime for the robot +dranik-python = { path = "./python" } +# Async runtime +tokio = { version = "1.35", features = ["rt", "signal"] } + +[dependencies.arc] +path = "../arc" +package = "arc-pylib" +features = [ + "dranik-only-builtins", + "math" +] + +[features] +default = ["only-builtins"] + +only-builtins = [ + "dranik-core/only_builtins", + "dranik-python/dranik-only-builtins", +] diff --git a/robot/README.md b/robot/README.md new file mode 100644 index 0000000..e69de29 diff --git a/robot/core/Cargo.toml b/robot/core/Cargo.toml new file mode 100644 index 0000000..36e5a9b --- /dev/null +++ b/robot/core/Cargo.toml @@ -0,0 +1,39 @@ +[package] +name = "dranik-core" +description = "Dranik Robot Core" +repository.workspace = true +version.workspace = true +edition.workspace = true +authors.workspace = true +license.workspace = true + +[lib] +name = "dranikcore" +path = "lib.rs" + +[dependencies.l2math] +git = "https://github.com/DranikiRobotics/librobomath" +package = "l2math" + +[dependencies.libtrig] +git = "https://github.com/DranikiRobotics/librobomath" +package = "libtrig" + +[dependencies.tokio] +version = "1.35" + +[dependencies.pyo3] +version = "0.20.0" + +[[test]] +name = "rust_only_robot" +path = "tests/rust_only_robot.rs" + +[features] +unstable = ["l2math/unstable", "libtrig/unstable"] + +only_builtins = [] +allow_external_impls = [] + +reveal_modules = [] +internals = [] diff --git a/robot/core/README.md b/robot/core/README.md new file mode 100644 index 0000000..e69de29 diff --git a/robot/core/config.rs b/robot/core/config.rs new file mode 100644 index 0000000..8161e13 --- /dev/null +++ b/robot/core/config.rs @@ -0,0 +1,61 @@ +//! This module contains the [RobotConfig] trait and the [DefaultRobotConfig] struct +//! +//! This is primarily used to configure the robot at a higher level than just changing the +//! entire codebase. +//! +//! This is useful for things like loading rust libraries before the python +//! interpreter is initialized (which is exactly how ARC is loaded). Or for customizing +//! things like the logging system. Or for anything else that needs to be done before +//! the actual robot code is run. + +use pyo3::prelude::*; +use pyo3::types::{PyDict, PyTuple}; +use crate::prelude::*; +use crate::internals::impls; +use impls::HardwareMapImpl as BuiltInHardwareMapImpl; +use impls::TelemetryImpl as BuiltInTelemetryImpl; +use impls::GamepadImpl as BuiltInGamepadImpl; + +/// A trait for common functionality that can be used to configure the robot +#[cfg(not(feature = "only_builtins"))] +pub trait RobotConfig< + H = BuiltInHardwareMapImpl, + T = BuiltInTelemetryImpl, + G = BuiltInGamepadImpl +> where + H: HardwareMap, + T: Telemetry, + G: Gamepad +{ + /// This function is called before the python interpreter is initialized + /// + /// This is useful for loading rust libraries that will then be used by python. + /// In fact, this is exactly how ARC is loaded. + fn python_preload() {} + /// The type that will be passed to the python main function + type Args: IntoPy> + Default; + /// This function is called to build the arguments that will be passed to the python main function + fn build_python_main_function_args<'a>( + _py: &pyo3::Python<'_>, _op: &crate::RuntimeOp + ) -> (Self::Args, Option<&'a PyDict>) { + (Self::Args::default(), None) + } +} + +/// A trait for common functionality that can be used to configure the robot +#[cfg(feature = "only_builtins")] +pub trait RobotConfig { + /// This function is called before the python interpreter is initialized + /// + /// This is useful for loading rust libraries that will then be used by python. + /// In fact, this is exactly how ARC is loaded. + fn python_preload() {} + /// The type that will be passed to the python main function + type Args: IntoPy> + Default; + /// This function is called to build the arguments that will be passed to the python main function + fn build_python_main_function_args<'a>( + _py: &pyo3::Python<'_>, _op: &crate::RuntimeOp + ) -> (Self::Args, Option<&'a PyDict>) { + (Self::Args::default(), None) + } +} diff --git a/robot/core/error.rs b/robot/core/error.rs new file mode 100644 index 0000000..0e47443 --- /dev/null +++ b/robot/core/error.rs @@ -0,0 +1,77 @@ +/// A type that can be used to represent a result that is always `Ok` +/// +/// ### O_O +/// +/// 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 +#[repr(C)] +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub enum HardwareError { + /// The device was disconnected + DeviceDisconnected, + /// The device was not found + DeviceNotFound, + /// Some other error occurred + Other { + /// The error message + message: &'static str, + }, +} + +impl HardwareError { + /// Creates a new `HardwareError::Other` with the given message + #[inline] + #[must_use = "This returns a new HardwareError"] + pub const fn new(message: &'static str) -> Self { + Self::Other { message } + } + /// Returns the error message + #[inline] + #[must_use = "This returns a new string slice"] + pub const fn as_str(&self) -> &'static str { + match self { + HardwareError::DeviceDisconnected => "Device disconnected", + HardwareError::DeviceNotFound => "Device not found", + HardwareError::Other { message } => message, + } + } +} + +/// 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 { + write!(f, "{}", self.as_str()) + } +} + +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 { + error.as_str() + } +} + +#[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/robot/core/gamepad.rs b/robot/core/gamepad.rs new file mode 100644 index 0000000..2bb3d7f --- /dev/null +++ b/robot/core/gamepad.rs @@ -0,0 +1,231 @@ +use crate::hardware::HardwareComponent; + +/// 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: HardwareComponent { + /// Returns the state of the dpad + /// + /// Includes up, down, left, and right + fn dpad(&self) -> crate::Result; + /// Returns the state of the left stick + /// + /// Includes x, y, and pressed + fn left_stick(&self) -> crate::Result; + /// Returns the state of the right stick + /// + /// Includes x, y, and pressed + fn right_stick(&self) -> crate::Result; + /// Returns the state of the left trigger + fn left_trigger(&self) -> crate::Result; + /// Returns the state of the right trigger + fn right_trigger(&self) -> crate::Result; + /// Is the A button pressed? + fn a(&self) -> crate::Result; + /// Is the B button pressed? + fn b(&self) -> crate::Result; + /// Is the X button pressed? + fn x(&self) -> crate::Result; + /// Is the Y button pressed? + fn y(&self) -> crate::Result; + /// Is the left bumper pressed? + fn left_bumper(&self) -> crate::Result; + /// Is the right bumper pressed? + fn right_bumper(&self) -> crate::Result; + + /// A non-standard 'back' button + #[inline] + 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) -> 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 { + /// 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 +/// +/// This is NOT a hardware component, but rather a struct +/// that is used as a wrapper for a sub-component of a gamepad. +#[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!(); + libtrig::Angle2D::from_radians(self.y.atan2(self.x)) + } +} + +impl From for GamepadStick { + #[inline] + #[must_use = "This returns a new GamepadStick"] + fn from(angle: libtrig::Angle2D) -> Self { + libtrig::prelude!(); + Self::new(angle.cos(), angle.sin(), false) + } +} + +impl From for libtrig::Angle2D { + #[inline] + #[must_use = "This returns a new Angle2D"] + fn from(stick: GamepadStick) -> Self { + stick.angle() + } +} + +impl From for GamepadStick { + #[inline] + #[must_use = "This returns a new GamepadStick"] + fn from(vec: libtrig::Vec2D) -> Self { + Self::new(vec.x(), vec.y(), false) + } +} + +impl From for libtrig::Vec2D { + #[inline] + #[must_use = "This returns a new Vec2D"] + fn from(stick: GamepadStick) -> Self { + Self::new(stick.x, stick.y) + } +} + +impl From<(f64, f64, bool)> for GamepadStick { + #[inline] + #[must_use = "This returns a new GamepadStick"] + fn from((x, y, pressed): (f64, f64, bool)) -> Self { + Self::new(x, y, pressed) + } +} + +impl From<(f64, f64)> for GamepadStick { + #[inline] + #[must_use = "This returns a new GamepadStick"] + fn from((x, y): (f64, f64)) -> Self { + Self::new(x, y, false) + } +} + +impl Eq for GamepadStick {} + +/// A struct that holds the state of a gamepad's Dpad +/// +/// Includes up, down, left, and right. +/// +/// This is NOT a hardware component, but rather a struct +/// that is used as a wrapper for a sub-component of a gamepad. +/// +/// (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 { + /// 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 { + Self { up, down, left, 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 { + Self::new(false, false, false, false) + } +} + +impl From<(bool, bool, bool, bool)> for GamepadDpad { + #[inline] + #[must_use = "This returns a new GamepadDpad"] + fn from((up, down, left, right): (bool, bool, bool, bool)) -> Self { + Self::new(up, down, left, right) + } +} + +impl From for (bool, bool, bool, bool) { + #[inline] + #[must_use = "This returns a new tuple"] + fn from(dpad: GamepadDpad) -> Self { + (dpad.up, dpad.down, dpad.left, dpad.right) + } +} diff --git a/robot/core/hardware.rs b/robot/core/hardware.rs new file mode 100644 index 0000000..9cfa3f4 --- /dev/null +++ b/robot/core/hardware.rs @@ -0,0 +1,38 @@ +//! Hardware components used by the robot + +use crate::*; + +mod dcmotor; + +pub use dcmotor::DcMotor; + +/// The hardware map. +/// +/// This is used to load hardware components. +/// +/// # Example +/// ```rust +/// # use dranikcore as robot; +/// # use robot::HardwareMap; +/// # fn example(hardware_map: impl HardwareMap) -> robot::Result<()> { +/// let motor = hardware_map.load::("motor")?; +/// # Ok(()) +/// # } +/// ``` +pub trait HardwareMap { + /// Loads a hardware component with the given UUID + fn load(&self, uuid: impl Into) -> Result; +} + +/// A trait that represents a hardware component +pub trait HardwareComponent: core::fmt::Debug + Send { + /// Returns the UUID of this component + #[allow(non_snake_case)] + fn getUUID(&self) -> HardwareUUID; + /// Internal method used to load a component + #[inline] + #[doc(hidden)] + fn __load_self(_: internals::HardwareComponentLoadMetadata) -> Result where Self: Sized { + Err(HardwareError::Other { message: "This component does not support loading" }) + } +} diff --git a/robot/core/hardware/dcmotor.rs b/robot/core/hardware/dcmotor.rs new file mode 100644 index 0000000..c217eb6 --- /dev/null +++ b/robot/core/hardware/dcmotor.rs @@ -0,0 +1,10 @@ +use super::HardwareComponent; +use l2math::Float64; + +use crate::*; + +/// A simple DC motor +pub trait DcMotor: HardwareComponent { + /// Sets the power of the motor + fn set_power(&mut self, power: Float64) -> Result<()>; +} diff --git a/robot/core/hardware/uuid.rs b/robot/core/hardware/uuid.rs new file mode 100644 index 0000000..62eb377 --- /dev/null +++ b/robot/core/hardware/uuid.rs @@ -0,0 +1,83 @@ +/// A Unique Hardware Identifier +/// +/// This is a struct that is used to identify a specific piece of hardware. +/// This is can be used to load a hardware component from the hardware map. +/// +/// This is a wrapper around `[char; 8]` +#[repr(C)] +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub struct HardwareUUID([char; 8]); + +impl HardwareUUID { + /// Creates a new HardwareUUID from a `[char; 8]` + #[inline(always)] + pub const fn new(uuid: [char; 8]) -> Self { + Self(uuid) + } + /// Converts something that can be converted into a string into a HardwareUUID + /// + /// ### Note + /// + /// If the string is less than 8 characters, the rest of the UUID will be filled with `0`s + #[inline] + pub fn from_to_string(input: impl ToString) -> Self { + let mut uuid = ['0'; 8]; + input.to_string() + .chars() + .enumerate() + .for_each(|(i, c)| uuid[i] = c); + Self::new(uuid) + } +} + +impl From<[char; 8]> for HardwareUUID { + #[inline(always)] + fn from(uuid: [char; 8]) -> Self { + Self::new(uuid) + } +} + +impl From for HardwareUUID { + /// Converts a string into a HardwareUUID + /// + /// ### Note + /// + /// If the string is less than 8 characters, the rest of the UUID will be filled with `0`s + #[inline] + fn from(input: String) -> Self { + Self::from_to_string(input) + } +} + +impl<'a> From<&'a str> for HardwareUUID { + /// Converts a `str` into a HardwareUUID + /// + /// ### Note + /// + /// If the string is less than 8 characters, the rest of the UUID will be filled with `0`s + #[inline] + fn from(input: &'a str) -> Self { + Self::from_to_string(input) + } +} + +impl From for [char; 8] { + #[inline(always)] + fn from(uuid: HardwareUUID) -> Self { + uuid.0 + } +} + +impl From for String { + #[inline(always)] + fn from(uuid: HardwareUUID) -> Self { + uuid.0.iter().collect() + } +} + +impl ToString for HardwareUUID { + #[inline(always)] + fn to_string(&self) -> String { + self.0.iter().collect() + } +} diff --git a/robot/core/internals/INTERNALS.md b/robot/core/internals/INTERNALS.md new file mode 100644 index 0000000..cc9a822 --- /dev/null +++ b/robot/core/internals/INTERNALS.md @@ -0,0 +1,6 @@ +Internals of the robot + +This contains things like: the impls of [`HardwareMap`] or [`Telemetry`], etc. + +[`HardwareMap`]: crate::HardwareMap +[`Telemetry`]: crate::Telemetry \ No newline at end of file diff --git a/robot/core/internals/impls.rs b/robot/core/internals/impls.rs new file mode 100644 index 0000000..7502d0c --- /dev/null +++ b/robot/core/internals/impls.rs @@ -0,0 +1,57 @@ +use std::collections::HashMap; + +use super::HardwareComponentLoadMetadata; +use crate::hardware as h; +use crate::prelude::*; + +mod dcmotor; +mod gamepad; +pub use dcmotor::DcMotorImpl; +pub use gamepad::GamepadImpl; + +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub enum HardwareComponentType { + DcMotor, +} + +#[derive(Default, Debug, Clone)] +pub struct HardwareMapImpl { + components: HashMap, +} + +impl HardwareMapImpl { + pub fn new() -> Self { + Self { components: HashMap::new() } + } +} + +impl h::HardwareMap for HardwareMapImpl { + fn load(&self, uuid: impl Into) -> crate::Result { + let uuid: crate::HardwareUUID = uuid.into(); + if let Some(component) = self.components.get(&uuid) { + return C::__load_self(HardwareComponentLoadMetadata { + uuid: uuid.into(), + }); + } + Err(crate::HardwareError::DeviceNotFound) + } +} + +#[derive(Default, Debug, Clone)] +pub struct TelemetryImpl; + +impl TelemetryImpl { + pub fn new() -> Self { + Self {} + } +} + +impl Telemetry for TelemetryImpl { + fn debug(&self, message: T) { + todo!() + } + fn send(&self, message: T) { + todo!() + } +} + diff --git a/robot/core/internals/impls/dcmotor.rs b/robot/core/internals/impls/dcmotor.rs new file mode 100644 index 0000000..115ed06 --- /dev/null +++ b/robot/core/internals/impls/dcmotor.rs @@ -0,0 +1,29 @@ +use crate::hardware as h; +use crate::*; + +struct DcMotorConfig { + +} + +#[derive(Debug, Clone)] +pub struct DcMotorImpl { + +} + +impl h::HardwareComponent for DcMotorImpl { + #[allow(non_snake_case)] + fn getUUID(&self) -> HardwareUUID { + todo!() + } + + fn __load_self(_: super::HardwareComponentLoadMetadata) -> Result where Self: Sized { + todo!() + } + +} + +impl h::DcMotor for DcMotorImpl { + fn set_power(&mut self, power: f64) -> Result<()> { + todo!() + } +} diff --git a/robot/core/internals/impls/gamepad.rs b/robot/core/internals/impls/gamepad.rs new file mode 100644 index 0000000..38f807b --- /dev/null +++ b/robot/core/internals/impls/gamepad.rs @@ -0,0 +1,125 @@ +use crate::hardware as h; +use crate::*; + +#[derive(Default, Debug, Clone)] +pub struct GamepadImpl { + +} + +impl GamepadImpl { + pub(crate) fn new() -> Self { + Self { + + } + } +} + +impl h::HardwareComponent for GamepadImpl { + /// Returns the UUID of the gamepad + /// + /// Note: This will always be "gamepad0" + #[allow(non_snake_case)] + fn getUUID(&self) -> HardwareUUID { + HardwareUUID::new(['g', 'a', 'm', 'e', 'p', 'a', 'd', '0']) + } + /// Loads the gamepad from the hardware map + /// + /// ### This will always panic + /// + /// This is because the gamepad is not supposed to be loaded from the hardware map. + fn __load_self(_: super::HardwareComponentLoadMetadata) -> Result where Self: Sized { + ::core::panic!("Attempted to load gamepad from hardware map"); + } +} + +impl crate::prelude::Gamepad for GamepadImpl { + fn dpad(&self) -> crate::Result { + todo!() + } + + fn left_stick(&self) -> crate::Result { + todo!() + } + + fn right_stick(&self) -> crate::Result { + todo!() + } + + fn left_trigger(&self) -> crate::Result { + todo!() + } + + fn right_trigger(&self) -> crate::Result { + todo!() + } + + fn a(&self) -> crate::Result { + todo!() + } + + fn b(&self) -> crate::Result { + todo!() + } + + fn x(&self) -> crate::Result { + todo!() + } + + fn y(&self) -> crate::Result { + todo!() + } + + fn left_bumper(&self) -> crate::Result { + todo!() + } + + fn right_bumper(&self) -> crate::Result { + todo!() + } +} + +impl crate::gamepad::MutableGamepad for GamepadImpl { + fn set_dpad(&mut self, _: gamepad::GamepadDpad) -> crate::Result<()> { + todo!() + } + + fn set_left_stick(&mut self, _: gamepad::GamepadStick) -> crate::Result<()> { + todo!() + } + + fn set_right_stick(&mut self, _: gamepad::GamepadStick) -> crate::Result<()> { + todo!() + } + + fn set_left_trigger(&mut self, _: l2math::Float64) -> crate::Result<()> { + todo!() + } + + fn set_right_trigger(&mut self, _: l2math::Float64) -> crate::Result<()> { + todo!() + } + + fn set_a(&mut self, _: bool) -> crate::Result<()> { + todo!() + } + + fn set_b(&mut self, _: bool) -> crate::Result<()> { + todo!() + } + + fn set_x(&mut self, _: bool) -> crate::Result<()> { + todo!() + } + + fn set_y(&mut self, _: bool) -> crate::Result<()> { + todo!() + } + + fn set_left_bumper(&mut self, _: bool) -> crate::Result<()> { + todo!() + } + + fn set_right_bumper(&mut self, _: bool) -> crate::Result<()> { + todo!() + } +} diff --git a/robot/core/internals/mod.rs b/robot/core/internals/mod.rs new file mode 100644 index 0000000..3883e2f --- /dev/null +++ b/robot/core/internals/mod.rs @@ -0,0 +1,21 @@ +#![doc = include_str!("./INTERNALS.md")] + +use super::*; + +pub(crate) mod impls; + +/// Metadata used to load a hardware component +#[derive(Debug, Clone)] +pub struct HardwareComponentLoadMetadata { + pub uuid: HardwareUUID, +} + +pub mod builtins { + use super::*; + + pub type BuiltInHardwareMapImpl = impls::HardwareMapImpl; + pub type BuiltInTelemetryImpl = impls::TelemetryImpl; + pub type BuiltInGamepadImpl = impls::GamepadImpl; + + pub type BuiltInDcMotorImpl = impls::DcMotorImpl; +} diff --git a/robot/core/lib.rs b/robot/core/lib.rs new file mode 100644 index 0000000..29a9082 --- /dev/null +++ b/robot/core/lib.rs @@ -0,0 +1,78 @@ +#![doc = include_str!("./README.md")] +#![warn(missing_docs, unused, clippy::all, unsafe_code)] +#![deny(missing_debug_implementations)] + +#[cfg(any( + all( + feature = "only_builtins", + feature = "allow_external_impls" + ), + all( + not(feature = "only_builtins"), + not(feature = "allow_external_impls") + ) +))] +compile_error!("The `only_builtins` and `allow_external_impls` features are mutually exclusive."); + +#[cfg(not(feature = "reveal_modules"))] +mod threadsafe; +#[cfg(not(feature = "reveal_modules"))] +mod hardware; +#[cfg(not(feature = "reveal_modules"))] +mod gamepad; +#[cfg(feature = "reveal_modules")] +pub mod threadsafe; +#[cfg(feature = "reveal_modules")] +pub mod hardware; +#[cfg(feature = "reveal_modules")] +pub mod gamepad; + +#[cfg(all( + feature = "reveal_modules", + feature = "internals" +))] +#[doc(hidden)] +pub mod internals; +#[cfg(not(all( + feature = "reveal_modules", + feature = "internals" +)))] +mod internals; + +mod telemetry; +mod config; +mod error; +mod op; + +/// The prelude for the `arc` crate. +/// +/// This prelude re-exports all of the important types and traits +pub mod prelude { + pub use super::hardware::*; + pub use super::config::RobotConfig; + pub use super::gamepad::{Gamepad, MutableGamepad}; + pub use super::telemetry::Telemetry; + pub use super::threadsafe::ThreadSafeError; + pub use super::thread_safe; +} + +pub use op::{Op, RuntimeOp}; +pub use error::{HardwareError, Result, IO_OK}; + +#[path = "hardware/uuid.rs"] +mod __uuid; +pub use __uuid::HardwareUUID; + +#[doc(hidden)] +type DeblockResult = core::result::Result; + +/// Deblocks a blocking piece of code. +/// +/// This function is used to take a blocking piece of code and run it in such a way +/// that it doesn't block the entire runtime. +pub async fn deblock(f: F) -> DeblockResult where + F: FnOnce() -> R + Send + 'static, + R: Send + 'static, +{ + tokio::task::spawn_blocking(f).await +} diff --git a/robot/core/op.rs b/robot/core/op.rs new file mode 100644 index 0000000..b3f4d78 --- /dev/null +++ b/robot/core/op.rs @@ -0,0 +1,320 @@ +use crate::prelude::*; +use crate::threadsafe::*; +use crate::{Result, HardwareUUID}; + +#[cfg(not(feature = "only_builtins"))] +#[derive(Debug, Clone)] +pub struct Op where + H: HardwareMap, + T: Telemetry, + G: Gamepad, +{ + running: ThreadSafeBool, + start_time: std::time::Instant, + hardware_map: ThreadSafe, + telemetry: ThreadSafe, + gamepad: ThreadSafe, +} + +#[cfg(feature = "only_builtins")] +#[derive(Debug, Clone)] +pub struct Op { + running: ThreadSafeBool, + start_time: std::time::Instant, + hardware_map: ThreadSafe, + telemetry: ThreadSafe, + gamepad: ThreadSafe, +} + +#[cfg(feature = "only_builtins")] +impl Default for Op { + #[inline(always)] + fn default() -> Self { + Self::new( + crate::internals::builtins::BuiltInHardwareMapImpl::default(), + crate::internals::builtins::BuiltInTelemetryImpl::default(), + crate::internals::builtins::BuiltInGamepadImpl::default() + ) + } +} + +use crate::internals::impls; +use impls::HardwareMapImpl as BuiltInHardwareMapImpl; +use impls::TelemetryImpl as BuiltInTelemetryImpl; +use impls::GamepadImpl as BuiltInGamepadImpl; + +#[cfg(not(feature = "only_builtins"))] +pub type RuntimeOp< + H: HardwareMap, + T: Telemetry, + G: Gamepad, +> = ThreadSafe>; + +#[cfg(feature = "only_builtins")] +pub type RuntimeOp = ThreadSafe; + +#[cfg(not(feature = "only_builtins"))] +impl Op where + H: HardwareMap, + T: Telemetry, + G: Gamepad, +{ + /// Creates a new `Op` with the given hardware map, telemetry, and gamepad + #[inline(always)] + #[must_use = "This returns a new Op"] + pub fn new(hardware_map: H, telemetry: T, gamepad: G) -> Self { + Self { + hardware_map: ThreadSafe::new(hardware_map), + telemetry: ThreadSafe::new(telemetry), + gamepad: ThreadSafe::new(gamepad), + running: ThreadSafeBool::new(false), + start_time: std::time::Instant::now() + } + } + + /// Returns if the op mode is running + /// + /// If the mutex is poisoned, this will return false + #[inline] + pub fn running(&self) -> bool { + match self.running.get() { + Ok(r) => **r, + Err(_) => false, + } + } + /// Returns if the op mode is running + /// + /// The result is wrapped in a `Result` because + /// internally, the value is wrapped in a `Mutex` + /// + /// If the [`Mutex`] is poisoned, this will return an error + /// + /// [`Mutex`]: https://doc.rust-lang.org/std/sync/struct.Mutex.html + #[inline] + pub fn running_result(&self) -> Result { + Ok(self.running.get().map(|r| **r)?) + } + /// Returns a reference to the [`HardwareMap`] of the op mode. + /// + /// It is mostly used for loading hardware components. + /// However, you may use this struct to load hardware components as well. + /// + /// See [`HardwareMap`] for more information. + /// + /// [`HardwareMap`]: ./trait.HardwareMap.html + #[inline] + pub fn hardware_map(&self) -> GetResult<'_, H> { + self.hardware_map.get() + } + /// Returns a cloned version of the [`HardwareMap`] of the op mode. + /// + /// This doesn't do bit for bit cloning, but instead, it clones the + /// internal [`Arc`] that holds the data. + /// + /// See [`HardwareMap`] for more information. + /// + /// [`HardwareMap`]: ./trait.HardwareMap.html + /// [`Arc`]: https://doc.rust-lang.org/std/sync/struct.Arc.html + #[inline] + pub fn get_hardware_map(&self) -> ThreadSafe { + self.hardware_map.clone() + } + /// Returns the [`Telemetry`] of the op mode. + /// + /// It is mostly used for sending log messages to the driver control station. + /// + /// See [`Telemetry`] for more information. + /// + /// [`Telemetry`]: ./trait.Telemetry.html + #[inline] + pub fn telemetry(&self) -> GetResult<'_, T> { + self.telemetry.get() + } + /// Returns a cloned version of the [`Telemetry`] of the op mode. + /// + /// This doesn't do bit for bit cloning, but instead, it clones the + /// internal [`Arc`] that holds the data. + /// + /// See [`Telemetry`] for more information. + /// + /// [`Telemetry`]: ./trait.Telemetry.html + /// [`Arc`]: https://doc.rust-lang.org/std/sync/struct.Arc.html + #[inline] + pub fn get_telemetry(&self) -> ThreadSafe { + self.telemetry.clone() + } + /// Returns the [`Gamepad`] of the op mode. + /// + /// Used for controlling the in TeleOp Modes. + /// + /// See [`Gamepad`] for more information. + /// + /// [`Gamepad`]: ./trait.Gamepad.html + #[inline] + pub fn gamepad(&self) -> GetResult<'_, G> { + self.gamepad.get() + } + /// Returns a cloned version of the [`Gamepad`] of the op mode. + /// + /// This doesn't do bit for bit cloning, but instead, it clones the + /// internal [`Arc`] that holds the data. + /// + /// See [`Gamepad`] for more information. + /// + /// [`Gamepad`]: ./trait.Gamepad.html + /// [`Arc`]: https://doc.rust-lang.org/std/sync/struct.Arc.html + #[inline] + pub fn get_gamepad(&self) -> ThreadSafe { + self.gamepad.clone() + } +} + +#[cfg(feature = "only_builtins")] +impl Op { + pub fn init() -> RuntimeOp { + let hardware_map = crate::internals::builtins::BuiltInHardwareMapImpl::default(); + let telemetry = crate::internals::builtins::BuiltInTelemetryImpl::default(); + let gamepad = crate::internals::builtins::BuiltInGamepadImpl::default(); + let op = Self::new(hardware_map, telemetry, gamepad); + ThreadSafe::new(op) + } + + /// Creates a new `Op` with the given hardware map, telemetry, and gamepad + #[inline(always)] + #[must_use = "This returns a new Op"] + pub fn new( + hardware_map: BuiltInHardwareMapImpl, + telemetry: BuiltInTelemetryImpl, + gamepad: BuiltInGamepadImpl + ) -> Self { + Self { + hardware_map: ThreadSafe::new(hardware_map), + telemetry: ThreadSafe::new(telemetry), + gamepad: ThreadSafe::new(gamepad), + running: ThreadSafeBool::new(false), + start_time: std::time::Instant::now() + } + } + + /// Returns if the op mode is running + /// + /// If the mutex is poisoned, this will return false + #[inline] + pub fn running(&self) -> bool { + match self.running.get() { + Ok(r) => **r, + Err(_) => false, + } + } + /// Returns if the op mode is running + /// + /// The result is wrapped in a `Result` because + /// internally, the value is wrapped in a `Mutex` + /// + /// If the [`Mutex`] is poisoned, this will return an error + /// + /// [`Mutex`]: https://doc.rust-lang.org/std/sync/struct.Mutex.html + #[inline] + pub fn running_result(&self) -> Result { + Ok(self.running.get().map(|r| **r)?) + } + /// Returns a reference to the [`HardwareMap`] of the op mode. + /// + /// It is mostly used for loading hardware components. + /// However, you may use this struct to load hardware components as well. + /// + /// See [`HardwareMap`] for more information. + /// + /// [`HardwareMap`]: ./trait.HardwareMap.html + #[inline] + pub fn hardware_map(&self) -> GetResult<'_, BuiltInHardwareMapImpl> { + self.hardware_map.get() + } + /// Returns a cloned version of the [`HardwareMap`] of the op mode. + /// + /// This doesn't do bit for bit cloning, but instead, it clones the + /// internal [`Arc`] that holds the data. + /// + /// See [`HardwareMap`] for more information. + /// + /// [`HardwareMap`]: ./trait.HardwareMap.html + /// [`Arc`]: https://doc.rust-lang.org/std/sync/struct.Arc.html + #[inline] + pub fn get_hardware_map(&self) -> ThreadSafe { + self.hardware_map.clone() + } + /// Returns the [`Telemetry`] of the op mode. + /// + /// It is mostly used for sending log messages to the driver control station. + /// + /// See [`Telemetry`] for more information. + /// + /// [`Telemetry`]: ./trait.Telemetry.html + #[inline] + pub fn telemetry(&self) -> GetResult<'_, BuiltInTelemetryImpl> { + self.telemetry.get() + } + /// Returns a cloned version of the [`Telemetry`] of the op mode. + /// + /// This doesn't do bit for bit cloning, but instead, it clones the + /// internal [`Arc`] that holds the data. + /// + /// See [`Telemetry`] for more information. + /// + /// [`Telemetry`]: ./trait.Telemetry.html + /// [`Arc`]: https://doc.rust-lang.org/std/sync/struct.Arc.html + #[inline] + pub fn get_telemetry(&self) -> ThreadSafe { + self.telemetry.clone() + } + /// Returns the [`Gamepad`] of the op mode. + /// + /// Used for controlling the in TeleOp Modes. + /// + /// See [`Gamepad`] for more information. + /// + /// [`Gamepad`]: ./trait.Gamepad.html + #[inline] + pub fn gamepad(&self) -> GetResult<'_, BuiltInGamepadImpl> { + self.gamepad.get() + } + /// Returns a cloned version of the [`Gamepad`] of the op mode. + /// + /// This doesn't do bit for bit cloning, but instead, it clones the + /// internal [`Arc`] that holds the data. + /// + /// See [`Gamepad`] for more information. + /// + /// [`Gamepad`]: ./trait.Gamepad.html + /// [`Arc`]: https://doc.rust-lang.org/std/sync/struct.Arc.html + #[inline] + pub fn get_gamepad(&self) -> ThreadSafe { + self.gamepad.clone() + } +} + +#[cfg(not(feature = "only_builtins"))] +impl HardwareMap for Op where + H: HardwareMap, + T: Telemetry, + G: Gamepad, +{ + /// Loads a hardware component with the given UUID + /// + /// This is just a shortcut for `self.hardware_map.load(uuid)` + #[inline] + fn load(&self, uuid: impl Into) -> Result { + self.hardware_map.get()?.load(uuid) + } +} + +#[cfg(feature = "only_builtins")] +impl HardwareMap for Op { + /// Loads a hardware component with the given UUID + /// + /// This is just a shortcut for `self.hardware_map.load(uuid)` + #[inline] + fn load(&self, uuid: impl Into) -> Result { + self.hardware_map()?.load(uuid) + } +} diff --git a/robot/core/telemetry.rs b/robot/core/telemetry.rs new file mode 100644 index 0000000..615c995 --- /dev/null +++ b/robot/core/telemetry.rs @@ -0,0 +1,19 @@ +use core::fmt::{Debug, Display}; + +/// A type that allows sending telemetry data to the driver control station +/// +/// Although this is a trait, it is not meant to be implemented by the user. +/// Instead, it is implemented by the robot crate. +/// +/// Even though it is marked as `HardwareComponent`, it is not meant to be loaded. +/// Infact, it will always return `HardwareError::DeviceNotFound` when loaded. +pub trait Telemetry { + /// Sends a debug message to the driver control station + /// + /// It will not be displayed to the driver control station if the robot is not in debug mode. + fn debug(&self, message: T); + /// Sends a message to the driver control station + /// + /// This will always be displayed to the driver control station. + fn send(&self, message: T); +} diff --git a/robot/core/tests/rust_only_robot.rs b/robot/core/tests/rust_only_robot.rs new file mode 100644 index 0000000..b476064 --- /dev/null +++ b/robot/core/tests/rust_only_robot.rs @@ -0,0 +1,5 @@ +#[test] +fn rust_only_robot() -> ::core::result::Result<(), Box> { + + Ok(()) +} \ No newline at end of file diff --git a/robot/core/threadsafe.rs b/robot/core/threadsafe.rs new file mode 100644 index 0000000..6a21948 --- /dev/null +++ b/robot/core/threadsafe.rs @@ -0,0 +1,334 @@ +//! Thread-safe values. +//! +//! This module contains thread-safe values that can be used in a +//! multi-threaded environment. + +/// Macro for implementing [`Send`] and [`Sync`] for a struct. +#[macro_export] +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 {} + }; +} + +mod holders { + use std::sync::{Arc, Mutex, MutexGuard}; + + const POISON_MESSAGE: &str = "Poisoned mutex"; + + #[derive(Debug, Default)] + pub struct ThreadSafeHolder(Arc>); + + /// The Standard Result type retuned by the thread-safe holders. + pub type StandardResult = ::core::result::Result; + + /// The result type returned when borrowing from a thread-safe holder. + pub type GetResult<'a, T> = StandardResult>; + /// The result type returned when mutably borrowing from a thread-safe holder. + pub type GetResultMut<'a, T> = StandardResult>; + + /// A Holder for immutable thread-safe values. + #[derive(Debug)] + #[repr(transparent)] + pub struct SafeHeld<'a, T>(MutexGuard<'a, T>); + + /// A Holder for mutable thread-safe values. + #[derive(Debug)] + #[repr(transparent)] + pub struct SafeHeldMut<'a, T>(MutexGuard<'a, T>); + + impl<'a, T> std::ops::Deref for SafeHeld<'a, T> { + type Target = T; + #[inline] + fn deref(&self) -> &Self::Target { + &*self.0 + } + } + + impl<'a, T> std::ops::Deref for SafeHeldMut<'a, T> { + type Target = T; + #[inline] + fn deref(&self) -> &Self::Target { + &*self.0 + } + } + + impl<'a, T> std::ops::DerefMut for SafeHeldMut<'a, T> { + #[inline] + fn deref_mut(&mut self) -> &mut Self::Target { + &mut *self.0 + } + } + + impl Clone for ThreadSafeHolder { + #[inline] + fn clone(&self) -> Self { + Self(self.0.clone()) + } + } + + impl ThreadSafeHolder { + #[inline] + pub fn new(value: T) -> Self { + ThreadSafeHolder(Arc::new(Mutex::new(value))) + } + pub fn get(&self) -> GetResult<'_, T> { + Ok(SafeHeld(self.0.lock().map_err(|_| POISON_MESSAGE)?)) + } + pub fn get_mut(&self) -> GetResultMut<'_, T> { + Ok(SafeHeldMut(self.0.lock().map_err(|_| POISON_MESSAGE)?)) + } + } + + crate::prelude::thread_safe!(ThreadSafeHolder); +} + +#[cfg(not(feature = "internals"))] +pub(crate) use holders::{GetResult, GetResultMut, SafeHeld, SafeHeldMut, StandardResult}; +#[cfg(feature = "internals")] +pub use holders::{GetResult, GetResultMut, SafeHeld, SafeHeldMut, StandardResult}; + +/// A thread-safe value. +/// +/// 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::*; + + #[repr(transparent)] + #[derive(Default, Debug, Clone)] + pub struct $name(ThreadSafe); + + thread_safe!($name); + + impl PartialEq for $name { + #[inline] + 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 PartialOrd for $name { + #[inline] + 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 { + #[inline] + 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 { + #[inline] + pub fn new(value: $primitive) -> Self { + Self(ThreadSafe::new(Primitive::new(value))) + } + #[inline] + pub fn get(&self) -> holders::GetResult<'_, Primitive> { + self.0.get() + } + #[inline] + 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); + + thread_safe!(Primitive); + + impl Primitive { + #[inline] + pub const fn new(value: $primitive) -> Self { + Self(value) + } + #[inline] + pub fn get(&self) -> $primitive { + self.0 + } + #[inline] + pub fn set(&mut self, value: $primitive) { + self.0 = value; + } + } + + impl core::ops::Deref for Primitive { + type Target = $primitive; + #[inline] + fn deref(&self) -> &Self::Target { + &self.0 + } + } + + impl core::ops::DerefMut for Primitive { + #[inline] + fn deref_mut(&mut self) -> &mut Self::Target { + &mut self.0 + } + } + + impl From<$primitive> for Primitive { + #[inline] + fn from(value: $primitive) -> Self { + Self(value) + } + } + + impl From for $primitive { + #[inline] + fn from(value: Primitive) -> Self { + value.0 + } + } + + impl From<$primitive> for $name { + #[inline] + fn from(value: $primitive) -> Self { + Self::new(value) + } + } + + impl From<$name> for $primitive { + #[inline] + fn from(value: $name) -> Self { + match value.get() { + Ok(b) => b.get(), + Err(_) => $default_value, + } + } + } + } + + thread_safe_primitive!($($rest)*); + ); + () => (); +} + +thread_safe_primitive! { + /// A thread-safe boolean. + /// + /// This is a wrapper around a [`bool`] that is thread-safe. + /// + /// [`bool`]: https://doc.rust-lang.org/std/primitive.bool.html + pub type ThreadSafeBool = bool; {thread_safe_bool; false} + + /// A thread-safe 8-bit signed integer. + /// + /// This is a wrapper around a [`i8`] that is thread-safe. + /// + /// [`i8`]: https://doc.rust-lang.org/std/primitive.i8.html + #[allow(dead_code)] + pub type ThreadSafeI8 = i8; {thread_safe_i8; 0} + + /// A thread-safe 8-bit unsigned integer. + /// + /// This is a wrapper around a [`u8`] that is thread-safe. + /// + /// [`u8`]: https://doc.rust-lang.org/std/primitive.u8.html + #[allow(dead_code)] + pub type ThreadSafeU8 = u8; {thread_safe_u8; 0} + + /// A thread-safe 16-bit signed integer. + /// + /// This is a wrapper around a [`i16`] that is thread-safe. + /// + /// [`i16`]: https://doc.rust-lang.org/std/primitive.i16.html + #[allow(dead_code)] + pub type ThreadSafeI16 = i16; {thread_safe_i16; 0} + + /// A thread-safe 16-bit unsigned integer. + /// + /// This is a wrapper around a [`u16`] that is thread-safe. + /// + /// [`u16`]: https://doc.rust-lang.org/std/primitive.u16.html + #[allow(dead_code)] + pub type ThreadSafeU16 = u16; {thread_safe_u16; 0} + + /// A thread-safe 32-bit signed integer. + /// + /// This is a wrapper around a [`i32`] that is thread-safe. + /// + /// [`i32`]: https://doc.rust-lang.org/std/primitive.i32.html + #[allow(dead_code)] + pub type ThreadSafeI32 = i32; {thread_safe_i32; 0} + + /// A thread-safe 32-bit unsigned integer. + /// + /// This is a wrapper around a [`u32`] that is thread-safe. + /// + /// [`u32`]: https://doc.rust-lang.org/std/primitive.u32.html + #[allow(dead_code)] + pub type ThreadSafeU32 = u32; {thread_safe_u32; 0} + + /// A thread-safe 64-bit signed integer. + /// + /// This is a wrapper around a [`i64`] that is thread-safe. + /// + /// [`i64`]: https://doc.rust-lang.org/std/primitive.i64.html + #[allow(dead_code)] + pub type ThreadSafeI64 = i64; {thread_safe_i64; 0} + + /// A thread-safe 64-bit unsigned integer. + /// + /// This is a wrapper around a [`u64`] that is thread-safe. + /// + /// [`u64`]: https://doc.rust-lang.org/std/primitive.u64.html + #[allow(dead_code)] + pub type ThreadSafeU64 = u64; {thread_safe_u64; 0} + + /// A thread-safe 128-bit signed integer. + /// + /// This is a wrapper around a [`i128`] that is thread-safe. + /// + /// [`i128`]: https://doc.rust-lang.org/std/primitive.i128.html + #[allow(dead_code)] + pub type ThreadSafeI128 = i128; {thread_safe_i128; 0} + + /// A thread-safe 128-bit unsigned integer. + /// + /// This is a wrapper around a [`u128`] that is thread-safe. + /// + /// [`u128`]: https://doc.rust-lang.org/std/primitive.u128.html + #[allow(dead_code)] + pub type ThreadSafeU128 = u128; {thread_safe_u128; 0} +} diff --git a/robot/lib.rs b/robot/lib.rs new file mode 100644 index 0000000..9f0d85c --- /dev/null +++ b/robot/lib.rs @@ -0,0 +1,127 @@ +#![doc = include_str!("./README.md")] +#![warn(missing_docs, unused, clippy::all, unsafe_code)] +#![deny(missing_debug_implementations)] + +/// Helps with loading robot configurations. +/// +/// ## Example using the default config +/// +/// ```rust +/// dranik::use_config!(); +/// dranik::main!(); +/// ``` +/// +/// ## Example using ARC +/// +/// ```rust +/// dranik::use_config!(arc); +/// dranik::main!(); +/// ``` +#[macro_export(local_inner_macros)] +macro_rules! use_config { + () => ( $crate::use_config!($crate); ); + ($namespace: path) => ( + use $namespace::{__dranik_config as __DranikRobotConfig}; + ); +} + +/// Creates the main function for the robot. +/// +/// ## Example +/// +/// ```rust +/// dranik::use_config!(); +/// dranik::main!(); +/// ``` +/// +/// This macro is used to create the main function for the robot. +/// It is guaranteed to be stable and it's API will not change. +/// +/// The reason this macro exists is because the main function +/// is not guaranteed to be stable and may change at any time. +/// That mostly includes the generic parameters. +#[macro_export(local_inner_macros)] +macro_rules! main { + () => ( fn main() { + $crate::main::<__DranikRobotConfig>(); + } ); +} + +pub use dranikcore::prelude::RobotConfig; +use std::sync::atomic::AtomicU8; +use std::sync::atomic::Ordering; +use tokio::runtime::Builder; + +static ID: AtomicU8 = AtomicU8::new(0); + +/// This is the actual main function that is called by the robot. +/// +/// It isn't recommended to call this function directly. +/// However, if you do, be careful as this function is not +/// guaranteed to be stable and may change at any time. +/// See [`main!`] for more information. +/// +/// ## Recommended Usage +/// +/// ```rust +/// dranik::use_config!(); +/// dranik::main!(); +/// ``` +/// +/// ## Example using ARC +/// +/// ```rust +/// dranik::use_config!(arc); +/// dranik::main!(); +/// ``` +/// +/// ## Internal workings +/// +/// What it does in a nutshell is: +/// 1. Initialize the async runtime. +/// 2. Setup the IO. +/// 3. Spawn the web server. +/// 4. Spawn the python thread. +/// 5. inject shutdown handler. +/// 6. Block until shutdown. +/// 7. Shutdown the runtime. +/// 8. Exit. +/// +/// [`main!`]: crate::main +pub fn main() { + let runtime = Builder::new_multi_thread() + .enable_all() + .worker_threads(4) + .thread_name_fn(|| { + let id = ID.fetch_add(1, Ordering::SeqCst); + format!("Dranik worker#{}", id) + }) + .build() + .expect("Failed to initialize runtime"); + + // This isn't how it's supposed to be done, but it works for now + // + // In the future the http server will be the one creating the OpMode + // and then feeding it to the Python thread. + let op = dranikcore::Op::init(); + + // runtime.spawn(dranikweb::main()); + runtime.spawn(dranikpy::main::(op)); + runtime.block_on(async { + tokio::signal::ctrl_c().await.expect("Failed to listen for ctrl-c"); + print!("Exiting..."); + }); + runtime.shutdown_timeout(std::time::Duration::from_secs(5)); + std::process::exit(0); +} + +/// Internal struct that is used to hold the robot config. +/// +/// This contains the default config. +#[doc(hidden)] +#[allow(non_camel_case_types)] +#[derive(Default, Debug, Clone, Copy)] +pub struct __dranik_config; +impl dranikcore::prelude::RobotConfig for __dranik_config { + type Args = (); +} diff --git a/robot/python/Cargo.toml b/robot/python/Cargo.toml new file mode 100644 index 0000000..e6949c5 --- /dev/null +++ b/robot/python/Cargo.toml @@ -0,0 +1,22 @@ +[package] +name = "dranik-python" +description = "Dranik Robot Python Runtime" +repository.workspace = true +version.workspace = true +edition.workspace = true +authors.workspace = true +license.workspace = true + +[lib] +name = "dranikpy" +path = "lib.rs" + +[dependencies.dranikcore] +package = "dranik-core" +path = "../core" + +[dependencies.pyo3] +version = "0.20.0" + +[features] +dranik-only-builtins = ["dranikcore/only_builtins"] diff --git a/robot/python/README.md b/robot/python/README.md new file mode 100644 index 0000000..e69de29 diff --git a/robot/python/lib.rs b/robot/python/lib.rs new file mode 100644 index 0000000..9366dac --- /dev/null +++ b/robot/python/lib.rs @@ -0,0 +1,91 @@ +#![doc = include_str!("./README.md")] + +#![warn(missing_docs, unused, clippy::all, unsafe_code)] +#![deny(missing_debug_implementations)] + +const PY_OK: pyo3::PyResult<()> = Ok(()); + +use dranikcore::prelude::*; +use pyo3::prelude::*; +use pyo3::Python; + +#[cfg(not(feature = "dranik-only-builtins"))] +pub async fn main< + PyArgs, + RoboConf, + HardwareMapImpl, + TelemetryImpl, + GamepadImpl, +>( + op: dranikcore::RuntimeOp< + HardwareMapImpl, + TelemetryImpl, + GamepadImpl, + >, +) where + PyArgs: IntoPy> + Default, + RoboConf: RobotConfig< + HardwareMapImpl, + TelemetryImpl, + GamepadImpl, + Args = PyArgs + >, + HardwareMapImpl: HardwareMap + 'static, + TelemetryImpl: Telemetry + 'static, + GamepadImpl: Gamepad + 'static, +{ + RoboConf::python_preload(); + + pyo3::prepare_freethreaded_python(); + + dranikcore::deblock(move || Python::with_gil(|py| { + // add files to sys.path + let syspath = py.import("sys")? + .getattr("path")? + .downcast::()?; + syspath.insert(0, "./examples")?; + + let args = RoboConf::build_python_main_function_args(&py, &op); + + // import main module + let main = py.import("auto")?; + let mainfunc = main.getattr("main")?; + mainfunc.call(args.0, args.1)?; + PY_OK + })) + .await + .expect("Failed to run python main") + .expect("Python main returned an error"); +} + +#[cfg(feature = "dranik-only-builtins")] +pub async fn main< + PyArgs, + RoboConf, +>(op: dranikcore::RuntimeOp) where + PyArgs: IntoPy> + Default, + RoboConf: RobotConfig +{ + RoboConf::python_preload(); + + pyo3::prepare_freethreaded_python(); + + dranikcore::deblock(move || Python::with_gil(|py| { + // add files to sys.path + let syspath = py.import("sys")? + .getattr("path")? + .downcast::()?; + syspath.insert(0, "./examples")?; + + let args = RoboConf::build_python_main_function_args(&py, &op); + + // import main module + let main = py.import("auto")?; + let mainfunc = main.getattr("main")?; + mainfunc.call(args.0, args.1)?; + PY_OK + })) + .await + .expect("Failed to run python main") + .expect("Python main returned an error"); +} diff --git a/robot/web/Cargo.toml b/robot/web/Cargo.toml new file mode 100644 index 0000000..5db70e0 --- /dev/null +++ b/robot/web/Cargo.toml @@ -0,0 +1,19 @@ +[package] +name = "dranik-web" +description = "Dranik HTTP Server" +repository.workspace = true +version.workspace = true +edition.workspace = true +authors.workspace = true +license.workspace = true + +[lib] +name = "dranikweb" +path = "lib.rs" + +[dependencies.rocket] +version = "0.5" +features = [] + +[dependencies.log] +version = "0.4" diff --git a/robot/web/README.md b/robot/web/README.md new file mode 100644 index 0000000..e69de29 diff --git a/robot/web/assets/logo.png b/robot/web/assets/logo.png new file mode 100644 index 0000000..8a14d8c Binary files /dev/null and b/robot/web/assets/logo.png differ diff --git a/robot/web/assets/logo.svg b/robot/web/assets/logo.svg new file mode 100644 index 0000000..6ff0791 --- /dev/null +++ b/robot/web/assets/logo.svg @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/robot/web/assets/logo192.ico b/robot/web/assets/logo192.ico new file mode 100644 index 0000000..d1ed52f Binary files /dev/null and b/robot/web/assets/logo192.ico differ diff --git a/robot/web/assets/logo256.ico b/robot/web/assets/logo256.ico new file mode 100644 index 0000000..13f1822 Binary files /dev/null and b/robot/web/assets/logo256.ico differ diff --git a/robot/web/assets/logo64.ico b/robot/web/assets/logo64.ico new file mode 100644 index 0000000..a7c964b Binary files /dev/null and b/robot/web/assets/logo64.ico differ diff --git a/robot/web/assets/logo_white.png b/robot/web/assets/logo_white.png new file mode 100644 index 0000000..821a899 Binary files /dev/null and b/robot/web/assets/logo_white.png differ diff --git a/robot/web/assets/logo_white_a.png b/robot/web/assets/logo_white_a.png new file mode 100644 index 0000000..52270de Binary files /dev/null and b/robot/web/assets/logo_white_a.png differ diff --git a/robot/web/assets/logo_white_gear.png b/robot/web/assets/logo_white_gear.png new file mode 100644 index 0000000..f75d501 Binary files /dev/null and b/robot/web/assets/logo_white_gear.png differ diff --git a/robot/web/lib.rs b/robot/web/lib.rs new file mode 100644 index 0000000..a3f7205 --- /dev/null +++ b/robot/web/lib.rs @@ -0,0 +1,11 @@ +#[macro_export] +macro_rules! include_hbs { + ($file:expr $(,)?) => ( + ::core::include_str!($file) + .handlebars("debug", $crate::html::SERVER) + ) +} + +pub async fn main() { + +}