antaeus 0.3.10

A Versatile Framework for Vexide
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154

//! Drivetrain Control
//!
//! This module defines traits and enums related to drivetrain types
// TODO: Add Docs

use std::cell::BorrowMutError;

use snafu::Snafu;
use vexide::{
    controller::ControllerError,
    math::Angle,
    prelude::Controller,
    smart::{PortError, motor::BrakeMode},
};

use crate::utils::error::Report;

pub mod differential;

/// Any form of Drivable Drivetrain that can be controlled using a controller
pub trait Drivable {
    /// Controls a tank-style drivetrain using the input from a controller.
    ///
    /// In tank drive mode, each joystick directly controls one side of the
    /// drivetrain. The left stick Y-axis controls the left motors, and the
    /// right stick Y-axis controls the right motors.
    fn tank(&mut self, controller: &Controller) -> Result<(), DrivetrainError>;
    /// Drive the robot using arcade controls (single-stick forward/back + single-stick turn).
    ///
    /// Behavior:
    /// * Forward/backward is read from the left stick Y axis.
    /// * Turning is read from the right stick X axis.
    /// * The two values are mixed into left/right voltages as:
    ///   * left = (fwd + turn) * 12.0
    ///   * right = (fwd - turn) * 12.0
    fn arcade(&mut self, controller: &Controller) -> Result<(), DrivetrainError>;
    /// Drive the robot using reversed tank controls (sticks swapped and inverted).
    ///
    /// Behavior:
    /// * Left motors take input from the RIGHT stick Y axis, inverted.
    /// * Right motors take input from the LEFT stick Y axis, inverted.
    /// * Computation:
    ///   * left = (-right_y) * 12.0
    ///   * right = (-left_y) * 12.0
    /// * This is useful when the robot is driving backwards but you want the sticks
    ///   to maintain an intuitive left/right mapping relative to the robot's new front.
    fn reverse_tank(&mut self, controller: &Controller) -> Result<(), DrivetrainError>;
    /// Drive the robot using reversed arcade controls (forward/turn both inverted).
    ///
    /// Behavior:
    /// * Forward/backward is read from the left stick Y axis, but inverted (fwd = -left_y).
    /// * Turning is read from the right stick X axis, also inverted (turn = -right_x).
    /// * Mixed into left/right voltages as:
    ///   * left = (fwd + turn) * 12.0
    ///   * right = (fwd - turn) * 12.0
    /// * This inversion preserves intuitive steering when the robot is driving backwards
    ///   (pushing the right stick right still causes a clockwise turn relative to the driver).
    /// * On controller read error, zeroed inputs are used and a warning is logged.
    ///
    /// Notes:
    /// * Inputs are assumed to be in the range [-1.0, 1.0] and are scaled to volts by 12.0.
    fn reverse_arcade(&mut self, controller: &Controller) -> Result<(), DrivetrainError>;
}

/// A Differential Drivetrain (or tank drive) is a drivetrain that has 2
/// separate sides that move independently to moe the robot.
pub trait Differential {
    /// Sets the brake mode for all motors in the drivetrain.
    ///
    /// The brake mode determines how motors behave when no voltage is applied:
    ///
    /// * [`BrakeMode::Coast`]: Motors spin freely.
    /// * [`BrakeMode::Brake`]: Motors actively resist rotation.
    /// * [`BrakeMode::Hold`]: Motors actively hold their position.
    fn set_brakemode(&self, brakemode: BrakeMode) -> Result<(), DrivetrainError>;
    /// Returns the average encoder position of all motors (left + right).
    ///
    /// The position is read from each motor’s integrated encoder and averaged.
    /// The result is returned as an [`Angle`].
    ///
    /// ## Error behavior
    ///
    /// - If reading any motor fails, that motor contributes `0` to the sum and an
    ///   error is recorded/logged.
    /// - If a motor group cannot be borrowed, the group is skipped and an error is
    ///   recorded/logged.
    ///
    /// Notes:
    /// - The current implementation divides by the number of motors successfully
    ///   iterated. If *no* motors are available, the divisor becomes `0.0` and the
    ///   returned angle will be non-finite. If you need a stricter guarantee,
    ///   consider handling the `errors` vector and/or adding your own guard.
    fn position(&self) -> Report<Angle, Vec<DrivetrainError>>;
    /// Returns the average encoder position of all left motors.
    ///
    /// See [`Differential::position`] for notes on error behavior and averaging.
    fn left_position(&self) -> Report<Angle, Vec<DrivetrainError>>;
    /// Returns the average encoder position of all right motors.
    ///
    /// See [`Differential::position`] for notes on error behavior and averaging.
    fn right_position(&self) -> Report<Angle, Vec<DrivetrainError>>;

    /// Resets the integrated encoder position on all drivetrain motors.
    ///
    /// This will attempt to reset both left and right motor groups.
    fn reset_position(&self) -> Result<(), DrivetrainError>;
    /// Sets the integrated encoder position on all drivetrain motors.
    fn set_position(&self, position: Angle) -> Result<(), DrivetrainError>;
    /// Sets the same voltage on all drivetrain motors.
    ///
    /// `voltage` is in volts.
    fn set_voltage(&self, voltage: f64) -> Result<(), DrivetrainError>;

    /// Sets the same voltage on all *left* drivetrain motors.
    ///
    /// `voltage` is in volts.
    fn set_left_voltage(&self, voltage: f64) -> Result<(), DrivetrainError>;
    /// Sets the same voltage on all *right* drivetrain motors.
    ///
    /// `voltage` is in volts.
    fn set_right_voltage(&self, voltage: f64) -> Result<(), DrivetrainError>;
}

/// Errors that can occur while commanding or reading from the drivetrain.
#[derive(Debug, Snafu)]
pub enum DrivetrainError {
    /// An error occurred while accessing a motor port (e.g. invalid port
    /// number, hardware failure, etc.).
    #[snafu(transparent)]
    PortError {
        /// The underlying error from the when trying to access a motor port.
        source: PortError,
    },
    /// An error occurred while reading the controller state (e.g. disconnected
    /// controller, communication error, etc.).
    #[snafu(transparent)]
    ControllerError {
        /// The underlying error from the when trying to read the controller
        /// state.
        source: ControllerError,
    },
    /// Failed to borrow the motor group mutably (e.g. already borrowed
    /// elsewhere).
    #[snafu(transparent)]
    BorrowMutError {
        /// The underlying error from trying to borrow the motor group mutably.
        source: BorrowMutError,
    },
    /// An unknown error occurred (catch-all for unexpected issues).
    Unknown {
        /// A string describing the unknown error.
        string: String,
    },
}