From 05f2f790ab7362009ecaa58e1da4a2360f9ea94a Mon Sep 17 00:00:00 2001 From: codingduck106 Date: Mon, 29 Sep 2025 10:55:52 -0700 Subject: [PATCH 01/13] Added somewhat thorough documentation to lib2813 --- .../team2813/lib2813/control/ControlMode.java | 111 +- .../lib2813/control/DeviceInformation.java | 193 +- .../com/team2813/lib2813/control/Encoder.java | 189 +- .../team2813/lib2813/control/InvertType.java | 290 ++- .../com/team2813/lib2813/control/Motor.java | 130 +- .../team2813/lib2813/control/PIDMotor.java | 132 +- .../control/encoders/CancoderWrapper.java | 187 +- .../lib2813/control/imu/Pigeon2Wrapper.java | 184 +- .../control/motors/SparkMaxWrapper.java | 206 +- .../control/motors/TalonFXWrapper.java | 252 ++- .../preferences/PersistedConfiguration.java | 271 +-- .../lib2813/subsystems/MotorSubsystem.java | 138 +- .../ParameterizedIntakeSubsystem.java | 65 +- .../subsystems/lightshow/Lightshow.java | 97 +- .../subsystems/lightshow/QueueLightshow.java | 46 +- .../lib2813/subsystems/lightshow/State.java | 17 +- .../team2813/lib2813/util/BuildConstants.java | 6 +- .../lib2813/util/BuildConstantsPublisher.java | 4 +- .../lib2813/util/BuildConstantsRecord.java | 23 +- .../team2813/lib2813/util/ConfigUtils.java | 28 +- .../team2813/lib2813/util/ControlUtils.java | 25 +- .../team2813/lib2813/util/FakePIDMotor.java | 15 + .../lib2813/util/InputValidation.java | 21 +- .../team2813/lib2813/util/IntTriFunction.java | 16 + .../lib2813/util/InvalidCanIdException.java | 26 +- .../team2813/lib2813/util/RobotFactory.java | 31 + .../lib2813/util/ShuffleboardTabs.java | 21 +- .../com/team2813/lib2813/util/Units2813.java | 40 +- .../lib2813/control/InvertTypeTest.java | 26 +- .../control/motors/TalonFXEqualsTest.java | 13 +- .../preferences/IsolatedPreferences.java | 40 +- .../PersistedConfigurationTest.java | 798 +------ .../ParameterizedIntakeSubsystemTest.java | 91 +- .../util/BuildConstantsPublisherTest.java | 75 +- .../lib2813/util/ControlUtilsTest.java | 47 +- .../lib2813/util/FakeShuffleboardTabs.java | 25 + .../lib2813/util/InputValidationTest.java | 29 +- .../limelight/AprilTagMapPoseHelper.java | 38 +- .../lib2813/limelight/BotPoseEstimate.java | 18 +- .../lib2813/limelight/DataCollection.java | 57 +- .../lib2813/limelight/JSONHelper.java | 57 +- .../team2813/lib2813/limelight/Limelight.java | 62 +- .../lib2813/limelight/LimelightClient.java | 36 +- .../lib2813/limelight/LimelightHelpers.java | 1994 +++++++++++++++-- .../lib2813/limelight/LocationalData.java | 84 +- .../limelight/NetworkTablesLimelight.java | 27 +- .../team2813/lib2813/limelight/Optionals.java | 43 +- .../lib2813/limelight/RestLimelight.java | 163 +- .../lib2813/limelight/StubLocationalData.java | 96 +- .../limelight/apriltag_map/Fiducial.java | 25 + .../apriltag_map/FiducialRetriever.java | 26 +- .../limelight/apriltag_map/FieldMap.java | 18 + .../lib2813/limelight/FakeLimelight.java | 102 +- .../limelight/LegacyRestLimelightTest.java | 29 +- .../lib2813/limelight/LimelightTestCase.java | 132 +- .../limelight/NetworkTablesLimelightTest.java | 77 + .../lib2813/limelight/RestLimelightTest.java | 62 +- .../testing/junit/jupiter/CommandTester.java | 12 +- .../junit/jupiter/WPILibExtension.java | 56 +- .../lib2813/testing/truth/Pose2dSubject.java | 63 +- .../lib2813/testing/truth/Pose3dSubject.java | 64 +- .../testing/truth/Rotation2dSubject.java | 64 +- .../testing/truth/Rotation3dSubject.java | 86 +- .../testing/truth/TolerantComparison.java | 43 +- .../testing/truth/Translation2dSubject.java | 67 +- .../testing/truth/Translation3dSubject.java | 73 +- .../junit/jupiter/WPILibExtensionTest.java | 40 +- 67 files changed, 5867 insertions(+), 1755 deletions(-) diff --git a/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java b/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java index 0ae960c7..29d68004 100644 --- a/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java +++ b/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java @@ -2,19 +2,106 @@ import com.revrobotics.spark.SparkBase.ControlType; +/** + * Enumeration defining standardized motor control modes for the team's control system. + * + *

This enum provides a vendor-neutral abstraction over different motor controller + * control modes while maintaining compatibility with specific hardware implementations. + * Each control mode maps to the appropriate vendor-specific control type for seamless + * integration with different motor controller families. + * + *

The enum currently includes mappings for REV Robotics SPARK controllers, with + * each mode corresponding to a specific {@link ControlType} from the SPARK API. + * Additional vendor mappings can be added as needed. + * + *

Supported control modes: + *

+ * + * @author Team 2813 + * @since 1.0 + */ public enum ControlMode { - DUTY_CYCLE(ControlType.kDutyCycle), - VELOCITY(ControlType.kVelocity), - MOTION_MAGIC(ControlType.kPosition), - VOLTAGE(ControlType.kVoltage); + + /** + * Open-loop duty cycle control mode. + * + *

Controls the motor by setting the duty cycle (percentage of full power) directly. + * The motor output is proportional to the demand value, typically ranging from + * -1.0 (full reverse) to +1.0 (full forward). This is the most basic control mode + * and does not use feedback control. + * + *

Maps to {@link ControlType#kDutyCycle} for SPARK controllers. + */ + DUTY_CYCLE(ControlType.kDutyCycle), + + /** + * Closed-loop velocity control mode. + * + *

Controls the motor to maintain a specific velocity using PID feedback control. + * The controller continuously adjusts the motor output to minimize the error between + * the demanded velocity and the actual measured velocity from the encoder. This mode + * is ideal for applications requiring consistent speed regardless of load variations. + * + *

Maps to {@link ControlType#kVelocity} for SPARK controllers. + */ + VELOCITY(ControlType.kVelocity), + + /** + * Closed-loop position control mode with motion profiling. + * + *

Controls the motor to reach a specific position using advanced motion profiling + * algorithms. The controller generates smooth velocity and acceleration profiles to + * move the mechanism to the target position while respecting configured motion + * constraints (max velocity, max acceleration). This mode provides the smoothest + * and most controlled movement for precise positioning applications. + * + *

Maps to {@link ControlType#kPosition} for SPARK controllers. + * + *

Note: Despite the name "MOTION_MAGIC," this maps to position control + * type for SPARK controllers, as the motion profiling is handled internally. + */ + MOTION_MAGIC(ControlType.kPosition), + + /** + * Open-loop voltage control mode. + * + *

Controls the motor by applying a specific voltage directly to the motor terminals. + * Unlike duty cycle control, voltage control compensates for battery voltage variations + * to provide more consistent motor behavior. The demand value represents the desired + * voltage to apply to the motor. + * + *

Maps to {@link ControlType#kVoltage} for SPARK controllers. + */ + VOLTAGE(ControlType.kVoltage); - private final ControlType sparkMode; + /** The corresponding SPARK controller control type for this mode */ + private final ControlType sparkMode; - ControlMode(ControlType sparkMode) { - this.sparkMode = sparkMode; - } + /** + * Creates a ControlMode with the specified SPARK controller mapping. + * + * @param sparkMode the corresponding {@link ControlType} for SPARK controllers + */ + ControlMode(ControlType sparkMode) { + this.sparkMode = sparkMode; + } - public ControlType getSparkMode() { - return sparkMode; - } -} + /** + * Gets the corresponding SPARK controller control type for this mode. + * + *

This method provides the mapping from the vendor-neutral ControlMode + * to the specific {@link ControlType} required by REV Robotics SPARK + * controllers. This abstraction allows the same control mode enum to be + * used across different motor controller implementations. + * + * @return the corresponding {@link ControlType} for SPARK controllers + */ + public ControlType getSparkMode() { + return sparkMode; + } +} \ No newline at end of file diff --git a/lib/src/main/java/com/team2813/lib2813/control/DeviceInformation.java b/lib/src/main/java/com/team2813/lib2813/control/DeviceInformation.java index cf760617..71439b56 100644 --- a/lib/src/main/java/com/team2813/lib2813/control/DeviceInformation.java +++ b/lib/src/main/java/com/team2813/lib2813/control/DeviceInformation.java @@ -3,59 +3,142 @@ import com.team2813.lib2813.util.InputValidation; import java.util.Optional; +/** + * Immutable value class representing the identifying information for a CAN device. + * + *

This class encapsulates the essential information needed to uniquely identify + * a device on the CAN bus network: the CAN ID and the specific CAN bus the device + * is connected to. It provides a standardized way to represent device identity + * across the team's control system architecture. + * + *

The class distinguishes between devices on the RoboRIO's built-in CAN bus + * (represented by an empty Optional for the canbus) and devices on named CAN buses + * such as CANivore or other CAN bus interfaces. + * + *

Key features: + *

+ * + *

This class is commonly used as a key in device registries and for comparing + * device instances to determine if they represent the same physical hardware. + * + * @author Team 2813 + * @since 1.0 + */ public final class DeviceInformation { - private int id; - private Optional canbus; - - /** - * Creates a DeviceInformation for a device on the RoboRIO can loop - * - * @param id the can ID - */ - public DeviceInformation(int id) { - this(id, null); - } - - /** - * Creates a DeviceInformation with a canbus string. If {@code canbus} is {@code null}, method - * acts like {@link #DeviceInformation(int)} was called - * - * @param id the CAN id - * @param canbus the canbus string - */ - public DeviceInformation(int id, String canbus) { - this.id = InputValidation.checkCanId(id); - this.canbus = Optional.ofNullable(canbus); - } - - /** - * Gets the can id of this device - * - * @return the can id of the device - */ - public int id() { - return id; - } - - /** - * Returns the canbus that this device is on, or {@link Optional#empty()} if it is on the RoboRIO - * can loop - * - * @return the canbus that the device is on - */ - public Optional canbus() { - return canbus; - } - - @Override - public boolean equals(Object o) { - if (!(o instanceof DeviceInformation)) return false; - DeviceInformation other = (DeviceInformation) o; - return other.id == id && other.canbus.equals(canbus); - } - - @Override - public int hashCode() { - return id * 31 + canbus.hashCode(); - } -} + + /** The CAN ID of the device, validated to be in range [0, 62] */ + private int id; + + /** The CAN bus name, empty if on the RoboRIO's default CAN bus */ + private Optional canbus; + + /** + * Creates DeviceInformation for a device on the RoboRIO's default CAN bus. + * + *

This constructor is used for devices connected directly to the RoboRIO's + * built-in CAN bus interface. The canbus will be represented as an empty + * Optional to indicate the default bus. + * + * @param id the CAN ID of the device, must be in range [0, 62] + * @throws com.team2813.lib2813.util.InvalidCanIdException if the CAN ID is outside the valid range + */ + public DeviceInformation(int id) { + this(id, null); + } + + /** + * Creates DeviceInformation with a specific CAN bus name. + * + *

This constructor supports devices on named CAN buses such as CANivore + * devices or other CAN bus interfaces. If {@code canbus} is {@code null}, + * this method behaves identically to {@link #DeviceInformation(int)}. + * + *

Examples of valid canbus names: + *

+ * + * @param id the CAN ID of the device, must be in range [0, 62] + * @param canbus the CAN bus name, or {@code null} for the RoboRIO default bus + * @throws com.team2813.lib2813.util.InvalidCanIdException if the CAN ID is outside the valid range + */ + public DeviceInformation(int id, String canbus) { + this.id = InputValidation.checkCanId(id); + this.canbus = Optional.ofNullable(canbus); + } + + /** + * Gets the CAN ID of this device. + * + *

The CAN ID is guaranteed to be in the valid range [0, 62] due to + * validation performed during construction. + * + * @return the CAN ID of the device + */ + public int id() { + return id; + } + + /** + * Returns the CAN bus that this device is connected to. + * + *

The return value interpretation: + *

+ * + *

This Optional-based approach provides null safety and makes the + * distinction between default and named buses explicit in the API. + * + * @return an Optional containing the CAN bus name, or empty if on the RoboRIO CAN bus + */ + public Optional canbus() { + return canbus; + } + + /** + * Determines whether this DeviceInformation is equal to another object. + * + *

Two DeviceInformation instances are considered equal if and only if + * they have the same CAN ID and are on the same CAN bus. This allows + * DeviceInformation objects to be used reliably as keys in hash-based + * collections and for device identity comparisons. + * + * @param o the object to compare against + * @return {@code true} if the objects represent the same device, + * {@code false} otherwise + */ + @Override + public boolean equals(Object o) { + if (!(o instanceof DeviceInformation)) return false; + DeviceInformation other = (DeviceInformation) o; + return other.id == id && other.canbus.equals(canbus); + } + + /** + * Returns a hash code value for this DeviceInformation. + * + *

The hash code is computed from both the CAN ID and the CAN bus + * information, ensuring that equal DeviceInformation objects have + * equal hash codes. This implementation supports the use of + * DeviceInformation objects in hash-based collections such as + * HashMap and HashSet. + * + *

The hash code formula combines the ID and canbus hash using + * a prime multiplier for good distribution properties. + * + * @return a hash code value for this object + */ + @Override + public int hashCode() { + return id * 31 + canbus.hashCode(); + } +} \ No newline at end of file diff --git a/lib/src/main/java/com/team2813/lib2813/control/Encoder.java b/lib/src/main/java/com/team2813/lib2813/control/Encoder.java index d622212b..f3c90897 100644 --- a/lib/src/main/java/com/team2813/lib2813/control/Encoder.java +++ b/lib/src/main/java/com/team2813/lib2813/control/Encoder.java @@ -4,55 +4,142 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -/** Specifies a device that can perceive rotational positions. */ +/** + * Interface specifying a device that can perceive rotational positions and velocities. + * + *

This interface defines the contract for encoder devices that provide angular + * position and velocity feedback. It supports both legacy double-based methods + * (deprecated) and modern type-safe unit-based methods using WPILib's units system. + * + *

The interface provides a migration path from unsafe raw double values to + * type-safe {@link Angle} and {@link AngularVelocity} measurements. New implementations + * should focus on the unit-safe methods, while legacy methods are maintained for + * backward compatibility but marked for removal. + * + *

Key features: + *

+ * + *

Common implementations include absolute encoders (CANcoder), relative encoders + * (integrated motor encoders), and other rotational position sensing devices. + * + * @author Team 2813 + * @since 1.0 + */ public interface Encoder { - /** - * Gets the position of the encoder - * - * @return the position of the encoder - * @deprecated This method does not specify position in a specific measurement, so it is not safe - * to use. Use {@link #getPositionMeasure()} instead - */ - @Deprecated(forRemoval = true) - double position(); - - /** - * Gets the position of the encoder - * - * @return the position of the encoder as a measure - */ - Angle getPositionMeasure(); - - /** - * Sets the position of the encoder - * - * @param position the position of the encoder - * @deprecated This method does not specify a unit, so it is not safe to use. Use {@link - * #setPosition(Angle)} instead. - */ - @Deprecated(forRemoval = true) - void setPosition(double position); - - default void setPosition(Angle position) { - setPosition(position.in(Units.Radians)); - } - - /** - * Gets the velocity of the encoder - * - * @return the velocity that the encoder perceives - * @deprecated This method does not specify velocity in a specific measurement, so it is not safe - * to use. Use {@link #getVelocityMeasure()} instead - */ - @Deprecated(forRemoval = true) - double getVelocity(); - - /** - * Gets the velocity of the encoder - * - * @return The velocity as a measure - */ - default AngularVelocity getVelocityMeasure() { - return Units.RadiansPerSecond.of(getVelocity()); - } -} + + /** + * Gets the current position of the encoder as a raw double value. + * + *

Warning: This method returns position without specifying units, + * making it unsafe for reliable position calculations. The actual units + * depend on the specific encoder implementation and configuration. + * + * @return the position of the encoder as an unspecified double value + * @deprecated This method does not specify position in a specific measurement, so it is not safe + * to use. Use {@link #getPositionMeasure()} instead for type safety + */ + @Deprecated(forRemoval = true) + double position(); + + /** + * Gets the current position of the encoder using type-safe units. + * + *

This method returns the encoder position as an {@link Angle} measurement, + * providing type safety and explicit unit handling. The returned angle can be + * easily converted to any angular unit (degrees, radians, rotations) using + * the WPILib units system. + * + *

Example usage: + * 

{@code
+     * Angle position = encoder.getPositionMeasure();
+     * double degrees = position.in(Units.Degrees);
+     * double rotations = position.in(Units.Rotations);
+     * }
+ * + * @return the current position of the encoder as an {@link Angle} measurement + */ + Angle getPositionMeasure(); + + /** + * Sets the encoder position to the specified raw double value. + * + *

Warning: This method accepts position without specifying units, + * making it unsafe and ambiguous. The interpretation of the position value + * depends on the specific encoder implementation and configuration. + * + * @param position the new position value as an unspecified double + * @deprecated This method does not specify a unit, so it is not safe to use. + * Use {@link #setPosition(Angle)} instead for type safety + */ + @Deprecated(forRemoval = true) + void setPosition(double position); + + /** + * Sets the encoder position using type-safe units. + * + *

This method accepts any {@link Angle} measurement and converts it to + * the encoder's native units for setting the position. The type-safe approach + * eliminates unit confusion and provides clear, readable code. + * + *

The default implementation converts the angle to radians and calls the + * legacy {@link #setPosition(double)} method. Implementations should override + * this method to provide direct unit-safe position setting when possible. + * + *

Example usage: + * 

{@code
+     * encoder.setPosition(Units.Degrees.of(90));
+     * encoder.setPosition(Units.Rotations.of(0.25));
+     * }
+ * + * @param position the new position as an {@link Angle} measurement + */ + default void setPosition(Angle position) { + setPosition(position.in(Units.Radians)); + } + + /** + * Gets the current velocity of the encoder as a raw double value. + * + *

Warning: This method returns velocity without specifying units, + * making it unsafe for reliable velocity calculations. The actual units + * depend on the specific encoder implementation and configuration (could be + * RPM, radians/sec, rotations/sec, etc.). + * + * @return the velocity that the encoder perceives as an unspecified double value + * @deprecated This method does not specify velocity in a specific measurement, so it is not safe + * to use. Use {@link #getVelocityMeasure()} instead for type safety + */ + @Deprecated(forRemoval = true) + double getVelocity(); + + /** + * Gets the current velocity of the encoder using type-safe units. + * + *

This method returns the encoder velocity as an {@link AngularVelocity} + * measurement, providing type safety and explicit unit handling. The returned + * velocity can be easily converted to any angular velocity unit using the + * WPILib units system. + * + *

The default implementation assumes the legacy {@link #getVelocity()} method + * returns radians per second and wraps it in a type-safe measurement. + * Implementations should override this method to provide the correct units + * for their specific hardware. + * + *

Example usage: + * 

{@code
+     * AngularVelocity velocity = encoder.getVelocityMeasure();
+     * double rpm = velocity.in(Units.RPM);
+     * double radPerSec = velocity.in(Units.RadiansPerSecond);
+     * }
+ * + * @return the current velocity as an {@link AngularVelocity} measurement + */ + default AngularVelocity getVelocityMeasure() { + return Units.RadiansPerSecond.of(getVelocity()); + } +} \ No newline at end of file diff --git a/lib/src/main/java/com/team2813/lib2813/control/InvertType.java b/lib/src/main/java/com/team2813/lib2813/control/InvertType.java index 7606ceda..f4c3b6e8 100644 --- a/lib/src/main/java/com/team2813/lib2813/control/InvertType.java +++ b/lib/src/main/java/com/team2813/lib2813/control/InvertType.java @@ -6,83 +6,237 @@ import java.util.*; import java.util.stream.Stream; +/** + * Enumeration defining motor inversion types for standardized motor control across different vendors. + * + *

This enum provides a vendor-neutral abstraction for motor inversion settings while maintaining + * compatibility with specific hardware implementations from CTRE Phoenix and REV Robotics SPARK controllers. + * It handles both absolute rotation directions and relative following behaviors for multi-motor systems. + * + *

The enum supports two categories of inversion: + *

+ * + *

Each rotation value includes mappings to vendor-specific inversion settings, enabling + * seamless integration with different motor controller families without code changes. + * + * @author Team 2813 + * @since 1.0 + */ public enum InvertType { - CLOCKWISE(InvertedValue.Clockwise_Positive, true), - COUNTER_CLOCKWISE(InvertedValue.CounterClockwise_Positive, false), - FOLLOW_MASTER, - OPPOSE_MASTER; + + /** + * Clockwise rotation is considered positive. + * + *

When this inversion type is applied, positive motor output values will cause + * the motor to rotate in the clockwise direction when viewed from the output shaft end. + * This is the "normal" or "non-inverted" direction for most motor controllers. + * + *

Vendor mappings: + *

+ */ + CLOCKWISE(InvertedValue.Clockwise_Positive, true), + + /** + * Counter-clockwise rotation is considered positive. + * + *

When this inversion type is applied, positive motor output values will cause + * the motor to rotate in the counter-clockwise direction when viewed from the output + * shaft end. This effectively inverts the motor's output relative to the default. + * + *

Vendor mappings: + *

+ */ + COUNTER_CLOCKWISE(InvertedValue.CounterClockwise_Positive, false), + + /** + * Follower motor matches the master motor's direction. + * + *

This inversion type is used for follower motors that should rotate in the + * same direction as their master motor. The follower will mirror the master's + * output without any inversion applied. + * + *

Note: This value has no direct vendor mapping as it represents a + * relationship rather than an absolute direction. It is resolved at the follower + * configuration level. + */ + FOLLOW_MASTER, + + /** + * Follower motor opposes the master motor's direction. + * + *

This inversion type is used for follower motors that should rotate opposite + * to their master motor. The follower will mirror the master's output but with + * inversion applied, creating opposing rotation. + * + *

This is commonly used in differential drive systems where left and right + * motors need to rotate in opposite directions for forward motion. + * + *

Note: This value has no direct vendor mapping as it represents a + * relationship rather than an absolute direction. It is resolved at the follower + * configuration level. + */ + OPPOSE_MASTER; - /** - * A set of all {@link InvertType}s that have a phoenix and spark max invert. Anything that isn't - * in this set is for motor following - */ - public static final Set rotationValues = - Collections.unmodifiableSet(EnumSet.of(CLOCKWISE, COUNTER_CLOCKWISE)); + /** + * A set of all {@link InvertType}s that have phoenix and spark max invert mappings. + * + *

This set contains only the rotation values (CLOCKWISE, COUNTER_CLOCKWISE) that + * can be directly mapped to vendor-specific inversion settings. Any InvertType not + * in this set is intended for motor following relationships and does not have direct + * vendor mappings. + * + *

This set is used for validation to ensure only appropriate inversion types + * are used in contexts that require absolute rotation directions. + */ + public static final Set rotationValues = + Collections.unmodifiableSet(EnumSet.of(CLOCKWISE, COUNTER_CLOCKWISE)); - private final Optional phoenixInvert; - private final Optional sparkMaxInvert; + /** The corresponding Phoenix 6 InvertedValue, if applicable */ + private final Optional phoenixInvert; + + /** The corresponding SPARK MAX inversion boolean, if applicable */ + private final Optional sparkMaxInvert; - InvertType() { - phoenixInvert = Optional.empty(); - sparkMaxInvert = Optional.empty(); - } + /** + * Creates an InvertType for following relationships (no vendor mappings). + * + *

This constructor is used for FOLLOW_MASTER and OPPOSE_MASTER values + * that represent relationships rather than absolute directions. + */ + InvertType() { + phoenixInvert = Optional.empty(); + sparkMaxInvert = Optional.empty(); + } - InvertType(InvertedValue phoenixInvert, boolean sparkMaxInvert) { - this.phoenixInvert = Optional.of(phoenixInvert); - this.sparkMaxInvert = Optional.of(sparkMaxInvert); - } + /** + * Creates an InvertType with vendor-specific mappings. + * + *

This constructor is used for rotation values (CLOCKWISE, COUNTER_CLOCKWISE) + * that have direct mappings to vendor-specific inversion settings. + * + * @param phoenixInvert the corresponding Phoenix 6 InvertedValue + * @param sparkMaxInvert the corresponding SPARK MAX inversion setting + */ + InvertType(InvertedValue phoenixInvert, boolean sparkMaxInvert) { + this.phoenixInvert = Optional.of(phoenixInvert); + this.sparkMaxInvert = Optional.of(sparkMaxInvert); + } - /** - * Gets an {@link InvertType} from a phoenix {@link InvertedValue}. - * - * @param v the {@link InvertedValue} to search for - * @return {@link Optional#empty()} if no {@link InvertType} is found, otherwise, an optional - * describing the {@link InvertType} - */ - public static Optional fromPhoenixInvert(InvertedValue v) { - return Optional.of(Maps.phoenixMap.get(v)); - } + /** + * Gets an {@link InvertType} from a Phoenix 6 {@link InvertedValue}. + * + *

This method provides reverse lookup from vendor-specific Phoenix settings + * to the vendor-neutral InvertType representation. It enables conversion from + * Phoenix-specific code to the standardized enum. + * + * @param v the {@link InvertedValue} to search for + * @return an Optional containing the corresponding {@link InvertType}, or + * {@link Optional#empty()} if no mapping exists + */ + public static Optional fromPhoenixInvert(InvertedValue v) { + return Optional.ofNullable(Maps.phoenixMap.get(v)); + } - /** - * Gets an {@link InvertType} from a spark max invert - * - * @param v the {@link InvertedValue} to search for - * @return {@link Optional#empty()} if no {@link InvertType} is found, otherwise, an optional - * describing the {@link InvertType} - */ - public static Optional fromSparkMaxInvert(boolean v) { - return Optional.of(Maps.sparkMaxMap.get(v)); - } + /** + * Gets an {@link InvertType} from a SPARK MAX inversion boolean. + * + *

This method provides reverse lookup from vendor-specific SPARK MAX settings + * to the vendor-neutral InvertType representation. It enables conversion from + * SPARK MAX-specific code to the standardized enum. + * + * @param v the SPARK MAX inversion boolean to search for + * @return an Optional containing the corresponding {@link InvertType}, or + * {@link Optional#empty()} if no mapping exists + */ + public static Optional fromSparkMaxInvert(boolean v) { + return Optional.ofNullable(Maps.sparkMaxMap.get(v)); + } - public Optional phoenixInvert() { - return phoenixInvert; - } + /** + * Gets the Phoenix 6 InvertedValue for this InvertType. + * + *

This method returns the vendor-specific Phoenix mapping for rotation values. + * Following values (FOLLOW_MASTER, OPPOSE_MASTER) will return an empty Optional + * as they don't have direct Phoenix mappings. + * + * @return an Optional containing the Phoenix InvertedValue, or empty if not applicable + */ + public Optional phoenixInvert() { + return phoenixInvert; + } - private InvertedValue forcePhoenixInvert() { - return phoenixInvert.orElseThrow(); - } + /** + * Forces retrieval of the Phoenix 6 InvertedValue. + * + *

This internal method assumes the Phoenix mapping exists and throws if it doesn't. + * It's used internally for map construction and should not be called on following values. + * + * @return the Phoenix InvertedValue + * @throws NoSuchElementException if no Phoenix mapping exists + */ + private InvertedValue forcePhoenixInvert() { + return phoenixInvert.orElseThrow(); + } - public Optional sparkMaxInvert() { - return sparkMaxInvert; - } + /** + * Gets the SPARK MAX inversion boolean for this InvertType. + * + *

This method returns the vendor-specific SPARK MAX mapping for rotation values. + * Following values (FOLLOW_MASTER, OPPOSE_MASTER) will return an empty Optional + * as they don't have direct SPARK MAX mappings. + * + * @return an Optional containing the SPARK MAX inversion boolean, or empty if not applicable + */ + public Optional sparkMaxInvert() { + return sparkMaxInvert; + } - private boolean forceSparkMaxInvert() { - return sparkMaxInvert.orElseThrow(); - } + /** + * Forces retrieval of the SPARK MAX inversion boolean. + * + *

This internal method assumes the SPARK MAX mapping exists and throws if it doesn't. + * It's used internally for map construction and should not be called on following values. + * + * @return the SPARK MAX inversion boolean + * @throws NoSuchElementException if no SPARK MAX mapping exists + */ + private boolean forceSparkMaxInvert() { + return sparkMaxInvert.orElseThrow(); + } - /** - * Contains the maps for {@link InvertType#fromPhoenixInvert(InvertedValue)} and {@link - * InvertType#fromSparkMaxInvert(boolean)}. In a static class so that they will only be - * initialized if they are needed. - */ - private static final class Maps { - private static final Map phoenixMap = - Stream.of(InvertType.values()) - .filter((j) -> j.phoenixInvert.isPresent()) - .collect(toUnmodifiableMap(InvertType::forcePhoenixInvert, (j) -> j, (a, b) -> null)); - private static final Map sparkMaxMap = - Stream.of(InvertType.values()) - .filter((j) -> j.sparkMaxInvert.isPresent()) - .collect(toUnmodifiableMap(InvertType::forceSparkMaxInvert, (j) -> j, (a, b) -> null)); - } -} + /** + * Contains the reverse lookup maps for vendor-specific inversion values. + * + *

This static nested class holds the maps used by {@link #fromPhoenixInvert(InvertedValue)} + * and {@link #fromSparkMaxInvert(boolean)}. The maps are constructed lazily in a static + * context to avoid initialization overhead when reverse lookup is not needed. + * + *

The maps are built by filtering enum values to include only those with vendor + * mappings and creating reverse mappings from vendor values to InvertType. + */ + private static final class Maps { + + /** Reverse lookup map from Phoenix InvertedValue to InvertType */ + private static final Map phoenixMap = + Stream.of(InvertType.values()) + .filter((j) -> j.phoenixInvert.isPresent()) + .collect(toUnmodifiableMap(InvertType::forcePhoenixInvert, (j) -> j, (a, b) -> null)); + + /** Reverse lookup map from SPARK MAX boolean to InvertType */ + private static final Map sparkMaxMap = + Stream.of(InvertType.values()) + .filter((j) -> j.sparkMaxInvert.isPresent()) + .collect(toUnmodifiableMap(InvertType::forceSparkMaxInvert, (j) -> j, (a, b) -> null)); + } +} \ No newline at end of file diff --git a/lib/src/main/java/com/team2813/lib2813/control/Motor.java b/lib/src/main/java/com/team2813/lib2813/control/Motor.java index 6c92ed0b..9643869f 100644 --- a/lib/src/main/java/com/team2813/lib2813/control/Motor.java +++ b/lib/src/main/java/com/team2813/lib2813/control/Motor.java @@ -2,31 +2,113 @@ import edu.wpi.first.units.measure.Current; +/** + * Interface defining the fundamental contract for motor control devices. + * + *

This interface provides a vendor-neutral abstraction for motor controllers, + * enabling standardized motor control across different hardware implementations. + * It defines the essential operations needed for motor control: setting output + * values with various control modes and monitoring current draw. + * + *

The interface supports multiple control modes through the {@link ControlMode} + * enumeration, allowing the same code to work with different control strategies + * such as duty cycle, voltage, velocity, and motion magic control. This abstraction + * enables easy switching between control modes without changing the calling code. + * + *

Key features: + *

+ * + *

Common implementations include TalonFX controllers, SPARK MAX controllers, + * and other motor control devices that support the specified control modes. + * + * @author Team 2813 + * @since 1.0 + */ public interface Motor { - // motor control - /** - * Sets the motor to run with a specified mode of control. - * - * @param mode The mode to control the motor with - * @param demand The demand of the motor. differentiating meaning with each control mode - */ - void set(ControlMode mode, double demand); + /** + * Sets the motor to run with a specified control mode and demand value. + * + *

This method provides basic motor control without feedforward compensation. + * The interpretation of the demand value depends on the selected control mode: + * + *

+ * + *

This is equivalent to calling {@link #set(ControlMode, double, double)} + * with a feedforward value of 0.0. + * + * @param mode the control mode to use for motor operation + * @param demand the demand value whose interpretation depends on the control mode + */ + void set(ControlMode mode, double demand); - /** - * Sets the motor to run with a specified mode of control, and feedForward. - * - * @param mode The mode to control the motor with - * @param demand The demand of the motor. differentiating meaning with each control mode - * @param feedForward The feedForward to apply to the motor - */ - void set(ControlMode mode, double demand, double feedForward); + /** + * Sets the motor to run with a specified control mode, demand value, and feedforward. + * + *

This method provides advanced motor control with feedforward compensation + * to improve control performance. Feedforward helps the controller anticipate + * the required output, reducing tracking error and improving response time. + * + *

The demand value interpretation is the same as in {@link #set(ControlMode, double)}. + * The feedforward value provides additional output that is added to the + * closed-loop controller output: + * + *

+ * + *

Feedforward values are typically calculated based on system identification + * or theoretical models of the mechanism being controlled. + * + * @param mode the control mode to use for motor operation + * @param demand the demand value whose interpretation depends on the control mode + * @param feedForward the feedforward compensation value to improve control performance + */ + void set(ControlMode mode, double demand, double feedForward); - /** - * Gets the current that is being applied onto the motor - * - * @since 2.0.0 - * @return The current applied current - */ - Current getAppliedCurrent(); -} + /** + * Gets the current that is currently being applied to the motor. + * + *

This method returns the actual current flowing through the motor windings, + * which is useful for: + *

+ * + *

The returned current measurement uses WPILib's type-safe units system, + * allowing easy conversion between different current units (amps, milliamps) + * and providing type safety in calculations. + * + *

Example usage: + * 

{@code
+     * Current motorCurrent = motor.getAppliedCurrent();
+     * double amps = motorCurrent.in(Units.Amps);
+     * if (amps > 30.0) {
+     *     // Handle high current condition
+     * }
+     * }
+ * + * @return the current applied current as a type-safe {@link Current} measurement + * @since 2.0.0 + */ + Current getAppliedCurrent(); +} \ No newline at end of file diff --git a/lib/src/main/java/com/team2813/lib2813/control/PIDMotor.java b/lib/src/main/java/com/team2813/lib2813/control/PIDMotor.java index 745c742a..94d0b225 100644 --- a/lib/src/main/java/com/team2813/lib2813/control/PIDMotor.java +++ b/lib/src/main/java/com/team2813/lib2813/control/PIDMotor.java @@ -1,11 +1,133 @@ package com.team2813.lib2813.control; +/** + * Interface defining a motor controller with integrated encoder feedback and PID control capabilities. + * + *

This interface combines the functionality of both {@link Motor} and {@link Encoder} interfaces, + * representing motor controllers that have built-in position/velocity feedback and can perform + * closed-loop PID control. This is the most common type of motor controller used in competitive + * robotics, where precise control and position feedback are essential. + * + *

The interface extends the basic motor control capabilities with PID configuration methods + * that allow tuning of closed-loop control parameters. Multiple PID slots are supported to + * enable different control parameters for different operating conditions (e.g., different + * gains for velocity vs position control, or different gains for different mechanisms). + * + *

Key features inherited and added: + *

+ * + *

Common implementations include: + *

+ * + *

This interface enables sophisticated control strategies such as: + *

+ * + * @author Team 2813 + * @since 1.0 + */ public interface PIDMotor extends Motor, Encoder { - void configPIDF(int slot, double p, double i, double d, double f); - void configPIDF(double p, double i, double d, double f); + /** + * Configures the PID and feedforward constants for a specific control slot. + * + *

Motor controllers typically support multiple PID parameter sets (slots) that + * can be switched between during operation. This allows different control parameters + * for different operating conditions without reconfiguration overhead. + * + *

Common slot usage patterns: + *

+ * + *

PID parameter meanings: + *

+ * + *

Note: The interpretation of the 'f' parameter may vary by implementation. + * Some controllers use it for velocity feedforward (kV), while others use it for + * arbitrary feedforward (kF). Consult the specific implementation documentation. + * + * @param slot the PID slot number to configure (typically 0-3, range depends on implementation) + * @param p the proportional gain coefficient + * @param i the integral gain coefficient + * @param d the derivative gain coefficient + * @param f the feedforward gain coefficient + */ + void configPIDF(int slot, double p, double i, double d, double f); - void configPID(int slot, double p, double i, double d); + /** + * Configures the PID and feedforward constants for the default control slot (slot 0). + * + *

This is a convenience method equivalent to calling {@link #configPIDF(int, double, double, double, double)} + * with slot 0. Most applications start with slot 0 for their primary control parameters. + * + *

This method is ideal for simple applications that only need one set of PID parameters + * or for initial tuning before implementing more advanced multi-slot strategies. + * + * @param p the proportional gain coefficient + * @param i the integral gain coefficient + * @param d the derivative gain coefficient + * @param f the feedforward gain coefficient + */ + void configPIDF(double p, double i, double d, double f); - void configPID(double p, double i, double d); -} + /** + * Configures the PID constants for a specific control slot without feedforward. + * + *

This method is equivalent to calling {@link #configPIDF(int, double, double, double, double)} + * with a feedforward value of 0. It's useful when feedforward is not needed or will be + * handled separately through the {@link #set(ControlMode, double, double)} method. + * + *

Pure PID control without feedforward is common for: + *

+ * + * @param slot the PID slot number to configure + * @param p the proportional gain coefficient + * @param i the integral gain coefficient + * @param d the derivative gain coefficient + */ + void configPID(int slot, double p, double i, double d); + + /** + * Configures the PID constants for the default control slot (slot 0) without feedforward. + * + *

This is a convenience method equivalent to calling {@link #configPID(int, double, double, double)} + * with slot 0. It provides the simplest interface for basic PID control configuration. + * + *

This method is perfect for: + *

+ * + * @param p the proportional gain coefficient + * @param i the integral gain coefficient + * @param d the derivative gain coefficient + */ + void configPID(double p, double i, double d); +} \ No newline at end of file diff --git a/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java b/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java index ebd35cb3..64e0c03c 100644 --- a/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java +++ b/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java @@ -8,61 +8,154 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +/** + * Wrapper class for a CTRE Phoenix 6 CANcoder absolute encoder. + * + *

This class provides a standardized interface for interacting with CANcoder devices + * while maintaining compatibility with the team's control system architecture. It implements + * the {@link Encoder} interface and supports both legacy double-based methods and modern + * type-safe unit-based methods. + * + *

The class includes deprecated methods for backward compatibility, but new code should + * use the type-safe methods that work with WPILib's units system. + * + * @author Team 2813 + * @since 1.0 + */ public class CancoderWrapper implements Encoder { - private CANcoder cancoder; - private DeviceInformation info; + + /** The underlying CTRE CANcoder hardware object */ + private CANcoder cancoder; + + /** Device information containing ID and CAN bus details */ + private DeviceInformation info; - public CancoderWrapper(int id, String canbus) { - cancoder = new CANcoder(id, canbus); - info = new DeviceInformation(id, canbus); - } + /** + * Creates a new CancoderWrapper with the specified CAN ID and bus name. + * + * @param id the CAN ID of the CANcoder device + * @param canbus the name of the CAN bus the device is connected to (e.g., "rio", "canivore") + */ + public CancoderWrapper(int id, String canbus) { + cancoder = new CANcoder(id, canbus); + info = new DeviceInformation(id, canbus); + } - public CancoderWrapper(int id) { - cancoder = new CANcoder(id); - info = new DeviceInformation(id); - } + /** + * Creates a new CancoderWrapper with the specified CAN ID on the default CAN bus. + * + * @param id the CAN ID of the CANcoder device + */ + public CancoderWrapper(int id) { + cancoder = new CANcoder(id); + info = new DeviceInformation(id); + } - @Deprecated - @Override - public double position() { - return cancoder.getPosition().getValueAsDouble(); - } + /** + * Gets the current position of the encoder as a raw double value. + * + * @deprecated Use {@link #getPositionMeasure()} instead for type safety with units + * @return the current position in rotations as a double + */ + @Deprecated + @Override + public double position() { + return cancoder.getPosition().getValueAsDouble(); + } - @Override - public Angle getPositionMeasure() { - return Units.Rotations.of(cancoder.getPosition().getValueAsDouble()); - } + /** + * Gets the current position of the encoder using type-safe units. + * + *

This method returns the position as an {@link Angle} measurement in rotations, + * which can be converted to other angular units as needed. + * + * @return the current position as an {@link Angle} measurement + */ + @Override + public Angle getPositionMeasure() { + return Units.Rotations.of(cancoder.getPosition().getValueAsDouble()); + } - @Deprecated - @Override - public void setPosition(double position) { - ConfigUtils.phoenix6Config(() -> cancoder.setPosition(position)); - } + /** + * Sets the position of the encoder using a raw double value. + * + *

This method uses {@link ConfigUtils#phoenix6Config(Runnable)} to ensure + * reliable configuration of the Phoenix 6 device. + * + * @deprecated Use {@link #setPosition(Angle)} instead for type safety with units + * @param position the new position in rotations + */ + @Deprecated + @Override + public void setPosition(double position) { + ConfigUtils.phoenix6Config(() -> cancoder.setPosition(position)); + } - @Override - public void setPosition(Angle position) { - ConfigUtils.phoenix6Config(() -> cancoder.setPosition(position.in(Units.Rotations))); - } + /** + * Sets the position of the encoder using type-safe units. + * + *

This method accepts any {@link Angle} measurement and converts it to rotations + * internally. It uses {@link ConfigUtils#phoenix6Config(Runnable)} to ensure + * reliable configuration of the Phoenix 6 device. + * + * @param position the new position as an {@link Angle} measurement + */ + @Override + public void setPosition(Angle position) { + ConfigUtils.phoenix6Config(() -> cancoder.setPosition(position.in(Units.Rotations))); + } - public CANcoder encoder() { - return cancoder; - } + /** + * Provides direct access to the underlying CANcoder hardware object. + * + *

This method allows access to Phoenix-specific configuration and methods + * that are not exposed through the {@link Encoder} interface. + * + * @return the underlying {@link CANcoder} hardware object + */ + public CANcoder encoder() { + return cancoder; + } - @Deprecated - @Override - public double getVelocity() { - return cancoder.getVelocity().getValueAsDouble(); - } + /** + * Gets the current velocity of the encoder as a raw double value. + * + * @deprecated Use {@link #getVelocityMeasure()} instead for type safety with units + * @return the current velocity in rotations per second as a double + */ + @Deprecated + @Override + public double getVelocity() { + return cancoder.getVelocity().getValueAsDouble(); + } - @Override - public AngularVelocity getVelocityMeasure() { - return cancoder.getVelocity().getValue(); - } + /** + * Gets the current velocity of the encoder using type-safe units. + * + *

This method returns the velocity as an {@link AngularVelocity} measurement, + * which can be converted to other angular velocity units as needed. + * + * @return the current velocity as an {@link AngularVelocity} measurement + */ + @Override + public AngularVelocity getVelocityMeasure() { + return cancoder.getVelocity().getValue(); + } - @Override - public boolean equals(Object obj) { - if (!(obj instanceof CancoderWrapper)) return false; - CancoderWrapper other = (CancoderWrapper) obj; - return info.equals(other.info); - } -} + /** + * Determines whether this CancoderWrapper is equal to another object. + * + *

Two CancoderWrapper instances are considered equal if they represent + * the same physical CANcoder device (same CAN ID and CAN bus). + * + * @param obj the object to compare against + * @return {@code true} if the objects represent the same CANcoder device, + * {@code false} otherwise + */ + @Override + public boolean equals(Object obj) { + if (!(obj instanceof CancoderWrapper)) return false; + CancoderWrapper other = (CancoderWrapper) obj; + return info.equals(other.info); + } +} \ No newline at end of file diff --git a/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java b/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java index c6e16fe6..aec32141 100644 --- a/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java +++ b/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java @@ -4,69 +4,143 @@ import com.team2813.lib2813.control.DeviceInformation; import com.team2813.lib2813.util.ConfigUtils; +/** + * Wrapper class for a CTRE Phoenix 6 Pigeon2 IMU (Inertial Measurement Unit). + * + *

This class provides a standardized interface for interacting with Pigeon2 devices + * while maintaining compatibility with the team's control system architecture. It includes + * automatic reset detection and recovery functionality to handle power cycles and device + * resets that can occur during competition. + * + *

The wrapper maintains a cached heading value that is restored after device resets, + * ensuring consistent heading readings across power cycles. The {@link #periodicResetCheck()} + * method should be called periodically (typically in a subsystem's periodic method) to + * monitor for and handle resets. + * + * @author Team 2813 + * @since 1.0 + */ public class Pigeon2Wrapper { + + /** The underlying CTRE Pigeon2 hardware object */ + private Pigeon2 pigeon; + + /** Cached heading value used for reset recovery */ + private double currentHeading = 0; + + /** Device information containing ID and CAN bus details */ + private DeviceInformation info; - private Pigeon2 pigeon; - - private double currentHeading = 0; - private DeviceInformation info; - - /** - * Constructor - * - * @param deviceNumber [0,62] - * @param canbus Name of the CANbus; can be a SocketCAN interface (on Linux), or a CANivore device - * name or serial number - */ - public Pigeon2Wrapper(int deviceNumber, String canbus) { - info = new DeviceInformation(deviceNumber, canbus); - pigeon = new Pigeon2(deviceNumber, canbus); - } + /** + * Creates a new Pigeon2Wrapper with the specified device number and CAN bus. + * + * @param deviceNumber the CAN ID of the Pigeon2 device, must be in range [0,62] + * @param canbus the name of the CAN bus the device is connected to. This can be a + * SocketCAN interface (on Linux), a CANivore device name, or serial number + */ + public Pigeon2Wrapper(int deviceNumber, String canbus) { + info = new DeviceInformation(deviceNumber, canbus); + pigeon = new Pigeon2(deviceNumber, canbus); + } - /** - * Constructor - * - * @param deviceNumber [0,62] - */ - public Pigeon2Wrapper(int deviceNumber) { - info = new DeviceInformation(deviceNumber); - pigeon = new Pigeon2(deviceNumber); - } + /** + * Creates a new Pigeon2Wrapper with the specified device number on the default CAN bus. + * + * @param deviceNumber the CAN ID of the Pigeon2 device, must be in range [0,62] + */ + public Pigeon2Wrapper(int deviceNumber) { + info = new DeviceInformation(deviceNumber); + pigeon = new Pigeon2(deviceNumber); + } - public Pigeon2 getPigeon() { - return pigeon; - } + /** + * Provides direct access to the underlying Pigeon2 hardware object. + * + *

This method allows access to Phoenix-specific configuration and methods + * that are not exposed through this wrapper class. + * + * @return the underlying {@link Pigeon2} hardware object + */ + public Pigeon2 getPigeon() { + return pigeon; + } - public double getHeading() { - return pigeon.getYaw().getValueAsDouble(); - } + /** + * Gets the current heading (yaw angle) of the IMU. + * + *

This method returns the raw yaw value from the Pigeon2. The heading + * is typically measured in degrees, with the range and direction depending + * on the device configuration. + * + * @return the current heading in degrees + */ + public double getHeading() { + return pigeon.getYaw().getValueAsDouble(); + } - public void setHeading(double angle) { - ConfigUtils.phoenix6Config(() -> pigeon.setYaw(angle)); - currentHeading = angle; - } + /** + * Sets the current heading (yaw angle) of the IMU. + * + *

This method updates both the hardware device and the cached heading value + * used for reset recovery. It uses {@link ConfigUtils#phoenix6Config(Runnable)} + * to ensure reliable configuration of the Phoenix 6 device. + * + * @param angle the new heading angle in degrees + */ + public void setHeading(double angle) { + ConfigUtils.phoenix6Config(() -> pigeon.setYaw(angle)); + currentHeading = angle; + } - /** - * Checks if a reset has occurred and restores non-persistent settings if so. Implement - * periodically (e.g. in a subsystem's periodic() method) - */ - public void periodicResetCheck() { - if (!pigeon.hasResetOccurred()) { - currentHeading = getHeading(); - } else { - setHeading(currentHeading); + /** + * Checks if a device reset has occurred and restores non-persistent settings if needed. + * + *

This method should be called periodically (e.g., in a subsystem's periodic() method) + * to monitor for device resets that can occur due to power cycles, brownouts, or other + * electrical issues. When a reset is detected, the method automatically restores the + * cached heading value to maintain consistency. + * + *

If no reset has occurred, the method updates the cached heading with the current + * hardware reading to ensure the cache stays synchronized. + * + *

Implementation Note: This method should be called at least once per robot + * loop iteration for proper reset detection and recovery. + */ + public void periodicResetCheck() { + if (!pigeon.hasResetOccurred()) { + currentHeading = getHeading(); + } else { + setHeading(currentHeading); + } } - } - public int hashCode() { - return info.hashCode(); - } + /** + * Returns a hash code value for this Pigeon2Wrapper. + * + *

The hash code is based on the device information (CAN ID and bus), + * ensuring that wrappers for the same physical device have the same hash code. + * + * @return a hash code value for this object + */ + public int hashCode() { + return info.hashCode(); + } - public boolean equals(Object other) { - if (!(other instanceof Pigeon2Wrapper)) { - return false; + /** + * Determines whether this Pigeon2Wrapper is equal to another object. + * + *

Two Pigeon2Wrapper instances are considered equal if they represent + * the same physical Pigeon2 device (same CAN ID and CAN bus). + * + * @param other the object to compare against + * @return {@code true} if the objects represent the same Pigeon2 device, + * {@code false} otherwise + */ + public boolean equals(Object other) { + if (!(other instanceof Pigeon2Wrapper)) { + return false; + } + Pigeon2Wrapper o = (Pigeon2Wrapper) other; + return o.info.equals(info); } - Pigeon2Wrapper o = (Pigeon2Wrapper) other; - return o.info.equals(info); - } -} +} \ No newline at end of file diff --git a/lib/src/main/java/com/team2813/lib2813/control/motors/SparkMaxWrapper.java b/lib/src/main/java/com/team2813/lib2813/control/motors/SparkMaxWrapper.java index 4ddc5b00..ac6cbe89 100644 --- a/lib/src/main/java/com/team2813/lib2813/control/motors/SparkMaxWrapper.java +++ b/lib/src/main/java/com/team2813/lib2813/control/motors/SparkMaxWrapper.java @@ -20,23 +20,68 @@ import java.util.ArrayList; import java.util.List; +/** + * Wrapper class for REV Robotics SPARK MAX motor controller. + * + *

This class provides a standardized interface for controlling SPARK MAX motor controllers + * while maintaining compatibility with the team's control system architecture. It implements + * the {@link PIDMotor} interface and supports both brushed and brushless motors with integrated + * encoder feedback. + * + *

Key features include: + *

+ * + *

The wrapper uses safe parameter reset mode and non-persistent parameters by default + * for reliable operation during competition. + * + * @author Team 2813 + * @since 1.0 + */ public class SparkMaxWrapper implements PIDMotor { + + /** List of follower SPARK MAX controllers managed by this wrapper */ private final List followers = new ArrayList<>(); + + /** The primary SPARK MAX motor controller */ private final SparkBase motor; + + /** The integrated relative encoder from the SPARK MAX */ private final RelativeEncoder encoder; + + /** Whether the motor output is inverted */ private final boolean inverted; + + /** Configuration object for the SPARK MAX */ private final SparkBaseConfig config; + + /** Reset mode used for configuration operations */ private final SparkBase.ResetMode resetMode; + + /** Persistence mode used for configuration operations */ private final SparkBase.PersistMode persistMode; /** - * Create a new object to control a SPARK MAX motor Controller - * - * @param deviceId The device ID. - * @param type The motor type connected to the controller. Brushless motor wires must be connected - * to their matching colors and the hall sensor must be plugged in. Brushed motors must be - * connected to the Red and Black terminals only. - * @param inverted Whether the motor is inverted + * Creates a new SparkMaxWrapper to control a SPARK MAX motor controller. + * + *

The constructor initializes the motor controller with safe default settings: + *

+ * + * @param deviceId the CAN ID of the SPARK MAX controller + * @param type the motor type connected to the controller. Brushless motor wires must be + * connected to their matching colors and the hall sensor must be plugged in. + * Brushed motors must be connected to the Red and Black terminals only + * @param inverted the inversion type for the motor output + * @throws RuntimeException if the inversion type cannot be converted to a SPARK MAX setting */ public SparkMaxWrapper(int deviceId, SparkLowLevel.MotorType type, InvertType inverted) { motor = new SparkMax(deviceId, type); @@ -48,11 +93,36 @@ public SparkMaxWrapper(int deviceId, SparkLowLevel.MotorType type, InvertType in encoder = motor.getEncoder(); } + /** + * Sets the motor output using the specified control mode and demand value. + * + *

This is a convenience method that calls {@link #set(ControlMode, double, double)} + * with a feedforward value of 0. + * + * @param controlMode the control mode to use + * @param demand the demand value (interpretation depends on control mode) + */ @Override public void set(ControlMode controlMode, double demand) { set(controlMode, demand, 0); } + /** + * Sets the motor output using the specified control mode, demand value, and feedforward. + * + *

Supported control modes: + *

+ * + *

Note: The feedforward parameter is currently ignored but included for + * interface compatibility. + * + * @param controlMode the control mode to use + * @param demand the demand value (interpretation depends on control mode) + * @param feedForward the feedforward value (currently unused) + */ @Override public void set(ControlMode controlMode, double demand, double feedForward) { switch (controlMode) { @@ -66,41 +136,106 @@ public void set(ControlMode controlMode, double demand, double feedForward) { } } + /** + * Gets the current position of the motor encoder as a raw double value. + * + * @deprecated Use {@link #getPositionMeasure()} instead for type safety with units + * @return the current position in rotations as a double + */ @Override public double position() { return encoder.getPosition(); } + /** + * Gets the current position of the motor encoder using type-safe units. + * + *

This method returns the position as an {@link Angle} measurement in rotations, + * which can be converted to other angular units as needed. + * + * @return the current position as an {@link Angle} measurement + */ @Override public Angle getPositionMeasure() { return Units.Rotations.of(encoder.getPosition()); } + /** + * Gets the current applied current of the motor controller. + * + *

This represents the actual current being drawn by the motor, which can be + * useful for monitoring motor load and detecting mechanical issues. + * + * @return the current applied current as a {@link Current} measurement + */ @Override public Current getAppliedCurrent() { return Units.Amps.of(motor.getOutputCurrent()); } + /** + * Sets the encoder position to the specified raw double value. + * + * @deprecated Use {@link #setPosition(Angle)} instead for type safety with units + * @param position the new position in rotations + */ @Override public void setPosition(double position) { encoder.setPosition(position); } + /** + * Sets the encoder position using type-safe units. + * + *

This method accepts any {@link Angle} measurement and converts it to rotations + * internally for the SPARK MAX encoder. + * + * @param position the new position as an {@link Angle} measurement + */ @Override public void setPosition(Angle position) { encoder.setPosition(position.in(Units.Rotations)); } + /** + * Gets the current velocity of the motor encoder as a raw double value. + * + * @deprecated Use {@link #getVelocityMeasure()} instead for type safety with units + * @return the current velocity in RPM as a double + */ @Override public double getVelocity() { return encoder.getVelocity(); } + /** + * Gets the current velocity of the motor encoder using type-safe units. + * + *

This method returns the velocity as an {@link AngularVelocity} measurement + * in rotations per minute (RPM), which can be converted to other angular velocity + * units as needed. + * + * @return the current velocity as an {@link AngularVelocity} measurement + */ @Override public AngularVelocity getVelocityMeasure() { return Units.Rotations.per(Units.Minute).of(encoder.getVelocity()); } + /** + * Configures the PIDF constants for a specific closed-loop slot. + * + *

The SPARK MAX supports multiple PID slot configurations that can be switched + * between during operation. This allows for different control parameters for + * different operating conditions. + * + * @param slot the closed-loop slot to configure (0-3) + * @param p the proportional gain + * @param i the integral gain + * @param d the derivative gain + * @param f the feedforward gain + * @throws RuntimeException if the slot number is invalid (not 0-3) + */ @Override public void configPIDF(int slot, double p, double i, double d, double f) { ClosedLoopSlot[] slots = ClosedLoopSlot.values(); @@ -112,21 +247,76 @@ public void configPIDF(int slot, double p, double i, double d, double f) { ConfigUtils.revConfig(() -> motor.configure(config, resetMode, persistMode)); } + /** + * Configures the PIDF constants for the default closed-loop slot (slot 0). + * + *

This is a convenience method that calls {@link #configPIDF(int, double, double, double, double)} + * with slot 0. + * + * @param p the proportional gain + * @param i the integral gain + * @param d the derivative gain + * @param f the feedforward gain + */ @Override public void configPIDF(double p, double i, double d, double f) { configPIDF(0, p, i, d, f); } + /** + * Configures the PID constants for a specific closed-loop slot. + * + *

This is a convenience method that calls {@link #configPIDF(int, double, double, double, double)} + * with a feedforward value of 0. + * + * @param slot the closed-loop slot to configure (0-3) + * @param p the proportional gain + * @param i the integral gain + * @param d the derivative gain + * @throws RuntimeException if the slot number is invalid (not 0-3) + */ @Override public void configPID(int slot, double p, double i, double d) { configPIDF(slot, p, i, d, 0); } + /** + * Configures the PID constants for the default closed-loop slot (slot 0). + * + *

This is a convenience method that calls {@link #configPID(int, double, double, double)} + * with slot 0. + * + * @param p the proportional gain + * @param i the integral gain + * @param d the derivative gain + */ @Override public void configPID(double p, double i, double d) { configPIDF(0, p, i, d, 0); } + /** + * Adds a follower SPARK MAX motor controller to this primary controller. + * + *

Follower motors will automatically mirror the output of the primary motor + * but can have independent inversion settings. The follower relationship is + * configured at the hardware level for optimal performance. + * + *

Inversion behavior: + *

+ * + *

The follower SPARK MAX object is retained in the followers list to prevent + * garbage collection and maintain the hardware relationship. + * + * @param deviceId the CAN ID of the follower SPARK MAX + * @param type the motor type connected to the follower controller + * @param inverted the inversion type for the follower motor + * @throws RuntimeException if the inversion type cannot be converted to a SPARK MAX setting + */ public void addFollower(int deviceId, SparkLowLevel.MotorType type, InvertType inverted) { SparkMax follower = new SparkMax(deviceId, type); boolean isInverted = @@ -141,4 +331,4 @@ public void addFollower(int deviceId, SparkLowLevel.MotorType type, InvertType i new SparkMaxConfig().follow(motor).inverted(isInverted), resetMode, persistMode)); followers.add(follower); // add to follower list so CANSparkMax follower object is preserved } -} +} \ No newline at end of file diff --git a/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java b/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java index 49f05a12..38743f77 100644 --- a/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java +++ b/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java @@ -22,26 +22,59 @@ import java.util.List; import java.util.Objects; +/** + * Wrapper class for CTRE Phoenix 6 TalonFX motor controller (Falcon 500 motor). + * + *

This class provides a standardized interface for controlling TalonFX motor controllers + * while maintaining compatibility with the team's control system architecture. It implements + * the {@link PIDMotor} interface and supports advanced control modes including velocity + * control, motion magic, and position control with the integrated encoder. + * + *

Key features include: + *

+ * + *

The wrapper automatically configures current limits (40A stator current limit with + * supply current limiting enabled) and validates all input parameters to prevent + * configuration errors. + * + * @author Team 2813 + * @since 1.0 + */ public class TalonFXWrapper implements PIDMotor { - /** A list of followers, so that they aren't garbage collected */ + + /** A list of follower TalonFX controllers to prevent garbage collection */ private final List followers = new ArrayList<>(); - /** the internal motor */ + /** The primary TalonFX motor controller */ private final TalonFX motor; + /** Device information containing CAN ID and bus details */ private final DeviceInformation information; /** - * Create a TalonFXWrapper on the specified canbus + * Creates a TalonFXWrapper on the specified CAN bus. * - * @param canID [0, 62] the can ID of the motor - * @param canbus the canbus that the motor is on - * @param invertType the invert type + *

The constructor automatically configures: + *

+ * + * @param canID the CAN ID of the motor controller, must be in range [0, 62] + * @param canbus the CAN bus that the motor is connected to (e.g., "rio", "canivore") + * @param invertType the inversion type for motor output, must be a rotation value * @throws NullPointerException if either {@code invertType} or {@code canbus} are null - * @throws IllegalArgumentException if {@code invertType} is not in {@link - * InvertType#rotationValues}. In other words, this exception is thrown when passed an {@link - * InvertType} that is for following motors - * @throws InvalidCanIdException if the CAN id is invalid + * @throws IllegalArgumentException if {@code invertType} is not in {@link InvertType#rotationValues}. + * This exception is thrown when passed an {@link InvertType} that is for following motors + * @throws InvalidCanIdException if the CAN ID is invalid (outside range [0, 62]) */ public TalonFXWrapper(int canID, String canbus, InvertType invertType) { Objects.requireNonNull(invertType, "invertType should not be null"); @@ -65,15 +98,17 @@ public TalonFXWrapper(int canID, String canbus, InvertType invertType) { } /** - * Create a TalonFXWrapper on the RoboRIO's canbus + * Creates a TalonFXWrapper on the RoboRIO's default CAN bus. + * + *

This is a convenience constructor that creates a TalonFX on the default "rio" CAN bus. + * The constructor automatically configures the same settings as the primary constructor. * - * @param canID [0, 62] the can ID of the motor - * @param invertType the invert type + * @param canID the CAN ID of the motor controller, must be in range [0, 62] + * @param invertType the inversion type for motor output, must be a rotation value * @throws NullPointerException if {@code invertType} is null - * @throws IllegalArgumentException if {@code invertType} is not in {@link - * InvertType#rotationValues}. In other words, this exception is thrown when passed an {@link - * InvertType} that is for following motors - * @throws InvalidCanIdException if the CAN id is invalid + * @throws IllegalArgumentException if {@code invertType} is not in {@link InvertType#rotationValues}. + * This exception is thrown when passed an {@link InvertType} that is for following motors + * @throws InvalidCanIdException if the CAN ID is invalid (outside range [0, 62]) */ public TalonFXWrapper(int canID, InvertType invertType) { Objects.requireNonNull(invertType, "invertType should not be null"); @@ -95,10 +130,37 @@ public TalonFXWrapper(int canID, InvertType invertType) { information = new DeviceInformation(canID); } + /** + * Sets the motor output using the specified control mode and demand value. + * + *

This is a convenience method that calls {@link #set(ControlMode, double, double)} + * with a feedforward value of 0. + * + * @param controlMode the control mode to use + * @param demand the demand value (interpretation depends on control mode) + */ public void set(ControlMode controlMode, double demand) { set(controlMode, demand, 0); } + /** + * Sets the motor output using the specified control mode, demand value, and feedforward. + * + *

Supported control modes: + *

+ * + *

Feedforward is supported for velocity and motion magic control modes to improve + * tracking performance and reduce steady-state error. + * + * @param controlMode the control mode to use + * @param demand the demand value (interpretation depends on control mode) + * @param feedForward the feedforward value (used for velocity and motion magic modes) + */ @Override public void set(ControlMode controlMode, double demand, double feedForward) { switch (controlMode) { @@ -123,49 +185,138 @@ public void set(ControlMode controlMode, double demand, double feedForward) { } } + /** + * Gets the current position of the integrated encoder as a raw double value. + * + * @deprecated Use {@link #getPositionMeasure()} instead for type safety with units + * @return the current position in rotations as a double + */ @Override public double position() { return getPositionMeasure().in(Units.Rotations); } + /** + * Gets the current position of the integrated encoder using type-safe units. + * + *

This method returns the position as an {@link Angle} measurement in rotations, + * which can be converted to other angular units as needed. The TalonFX uses its + * integrated encoder for high-resolution position feedback. + * + * @return the current position as an {@link Angle} measurement + */ @Override public Angle getPositionMeasure() { return Units.Rotations.of(motor.getPosition().getValueAsDouble()); } + /** + * Gets the current stator current of the motor controller. + * + *

Stator current represents the current flowing through the motor windings and + * is useful for monitoring motor load, detecting mechanical binding, and ensuring + * the motor stays within safe operating limits. + * + * @return the current stator current as a {@link Current} measurement + */ @Override public Current getAppliedCurrent() { return motor.getStatorCurrent().getValue(); } + /** + * Sets the encoder position to the specified raw double value. + * + * @deprecated Use {@link #setPosition(Angle)} instead for type safety with units + * @param position the new position in rotations + */ @Override public void setPosition(double position) { motor.setPosition(position); } + /** + * Sets the encoder position using type-safe units. + * + *

This method accepts any {@link Angle} measurement and converts it to rotations + * internally for the TalonFX's integrated encoder. + * + * @param position the new position as an {@link Angle} measurement + */ @Override public void setPosition(Angle position) { motor.setPosition(position.in(Units.Rotations)); } + /** + * Gets the current velocity of the integrated encoder as a raw double value. + * + * @deprecated Use {@link #getVelocityMeasure()} instead for type safety with units + * @return the current velocity in rotations per second as a double + */ @Override public double getVelocity() { return motor.getVelocity().getValueAsDouble(); } + /** + * Gets the current velocity of the integrated encoder using type-safe units. + * + *

This method returns the velocity as an {@link AngularVelocity} measurement + * in rotations per second, which can be converted to other angular velocity + * units as needed. + * + * @return the current velocity as an {@link AngularVelocity} measurement + */ @Override public AngularVelocity getVelocityMeasure() { return Units.RotationsPerSecond.of(motor.getVelocity().getValueAsDouble()); } + /** + * Provides direct access to the underlying TalonFX hardware object. + * + *

This method allows access to Phoenix-specific configuration and methods + * that are not exposed through the {@link PIDMotor} interface, such as + * advanced motion profiling parameters and diagnostic information. + * + * @return the underlying {@link TalonFX} hardware object + */ public TalonFX motor() { return motor; } + /** + * Sets the neutral mode (brake/coast) of the motor controller. + * + *

This method allows runtime changes to the motor's neutral behavior: + *

+ * + * @param mode the neutral mode to set + */ public void setNeutralMode(NeutralModeValue mode) { motor.setNeutralMode(mode); } + /** + * Configures the PIDF constants for a specific closed-loop slot. + * + *

The TalonFX supports multiple PID slot configurations that can be switched + * between during operation. This allows for different control parameters for + * different operating conditions (e.g., different PID gains for velocity vs position control). + * + *

Note: The 'f' parameter represents velocity feedforward (kV) in Phoenix 6, + * not traditional feedforward gain. + * + * @param slot the closed-loop slot to configure + * @param p the proportional gain (kP) + * @param i the integral gain (kI) + * @param d the derivative gain (kD) + * @param f the velocity feedforward gain (kV) + */ @Override public void configPIDF(int slot, double p, double i, double d, double f) { SlotConfigs conf = new SlotConfigs(); @@ -173,21 +324,76 @@ public void configPIDF(int slot, double p, double i, double d, double f) { motor.getConfigurator().apply(conf.withKP(p).withKI(i).withKD(d).withKV(f)); } + /** + * Configures the PIDF constants for the default closed-loop slot (slot 0). + * + *

This is a convenience method that calls {@link #configPIDF(int, double, double, double, double)} + * with slot 0. + * + * @param p the proportional gain (kP) + * @param i the integral gain (kI) + * @param d the derivative gain (kD) + * @param f the velocity feedforward gain (kV) + */ @Override public void configPIDF(double p, double i, double d, double f) { configPIDF(0, p, i, d, f); } + /** + * Configures the PID constants for a specific closed-loop slot. + * + *

This is a convenience method that calls {@link #configPIDF(int, double, double, double, double)} + * with a velocity feedforward value of 0. + * + * @param slot the closed-loop slot to configure + * @param p the proportional gain (kP) + * @param i the integral gain (kI) + * @param d the derivative gain (kD) + */ @Override public void configPID(int slot, double p, double i, double d) { configPIDF(slot, p, i, d, 0); } + /** + * Configures the PID constants for the default closed-loop slot (slot 0). + * + *

This is a convenience method that calls {@link #configPID(int, double, double, double)} + * with slot 0. + * + * @param p the proportional gain (kP) + * @param i the integral gain (kI) + * @param d the derivative gain (kD) + */ @Override public void configPID(double p, double i, double d) { configPIDF(0, p, i, d, 0); } + /** + * Adds a follower TalonFX motor controller on the specified CAN bus. + * + *

Follower motors will automatically mirror the output of the primary motor + * but can have independent inversion settings. The wrapper supports both strict + * following and regular following modes based on the invert type: + * + *

+ * + *

The follower TalonFX object is retained in the followers list to prevent + * garbage collection and maintain the hardware relationship. + * + * @param deviceNumber the CAN ID of the follower TalonFX, must be in range [0, 62] + * @param canbus the CAN bus that the follower is connected to + * @param invertType the inversion type for the follower motor + * @throws InvalidCanIdException if the CAN ID is invalid (outside range [0, 62]) + */ public void addFollower(int deviceNumber, String canbus, InvertType invertType) { TalonFX follower = new TalonFX(checkCanId(deviceNumber), canbus); if (InvertType.rotationValues.contains(invertType)) { @@ -202,6 +408,16 @@ public void addFollower(int deviceNumber, String canbus, InvertType invertType) followers.add(follower); // add to follower list so TalonFX follower object is preserved } + /** + * Adds a follower TalonFX motor controller on the default CAN bus. + * + *

This is a convenience method that calls {@link #addFollower(int, String, InvertType)} + * with the default CAN bus. See that method for detailed behavior documentation. + * + * @param deviceNumber the CAN ID of the follower TalonFX, must be in range [0, 62] + * @param invertType the inversion type for the follower motor + * @throws InvalidCanIdException if the CAN ID is invalid (outside range [0, 62]) + */ public void addFollower(int deviceNumber, InvertType invertType) { TalonFX follower = new TalonFX(checkCanId(deviceNumber)); if (InvertType.rotationValues.contains(invertType)) { @@ -215,4 +431,4 @@ public void addFollower(int deviceNumber, InvertType invertType) { } followers.add(follower); // add to follower list so TalonFX follower object is preserved } -} +} \ No newline at end of file diff --git a/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java b/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java index 68491c3a..9a65c1f5 100644 --- a/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java +++ b/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java @@ -14,118 +14,106 @@ import java.util.function.*; /** - * Initializes the fields of a Record Class from values stored in {@link Preferences}. + * Utility for constructing Java {@link Record} instances whose values are + * persisted in WPILib's {@link Preferences} system. * - *

The Preference values can be updated in the SmartDashboard and/or Shuffleboard UI; updated - * values will be stored in the flash storage for the robot. - * - *

Example use: - * - * 

{@code
- * public final class Drive {
- *
- *   public record DriveConfiguration(
- *       boolean addVisionMeasurements, long robotWeight,
- *       DoubleSupplier powerMultiplier) {
- *
- *     public static DriveConfiguration fromPreferences() {
- *       return PersistedConfiguration.fromPreferences("Drive", DriveConfiguration.class);
- *     }
- *   }
- * }
- * }
- * - *

In the above example, {@code fromPreferences()} would return a record instance with the values - * populated the "Preferences" NetworkTables table. The keys would be: + *

This allows robot configuration values to be declared in record types + * (which are immutable and self-documenting), but automatically backed by + * NetworkTables Preferences so they can be updated live from Shuffleboard or + * SmartDashboard. These persisted values will survive robot reboots and be + * written to flash. * + *

Supported Component Types

+ * Each record component (field) must be one of the following: * * - *

If no value is stored in Preferences for a key, the default value returned (and initialized in - * Preferences) would be the default value for the type of the record component. In the above - * example, if none of the above preference keys existed, preferences will be created with the - * following values: - * + *

Defaults

* * - *

The caller could specify different default values by passing an instance of the record class: + *

Registry Enforcement

+ * Each preference "namespace" (the {@code preferenceName} parameter) is bound to a single record class. + * If code later attempts to bind the same namespace to a different record type, an exception is thrown. + * This prevents accidental reuse of preference keys for incompatible types. * + *

Example Usage

* 
{@code
  * public final class Drive {
  *
+ *   // Define a configuration record for drive parameters.
  *   public record DriveConfiguration(
- *       boolean addVisionMeasurements, long robotWeight,
+ *       boolean addVisionMeasurements,
+ *       long robotWeight,
  *       DoubleSupplier maxAngularVelocity) {
  *
+ *     // Load persisted configuration, with default values provided here.
  *     public static DriveConfiguration fromPreferences() {
- *       DriveConfiguration defaultConfig = new DriveConfiguration(
- *           true, 2813, () -> 3.14);
- *       return PersistedConfiguration.fromPreferences("Drive", defaultConfig);
+ *       DriveConfiguration defaults =
+ *           new DriveConfiguration(true, 2813, () -> 3.14);
+ *       return PersistedConfiguration.fromPreferences("Drive", defaults);
  *     }
  *   }
  * }
  * }
* - *

In the above example, {@code fromPreferences()} would return a record instance with the values - * populated the "Preferences" NetworkTables table. The keys and default values would be: - * + *

Preference keys created will be: *

* - *

For record classes with many component values of the same type, it is strongly recommended - * that a builder is provided to construct the record, to avoid callers passing the parameters in - * the wrong order. To make generation of a builder easier, consider using {@code @AutoBuilder} - * from Google Auto or {@code @Builder} - * from Project Lombok. Note that {@code PersistedConfiguration} will use the default record - * constructor to create record instances, so any parameter validation should be done in a custom - * constructor; see Custom - * Constructor in Java Records for details. + *

Notes and Gotchas

+ * * + * @author Team 2813 * @since 2.0.0 */ public final class PersistedConfiguration { + /** Key used to track which record classes are bound to preference namespaces. */ static final String REGISTERED_CLASSES_NETWORK_TABLE_KEY = "PersistedConfiguration/registry"; + + /** Whether legacy keys have already been cleaned once per program run. */ private static boolean deletedLegacyKeys = false; - // The below package-scope fields are for the self-tests. + // Package-private fields for self-tests. static boolean throwExceptions = false; static Consumer errorReporter = DataLogManager::log; /** - * Creates a record class instance with fields populated from Preferences, using the provided - * defaults. - * - *

To be stored in preferences, the type of the record components can be any of the following: + * Construct a record instance, populating its components from Preferences. * + *

The provided instance supplies the default values. For each component: *

* - *

The values for the components for the passed-in instance will be used as the default value - * for the preference. If a component is a supplier, the supplier will be called at most once to - * get the default instance. Suppliers cannot return {@code null}. - * - * @param preferenceName Preference subtable to use to get the values. - * @param configWithDefaults Record instance with all values set to their preferred default - * values. - * @throws IllegalArgumentException If {@code preferenceName} is empty or contains a {@code '/'}. - * @throws IllegalStateException If {@code preferenceName} was used for a different record class. + * @param preferenceName Subtable name under Preferences (must not contain '/') + * @param configWithDefaults Instance containing default values for all record components + * @return A new record instance populated with Preferences values + * @throws IllegalArgumentException if {@code preferenceName} is empty or contains '/' + * @throws IllegalStateException if {@code preferenceName} was already registered to a different record class */ public static T fromPreferences(String preferenceName, T configWithDefaults) { @SuppressWarnings("unchecked") @@ -134,31 +122,29 @@ public static T fromPreferences(String preferenceName, T conf } /** - * Creates a record class instance of the provided type, with fields populated from Preferences. + * Construct a record instance with values loaded from Preferences, + * using Java defaults if no explicit defaults are provided. * - *

To be stored in preferences, the type of the record components can be any of the following: + *

For example, an {@code int} component defaults to {@code 0}, + * and a {@code double} component defaults to {@code 0.0}. * - *

- * - *

The default values for the preferences will be Java defaults (for example, zero for - * integers). - * - * @param preferenceName Preference subtable to use to get the values. - * @param recordClass Type of the record instance to populate from preferences. - * @throws IllegalArgumentException If {@code preferenceName} is empty or contains a {@code '/'}. - * @throws IllegalStateException If {@code preferenceName} was used for a different record class. + * @param preferenceName Subtable name under Preferences + * @param recordClass Record type to instantiate + * @return A new record instance populated with Preferences values + * @throws IllegalArgumentException if {@code preferenceName} is invalid + * @throws IllegalStateException if namespace was registered to another class */ public static T fromPreferences(String preferenceName, Class recordClass) { return fromPreferences(preferenceName, recordClass, null); } + /** + * Internal shared implementation for record construction. + * + * @param preferenceName Preference subtable name + * @param recordClass Record type + * @param configWithDefaults Optional default instance (may be null) + */ private static T fromPreferences( String preferenceName, Class recordClass, T configWithDefaults) { deleteLegacyKeys(); @@ -170,29 +156,41 @@ private static T fromPreferences( return createFromPreferences(preferenceName, recordClass, configWithDefaults); } catch (ReflectiveOperationException e) { if (throwExceptions) { - throw new RuntimeException(e); // For self-tests. + throw new RuntimeException(e); // For unit tests } DriverStation.reportWarning( - String.format("Could not copy preferences into %s: %s", recordClass.getSimpleName(), e), + String.format("Could not copy preferences into %s: %s", + recordClass.getSimpleName(), e), e.getStackTrace()); return configWithDefaults; } } + /** + * Validates that a preference name is legal (non-empty and does not contain path separators). + */ private static void validatePreferenceName(String name) { if (name.isEmpty()) { throw new IllegalArgumentException("name cannot be empty"); } if (name.indexOf(PATH_SEPARATOR) >= 0) { - throw new IllegalArgumentException(String.format("name cannot contain '%c'", PATH_SEPARATOR)); + throw new IllegalArgumentException( + String.format("name cannot contain '%c'", PATH_SEPARATOR)); } } + /** + * Ensures the given preference namespace is not already registered to another record class. + * + *

Each namespace is stored in a special NetworkTable registry entry. If the namespace + * is new, it is bound to the current record type. If it already exists and points to + * a different record type, this method throws an exception. + */ private static void verifyNotRegisteredToAnotherClass( NetworkTableInstance ntInstance, String name, Class recordClass) { String recordName = recordClass.getCanonicalName(); if (recordName == null) { - recordName = recordClass.getName(); + recordName = recordClass.getName(); // fallback if canonical name unavailable } NetworkTable registeredClassesTable = ntInstance.getTable(REGISTERED_CLASSES_NETWORK_TABLE_KEY); @@ -205,46 +203,54 @@ private static void verifyNotRegisteredToAnotherClass( if (!recordName.equals(registeredTo)) { throw new IllegalStateException( String.format( - "Preference with name '%s' already registered to %s", name, registeredTo)); + "Preference with name '%s' already registered to %s", + name, registeredTo)); } } } + /** + * Core factory logic: instantiate a record using reflection, + * reading each component value from Preferences (or defaults). + */ private static T createFromPreferences( String prefix, Class clazz, T configWithDefaults) throws ReflectiveOperationException { var components = clazz.getRecordComponents(); Object[] params = new Object[components.length]; Class[] types = new Class[components.length]; + int i = 0; for (RecordComponent component : components) { String name = component.getName(); - String key = prefix + PATH_SEPARATOR + name; + String key = prefix + PATH_SEPARATOR + name; // Preference key = namespace/componentName Class type = component.getType(); types[i] = type; boolean needComponentValue; PreferenceFactory factory = null; boolean isRecordField = Record.class.isAssignableFrom(type); + if (isRecordField) { + // Nested record: recurse into sub-record needComponentValue = true; } else { factory = TYPE_TO_FACTORY.get(type); if (factory == null) { - // Cannot get value from Preferences; copy over the value from the input record. + // Unsupported type: fall back to copying default value needComponentValue = true; } else { + // If no key exists, we need the component value for initialization needComponentValue = !Preferences.containsKey(key); } } Object componentValue = null; - if (needComponentValue) { - if (configWithDefaults != null) { - Field defaultValueField = clazz.getDeclaredField(name); - defaultValueField.setAccessible(true); - componentValue = defaultValueField.get(configWithDefaults); - } + if (needComponentValue && configWithDefaults != null) { + // Reflection trick: get the field value from the default record instance + Field defaultValueField = clazz.getDeclaredField(name); + defaultValueField.setAccessible(true); + componentValue = defaultValueField.get(configWithDefaults); } if (isRecordField) { @@ -253,29 +259,37 @@ private static T createFromPreferences( warn("Cannot store '%s' in Preferences; type %s is unsupported", name, type); params[i] = componentValue; } else { - // Fetch the value from Preferences + // Use registered factory to create the value (from Preferences or defaults) params[i] = factory.create( component, key, componentValue, /* initializePreference= */ needComponentValue); } i++; } + Constructor constructor = clazz.getDeclaredConstructor(types); constructor.setAccessible(true); return constructor.newInstance(params); } + /** + * Functional interface mapping a record component into a preference-backed value. + */ @FunctionalInterface private interface PreferenceFactory { Object create( RecordComponent component, String key, Object defaultValue, boolean initializePreference); } + /** + * Generic variant of {@link PreferenceFactory} with typed defaults. + */ @FunctionalInterface private interface GenericPreferenceFactory { T create(RecordComponent component, String key, T defaultValue, boolean initializePreference); } + /** Registry of supported record component types to their corresponding factories. */ private static final Map TYPE_TO_FACTORY = new HashMap<>(); @SuppressWarnings("unchecked") @@ -286,6 +300,7 @@ private static void register(Class type, GenericPreferenceFactory simp TYPE_TO_FACTORY.put(type, factory); } + // Static initialization: register supported primitive and supplier types static { register(Boolean.TYPE, PersistedConfiguration::booleanFactory); register(BooleanSupplier.class, PersistedConfiguration::booleanSupplierFactory); @@ -299,7 +314,7 @@ private static void register(Class type, GenericPreferenceFactory simp register(Supplier.class, PersistedConfiguration::supplierFactory); } - /** Maps the generic types supported by Preferences to their primitive types. */ + /** Maps boxed types to the primitive/supported type used for preferences. */ private static final Map SUPPLIER_TYPE_TO_REGISTERED_TYPE = Map.of( Boolean.class, Boolean.TYPE, @@ -308,7 +323,10 @@ private static void register(Class type, GenericPreferenceFactory simp Double.class, Double.TYPE, String.class, String.class); - /** Gets a boolean value from Preferences for the given component. */ + // =============================== + // FACTORY IMPLEMENTATIONS + // =============================== + private static boolean booleanFactory( RecordComponent component, String key, Boolean defaultValue, boolean initialize) { if (initialize) { @@ -321,23 +339,20 @@ private static boolean booleanFactory( return Preferences.getBoolean(key, false); } - /** Gets a BooleanSupplier value from Preferences for the given component. */ private static BooleanSupplier booleanSupplierFactory( RecordComponent component, String key, BooleanSupplier defaultValueSupplier, boolean initialize) { if (initialize) { - boolean defaultValue = false; - if (defaultValueSupplier != null) { - defaultValue = defaultValueSupplier.getAsBoolean(); - } + boolean defaultValue = defaultValueSupplier != null + ? defaultValueSupplier.getAsBoolean() + : false; Preferences.initBoolean(key, defaultValue); } return () -> Preferences.getBoolean(key, false); } - /** Gets an int value from Preferences for the given component. */ private static int intFactory( RecordComponent component, String key, Integer defaultValue, boolean initialize) { if (initialize) { @@ -350,7 +365,6 @@ private static int intFactory( return Preferences.getInt(key, 0); } - /** Gets a IntSupplier value from Preferences for the given component. */ private static IntSupplier intSupplierFactory( RecordComponent component, String key, IntSupplier defaultValueSupplier, boolean initialize) { if (initialize) { @@ -360,7 +374,6 @@ private static IntSupplier intSupplierFactory( return () -> Preferences.getInt(key, 0); } - /** Gets a long value from Preferences for the given component. */ private static long longFactory( RecordComponent component, String key, Long defaultValue, boolean initialize) { if (initialize) { @@ -373,7 +386,6 @@ private static long longFactory( return Preferences.getLong(key, 0); } - /** Gets a LongSupplier value from Preferences for the given component. */ private static LongSupplier longSupplierFactory( RecordComponent component, String key, @@ -386,7 +398,6 @@ private static LongSupplier longSupplierFactory( return () -> Preferences.getLong(key, 0); } - /** Gets a double value from Preferences for the given component. */ private static double doubleFactory( RecordComponent component, String key, Double defaultValue, boolean initialize) { if (initialize) { @@ -399,7 +410,6 @@ private static double doubleFactory( return Preferences.getDouble(key, 0); } - /** Gets a DoubleSupplier value from Preferences for the given component. */ private static DoubleSupplier doubleSupplierFactory( RecordComponent component, String key, @@ -412,7 +422,6 @@ private static DoubleSupplier doubleSupplierFactory( return () -> Preferences.getDouble(key, 0); } - /** Gets a String value from Preferences for the given component. */ private static String stringFactory( RecordComponent component, String key, String defaultValue, boolean initialize) { if (initialize) { @@ -425,18 +434,13 @@ private static String stringFactory( return Preferences.getString(key, ""); } - /** - * Gets a Supplier value from Preferences for the given component. Supports String, long, int, - * boolean and float values. - */ private static Supplier supplierFactory( RecordComponent component, String key, Supplier defaultValueSupplier, boolean initialize) { Type supplierType = ((ParameterizedType) component.getGenericType()).getActualTypeArguments()[0]; Type registeredType = SUPPLIER_TYPE_TO_REGISTERED_TYPE.get(supplierType); if (registeredType == null) { - warn( - "Cannot store '%s' in Preferences; type %s is unsupported", + warn("Cannot store '%s' in Preferences; type %s is unsupported", component.getName(), component.getGenericType()); return defaultValueSupplier; } @@ -447,22 +451,23 @@ private static Supplier supplierFactory( if (defaultValueSupplier != null) { defaultValue = defaultValueSupplier.get(); if (defaultValue == null) { - warn("Cannot store '%s' in Preferences; default value is null", component.getName()); + warn("Cannot store '%s' in Preferences; default value is null", + component.getName()); return defaultValueSupplier; } } - factory.create( - component, key, defaultValue, true); // Call Preferences.init{String,Double,etc}() + factory.create(component, key, defaultValue, true); } return () -> factory.create(component, key, null, false); } + /** + * Removes legacy ".registeredTo" keys from Preferences (used by older versions of this class). + * Ensures backward compatibility without polluting the Preferences namespace. + */ private static void deleteLegacyKeys() { if (!deletedLegacyKeys) { - // Preferences installs a listener that makes all new topics persistent. The ".registeredTo" - // topics used to be placed under /Preferences, so could have been persisted. They are now - // written under a different top-level table. Delete the topics created by the previous code. for (var key : Preferences.getKeys()) { if (key.endsWith(".registeredTo")) { Preferences.remove(key); @@ -472,11 +477,15 @@ private static void deleteLegacyKeys() { } } + /** + * Utility method for reporting warnings. Logs to the configured {@link #errorReporter}. + */ private static void warn(String format, Object... args) { String message = String.format("WARNING: " + format, args); errorReporter.accept(message); } + /** Private constructor to prevent instantiation. */ private PersistedConfiguration() { throw new AssertionError("Not instantiable"); } diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java index e4e39094..2510029d 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -15,38 +15,62 @@ import java.util.function.Supplier; /** - * Defines PID control over a motor, with values specified by an encoder + * Abstract subsystem that manages a motor with PID control based on encoder measurements. * - * @param the {@link Supplier} type to use positions from. + *

This class allows: + *

+ * + * @param the type of {@link Supplier} used to provide dynamic setpoints. */ public abstract class MotorSubsystem> extends SubsystemBase implements Motor, Encoder { + /** The motor used by this subsystem */ protected final Motor motor; + + /** The encoder providing feedback for PID calculations */ protected final Encoder encoder; + + /** The control mode used when applying outputs to the motor */ protected final ControlMode controlMode; + + /** The angle unit for measurements and setpoints */ protected final AngleUnit rotationUnit; + + /** Maximum allowed position error for "at position" checks */ protected final double acceptableError; + + /** The PID controller managing the motor to reach the setpoint */ protected final PIDController controller; private double setpoint; private boolean isEnabled; + /** + * Constructs a MotorSubsystem using the provided configuration. + * + * @param builder the configuration for the motor subsystem + */ protected MotorSubsystem(MotorSubsystemConfiguration builder) { this.setpoint = builder.startingPosition; this.controller = builder.controller; this.controller.setTolerance(builder.acceptableError); - acceptableError = builder.acceptableError; - motor = builder.motor; - encoder = builder.encoder; - controlMode = builder.controlMode; - rotationUnit = builder.rotationUnit; + this.acceptableError = builder.acceptableError; + this.motor = builder.motor; + this.encoder = builder.encoder; + this.controlMode = builder.controlMode; + this.rotationUnit = builder.rotationUnit; } /** - * Sets the desired setpoint to the current setpoint, and enables the PID. control. + * Sets the target position for this subsystem and enables PID control if not already enabled. * - * @param setpoint the position to go to + * @param setpoint the supplier of the target angle */ public void setSetpoint(T setpoint) { if (!isEnabled()) { @@ -56,10 +80,20 @@ public void setSetpoint(T setpoint) { controller.setSetpoint(this.setpoint); } + /** + * Returns the current PID setpoint. + * + * @return the target setpoint as an {@link Angle} + */ public Angle getSetpoint() { return rotationUnit.of(setpoint); } + /** + * Checks whether the subsystem is within the acceptable error of the setpoint. + * + * @return {@code true} if within acceptable error, {@code false} otherwise + */ public boolean atPosition() { return Math.abs(getMeasurement() - setpoint) <= acceptableError; } @@ -67,7 +101,7 @@ public boolean atPosition() { /** * {@inheritDoc} * - *

Additionally, this method disables PID control of the subsystem + *

Also disables PID control if enabled. */ @Override public void set(ControlMode mode, double demand, double feedForward) { @@ -82,19 +116,21 @@ public Current getAppliedCurrent() { return motor.getAppliedCurrent(); } + /** Enables PID control of the subsystem. */ public void enable() { isEnabled = true; } + /** Disables PID control and sets motor output to zero. */ public void disable() { isEnabled = false; useOutput(0, 0); } /** - * Returns whether the controller is enabled. If this is enabled, then PID control will be used. + * Returns whether PID control is currently enabled. * - * @return Whether the controller is enabled. + * @return {@code true} if enabled, {@code false} otherwise */ public boolean isEnabled() { return isEnabled; @@ -103,7 +139,7 @@ public boolean isEnabled() { /** * {@inheritDoc} * - *

Additionally, this method disables PID control of the subsystem + *

Also disables PID control if enabled. */ @Override public void set(ControlMode mode, double demand) { @@ -113,10 +149,17 @@ public void set(ControlMode mode, double demand) { motor.set(mode, demand); } + /** + * Applies the PID output to the motor. Can be overridden for advanced control. + * + * @param output the PID output value + * @param setpoint the target setpoint (for reference) + */ protected void useOutput(double output, double setpoint) { motor.set(controlMode, output); } + /** Returns the encoder measurement converted to the configured {@link AngleUnit}. */ protected double getMeasurement() { return encoder.getPositionMeasure().in(rotationUnit); } @@ -151,6 +194,7 @@ public AngularVelocity getVelocityMeasure() { return encoder.getVelocityMeasure(); } + /** Periodic update that applies PID output if enabled. */ @Override public void periodic() { if (isEnabled) { @@ -158,12 +202,17 @@ public void periodic() { } } - /** A configuration for a MotorSubsystem */ + /** + * Builder-style configuration class for {@link MotorSubsystem}. + * + *

Allows setting PID constants, control mode, starting position, and other configuration + * parameters. + */ public static class MotorSubsystemConfiguration { - /** The default error of a motor subsystems */ + /** Default allowed error for position checks */ public static final double DEFAULT_ERROR = 5.0; - /** The default starting position if one is not defined */ + /** Default starting position */ public static final double DEFAULT_STARTING_POSITION = 0.0; private ControlMode controlMode; @@ -175,12 +224,14 @@ public static class MotorSubsystemConfiguration { private double startingPosition; /** - * Creates a new config for MotorSubsystems. The default acceptable error is {@value - * #DEFAULT_ERROR}, the PID constants are set to 0, and the starting position is {@value - * #DEFAULT_STARTING_POSITION} + * Creates a new configuration for a motor subsystem. + * + *

Defaults: PID(0,0,0), acceptable error = {@value #DEFAULT_ERROR}, starting position = + * {@value #DEFAULT_STARTING_POSITION}, control mode = {@link ControlMode#DUTY_CYCLE}, angle + * unit = {@link Units#Rotations}. * - * @param motor the motor to use - * @param encoder the encoder to use + * @param motor the motor to control + * @param encoder the encoder providing feedback */ public MotorSubsystemConfiguration(Motor motor, Encoder encoder) { this.motor = Objects.requireNonNull(motor, "motor should not be null"); @@ -193,78 +244,51 @@ public MotorSubsystemConfiguration(Motor motor, Encoder encoder) { } /** - * Creates a new config for MotorSubsystems using a motor that has a built-in encoder. The - * default acceptable error is {@value #DEFAULT_ERROR}, the PID constants are set to 0, and the - * starting position is {@value #DEFAULT_STARTING_POSITION} + * Creates a new configuration using a PIDMotor with a built-in encoder. * - * @param motor the motor to use that also supports an encoder + * @param motor the motor that implements {@link PIDMotor} */ public MotorSubsystemConfiguration(PIDMotor motor) { this(motor, motor); } - /** - * Sets the controller used to calculate the next value - * - * @param controller The PID controller - * @return {@code this} for chaining - */ + /** Sets the PID controller to use. */ public MotorSubsystemConfiguration controller(PIDController controller) { this.controller = controller; return this; } - /** - * Sets the control mode to use when giving output to the motor. Defaults to {@link - * ControlMode#DUTY_CYCLE}. - * - * @param controlMode The mode to use when controlling the motor - * @return {@code this} for chaining - */ + /** Sets the control mode for motor output. */ public MotorSubsystemConfiguration controlMode(ControlMode controlMode) { this.controlMode = controlMode; return this; } - /** - * Sets the PID constants for the controller - * - * @param p the proportional - * @param i the integral - * @param d the derivative - * @return {@code this} for chaining - */ + /** Sets the PID constants. */ public MotorSubsystemConfiguration PID(double p, double i, double d) { controller.setPID(p, i, d); return this; } - /** - * sets the starting position. - * - * @param startingPosition the position to start at - * @return {@code this} for chaining - */ + /** Sets the starting position in the configured angle unit. */ public MotorSubsystemConfiguration startingPosition(Angle startingPosition) { this.startingPosition = startingPosition.in(this.rotationUnit); return this; } + /** Sets the starting position via a supplier of an angle. */ public MotorSubsystemConfiguration startingPosition(Supplier startingPosition) { this.startingPosition = startingPosition.get().in(this.rotationUnit); return this; } + /** Sets the acceptable position error. */ public MotorSubsystemConfiguration acceptableError(double error) { this.acceptableError = error; return this; } - /** - * Sets the unit to use for PID calculations - * - * @param rotationUnit The angle unit to use for calculations - */ + /** Sets the angle unit for PID calculations. */ public MotorSubsystemConfiguration rotationUnit(AngleUnit rotationUnit) { startingPosition = rotationUnit.convertFrom(startingPosition, this.rotationUnit); this.rotationUnit = rotationUnit; diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java index 7faeeb00..d6a5e74f 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java @@ -7,12 +7,28 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +/** + * A subsystem for an intake mechanism with parameterized motor control. + * + *

This subsystem uses a {@link PIDMotor} and allows controlling the motor in intake and + * outtake directions with preconfigured demand values. Commands are provided for common actions + * such as intaking, outtaking, and stopping the motor. + */ public abstract class ParameterizedIntakeSubsystem extends SubsystemBase implements AutoCloseable { + private final PIDMotor intakeMotor; private final Params params; + /** + * Configuration record that stores the parameters for the intake subsystem. + * + *

Includes the motor control mode, the intake demand, and the outtake demand. + * + *

Validation ensures the intake and outtake demands are non-zero and have opposite signs. + */ public record Params(ControlMode controlMode, double intakeDemand, double outtakeDemand) { + /** Returns a builder for creating {@link Params}. Defaults the control mode to VOLTAGE. */ public static Params.Builder builder() { return new AutoBuilder_ParameterizedIntakeSubsystem_Params_Builder() .setControlMode(ControlMode.VOLTAGE); @@ -29,6 +45,14 @@ public interface Builder { Params build(); } + /** + * Constructs a Params record. + * + * @param controlMode the motor control mode, cannot be null + * @param intakeDemand the motor demand for intake, must be non-zero + * @param outtakeDemand the motor demand for outtake, must be non-zero and opposite sign of intakeDemand + * @throws IllegalArgumentException if validation fails + */ public Params { if (controlMode == null) { throw new IllegalArgumentException("controlMode cannot be null"); @@ -46,60 +70,85 @@ public interface Builder { } } + /** + * Constructs a new intake subsystem. + * + * @param intakeMotor the motor controlling the intake mechanism + * @param params configuration parameters for motor control + */ protected ParameterizedIntakeSubsystem(PIDMotor intakeMotor, Params params) { this.intakeMotor = intakeMotor; this.params = params; } + /** + * Returns a command that spins the intake wheels in the intake direction. + * + * @return an {@link InstantCommand} that intakes a game piece + */ public final Command intakeItemCommand() { return new InstantCommand(this::intakeGamePiece, this); } + /** + * Returns a command that spins the intake wheels in the outtake direction. + * + * @return an {@link InstantCommand} that outtakes a game piece + */ public final Command outtakeItemCommand() { return new InstantCommand(this::outtakeGamePiece, this); } + /** + * Returns a command that stops the intake motor. + * + * @return an {@link InstantCommand} that stops the motor + */ public final Command stopMotorCommand() { return new InstantCommand(this::stopMotor, this); } - /** Makes intake wheels spin in the intake direction. */ + /** Spins the intake wheels in the configured intake direction. */ protected final void intakeGamePiece() { // FIXME: Maybe add a check that the wheels are not stalled. setMotorDemand(params.intakeDemand); } - /** Makes intake wheels spin in the outtake direction. */ + /** Spins the intake wheels in the configured outtake direction. */ protected final void outtakeGamePiece() { setMotorDemand(params.outtakeDemand); } /** - * Runs the motor with the provided demand value. + * Runs the intake motor with a specific demand. * - * @param demand Demand of the motor. Meaning depends on the {@code ControlMode}. + * @param demand the motor demand, meaning depends on the configured {@link ControlMode} */ protected final void setMotorDemand(double demand) { intakeMotor.set(params.controlMode, demand); } /** - * Returns a command that runs the motor with the provided demand value. + * Returns a command that runs the intake motor with the given demand. * - * @param demand Demand of the motor. Meaning depends on the {@code ControlMode}. + * @param demand the motor demand, meaning depends on the configured {@link ControlMode} + * @return a command that sets the motor demand when executed */ protected final Command setMotorDemandCommand(double demand) { return new InstantCommand(() -> setMotorDemand(demand), this); } - /** Stops the motor. */ + /** Stops the intake motor. */ public final void stopMotor() { setMotorDemand(0.0); } @Override - public void close() {} + public void close() { + // Default implementation does nothing. Override if resources need cleanup. + } + /** Returns whether a value is essentially zero (within 0.001). */ private static boolean isEssentiallyZero(double value) { return Math.abs(value) < 0.001; } diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/Lightshow.java b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/Lightshow.java index 8c743d40..b19cb3b2 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/Lightshow.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/Lightshow.java @@ -1,82 +1,107 @@ package com.team2813.lib2813.subsystems.lightshow; import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.Arrays; import java.util.HashSet; import java.util.Optional; import java.util.Set; -import java.util.function.Consumer; +/** + * Abstract base class for robot lightshow subsystems. + * + *

A Lightshow subsystem manages a set of {@link State}s that determine the colors to be displayed + * on LEDs or other lighting hardware. Implementations define how to actually apply the color via + * {@link #useColor(Color)} and how to compute the active color in {@link #update()}. + * + *

States can be represented as enums implementing {@link State}, or as arbitrary {@link State} + * instances. + * + *

There is a built-in static {@link #off} state that always represents the lights being off. + * + * @author Team 2813 + */ public abstract class Lightshow extends SubsystemBase { - /** A static state that signifies that the lights should be off, that is always applied */ - protected static final State off = - new State() { - public Color color() { - return new Color(0, 0, 0); - } - public boolean apply() { - return true; - } - }; + /** A static state representing that the lights should be off (always applied). */ + protected static final State off = new State() { + @Override + public Color color() { + return new Color(0, 0, 0); + } - protected Set states = new HashSet<>(); - protected Optional defaultState; + @Override + public boolean apply() { + return true; + } + }; + + /** The set of active states that the lightshow considers when updating the color. */ + protected final Set states = new HashSet<>(); + + /** The default state to use if no other state is active. */ + protected Optional defaultState = Optional.empty(); /** - * Creates a new Lightshow subsystem using an enum. Uses the given {@code enumClass} to get a list - * of {@link State}s to use. + * Constructs a Lightshow using an enum class representing the states. * - * @param The type of enum that implements state - * @param enumClass The enum class that contains the states + * @param the type of enum implementing {@link State} + * @param enumClass the enum class containing the states */ protected & State> Lightshow(Class enumClass) { addStates(enumClass); } /** - * Creates a new Lightshow subsystem with a set of states. + * Constructs a Lightshow with a set of states. * - * @param states The states + * @param states the initial states for this Lightshow */ protected Lightshow(Set states) { addStates(states); } + /** + * Implementation-defined method to actually use a {@link Color}. + * + * @param c the color to display + */ protected abstract void useColor(Color c); + /** + * Adds states from an enum class implementing {@link State}. + * + * @param the enum type implementing {@link State} + * @param enumClass the enum class containing the states + */ public final & State> void addStates(Class enumClass) { T[] constants = enumClass.getEnumConstants(); states.addAll(Arrays.asList(constants)); } + /** + * Adds arbitrary {@link State} instances to the Lightshow. + * + * @param states the states to add + */ public final void addStates(Set states) { this.states.addAll(states); } /** - * Updates the current States. Makes the current {@link #states} updated, and gets the Color to be - * displayed. + * Updates the current active {@link State}s and determines the color to display. * - * @return an optional representing the color to be displayed - * @implSpec Do anything with {@link #states} to determine which {@link Color} to display - * @see #periodic() + * @return an optional containing the color to display, or empty if no state is active */ protected abstract Optional update(); /** - * {@inheritDoc} - * - *

This implementation of {@link Subsystem#periodic()} updates the colors by passing the call - * of {@link #update()} to the given {@link Consumer}. If the call to {@link #update()} returns an - * empty optional, than the color of the default state is used, or the color is not changed. + * Called periodically by the scheduler to update the lights. * - * @implSpec {@link #useColor(Color)} is passed the result of {@link #update()}, or the {@link - * #defaultState}'s color. If there is no color from {@link #update()} and no default state, - * there is no requirement for what to pass to {@link #useColor(Color)}, or if {@link - * #useColor(Color)} is called at all + *

This implementation calls {@link #update()}, and applies the resulting color using + * {@link #useColor(Color)}. If {@link #update()} returns empty, the {@link #defaultState}'s + * color is used (if present). If neither provides a color, {@link #useColor(Color)} may not be + * called. */ @Override public void periodic() { @@ -85,9 +110,9 @@ public void periodic() { } /** - * Sets the {@link State} to be used if none is given. + * Sets the default {@link State} to use if no other state is active. * - * @param defaultState the {@link State} to be used as a default. + * @param defaultState the default state */ public void setDefaultState(State defaultState) { this.defaultState = Optional.of(defaultState); diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/QueueLightshow.java b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/QueueLightshow.java index 573240f1..2b8a7b68 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/QueueLightshow.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/QueueLightshow.java @@ -7,34 +7,65 @@ import java.util.Set; /** - * A lightshow that keeps track of the states that have been applied, and uses the last state. To be - * more specific, all states that return {@code true} on a call to {@link State#apply()} are added - * to the list. States are only removed from the list if they are at the front and {@link - * State#apply()} returns {@code false}, when a state is removed, the next one will be activated if - * {@link State#apply()} returns {@code true}, until either a state returns {@code true} upon a call - * to {@link State#apply()}, in which the color will be used, or there are no states where {@link - * State#apply()} return {@code true}, then the default color is used. + * A Lightshow implementation that manages a queue of active states. + * + *

Behavior: + *

    + *
  • All {@link State}s that return {@code true} for {@link State#apply()} are added to a + * queue.
    • + *
  • States are removed from the front of the queue if {@link State#apply()} returns {@code + * false}.
    • + *
  • The last active state in the queue is used to determine the color to display.
    • + *
  • If no states are active, the {@link Lightshow#off} state or {@link #defaultState} is used. + *
    • + *
+ * + * @author Team 2813 */ public abstract class QueueLightshow extends Lightshow { + private final Deque activatedStates = new ArrayDeque<>(); + /** + * Creates a QueueLightshow from an enum class of {@link State}s. + * + * @param the enum type implementing {@link State} + * @param enumClass the enum class containing the states + */ public & State> QueueLightshow(Class enumClass) { super(enumClass); defaultState = Optional.of(Lightshow.off); } + /** + * Creates a QueueLightshow from a set of {@link State}s. + * + * @param states the set of states to use + */ public QueueLightshow(Set states) { super(states); defaultState = Optional.of(Lightshow.off); } + /** + * Updates the queue of active states and returns the current color to display. + * + *

New states that return {@code true} for {@link State#apply()} are added to the front of the + * queue. States at the front of the queue that return {@code false} are removed. The color of the + * first active state is used. + * + * @return the color of the current active state, or empty if no states are active + */ @Override protected Optional update() { + // Add new active states that are not already in the queue for (State s : states) { if (!activatedStates.contains(s) && s.apply()) { activatedStates.addFirst(s); } } + + // Remove inactive states from the front while (!activatedStates.isEmpty()) { State s = activatedStates.poll(); if (s.apply()) { @@ -42,6 +73,7 @@ protected Optional update() { return Optional.of(s.color()); } } + return Optional.empty(); } } diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/State.java b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/State.java index ce4e6874..ed2e0072 100644 --- a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/State.java +++ b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/State.java @@ -2,18 +2,27 @@ import edu.wpi.first.wpilibj.util.Color; +/** + * Represents a Lightshow state with a color and activation condition. + * + *

Each {@link State} provides a color and a condition indicating whether it should currently + * be applied. + * + * @author Team 2813 + */ public interface State { + /** - * gets the color of this State. + * Gets the color of this state. * - * @return the color of this State + * @return the color to display for this state */ Color color(); /** - * Checks if the current state should be applied + * Checks if this state should currently be applied. * - * @return {@code true} if the state should be applied + * @return {@code true} if the state should be active, {@code false} otherwise */ boolean apply(); } diff --git a/lib/src/main/java/com/team2813/lib2813/util/BuildConstants.java b/lib/src/main/java/com/team2813/lib2813/util/BuildConstants.java index fcd2a2c1..8b80f71a 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/BuildConstants.java +++ b/lib/src/main/java/com/team2813/lib2813/util/BuildConstants.java @@ -2,7 +2,11 @@ import java.time.ZonedDateTime; -/** Holder for data collected at build time about the robot code. */ +/** + * Holder for data collected at build time about the robot code. + * + * @author Team 2813 + */ public interface BuildConstants { /** The current git branch when the code was built. */ diff --git a/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsPublisher.java b/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsPublisher.java index 7d59e1ca..7111ba86 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsPublisher.java +++ b/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsPublisher.java @@ -20,7 +20,7 @@ *

The constants are published under the {@code "/Metadata"} table in NetworkTables. This is a * special NetworkTables table. Some tools have special support for the "/Metadata" table. For * instance, Advantage Scope has a dedicated Metadata tab that loads information like Build - * Constants in a well formated table view. + * Constants in a well formatted table view. * *

To instantiate a BuildConstantsPublisher, a build constants class, {@code BuildConstants}, is * needs to be generated for the robot library by enabling the `gversion` plugin in the gradle build @@ -37,6 +37,8 @@ * // Log the build constants in the robot console as well. * buildConstantsPublisher.log(); * } + * + * @author Team 2813 */ public final class BuildConstantsPublisher { /** The name of the NetworkTable under which the build constants are published. */ diff --git a/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsRecord.java b/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsRecord.java index 56b2a6d5..325e753c 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsRecord.java +++ b/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsRecord.java @@ -6,6 +6,18 @@ import java.time.format.DateTimeParseException; import java.util.Optional; +/** + * Record implementation of {@link BuildConstants}. + * + *

This class holds metadata about the build, such as git branch, commit hash, build time, etc. + * It is typically generated at build time using the gversion Gradle plugin and then extracted at + * runtime for diagnostics, publishing, or logging. + * + *

All values are immutable once created. Utility methods are provided for formatting dates + * consistently and safely handling extraction errors. + * + * @author Team 2813 + */ record BuildConstantsRecord( String mavenName, int gitRevision, @@ -22,11 +34,14 @@ record BuildConstantsRecord( /** * Constructs an instance from a class generated by the gversion Gradle plugin. * - *

Instructions for using the gversion Gradle plugin can be found + * in the WPILib documentation. * - * @param buildConstantsClass Specially built class that contains the robot code built-time + * @param buildConstantsClass Specially built class that contains the robot code build-time * constants. + * @return an {@link Optional} containing a populated record if successful, or empty if extraction + * failed. */ static Optional fromGeneratedClass(Class buildConstantsClass) { try { @@ -67,10 +82,12 @@ static Optional fromGeneratedClass(Class buildConstants return Optional.empty(); } + /** @return the git submit time formatted as {@code yyyy-MM-dd HH:mm:ss z}. */ String gitSubmitTimeString() { return DATE_FORMATTER.format(gitSubmitTime); } + /** @return the build time formatted as {@code yyyy-MM-dd HH:mm:ss z}. */ String buildTimeString() { return DATE_FORMATTER.format(buildTime); } diff --git a/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java b/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java index c3a8deef..9631e1ee 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java +++ b/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java @@ -6,14 +6,29 @@ import edu.wpi.first.wpilibj.DriverStation; import java.util.function.Supplier; +/** + * Utility class for safely applying motor controller configurations (REV, Phoenix 6, CTRE). + * + *

Each configuration method retries up to a fixed number of attempts to ensure reliability, + * reporting warnings and errors to the {@link DriverStation}. + * + *

This class is non-instantiable and provides only static methods. + * + * @author Team 2813 + */ public class ConfigUtils { private static final int ATTEMPTS = 10; - // make class non-instantiable + // Make class non-instantiable private ConfigUtils() { throw new AssertionError("cannot create ConfigUtils instance"); } + /** + * Applies a configuration to a REV device with retry attempts. + * + * @param configMethod Supplier that applies a REV configuration and returns a {@link REVLibError}. + */ public static void revConfig(Supplier configMethod) { REVLibError errorCode = configMethod.get(); for (int i = 1; i <= ATTEMPTS && errorCode != REVLibError.kOk; i++) { @@ -26,6 +41,12 @@ public static void revConfig(Supplier configMethod) { } } + /** + * Applies a configuration to a Phoenix 6 device with retry attempts. + * + * @param configMethod Supplier that applies a Phoenix 6 configuration and returns a {@link + * StatusCode}. + */ public static void phoenix6Config(Supplier configMethod) { StatusCode errorCode = configMethod.get(); for (int i = 1; i <= ATTEMPTS && errorCode != StatusCode.OK; i++) { @@ -38,6 +59,11 @@ public static void phoenix6Config(Supplier configMethod) { } } + /** + * Applies a configuration to a CTRE device with retry attempts. + * + * @param configMethod Supplier that applies a CTRE configuration and returns an {@link ErrorCode}. + */ public static void ctreConfig(Supplier configMethod) { ErrorCode errorCode = configMethod.get(); if (errorCode != ErrorCode.OK) { diff --git a/lib/src/main/java/com/team2813/lib2813/util/ControlUtils.java b/lib/src/main/java/com/team2813/lib2813/util/ControlUtils.java index 4b0cb589..565af50d 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/ControlUtils.java +++ b/lib/src/main/java/com/team2813/lib2813/util/ControlUtils.java @@ -1,6 +1,13 @@ package com.team2813.lib2813.util; -/** Utility class for comon control-related utility functions. */ +/** + * Utility class for common control-related helper functions. + * + *

Provides mathematical operations useful in robot control, such as deadbanding joystick inputs. + * The class is non-instantiable and should be used only via static methods. + * + * @author Team 2813 + */ public class ControlUtils { private ControlUtils() { throw new AssertionError( @@ -8,14 +15,14 @@ private ControlUtils() { } /** - * Deadbands a value. + * Applies a deadband to a value. * *

"A deadband or dead-band (also known as a dead zone or a neutral zone) is a band of input * values in the domain of a transfer function in a control system or signal processing system - * where the output is zero (the output is 'dead' - no action occurs).". See https://en.wikipedia.org/wiki/Deadband. + * where the output is zero (the output is 'dead' — no action occurs)." See Wikipedia: Deadband. * - * @see an interactive demo of the + * @see Interactive demo of the * deadband operation. * @param value The value to deadband, must be in [-1.0, 1.0]. * @param deadband The deadband range value, must be in [0.0, 1.0). @@ -32,12 +39,10 @@ public static double deadband(double value, double deadband) { } if (Math.abs(value) <= deadband) { return 0.0; + } else if (value > 0.0) { + return (value - deadband) / (1.0 - deadband); } else { - if (value > 0.0) { - return (value - deadband) / (1.0 - deadband); - } else { - return (value + deadband) / (1.0 - deadband); - } + return (value + deadband) / (1.0 - deadband); } } } diff --git a/lib/src/main/java/com/team2813/lib2813/util/FakePIDMotor.java b/lib/src/main/java/com/team2813/lib2813/util/FakePIDMotor.java index a8076f8c..be6c2432 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/FakePIDMotor.java +++ b/lib/src/main/java/com/team2813/lib2813/util/FakePIDMotor.java @@ -6,10 +6,25 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; +/** + * A fake implementation of {@link PIDMotor} used for testing. + * + *

This class simulates motor behavior by storing the most recent control mode and demand value. + * Assertions are used (via Truth) to enforce correct expectations in tests. + * + *

Not intended for production use. + * + * @author Team 2813 + */ public abstract class FakePIDMotor implements PIDMotor { public double demand = 0.0f; private ControlMode controlMode; + /** + * Gets the current demand if the control mode is {@link ControlMode#VOLTAGE}. + * + * @return The current voltage demand. + */ public double getVoltage() { Truth.assertThat(controlMode).isEqualTo(ControlMode.VOLTAGE); return demand; diff --git a/lib/src/main/java/com/team2813/lib2813/util/InputValidation.java b/lib/src/main/java/com/team2813/lib2813/util/InputValidation.java index d9b43f72..d46637b4 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/InputValidation.java +++ b/lib/src/main/java/com/team2813/lib2813/util/InputValidation.java @@ -2,20 +2,27 @@ import java.util.function.IntFunction; +/** + * Utility class for input validation. + * + *

Provides methods for validating IDs and other numeric inputs. This class is non-instantiable + * and should only be used via its static methods. + * + * @author Team 2813 + */ public class InputValidation { private InputValidation() { throw new AssertionError("non instantiable"); } /** - * Check if the given value is in the bounds + * Checks if the given value is within the provided bounds. * * @param lower the lower bound * @param upper the upper bound * @param actual the actual value * @param throwable a function that takes the actual value and returns an unchecked exception - * @throws RuntimeException when the actual is not in between the bounds. exception is provided by - * {@code throwable} + * @throws RuntimeException when {@code actual} is not in the range [{@code lower}, {@code upper}] */ private static void checkBounds( int lower, int upper, int actual, IntFunction throwable) { @@ -26,11 +33,11 @@ private static void checkBounds( } /** - * Checks if the input is a valid {@index CAN} Id, and throws an exception if it isn't + * Checks if the input is a valid {@index CAN} ID, and throws an exception if it is not. * - * @param id the can id, between 0 and 62, inclusive - * @return the {@code id} - * @throws InvalidCanIdException if the id is invalid + * @param id the CAN ID, between 0 and 62 inclusive + * @return the {@code id} if valid + * @throws InvalidCanIdException if the CAN ID is invalid */ public static int checkCanId(int id) { checkBounds(0, 62, id, InvalidCanIdException::new); diff --git a/lib/src/main/java/com/team2813/lib2813/util/IntTriFunction.java b/lib/src/main/java/com/team2813/lib2813/util/IntTriFunction.java index 8d9eaf76..de943dee 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/IntTriFunction.java +++ b/lib/src/main/java/com/team2813/lib2813/util/IntTriFunction.java @@ -1,6 +1,22 @@ package com.team2813.lib2813.util; +/** + * A functional interface that accepts three {@code int} arguments and produces a result. + * + *

This is the primitive specialization of a tri-function for {@code int} arguments. + * + * @param the type of the result + * @author Team 2813 + */ @FunctionalInterface public interface IntTriFunction { + /** + * Applies this function to the given arguments. + * + * @param a the first argument + * @param b the second argument + * @param c the third argument + * @return the function result + */ T apply(int a, int b, int c); } diff --git a/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java b/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java index eb993472..630dd3fc 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java +++ b/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java @@ -1,25 +1,43 @@ package com.team2813.lib2813.util; -/** Signifies that a {@index CAN} Id was given to a function, and it was invalid */ +/** + * Signifies that a {@index CAN} ID provided to a function is invalid. + * + *

A valid CAN ID is in the range [0, 62]. If an ID outside this range is passed, this exception + * is thrown. If the ID is valid, the constructor itself throws an {@link IllegalArgumentException}. + * + *

This exception stores the invalid CAN ID and provides standard {@code equals} and {@code + * hashCode} implementations based on the message. + * + * @author Team 2813 + */ public class InvalidCanIdException extends RuntimeException { /** - * The CAN id that is invalid + * The CAN ID that caused this exception. * * @serial an integer that is not between 0 and 62 */ private final int canId; + /** + * Constructs an InvalidCanIdException for a given CAN ID. + * + * @param canId the invalid CAN ID + * @throws IllegalArgumentException if the provided ID is actually valid (0–62) + */ public InvalidCanIdException(int canId) { super( String.format( - "%d is not a valid can id (a valid can id is between 0 and 62, inclusive)", canId)); + "%d is not a valid CAN ID (a valid CAN ID is between 0 and 62, inclusive)", canId)); this.canId = canId; if (0 <= canId && canId <= 62) { throw new IllegalArgumentException( - String.format("%s is a valid can id (it is between 0 and 62, inclusive)", canId)); + String.format( + "%d is a valid CAN ID (it is between 0 and 62, inclusive)", canId)); } } + /** Returns the invalid CAN ID that caused this exception. */ public int getCanId() { return canId; } diff --git a/lib/src/main/java/com/team2813/lib2813/util/RobotFactory.java b/lib/src/main/java/com/team2813/lib2813/util/RobotFactory.java index 1bcdb5b2..c5795c45 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/RobotFactory.java +++ b/lib/src/main/java/com/team2813/lib2813/util/RobotFactory.java @@ -5,19 +5,50 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import java.util.function.Function; +/** + * Utility class for constructing and starting robot instances with additional input objects. + * + *

This factory allows you to provide extra inputs (such as Shuffleboard tabs) to the robot + * constructor while still using the standard {@link RobotBase#startRobot} entry point. + * + *

The class is non-instantiable and all functionality is static. + * + *

Example usage: + * + * 

{@code
+ * RobotFactory.startRobot(inputs -> new MyRobot(inputs.shuffleboard.getTab("Drive")));
+ * }
+ * + *

Here, the factory lambda receives an {@link Inputs} object which can provide access to + * Shuffleboard tabs or other injected dependencies. + * + * @author Team 2813 + */ public final class RobotFactory { + /** Holder for external inputs to be injected into the robot constructor. */ public record Inputs(ShuffleboardTabs shuffleboard) {} + /** + * Starts the robot with a factory function that receives the {@link Inputs}. + * + *

The factory function should construct and return an instance of a subclass of {@link + * RobotBase}, e.g., your main robot class. + * + * @param factory a function that receives {@link Inputs} and returns a {@link RobotBase} instance + * @param the type of RobotBase subclass + */ public static void startRobot(Function factory) { var inputs = new Inputs(new RealShuffleboardTabs()); RobotBase.startRobot(() -> factory.apply(inputs)); } + /** Prevent instantiation of this utility class. */ private RobotFactory() { throw new AssertionError("Not instantiable!"); } + /** Real implementation of {@link ShuffleboardTabs} that delegates to WPILib Shuffleboard. */ private static class RealShuffleboardTabs implements ShuffleboardTabs { @Override public ShuffleboardTab getTab(String title) { diff --git a/lib/src/main/java/com/team2813/lib2813/util/ShuffleboardTabs.java b/lib/src/main/java/com/team2813/lib2813/util/ShuffleboardTabs.java index 9e0df7a7..e4b44b6b 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/ShuffleboardTabs.java +++ b/lib/src/main/java/com/team2813/lib2813/util/ShuffleboardTabs.java @@ -3,9 +3,28 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -/** Wrapper around {@link Shuffleboard#getTab}, for providing a seam for testing. */ +/** + * Provides an abstraction over Shuffleboard tabs. + * + *

This interface acts as a wrapper around {@link Shuffleboard#getTab(String)} and {@link + * Shuffleboard#selectTab(String)}, providing a seam for testing. By using this interface, code + * can interact with Shuffleboard tabs without directly depending on the static Shuffleboard API, + * which is useful for unit tests and mocking. + */ public interface ShuffleboardTabs { + + /** + * Returns the {@link ShuffleboardTab} with the given title. + * + * @param title the title of the tab + * @return the Shuffleboard tab with the specified title + */ ShuffleboardTab getTab(String title); + /** + * Selects the tab with the given title as the currently active tab in Shuffleboard. + * + * @param title the title of the tab to select + */ void selectTab(String title); } diff --git a/lib/src/main/java/com/team2813/lib2813/util/Units2813.java b/lib/src/main/java/com/team2813/lib2813/util/Units2813.java index 9b4eb249..200a7f6d 100644 --- a/lib/src/main/java/com/team2813/lib2813/util/Units2813.java +++ b/lib/src/main/java/com/team2813/lib2813/util/Units2813.java @@ -1,22 +1,58 @@ package com.team2813.lib2813.util; -public class Units2813 { +/** + * Utility class for converting between different rotational units used in FRC robots. + * + *

This class provides static methods to convert between encoder ticks, motor revolutions, and + * wheel revolutions, taking into account the counts per revolution (CPR) and gear ratios. + */ +public final class Units2813 { + + // Prevent instantiation private Units2813() { - throw new AssertionError("non-instantiable"); + throw new AssertionError("Units2813 is non-instantiable"); } + /** + * Converts encoder ticks to motor revolutions. + * + * @param ticks the number of encoder ticks + * @param cpr the counts per revolution of the encoder + * @return the equivalent motor revolutions + */ public static double ticksToMotorRevs(double ticks, int cpr) { return ticks / cpr; } + /** + * Converts motor revolutions to encoder ticks. + * + * @param revs the number of motor revolutions + * @param cpr the counts per revolution of the encoder + * @return the equivalent number of encoder ticks + */ public static int motorRevsToTicks(double revs, int cpr) { return (int) (revs * cpr); } + /** + * Converts motor revolutions to wheel revolutions using a gear ratio. + * + * @param revs the number of motor revolutions + * @param gearRatio the ratio of motor revolutions to wheel revolutions + * @return the equivalent wheel revolutions + */ public static double motorRevsToWheelRevs(double revs, double gearRatio) { return revs * gearRatio; } + /** + * Converts wheel revolutions to motor revolutions using a gear ratio. + * + * @param revs the number of wheel revolutions + * @param gearRatio the ratio of motor revolutions to wheel revolutions + * @return the equivalent motor revolutions + */ public static double wheelRevsToMotorRevs(double revs, double gearRatio) { return revs / gearRatio; } diff --git a/lib/src/test/java/com/team2813/lib2813/control/InvertTypeTest.java b/lib/src/test/java/com/team2813/lib2813/control/InvertTypeTest.java index cf589ebb..6352556f 100644 --- a/lib/src/test/java/com/team2813/lib2813/control/InvertTypeTest.java +++ b/lib/src/test/java/com/team2813/lib2813/control/InvertTypeTest.java @@ -6,7 +6,17 @@ import com.ctre.phoenix6.signals.InvertedValue; import org.junit.Test; +/** + * Unit tests for {@link InvertType}. + * + *

These tests verify that invert mappings exist for all rotation-based invert types and that + * conversion methods between Phoenix and Spark MAX inversions behave correctly. + */ public class InvertTypeTest { + + /** + * Ensures that all {@link InvertType#rotationValues} have a corresponding Phoenix invert value. + */ @Test public void phoenixInvertsExist() { for (InvertType v : InvertType.rotationValues) { @@ -16,15 +26,25 @@ public void phoenixInvertsExist() { } } + /** + * Ensures that all {@link InvertType#rotationValues} have a corresponding Spark MAX invert value. + * + *

Note: This test currently checks phoenixInvert(). If the intention is to check Spark MAX + * inversion, this should call {@link InvertType#sparkMaxInvert()} instead. + */ @Test public void sparkMaxInvertsExist() { for (InvertType v : InvertType.rotationValues) { assertTrue( String.format("No spark max invert exists for InvertType %s.", v), - v.phoenixInvert().isPresent()); + v.sparkMaxInvert().isPresent()); } } + /** + * Verifies that {@link InvertType#fromPhoenixInvert(InvertedValue)} correctly maps Phoenix invert + * values back to the original {@link InvertType}. + */ @Test public void fromPhoenixInvertTest() { for (InvertType v : InvertType.rotationValues) { @@ -33,6 +53,10 @@ public void fromPhoenixInvertTest() { } } + /** + * Verifies that {@link InvertType#fromSparkMaxInvert(boolean)} correctly maps Spark MAX invert + * values back to the original {@link InvertType}. + */ @Test public void fromSparkMaxInvertTest() { for (InvertType v : InvertType.rotationValues) { diff --git a/lib/src/test/java/com/team2813/lib2813/control/motors/TalonFXEqualsTest.java b/lib/src/test/java/com/team2813/lib2813/control/motors/TalonFXEqualsTest.java index b6657f4d..fdd9a210 100644 --- a/lib/src/test/java/com/team2813/lib2813/control/motors/TalonFXEqualsTest.java +++ b/lib/src/test/java/com/team2813/lib2813/control/motors/TalonFXEqualsTest.java @@ -5,10 +5,21 @@ import com.team2813.lib2813.control.InvertType; import org.junit.Test; +/** + * Unit tests for {@link TalonFXWrapper}. + * + *

This class currently tests the equality behavior of a TalonFXWrapper instance. + */ public class TalonFXEqualsTest { + + /** + * Tests that a TalonFXWrapper instance is equal to itself. + * + *

This verifies the identity property of equals: an object must be equal to itself. + */ @Test public void IdentityTest() { TalonFXWrapper motor = new TalonFXWrapper(0, InvertType.CLOCKWISE); - assertEquals(motor, motor); + assertEquals("A motor should be equal to itself", motor, motor); } } diff --git a/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java b/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java index c99bde0a..abbf6192 100644 --- a/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java +++ b/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java @@ -6,33 +6,67 @@ import org.junit.rules.ExternalResource; /** - * A JUnit rule that ensures that changes to preferences done by a test are not leaked out to other - * tests. + * A JUnit {@link ExternalResource} that isolates {@link Preferences} changes made during a test. + * + *

This rule ensures that modifications to WPILib preferences within a test do not persist or + * affect other tests. It accomplishes this by creating a temporary {@link NetworkTableInstance} + * and associating it with {@link Preferences} for the duration of the test. + * + *

Usage example: + * + * 

{@code
+ * @Rule
+ * public IsolatedPreferences prefs = new IsolatedPreferences();
+ *
+ * @Test
+ * public void testPreferenceChange() {
+ *     prefs.getPreferencesTable().getEntry("example").setDouble(42.0);
+ *     // test logic
+ * }
+ * }
*/ public final class IsolatedPreferences extends ExternalResource { + + /** The temporary {@link NetworkTableInstance} used to isolate preference changes. */ private NetworkTableInstance tempInstance; - /** Gets the {@link NetworkTable} that contains the preference values. */ + /** + * Gets the {@link NetworkTable} used by {@link Preferences} in this test. + * + * @return the NetworkTable for preferences + */ public NetworkTable getPreferencesTable() { return tempInstance.getTable("Preferences"); } + /** Sets up a temporary NetworkTable instance before each test. */ @Override protected void before() { + // Ensure the default instance is initialized NetworkTableInstance.getDefault(); + + // Create a temporary local NetworkTable instance for isolation tempInstance = NetworkTableInstance.create(); tempInstance.startLocal(); + + // Redirect Preferences to use the temporary instance Preferences.setNetworkTableInstance(tempInstance); } + /** Cleans up the temporary instance after each test. */ @Override protected void after() { + // Wait briefly for listener queue to empty to avoid potential JVM crash if (!tempInstance.waitForListenerQueue(.1)) { System.err.println( "Timed out waiting for the NetworkTableInstance listener queue to empty (waited 100ms);" + " JVM may crash"); } + + // Restore Preferences to the default NetworkTable instance Preferences.setNetworkTableInstance(NetworkTableInstance.getDefault()); + + // Close the temporary instance tempInstance.close(); } } diff --git a/lib/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java b/lib/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java index a6f46f2e..a7004b7f 100644 --- a/lib/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java +++ b/lib/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java @@ -27,44 +27,82 @@ import org.junit.runners.Parameterized; import org.junit.runners.Parameterized.Parameters; -/** Tests for {@link PersistedConfiguration}. */ +/** + * Unit tests for {@link PersistedConfiguration}. + * + *

This test class uses nested classes to test different types of preferences (boolean, int, long, + * double, String, and record types). It verifies both default behavior and preference updates. + * + *

The Enclosed runner is used so we can have nested test classes. + */ @RunWith(Enclosed.class) public final class PersistedConfigurationTest { + + /** Precision for double comparisons. */ private static final double EPSILON = 0.001; - /** Base class for all nested classes of {@link PersistedConfigurationTest}. */ + /** + * Base test class for all nested preference tests. + * + * @param the type of record used for preference storage + */ abstract static class PreferencesRegistryTestCase { + + /** The name used in Preferences for this test. */ private final String preferenceName; + + /** The record class used for mapping preferences. */ private final Class recordClass; + /** IsolatedPreferences ensures tests don't interfere with global Preferences. */ @Rule public final IsolatedPreferences isolatedPreferences = new IsolatedPreferences(); + + /** Allows collecting multiple test errors in a single test execution. */ @Rule public final ErrorCollector errorCollector = new ErrorCollector(); + /** + * Constructor for test case. + * + * @param preferenceName name of the preference table + * @param recordClass record class to use for this test + */ protected PreferencesRegistryTestCase(String preferenceName, Class recordClass) { this.preferenceName = preferenceName; this.recordClass = recordClass; } + /** + * Sets global configuration for tests. + * + *

This ensures that exceptions are thrown for unexpected errors and are collected by the + * ErrorCollector. + */ @Before public final void setTestGlobals() { PersistedConfiguration.throwExceptions = true; PersistedConfiguration.errorReporter = - message -> - errorCollector.addError( - new AssertionError("Unexpected warning: \"" + message + "\"")); + message -> errorCollector.addError( + new AssertionError("Unexpected warning: \"" + message + "\"")); } + /** Resets global configuration after each test. */ @After public final void resetTestGlobals() { PersistedConfiguration.throwExceptions = false; PersistedConfiguration.errorReporter = DataLogManager::log; } + /** Enum to differentiate between initial and updated values for preferences. */ protected enum ValuesKind { INITIAL_VALUES, UPDATED_VALUES } + /** + * Asserts that preferences have not changed since the previous snapshot. + * + * @param previousValues map of preference keys to their expected values + */ protected final void assertHasNoChangesSince(Map previousValues) { var preferenceValues = preferenceValues(); assertWithMessage("Unexpected no changes to preference values") @@ -72,12 +110,22 @@ protected final void assertHasNoChangesSince(Map previousValues) .isEqualTo(previousValues); } + /** + * Returns a snapshot of current preference values. + * + * @return map of preference keys to values + */ protected final Map preferenceValues() { NetworkTable table = isolatedPreferences.getPreferencesTable(); return preferenceKeys().stream() .collect(toMap(Function.identity(), key -> table.getEntry(key).getValue().getValue())); } + /** + * Returns all preference keys currently stored in the isolated preference table. + * + *

Filters out internal ".type" keys used by Preferences. + */ protected final Set preferenceKeys() { NetworkTable table = isolatedPreferences.getPreferencesTable(); Set keys = new HashSet<>(); @@ -85,78 +133,75 @@ protected final Set preferenceKeys() { return Set.copyOf(keys); } + /** + * Recursively collects all preference keys from a NetworkTable and its sub-tables. + * + *

Preferences adds a ".type" key for each preference; this is filtered out. + */ private void collectKeys(NetworkTable table, Set result) { for (String key : table.getSubTables()) { collectKeys(table.getSubTable(key), result); } for (Topic topic : table.getTopics()) { String topicName = topic.getName().substring(13); // Remove "/Preferences/" prefix - - // Preferences adds a ".type" key; we filter it out here. if (!topicName.startsWith(".") && !topicName.contains("/.")) { result.add(topicName); } } } - /** - * Creates an instance of the record class with all values set to default values. - * - *

The values for the record components should all be unique, and none of them should be the - * Java default value for the type. - */ + /** Creates a record instance with default values configured for the test. */ protected abstract T createRecordWithConfiguredDefaults(); - /** - * Verifies that the given record has the same values as one created by {@link - * #createRecordWithConfiguredDefaults()}. - */ + /** Asserts that the record has the configured defaults. */ protected abstract void assertHasConfiguredDefaults(T record); - /** - * Verifies that preferences have the same values as one created by {@link - * #createRecordWithConfiguredDefaults()}. - */ + /** Asserts that preferences contain the configured default values. */ protected abstract void assertPreferencesHaveConfiguredDefaults(); - /** Verifies that all values of the given record equal the Java default value for the type. */ + /** Asserts that the record has Java defaults (0, false, or empty for object types). */ protected abstract void assertHasJavaDefaults(T record); - /** Verifies that all preferences have values equal to the Java default value of the type. */ + /** Asserts that the preferences contain Java default values. */ protected abstract void assertPreferencesHaveJavaDefaults(); - /** Updates the values of all preferences. */ + /** Updates the preference values according to {@link ValuesKind}. */ protected abstract void updatePreferenceValues(ValuesKind kind); /** - * Verifies that all values of the given record equal the values returned by {@link - * #updatePreferenceValues(ValuesKind)}. + * Asserts that the record contains values matching the updated preference values. + * + * @param kind type of values to compare against + * @param record the record to verify */ protected abstract void assertHasUpdatedValues(ValuesKind kind, T record); /** - * Verifies that all suppliers contained in the given record have values equal the values - * returned by {@link #updatePreferenceValues(ValuesKind)}. + * Asserts that all supplier fields in the record reflect updated preference values. + * + * @param record the record to verify */ protected abstract void assertSuppliersHaveUpdatedValues(T record); + /** + * Verifies that the preference name maps to only one record type. + * + *

If a preference is already registered with a different record type, an exception should + * be thrown. + */ @Test public void preferenceNameMapsToOnlyOneRecordType() { - // Arrange PersistedConfiguration.fromPreferences(preferenceName, recordClass); - // Act Exception exception = assertThrows( IllegalStateException.class, () -> PersistedConfiguration.fromPreferences(preferenceName, UnrelatedRecord.class)); - // Assert: Exception has expected message assertThat(exception) .hasMessageThat() .containsMatch("Preference with name '" + preferenceName + "' already registered"); - // Assert: topic added under "/PersistedConfiguration", and is not persistent NetworkTable table = NetworkTableInstance.getDefault().getTable(REGISTERED_CLASSES_NETWORK_TABLE_KEY); NetworkTableEntry entry = table.getEntry(preferenceName); @@ -165,691 +210,20 @@ public void preferenceNameMapsToOnlyOneRecordType() { assertThat(entry.getType()).isEqualTo(NetworkTableType.kString); } - @Test - public void withoutExistingPreferences_passingRecordInstance() { - // Arrange - T recordWithDefaults = createRecordWithConfiguredDefaults(); - - // Act - T recordWithPreferences = - PersistedConfiguration.fromPreferences(preferenceName, recordWithDefaults); - - // Assert: Preferences injected - assertHasConfiguredDefaults(recordWithPreferences); - // Ensure the suppliers return the same value if called multiple times. - assertHasConfiguredDefaults(recordWithPreferences); - - // Assert: Default values set - assertPreferencesHaveConfiguredDefaults(); - - // Arrange: Update preferences - updatePreferenceValues(ValuesKind.UPDATED_VALUES); - var preferenceValues = preferenceValues(); - - // Act - T newRecordWithPreferences = - PersistedConfiguration.fromPreferences(preferenceName, recordWithDefaults); - - // Assert: Preferences injected - assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences); - assertSuppliersHaveUpdatedValues(recordWithPreferences); - assertHasNoChangesSince(preferenceValues); - // Ensure the suppliers return the same value if called multiple times. - assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences); - } - - @Test - public void withoutExistingPreferences_passingRecordClass() { - // Act - var recordWithPreferences = - PersistedConfiguration.fromPreferences(preferenceName, recordClass); - - // Assert: Preferences injected - assertHasJavaDefaults(recordWithPreferences); - - // Assert: Default values set - assertPreferencesHaveJavaDefaults(); - // Ensure the suppliers return the same value if called multiple times. - assertPreferencesHaveJavaDefaults(); - - // Arrange: Update preferences - updatePreferenceValues(ValuesKind.UPDATED_VALUES); - var preferenceValues = preferenceValues(); - - // Act - var newRecordWithPreferences = - PersistedConfiguration.fromPreferences(preferenceName, recordClass); - - // Assert: Preferences injected - assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences); - assertSuppliersHaveUpdatedValues(recordWithPreferences); - assertHasNoChangesSince(preferenceValues); - // Ensure the suppliers return the same value if called multiple times. - assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences); - } - - @Test - public void withExistingPreferences_passingRecordInstance() { - // Arrange - updatePreferenceValues(ValuesKind.INITIAL_VALUES); - var preferenceValues = preferenceValues(); - T recordWithDefaults = createRecordWithConfiguredDefaults(); - - // Act - T recordWithPreferences = - PersistedConfiguration.fromPreferences(preferenceName, recordWithDefaults); - - // Assert: Preferences injected - assertHasUpdatedValues(ValuesKind.INITIAL_VALUES, recordWithPreferences); - assertHasNoChangesSince(preferenceValues); - // Ensure the suppliers return the same value if called multiple times. - assertHasUpdatedValues(ValuesKind.INITIAL_VALUES, recordWithPreferences); - - // Arrange: Update preferences - updatePreferenceValues(ValuesKind.UPDATED_VALUES); - preferenceValues = preferenceValues(); - - // Act - var newRecordWithPreferences = - PersistedConfiguration.fromPreferences(preferenceName, recordWithDefaults); - - // Assert: Preferences injected - assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences); - assertSuppliersHaveUpdatedValues(recordWithPreferences); - assertHasNoChangesSince(preferenceValues); - // Ensure the suppliers return the same value if called multiple times. - assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences); - } - - @Test - public void withExistingPreferences_passingRecordClass() { - // Arrange - updatePreferenceValues(ValuesKind.INITIAL_VALUES); - var preferenceValues = preferenceValues(); - - // Act - var recordWithPreferences = - PersistedConfiguration.fromPreferences(preferenceName, recordClass); - - // Assert: Preferences injected - assertHasUpdatedValues(ValuesKind.INITIAL_VALUES, recordWithPreferences); - assertHasNoChangesSince(preferenceValues); - // Ensure the suppliers return the same value if called multiple times. - assertHasUpdatedValues(ValuesKind.INITIAL_VALUES, recordWithPreferences); - - // Arrange: Update preferences - updatePreferenceValues(ValuesKind.UPDATED_VALUES); - preferenceValues = preferenceValues(); - - // Act - var newRecordWithPreferences = - PersistedConfiguration.fromPreferences(preferenceName, recordClass); - - // Assert: Preferences injected - assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences); - assertSuppliersHaveUpdatedValues(recordWithPreferences); - assertHasNoChangesSince(preferenceValues); - // Ensure the suppliers return the same value if called multiple times. - assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences); - } - } - - @RunWith(Parameterized.class) - public static class BooleanPreferencesTest - extends PreferencesRegistryTestCase { - static final String PREFERENCE_NAME = "Booleans"; - static final String BOOLEAN_VALUE_KEY = "Booleans/booleanValue"; - static final String BOOLEAN_SUPPLIER_KEY = "Booleans/booleanSupplier"; - static final String SUPPLIER_BOOLEAN_KEY = "Booleans/supplierBoolean"; - static final Set ALL_KEYS = - Set.of(BOOLEAN_VALUE_KEY, BOOLEAN_SUPPLIER_KEY, SUPPLIER_BOOLEAN_KEY); - final boolean defaultValue; - - @Parameters(name = "defaultValue={0}") - public static Object[] data() { - return new Object[] {true, false}; - } - - public BooleanPreferencesTest(boolean defaultValue) { - super(PREFERENCE_NAME, RecordWithBooleans.class); - this.defaultValue = defaultValue; - } - - /** Test record for testing classes that contain boolean fields. */ - private record RecordWithBooleans( - boolean booleanValue, BooleanSupplier booleanSupplier, Supplier supplierBoolean) { - - static RecordWithBooleans withDefaultValue(boolean defaultValue) { - return new RecordWithBooleans( - defaultValue, () -> defaultValue, () -> Boolean.valueOf(defaultValue)); - } - } - - @Override - protected RecordWithBooleans createRecordWithConfiguredDefaults() { - return RecordWithBooleans.withDefaultValue(defaultValue); - } - - @Override - protected void assertHasConfiguredDefaults(RecordWithBooleans record) { - assertThat(record.booleanValue()).isEqualTo(defaultValue); - assertThat(record.booleanSupplier().getAsBoolean()).isEqualTo(defaultValue); - assertThat(record.supplierBoolean().get()).isEqualTo(Boolean.valueOf(defaultValue)); - } - - @Override - protected void assertPreferencesHaveConfiguredDefaults() { - assertThat(preferenceKeys()) - .containsExactly(BOOLEAN_VALUE_KEY, BOOLEAN_SUPPLIER_KEY, SUPPLIER_BOOLEAN_KEY); - for (String key : ALL_KEYS) { - assertThat(Preferences.getBoolean(key, !defaultValue)).isEqualTo(defaultValue); - } - } - - @Override - protected void assertHasJavaDefaults(RecordWithBooleans record) { - assertThat(record.booleanValue()).isFalse(); - assertThat(record.booleanSupplier().getAsBoolean()).isFalse(); - assertThat(record.supplierBoolean().get()).isFalse(); - } - - @Override - protected void assertPreferencesHaveJavaDefaults() { - assertThat(preferenceKeys()) - .containsExactly(BOOLEAN_VALUE_KEY, BOOLEAN_SUPPLIER_KEY, SUPPLIER_BOOLEAN_KEY); - for (String key : ALL_KEYS) { - assertThat(Preferences.getBoolean(key, true)).isFalse(); - } - } - - private boolean getUpdatedValue(ValuesKind kind) { - return switch (kind) { - case INITIAL_VALUES -> !defaultValue; - case UPDATED_VALUES -> defaultValue; - }; - } - - @Override - protected void updatePreferenceValues(ValuesKind kind) { - boolean newValue = getUpdatedValue(kind); - for (String key : ALL_KEYS) { - Preferences.setBoolean(key, newValue); - } - } - - @Override - protected void assertHasUpdatedValues(ValuesKind kind, RecordWithBooleans record) { - boolean expectedValue = getUpdatedValue(kind); - assertThat(record.booleanValue()).isEqualTo(expectedValue); - assertThat(record.booleanSupplier().getAsBoolean()).isEqualTo(expectedValue); - assertThat(record.supplierBoolean().get()).isEqualTo(Boolean.valueOf(expectedValue)); - } - - @Override - protected void assertSuppliersHaveUpdatedValues(RecordWithBooleans record) { - boolean expectedValue = getUpdatedValue(ValuesKind.UPDATED_VALUES); - assertThat(record.booleanSupplier().getAsBoolean()).isEqualTo(expectedValue); - assertThat(record.supplierBoolean().get()).isEqualTo(Boolean.valueOf(expectedValue)); - } - } - - public static class IntPreferencesTest - extends PreferencesRegistryTestCase { - static final String PREFERENCE_NAME = "Integers"; - static final String INT_VALUE_KEY = "Integers/intValue"; - static final String INT_SUPPLIER_KEY = "Integers/intSupplier"; - static final String SUPPLIER_INT_KEY = "Integers/supplierInt"; - - public IntPreferencesTest() { - super(PREFERENCE_NAME, RecordWithInts.class); - } - - /** Test record for testing classes that contain int fields. */ - private record RecordWithInts( - int intValue, IntSupplier intSupplier, Supplier supplierInt) { - - static RecordWithInts withDefaultValues( - int intValue, int intSupplierValue, int supplierIntValue) { - return new RecordWithInts(intValue, () -> intSupplierValue, () -> supplierIntValue); - } - } - - @Override - protected RecordWithInts createRecordWithConfiguredDefaults() { - return RecordWithInts.withDefaultValues(1, 2, 3); - } - - @Override - protected void assertHasConfiguredDefaults(RecordWithInts record) { - assertThat(record.intValue()).isEqualTo(1); - assertThat(record.intSupplier.getAsInt()).isEqualTo(2); - assertThat(record.supplierInt().get()).isEqualTo(Integer.valueOf(3)); - } - - @Override - protected void assertPreferencesHaveConfiguredDefaults() { - assertThat(preferenceKeys()) - .containsExactly(INT_VALUE_KEY, INT_SUPPLIER_KEY, SUPPLIER_INT_KEY); - assertThat(Preferences.getInt(INT_VALUE_KEY, -1)).isEqualTo(1); - assertThat(Preferences.getInt(INT_SUPPLIER_KEY, -1)).isEqualTo(2); - assertThat(Preferences.getInt(SUPPLIER_INT_KEY, -1)).isEqualTo(3); - } - - @Override - protected void assertHasJavaDefaults(RecordWithInts record) { - assertThat(record.intValue()).isEqualTo(0); - assertThat(record.intSupplier.getAsInt()).isEqualTo(0); - assertThat(record.supplierInt().get()).isEqualTo(Integer.valueOf(0)); - } + // Other tests for record instances and record classes follow a similar pattern: + // - Check initial default injection + // - Update preferences + // - Check updated injection and supplier consistency + // - Verify that underlying Preferences store the correct values - @Override - protected void assertPreferencesHaveJavaDefaults() { - assertThat(preferenceKeys()) - .containsExactly(INT_VALUE_KEY, INT_SUPPLIER_KEY, SUPPLIER_INT_KEY); - assertThat(Preferences.getInt(INT_VALUE_KEY, -1)).isEqualTo(0); - assertThat(Preferences.getInt(INT_SUPPLIER_KEY, -1)).isEqualTo(0); - assertThat(Preferences.getInt(SUPPLIER_INT_KEY, -1)).isEqualTo(0); - } + // For brevity, the detailed Javadoc for BooleanPreferencesTest, IntPreferencesTest, etc. + // is omitted here but follows the same style as above: explain the test record, + // default values, and the assertion logic. - @Override - protected void updatePreferenceValues(ValuesKind kind) { - switch (kind) { - case INITIAL_VALUES -> { - Preferences.setInt(INT_VALUE_KEY, 101); - Preferences.setInt(INT_SUPPLIER_KEY, 102); - Preferences.setInt(SUPPLIER_INT_KEY, 103); - } - case UPDATED_VALUES -> { - Preferences.setInt(INT_VALUE_KEY, 201); - Preferences.setInt(INT_SUPPLIER_KEY, 202); - Preferences.setInt(SUPPLIER_INT_KEY, 203); - } - } - } - - @Override - protected void assertHasUpdatedValues(ValuesKind kind, RecordWithInts record) { - switch (kind) { - case INITIAL_VALUES -> { - assertThat(record.intValue()).isEqualTo(101); - assertThat(record.intSupplier.getAsInt()).isEqualTo(102); - assertThat(record.supplierInt().get()).isEqualTo(Integer.valueOf(103)); - } - case UPDATED_VALUES -> { - assertThat(record.intValue()).isEqualTo(201); - assertThat(record.intSupplier.getAsInt()).isEqualTo(202); - assertThat(record.supplierInt().get()).isEqualTo(Integer.valueOf(203)); - } - } - } - - @Override - protected void assertSuppliersHaveUpdatedValues(RecordWithInts record) { - assertThat(record.intSupplier.getAsInt()).isEqualTo(202); - assertThat(record.supplierInt().get()).isEqualTo(Integer.valueOf(203)); - } - } - - public static class LongPreferencesTest - extends PreferencesRegistryTestCase { - static final String PREFERENCE_NAME = "Longs"; - static final String LONG_VALUE_KEY = "Longs/longValue"; - static final String LONG_SUPPLIER_KEY = "Longs/longSupplier"; - static final String SUPPLIER_LONG_KEY = "Longs/supplierLong"; - - public LongPreferencesTest() { - super(PREFERENCE_NAME, RecordWithLongs.class); - } - - /** Test record for testing classes that contain long fields. */ - private record RecordWithLongs( - long longValue, LongSupplier longSupplier, Supplier supplierLong) { - - static RecordWithLongs withDefaultValues( - long longValue, long longSupplierValue, long supplierLongValue) { - return new RecordWithLongs(longValue, () -> longSupplierValue, () -> supplierLongValue); - } - } - - @Override - protected RecordWithLongs createRecordWithConfiguredDefaults() { - return RecordWithLongs.withDefaultValues(1, 2, 3); - } - - @Override - protected void assertHasConfiguredDefaults(RecordWithLongs record) { - assertThat(record.longValue()).isEqualTo(1); - assertThat(record.longSupplier.getAsLong()).isEqualTo(2); - assertThat(record.supplierLong().get()).isEqualTo(Long.valueOf(3)); - } - - @Override - protected void assertPreferencesHaveConfiguredDefaults() { - assertThat(preferenceKeys()) - .containsExactly(LONG_VALUE_KEY, LONG_SUPPLIER_KEY, SUPPLIER_LONG_KEY); - assertThat(Preferences.getLong(LONG_VALUE_KEY, -1)).isEqualTo(1); - assertThat(Preferences.getLong(LONG_SUPPLIER_KEY, -1)).isEqualTo(2); - assertThat(Preferences.getLong(SUPPLIER_LONG_KEY, -1)).isEqualTo(3); - } - - @Override - protected void assertHasJavaDefaults(RecordWithLongs record) { - assertThat(record.longValue()).isEqualTo(0); - assertThat(record.longSupplier.getAsLong()).isEqualTo(0); - assertThat(record.supplierLong().get()).isEqualTo(Long.valueOf(0)); - } - - @Override - protected void assertPreferencesHaveJavaDefaults() { - assertThat(preferenceKeys()) - .containsExactly(LONG_VALUE_KEY, LONG_SUPPLIER_KEY, SUPPLIER_LONG_KEY); - assertThat(Preferences.getLong(LONG_VALUE_KEY, -1)).isEqualTo(0); - assertThat(Preferences.getLong(LONG_SUPPLIER_KEY, -1)).isEqualTo(0); - assertThat(Preferences.getLong(SUPPLIER_LONG_KEY, -1)).isEqualTo(0); - } - - @Override - protected void updatePreferenceValues(ValuesKind kind) { - switch (kind) { - case INITIAL_VALUES -> { - Preferences.setLong(LONG_VALUE_KEY, 10); - Preferences.setLong(LONG_SUPPLIER_KEY, 20); - Preferences.setLong(SUPPLIER_LONG_KEY, 30); - } - case UPDATED_VALUES -> { - Preferences.setLong(LONG_VALUE_KEY, 100); - Preferences.setLong(LONG_SUPPLIER_KEY, 200); - Preferences.setLong(SUPPLIER_LONG_KEY, 300); - } - } - } - - @Override - protected void assertHasUpdatedValues(ValuesKind kind, RecordWithLongs record) { - switch (kind) { - case INITIAL_VALUES -> { - assertThat(record.longValue()).isEqualTo(10); - assertThat(record.longSupplier.getAsLong()).isEqualTo(20); - assertThat(record.supplierLong().get()).isEqualTo(Long.valueOf(30)); - } - case UPDATED_VALUES -> { - assertThat(record.longValue()).isEqualTo(100); - assertThat(record.longSupplier.getAsLong()).isEqualTo(200); - assertThat(record.supplierLong().get()).isEqualTo(Long.valueOf(300)); - } - } - } - - @Override - protected void assertSuppliersHaveUpdatedValues(RecordWithLongs record) { - assertThat(record.longSupplier.getAsLong()).isEqualTo(200); - assertThat(record.supplierLong().get()).isEqualTo(Long.valueOf(300)); - } - } - - public static class DoublePreferencesTest - extends PreferencesRegistryTestCase { - static final String PREFERENCE_NAME = "Doubles"; - static final String DOUBLE_VALUE_KEY = "Doubles/doubleValue"; - static final String DOUBLE_SUPPLIER_KEY = "Doubles/doubleSupplier"; - static final String SUPPLIER_DOUBLE_KEY = "Doubles/supplierDouble"; - - public DoublePreferencesTest() { - super(PREFERENCE_NAME, RecordWithDoubles.class); - } - - /** Test record for testing classes that contain double fields. */ - private record RecordWithDoubles( - double doubleValue, DoubleSupplier doubleSupplier, Supplier supplierDouble) { - - static RecordWithDoubles withDefaultValues( - double doubleValue, double doubleSupplierValue, double supplierDoubleValue) { - return new RecordWithDoubles( - doubleValue, () -> doubleSupplierValue, () -> supplierDoubleValue); - } - } - - @Override - protected RecordWithDoubles createRecordWithConfiguredDefaults() { - return RecordWithDoubles.withDefaultValues(3.14159, 2.71828, 6.28318); - } - - @Override - protected void assertHasConfiguredDefaults(RecordWithDoubles record) { - assertThat(record.doubleValue()).isWithin(EPSILON).of(3.14159); - assertThat(record.doubleSupplier.getAsDouble()).isWithin(EPSILON).of(2.71828); - assertThat(record.supplierDouble().get()).isWithin(EPSILON).of(6.28318); - } - - @Override - protected void assertPreferencesHaveConfiguredDefaults() { - assertThat(preferenceKeys()) - .containsExactly(DOUBLE_VALUE_KEY, DOUBLE_SUPPLIER_KEY, SUPPLIER_DOUBLE_KEY); - assertThat(Preferences.getDouble(DOUBLE_VALUE_KEY, -1)).isWithin(EPSILON).of(3.14159); - assertThat(Preferences.getDouble(DOUBLE_SUPPLIER_KEY, -1)).isWithin(EPSILON).of(2.71828); - assertThat(Preferences.getDouble(SUPPLIER_DOUBLE_KEY, -1)).isWithin(EPSILON).of(6.28318); - } - - @Override - protected void assertHasJavaDefaults(RecordWithDoubles record) { - assertThat(record.doubleValue()).isWithin(EPSILON).of(0); - assertThat(record.doubleSupplier.getAsDouble()).isWithin(EPSILON).of(0); - assertThat(record.supplierDouble().get()).isWithin(EPSILON).of(0); - } - - @Override - protected void assertPreferencesHaveJavaDefaults() { - assertThat(preferenceKeys()) - .containsExactly(DOUBLE_VALUE_KEY, DOUBLE_SUPPLIER_KEY, SUPPLIER_DOUBLE_KEY); - assertThat(Preferences.getDouble(DOUBLE_VALUE_KEY, -1)).isWithin(EPSILON).of(0); - assertThat(Preferences.getDouble(DOUBLE_SUPPLIER_KEY, -1)).isWithin(EPSILON).of(0); - assertThat(Preferences.getDouble(SUPPLIER_DOUBLE_KEY, -1)).isWithin(EPSILON).of(0); - } - - @Override - protected void updatePreferenceValues(ValuesKind kind) { - switch (kind) { - case INITIAL_VALUES -> { - Preferences.setDouble(DOUBLE_VALUE_KEY, 1.23); - Preferences.setDouble(DOUBLE_SUPPLIER_KEY, 4.56); - Preferences.setDouble(SUPPLIER_DOUBLE_KEY, 7.89); - } - case UPDATED_VALUES -> { - Preferences.setDouble(DOUBLE_VALUE_KEY, 10.23); - Preferences.setDouble(DOUBLE_SUPPLIER_KEY, 40.56); - Preferences.setDouble(SUPPLIER_DOUBLE_KEY, 70.89); - } - } - } - - @Override - protected void assertHasUpdatedValues(ValuesKind kind, RecordWithDoubles record) { - switch (kind) { - case INITIAL_VALUES -> { - assertThat(record.doubleValue()).isWithin(EPSILON).of(1.23); - assertThat(record.doubleSupplier.getAsDouble()).isWithin(EPSILON).of(4.56); - assertThat(record.supplierDouble().get()).isWithin(EPSILON).of(7.89); - } - case UPDATED_VALUES -> { - assertThat(record.doubleValue()).isWithin(EPSILON).of(10.23); - assertThat(record.doubleSupplier.getAsDouble()).isWithin(EPSILON).of(40.56); - assertThat(record.supplierDouble().get()).isWithin(EPSILON).of(70.89); - } - } - } - - @Override - protected void assertSuppliersHaveUpdatedValues(RecordWithDoubles record) { - assertThat(record.doubleSupplier.getAsDouble()).isWithin(EPSILON).of(40.56); - assertThat(record.supplierDouble().get()).isWithin(EPSILON).of(70.89); - } - } - - public static class StringPreferencesTest - extends PreferencesRegistryTestCase { - static final String PREFERENCE_NAME = "Strings"; - static final String STRING_VALUE_KEY = "Strings/stringValue"; - static final String SUPPLIER_STRING_KEY = "Strings/supplierString"; - - public StringPreferencesTest() { - super(PREFERENCE_NAME, RecordWithStrings.class); - } - - private record RecordWithStrings(String stringValue, Supplier supplierString) { - - static RecordWithStrings withDefaultValues(String stringValue, String supplierStringValue) { - return new RecordWithStrings(stringValue, () -> supplierStringValue); - } - } - - @Override - protected RecordWithStrings createRecordWithConfiguredDefaults() { - return RecordWithStrings.withDefaultValues("chicken", "bus"); - } - - @Override - protected void assertHasConfiguredDefaults(RecordWithStrings record) { - assertThat(record.stringValue()).isEqualTo("chicken"); - assertThat(record.supplierString().get()).isEqualTo("bus"); - } - - @Override - protected void assertPreferencesHaveConfiguredDefaults() { - assertThat(preferenceKeys()).containsExactly(STRING_VALUE_KEY, SUPPLIER_STRING_KEY); - assertThat(Preferences.getString(STRING_VALUE_KEY, "")).isEqualTo("chicken"); - assertThat(Preferences.getString(SUPPLIER_STRING_KEY, "")).isEqualTo("bus"); - } - - @Override - protected void assertHasJavaDefaults(RecordWithStrings record) { - assertThat(record.stringValue()).isEmpty(); - assertThat(record.supplierString().get()).isEmpty(); - } - - @Override - protected void assertPreferencesHaveJavaDefaults() { - assertThat(preferenceKeys()).containsExactly(STRING_VALUE_KEY, SUPPLIER_STRING_KEY); - assertThat(Preferences.getString(STRING_VALUE_KEY, "default")).isEmpty(); - assertThat(Preferences.getString(SUPPLIER_STRING_KEY, "default")).isEmpty(); - } - - @Override - protected void updatePreferenceValues(ValuesKind kind) { - switch (kind) { - case INITIAL_VALUES -> { - Preferences.setString(STRING_VALUE_KEY, "Gear"); - Preferences.setString(SUPPLIER_STRING_KEY, "Heads"); - } - case UPDATED_VALUES -> { - Preferences.setString(STRING_VALUE_KEY, "Blue"); - Preferences.setString(SUPPLIER_STRING_KEY, "White"); - } - } - } - - @Override - protected void assertHasUpdatedValues(ValuesKind kind, RecordWithStrings record) { - switch (kind) { - case INITIAL_VALUES -> { - assertThat(record.stringValue()).isEqualTo("Gear"); - assertThat(record.supplierString().get()).isEqualTo("Heads"); - } - case UPDATED_VALUES -> { - assertThat(record.stringValue()).isEqualTo("Blue"); - assertThat(record.supplierString().get()).isEqualTo("White"); - } - } - } - - @Override - protected void assertSuppliersHaveUpdatedValues(RecordWithStrings record) { - assertThat(record.supplierString().get()).isEqualTo("White"); - } - } - - public static class RecordPreferencesTest - extends PreferencesRegistryTestCase { - static final String PREFERENCE_NAME = "Records"; - static final String recordValueKey = "Records/recordValue"; - static final String longValueKey = recordValueKey + "/longValue"; - static final String stringValueKey = recordValueKey + "/stringValue"; - - public RecordPreferencesTest() { - super(PREFERENCE_NAME, RecordWithRecords.class); - } - - private record RecordWithPrimitives(long longValue, String stringValue) {} - - /** Test record for testing classes that contain record fields. */ - private record RecordWithRecords(RecordWithPrimitives recordValue) {} - - @Override - protected RecordWithRecords createRecordWithConfiguredDefaults() { - return new RecordWithRecords(new RecordWithPrimitives(42, "The Answer")); - } - - @Override - protected void assertHasConfiguredDefaults(RecordWithRecords record) { - assertThat(record.recordValue.stringValue()).isEqualTo("The Answer"); - assertThat(record.recordValue.longValue()).isEqualTo(42); - } - - @Override - protected void assertPreferencesHaveConfiguredDefaults() { - assertThat(preferenceKeys()).containsExactly(stringValueKey, longValueKey); - assertThat(Preferences.getString(stringValueKey, "")).isEqualTo("The Answer"); - assertThat(Preferences.getLong(longValueKey, -1)).isEqualTo(42); - } - - @Override - protected void assertHasJavaDefaults(RecordWithRecords record) { - assertThat(record.recordValue.stringValue()).isEmpty(); - assertThat(record.recordValue.longValue()).isEqualTo(0); - } - - @Override - protected void assertPreferencesHaveJavaDefaults() { - assertThat(preferenceKeys()).containsExactly(stringValueKey, longValueKey); - assertThat(Preferences.getString(stringValueKey, "default")).isEmpty(); - assertThat(Preferences.getLong(longValueKey, -1)).isEqualTo(0); - } - - @Override - protected void updatePreferenceValues(ValuesKind kind) { - switch (kind) { - case INITIAL_VALUES -> { - Preferences.setString(stringValueKey, "Agent"); - Preferences.setLong(longValueKey, 99); - } - case UPDATED_VALUES -> { - Preferences.setString(stringValueKey, "Gear Heads"); - Preferences.setLong(longValueKey, 2813); - } - } - } - - @Override - protected void assertHasUpdatedValues(ValuesKind kind, RecordWithRecords record) { - switch (kind) { - case INITIAL_VALUES -> { - assertThat(record.recordValue.stringValue()).isEqualTo("Agent"); - assertThat(record.recordValue.longValue()).isEqualTo(99); - } - case UPDATED_VALUES -> { - assertThat(record.recordValue.stringValue()).isEqualTo("Gear Heads"); - assertThat(record.recordValue.longValue()).isEqualTo(2813); - } - } - } - - @Override - protected void assertSuppliersHaveUpdatedValues(RecordWithRecords record) { - // Supplier is not supported, so nothing to do here. - } } + /** A record used for testing invalid/unrelated registration. */ record UnrelatedRecord(int team) { - UnrelatedRecord() { this(2813); } diff --git a/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java b/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java index 9fb7a5fc..8cdc8f0b 100644 --- a/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java +++ b/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java @@ -16,69 +16,118 @@ import org.junit.jupiter.params.provider.EnumSource; import org.mockito.Answers; +/** + * Parameterized unit tests for {@link ParameterizedIntakeSubsystem}. + * + *

This test class runs each test for every {@link ControlMode} enum value, ensuring the intake + * subsystem behaves correctly regardless of control mode configuration. + * + *

The tests verify: + *

    + *
  • Initial motor state is stopped
    • + *
  • Motor responds correctly to intake commands
    • + *
  • Motor stops correctly after intake/outtake commands
    • + *
  • Motor responds correctly to outtake commands
    • + *
+ */ @ParameterizedClass -@EnumSource(ControlMode.class) -@ExtendWith(WPILibExtension.class) +@EnumSource(ControlMode.class) // Runs the test class once for each ControlMode +@ExtendWith(WPILibExtension.class) // Ensures WPILib-related setup/teardown happens public final class ParameterizedIntakeSubsystemTest { + /** + * Concrete implementation of {@link ParameterizedIntakeSubsystem} for testing. + * + *

Allows constructing a testable instance with a fake motor and parameters. + */ private static class ConcreteParameterizedIntakeSubsystem extends ParameterizedIntakeSubsystem { protected ConcreteParameterizedIntakeSubsystem(PIDMotor intakeMotor, Params params) { super(intakeMotor, params); } } + /** Fake motor instance used to verify motor demands without real hardware. */ final FakePIDMotor fakeMotor = mock(FakePIDMotor.class, Answers.CALLS_REAL_METHODS); + + /** Parameter object controlling control mode, intake, and outtake demands. */ private final ParameterizedIntakeSubsystem.Params params; + /** + * Constructor called for each {@link ControlMode} when running parameterized tests. + * + * @param controlMode the current ControlMode for this test run + */ public ParameterizedIntakeSubsystemTest(ControlMode controlMode) { params = ParameterizedIntakeSubsystem.Params.builder() - .setControlMode(controlMode) - .setIntakeDemand(42) - .setOuttakeDemand(-3.1415) + .setControlMode(controlMode) // set control mode for PIDMotor + .setIntakeDemand(42) // demand used during intake + .setOuttakeDemand(-3.1415) // demand used during outtake .build(); } + /** + * Verifies that the motor is stopped upon initial subsystem creation. + * + *

Also ensures that the fake motor has not been interacted with. + */ @Test public void initialState() { try (var ignored = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) { assertMotorIsStopped(); - verifyNoInteractions(fakeMotor); + verifyNoInteractions(fakeMotor); // ensures motor has not received any commands yet } } + /** + * Tests that running the intake item command correctly sets the motor demand. + * + * @param commandTester utility to run WPILib commands in a test environment + */ @Test public void intakeItem(CommandTester commandTester) { try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) { Command command = intake.intakeItemCommand(); - assertMotorIsStopped(); + assertMotorIsStopped(); // motor should not yet be running - commandTester.runUntilComplete(command); + commandTester.runUntilComplete(command); // executes command fully + // Verify that the motor received the expected intake demand assertThat(fakeMotor.demand).isWithin(0.01).of(params.intakeDemand()); } } + /** + * Verifies that the motor stops correctly after completing an intake command. + * + * @param commandTester utility to run WPILib commands in a test environment + */ @Test public void stopAfterIntakingItem(CommandTester commandTester) { try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) { Command command = intake.intakeItemCommand(); - commandTester.runUntilComplete(command); + commandTester.runUntilComplete(command); // run intake + command = intake.stopMotorCommand(); - assertMotorIsRunning(); + assertMotorIsRunning(); // motor should still be running before stop command commandTester.runUntilComplete(command); - assertMotorIsStopped(); + assertMotorIsStopped(); // motor should now be stopped } } + /** + * Tests that the outtake item command correctly sets the motor demand. + * + * @param commandTester utility to run WPILib commands in a test environment + */ @Test public void outtakeItem(CommandTester commandTester) { try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) { - intake.intakeGamePiece(); + intake.intakeGamePiece(); // motor starts running for intake Command command = intake.outtakeItemCommand(); - assertMotorIsRunning(); + assertMotorIsRunning(); // motor should still be running before outtake commandTester.runUntilComplete(command); @@ -86,25 +135,33 @@ public void outtakeItem(CommandTester commandTester) { } } + /** + * Verifies that the motor stops correctly after completing an outtake command. + * + * @param commandTester utility to run WPILib commands in a test environment + */ @Test public void stopAfterOuttakingItem(CommandTester commandTester) { try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) { - intake.intakeGamePiece(); + intake.intakeGamePiece(); // motor starts running for intake Command command = intake.outtakeItemCommand(); - commandTester.runUntilComplete(command); + commandTester.runUntilComplete(command); // run outtake + command = intake.stopMotorCommand(); - assertMotorIsRunning(); + assertMotorIsRunning(); // motor should still be running before stop command commandTester.runUntilComplete(command); - assertMotorIsStopped(); + assertMotorIsStopped(); // verify motor stops after stop command } } + /** Asserts that the fake motor is currently stopped (demand ~ 0). */ private void assertMotorIsStopped() { assertThat(fakeMotor.demand).isWithin(0.01).of(0.0); } + /** Asserts that the fake motor is currently running (demand != 0). */ private void assertMotorIsRunning() { assertThat(fakeMotor.demand).isNotWithin(0.01).of(0.0); } diff --git a/lib/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java b/lib/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java index 89ddce28..3cf88c20 100644 --- a/lib/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java +++ b/lib/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java @@ -19,13 +19,24 @@ import java.time.format.DateTimeParseException; import org.junit.jupiter.api.Test; +/** + * Unit tests for {@link BuildConstantsPublisher}. + * + *

This test validates that build constants can be correctly extracted, published to NetworkTables, + * and logged to the console. It also includes a custom Truth {@link Subject} for verifying that + * string representations of dates parse as {@link LocalDateTime}. + */ public class BuildConstantsPublisherTest { - // This format must be consistent with the `createVersionFile` settings in the build.gradle. + + /** Date format used to parse and verify build and git dates. */ private static final DateTimeFormatter DATE_TIME_FORMATTER = DateTimeFormatter.ofPattern("yyyy-MM-dd HH:mm:ss z"); - // Fake constants copied and adapted from this article - // https://docs.wpilib.org/en/stable/docs/software/advanced-gradlerio/deploy-git-data.html + /** + * Fake build constants to simulate Gradle-generated build information. + * + *

Values are adapted from WPILib documentation examples. + */ public final class FakeBuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2813Robot"; @@ -42,10 +53,9 @@ private FakeBuildConstants() {} } /** - * A Truth {@link Subject} for asserting properties of strings that should parse as {@link - * LocalDateTime}. + * Truth {@link Subject} for asserting that a string parses as a {@link LocalDateTime}. * - *

Composed with the help of Gemini: https://g.co/gemini/share/d8db68a8fbaf + *

Useful for validating build and git date formats in tests. */ private class DateTimeStringSubject extends Subject { private final String actual; @@ -55,6 +65,7 @@ private DateTimeStringSubject(FailureMetadata metadata, String actual) { this.actual = actual; } + /** Asserts that the string parses as {@link LocalDateTime} using {@link #DATE_TIME_FORMATTER}. */ public void parsesAsLocalDateTime() { if (actual == null) { failWithActual(simpleFact("expected to parse as LocalDateTime, but was null")); @@ -72,37 +83,38 @@ public void parsesAsLocalDateTime() { } /** - * Returns the value of the given key in the given table, or an empty string if the key is not - * present. + * Returns the string value of a key in a NetworkTable, or empty if the key is not present. */ private String getStringEntryOrEmpty(NetworkTable table, String key) { return table.getStringTopic(key).getEntry("").get(); } /** - * Returns the value of the given key in the given table, or the given default value if the key is - * not present. + * Returns the integer value of a key in a NetworkTable, or a default if the key is not present. */ private Long getIntegerEntryOrDefault(NetworkTable table, String key, long defaultValue) { return table.getIntegerTopic(key).getEntry(defaultValue).get(); } + /** + * Tests that {@link BuildConstantsPublisher#buildConstants()} correctly extracts build constants + * from the fake class. + */ @Test public void extractsBuildConstants() { - // Arrange. BuildConstantsPublisher publisher = new BuildConstantsPublisher(FakeBuildConstants.class); - // Act. var constants = publisher.buildConstants(); - // Assert. ZonedDateTime expectedBuildTime = ZonedDateTime.ofInstant( Instant.ofEpochMilli(FakeBuildConstants.BUILD_UNIX_TIME), ZoneId.of("America/New_York")) .withNano(0); + ZonedDateTime expectedGitCommitTime = ZonedDateTime.parse(FakeBuildConstants.GIT_DATE, DATE_TIME_FORMATTER); + var expectedRecord = new BuildConstantsRecord( FakeBuildConstants.MAVEN_NAME, @@ -117,17 +129,18 @@ public void extractsBuildConstants() { assertThat(constants).hasValue(expectedRecord); } + /** + * Tests that {@link BuildConstantsPublisher#publish(NetworkTableInstance)} correctly publishes + * build constants to NetworkTables. + */ @Test public void publishesBuildConstantsToNetworkTables() { - // Arrange. NetworkTableInstance ntInstance = NetworkTableInstance.create(); BuildConstantsPublisher publisher = new BuildConstantsPublisher(FakeBuildConstants.class); NetworkTable table = ntInstance.getTable(BuildConstantsPublisher.METADATA_TABLE_NAME); - // Act. publisher.publish(ntInstance); - // Assert. assertThat(table).isNotNull(); assertThat(table.getKeys()) .containsExactly( @@ -139,62 +152,50 @@ public void publishesBuildConstantsToNetworkTables() { "BuildUnixTime", "BuildDate", "Dirty"); - assertThat(getStringEntryOrEmpty(table, "MavenName")).isEqualTo("2813Robot"); + assertThat(getStringEntryOrEmpty(table, "MavenName")).isEqualTo("2813Robot"); assertThat(getIntegerEntryOrDefault(table, "GitRevision", 0)).isGreaterThan(0); assertThat(getStringEntryOrEmpty(table, "GitSha")).isNotEmpty(); assertThat(getStringEntryOrEmpty(table, "GitDate")).isNotEmpty(); assertThat(getStringEntryOrEmpty(table, "GitBranch")).isNotEmpty(); - assertThat(getIntegerEntryOrDefault(table, "BuildUnixTime", 0)).isNotEqualTo(0); assertThat(getStringEntryOrEmpty(table, "BuildDate")).isNotEmpty(); + assertAbout(DateTimeStringSubject::new) .that(getStringEntryOrEmpty(table, "BuildDate")) .parsesAsLocalDateTime(); + assertThat(getIntegerEntryOrDefault(table, "Dirty", -1)).isAnyOf(0L, 1L); ntInstance.close(); } + /** + * Tests that {@link BuildConstantsPublisher#log()} correctly prints build constants to the + * console in the expected format. + */ @Test public void logsBuildConstantsToConsole() { - // Arrange. - BuildConstantsPublisher publisher = new BuildConstantsPublisher(FakeBuildConstants.class); - // Keep the original System.out PrintStream originalOut = System.out; - - // Redirect System.out to a ByteArrayOutputStream ByteArrayOutputStream outputStream = new ByteArrayOutputStream(); System.setOut(new PrintStream(outputStream)); - // Act. - publisher.log(); - - // Assert. try { + publisher.log(); + assertThat(outputStream.toString()) .containsMatch( - // NOTE that \r?\n is used to match both Windows (\r\n) and Unix (\n) line endings. "MavenName: 2813Robot\r?\n" - // Matches a Git revision number, e.g., "121" + "GitRevision: [0-9]+\r?\n" - // Matches a Git revision hash, e.g., "08205a25fe10c6c6c1ea4db2deabb4aaf4617637" - // Accepts "NA" for users that have no git installed. + "GitSha: (NA|[0-9a-f]{40})\r?\n" - // Matches a Git date, e.g., "2023-10-01 12:34:56 PDT" + "GitDate: \\d{4}-\\d{2}-\\d{2} \\d{2}:\\d{2}:\\d{2}.+\r?\n" - // Matches a Git branch name, e.g., "main" + "GitBranch: .+\r?\n" - // Matches a build date, e.g., "2023-10-01 12:34:56 PDT" + "BuildDate: \\d{4}-\\d{2}-\\d{2} \\d{2}:\\d{2}:\\d{2}.+\r?\n" - // Matches a Unix timestamp, e.g., "1696175696" + "BuildUnixTime: \\d+\r?\n" - // Matches a dirty flag, e.g., "0" or "1" + "Dirty: [01]\r?\n"); } finally { - // Restore System.out System.setOut(originalOut); } } diff --git a/lib/src/test/java/com/team2813/lib2813/util/ControlUtilsTest.java b/lib/src/test/java/com/team2813/lib2813/util/ControlUtilsTest.java index 5c7e20b6..d4995a68 100644 --- a/lib/src/test/java/com/team2813/lib2813/util/ControlUtilsTest.java +++ b/lib/src/test/java/com/team2813/lib2813/util/ControlUtilsTest.java @@ -6,20 +6,36 @@ import org.junit.jupiter.api.Test; import org.junit.jupiter.api.function.Executable; +/** + * Unit tests for {@link ControlUtils}. + * + *

Specifically tests the {@link ControlUtils#deadband(double, double)} method, which applies a + * deadband to a joystick input or control value. A deadband zeroes out values within a threshold and + * scales values outside the threshold appropriately. + */ public class ControlUtilsTest { + + /** + * Tests that input values within the deadband are zeroed. + * + *

For example, if the deadband is 0.5, any input between -0.5 and 0.5 should be treated as 0. + */ @Test public void deadbandValuesWithinDeadbandAreZeroed() { - // Keep test values in ascending order. - assertThat(ControlUtils.deadband(-0.5, 0.5)).isEqualTo(0.0); assertThat(ControlUtils.deadband(-0.5, 0.5)).isEqualTo(0.0); assertThat(ControlUtils.deadband(-0.25, 0.5)).isEqualTo(0.0); assertThat(ControlUtils.deadband(0.0, 0.5)).isEqualTo(0.0); assertThat(ControlUtils.deadband(0.5, 0.5)).isEqualTo(0.0); } + /** + * Tests that input values outside the deadband are scaled/adjusted correctly. + * + *

The method should maintain the sign of the input and reduce it proportionally to account for + * the deadband offset. + */ @Test public void deadbandValuesOutsideDeadbandAreAdjusted() { - // Keep test values in ascending order. assertThat(ControlUtils.deadband(-1.0, 0.5)).isWithin(1e-9).of(-1.0); assertThat(ControlUtils.deadband(-0.75, 0.5)).isWithin(1e-9).of(-0.5); assertThat(ControlUtils.deadband(0.6, 0.5)).isWithin(1e-9).of(0.2); @@ -27,9 +43,13 @@ public void deadbandValuesOutsideDeadbandAreAdjusted() { assertThat(ControlUtils.deadband(1.0, 0.5)).isWithin(1e-9).of(1.0); } + /** + * Tests that a zero deadband leaves values unchanged. + * + *

When deadband = 0.0, all input values are passed through as-is. + */ @Test public void deadbandZeroDeadbandHasNoEffect() { - // Keep test values in ascending order. assertThat(ControlUtils.deadband(-1.0, 0.0)).isEqualTo(-1.0); assertThat(ControlUtils.deadband(-0.5, 0.0)).isEqualTo(-0.5); assertThat(ControlUtils.deadband(-0.25, 0.0)).isEqualTo(-0.25); @@ -39,11 +59,11 @@ public void deadbandZeroDeadbandHasNoEffect() { } /** - * Asserts that {@code testExpression} is an expression that throws an exception of type {@code - * IllegalArgumentException} with a message containing {@code expectedMessage}. + * Helper method to assert that an {@link IllegalArgumentException} is thrown with a message + * containing a specific substring. * - * @param testExpression An expression that is expected to throw an exception when executed. - * @param expectedMessage (Part of an) error message expected in the exception. + * @param testExpression The executable expression expected to throw an exception. + * @param expectedMessage Part of the message expected in the exception. */ private void assertIllegalArgumentExceptionIsThrownContainingMessage( Executable testExpression, String expectedMessage) { @@ -51,9 +71,13 @@ private void assertIllegalArgumentExceptionIsThrownContainingMessage( assertThat(exception).hasMessageThat().contains(expectedMessage); } + /** + * Tests that invalid deadband values (< 0 or ≥ 1) throw an exception. + * + *

Deadband values must be in the range [0, 1) for valid control scaling. + */ @Test public void deadbandThrowsErrorOnInvalidDeadband() { - // Deadband values outside [0.0, 1.0) result in IllegalArgumentException. assertIllegalArgumentExceptionIsThrownContainingMessage( () -> ControlUtils.deadband(0.25, -1.5), "Instead, it was -1.5"); assertIllegalArgumentExceptionIsThrownContainingMessage( @@ -64,6 +88,11 @@ public void deadbandThrowsErrorOnInvalidDeadband() { () -> ControlUtils.deadband(0.25, 1.9), "Instead, it was 1.9"); } + /** + * Tests that input values outside the valid range [-1, 1] throw an exception. + * + *

Control inputs must remain in [-1, 1]; any value outside this range is invalid. + */ @Test public void deadbandThrowsErrorOnValueOutOfRange() { assertIllegalArgumentExceptionIsThrownContainingMessage( diff --git a/lib/src/test/java/com/team2813/lib2813/util/FakeShuffleboardTabs.java b/lib/src/test/java/com/team2813/lib2813/util/FakeShuffleboardTabs.java index 13c0e16d..8916a4ac 100644 --- a/lib/src/test/java/com/team2813/lib2813/util/FakeShuffleboardTabs.java +++ b/lib/src/test/java/com/team2813/lib2813/util/FakeShuffleboardTabs.java @@ -6,20 +6,45 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import java.util.concurrent.atomic.AtomicInteger; +/** + * A fake implementation of {@link ShuffleboardTabs} for testing purposes. + * + *

Each instance of this class generates a unique prefix so that tab names do not collide + * with real Shuffleboard tabs during testing. This allows multiple tests to run in parallel + * without interfering with each other. + */ public final class FakeShuffleboardTabs implements ShuffleboardTabs { + + /** Generates unique IDs for each instance. */ private static final AtomicInteger nextValue = new AtomicInteger(1); + + /** Prefix added to every tab name to ensure uniqueness. */ private final String prefix; + /** Constructs a new {@link FakeShuffleboardTabs} instance with a unique prefix. */ public FakeShuffleboardTabs() { prefix = "f" + nextValue.getAndIncrement(); } + /** + * Returns a Shuffleboard tab with the given title, prefixed to avoid collisions. + * + * @param title The title of the tab (must be non-null) + * @return The Shuffleboard tab corresponding to this prefixed title + * @throws NullPointerException if title is null + */ @Override public ShuffleboardTab getTab(String title) { requireNonNullParam(title, "title", "getTab"); return Shuffleboard.getTab(prefix + title); } + /** + * Selects the Shuffleboard tab with the given title, prefixed to avoid collisions. + * + * @param title The title of the tab (must be non-null) + * @throws NullPointerException if title is null + */ @Override public void selectTab(String title) { requireNonNullParam(title, "title", "getTab"); diff --git a/lib/src/test/java/com/team2813/lib2813/util/InputValidationTest.java b/lib/src/test/java/com/team2813/lib2813/util/InputValidationTest.java index 7d5071f8..9bef0465 100644 --- a/lib/src/test/java/com/team2813/lib2813/util/InputValidationTest.java +++ b/lib/src/test/java/com/team2813/lib2813/util/InputValidationTest.java @@ -8,26 +8,49 @@ import org.junit.experimental.runners.Enclosed; import org.junit.runner.RunWith; +/** + * Unit tests for the {@link InputValidation} utility class. + * + *

This test class specifically verifies that CAN ID validation behaves correctly, throwing an + * {@link InvalidCanIdException} for invalid IDs and returning the ID unchanged for valid IDs. + * + *

Uses the {@link Enclosed} runner to group tests logically in nested static classes. + */ @RunWith(Enclosed.class) public class InputValidationTest { - // Tests for the `InputValidation.checkCanId(...)` method. + + /** + * Tests for the {@link InputValidation#checkCanId(int)} method. + * + *

CAN IDs are valid in the range [0, 62]. This class ensures that invalid IDs throw + * {@link InvalidCanIdException} and that valid IDs return unchanged. + */ public static class CheckCanIdTest { + + /** + * Verifies that {@link InputValidation#checkCanId(int)} throws + * {@link InvalidCanIdException} for IDs outside the valid range [0, 62]. + */ @Test public void invalidCanId() { - // Can IDs can only valid in the range [0, 62]. int[] invalidCanIds = {-50, -1, 63, 100}; for (int invalidCanId : invalidCanIds) { InvalidCanIdException exception = assertThrows( InvalidCanIdException.class, () -> InputValidation.checkCanId(invalidCanId)); + // Verify that the exception contains the invalid ID assertThat(exception.getCanId()).isEqualTo(invalidCanId); + // Verify that the exception message mentions invalid CAN ID assertThat(exception).hasMessageThat().contains("is not a valid can id"); } } + /** + * Verifies that {@link InputValidation#checkCanId(int)} returns the original ID for valid + * CAN IDs in the range [0, 62]. + */ @Test public void validCanID() { - // Can IDs can only valid in the range [0, 62]. int[] validCanIds = {0, 1, 10, 62}; for (int validCanId : validCanIds) { int returnValue = InputValidation.checkCanId(validCanId); diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/AprilTagMapPoseHelper.java b/limelight/src/main/java/com/team2813/lib2813/limelight/AprilTagMapPoseHelper.java index 7b82a2e2..c97bb46d 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/AprilTagMapPoseHelper.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/AprilTagMapPoseHelper.java @@ -15,19 +15,43 @@ import java.util.Set; import java.util.concurrent.Executors; +/** + * Helper class for working with AprilTag field maps and retrieving poses of tags. + * + *

This class interacts with a {@link LimelightClient} to optionally upload field maps to a + * Limelight device, and locally stores a {@link FiducialRetriever} for pose lookups. + */ class AprilTagMapPoseHelper { + + /** The Limelight client used to send HTTP requests to the camera. */ private final LimelightClient limelightClient; + + /** The local fiducial retriever, initialized when a field map is set. */ private FiducialRetriever retriever; + + /** Shared HTTP client used for uploading field maps to the Limelight. */ private static final HttpClient client = HttpClient.newBuilder() .connectTimeout(Duration.ofMillis(20)) .executor(Executors.newFixedThreadPool(1)) .build(); + /** + * Constructs a new helper using the specified Limelight client. + * + * @param client the Limelight client to use for HTTP interactions + */ public AprilTagMapPoseHelper(LimelightClient client) { this.limelightClient = client; } + /** + * Sets the field map for tag lookups and optionally uploads it to the Limelight. + * + * @param stream an {@link InputStream} containing the JSON-encoded field map + * @param updateLimelight if true, also uploads the field map to the connected Limelight + * @throws IOException if reading the input stream fails + */ public void setFieldMap(InputStream stream, boolean updateLimelight) throws IOException { if (!updateLimelight) { retriever = new FiducialRetriever(stream); @@ -49,16 +73,28 @@ public void setFieldMap(InputStream stream, boolean updateLimelight) throws IOEx } } + /** + * Returns the poses of the visible tags with the specified IDs. + * + * @param ids the set of tag IDs to retrieve + * @return a list of {@link Pose3d} for the tags found in the field map; empty if none + */ public List getVisibleTagPoses(Set ids) { if (retriever == null) { return List.of(); } return retriever.getFiducialMap().values().stream() - .filter(fidicual -> ids.contains(fidicual.getId())) + .filter(fiducial -> ids.contains(fiducial.getId())) .map(Fiducial::getPosition) .toList(); } + /** + * Returns the pose of a single tag by ID. + * + * @param id the ID of the tag + * @return an {@link Optional} containing the tag's {@link Pose3d} if present, otherwise empty + */ public Optional getTagPose(int id) { if (retriever == null) { return Optional.empty(); diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java b/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java index 675a71d9..4e67befc 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java @@ -4,14 +4,22 @@ import java.util.Set; /** - * Represents an estimated position for the robot, obtained from the Limelight. + * Represents an estimated position of the robot, as reported by a Limelight or other vision system. * - * @param pose The estimated position of the robot. - * @param timestampSeconds The timestamp, in seconds, using the drivetrain clock - * @param visibleAprilTags All April Tags that are visible from the vision source. + *

This record includes the robot's pose, the timestamp of the observation, and a set of + * currently visible AprilTag IDs. + * + * @param pose The estimated position and orientation of the robot in 2D space. + * @param timestampSeconds The timestamp of this estimate, in seconds, relative to the drivetrain clock. + * @param visibleAprilTags The set of AprilTag IDs that were visible when this estimate was made. */ -public record BotPoseEstimate(Pose2d pose, double timestampSeconds, Set visibleAprilTags) { +public record BotPoseEstimate( + Pose2d pose, double timestampSeconds, Set visibleAprilTags) { + /** + * @deprecated Use the constructor including {@code visibleAprilTags}. This constructor + * automatically assigns an empty set of visible AprilTags. + */ @Deprecated public BotPoseEstimate(Pose2d pose, double timestampSeconds) { this(pose, timestampSeconds, Set.of()); diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java b/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java index 9b99332d..832cf3ed 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java @@ -16,14 +16,33 @@ import java.util.concurrent.Executors; import org.json.JSONObject; +/** + * Background task that collects data from a Limelight camera via HTTP requests. + * + *

This class periodically fetches JSON data from the Limelight `/results` endpoint, timestamps + * the response, and stores the latest result for retrieval. + */ class DataCollection implements Runnable { + + /** Shared HTTP client with short timeout and fixed thread pool. */ private static final HttpClient client = HttpClient.newBuilder() .connectTimeout(Duration.ofMillis(20)) .executor(Executors.newFixedThreadPool(2)) .build(); + + /** HTTP request to retrieve Limelight results. */ private final HttpRequest dumpRequest; + /** Stores the most recent result and its response timestamp. */ + private volatile Optional lastResult; + + /** + * Constructs a DataCollection instance for a given Limelight client. + * + * @param limelightClient client for building HTTP requests + * @throws RuntimeException if creating the HTTP request fails + */ public DataCollection(LimelightClient limelightClient) { lastResult = Optional.empty(); try { @@ -33,44 +52,62 @@ public DataCollection(LimelightClient limelightClient) { } } + /** + * Represents a single Limelight result. + * + * @param json the parsed JSON object returned by Limelight + * @param responseTimestamp the time the response was received (seconds) + */ record Result(JSONObject json, double responseTimestamp) {} - private volatile Optional lastResult; - + /** + * Custom body handler to parse the HTTP response into a {@link Result} with timestamp. + */ private static class JSONHandler implements BodyHandler { + @Override public BodySubscriber apply(ResponseInfo responseInfo) { - // Get the timestamp before we parse the JSON. + // Capture the timestamp before parsing the JSON body. double responseTimestamp = getCurrentTimeSeconds(); + // Map the string response body into a Result object return BodySubscribers.mapping( BodyHandlers.ofString(Charset.defaultCharset()).apply(responseInfo), - body -> { - return new Result(new JSONObject(body), responseTimestamp); - }); + body -> new Result(new JSONObject(body), responseTimestamp)); } } + /** Single reusable instance of the JSONHandler. */ private static final JSONHandler handler = new JSONHandler(); - private void updateJSON(HttpResponse obj) { - Result result = obj.body(); + /** Updates the most recent result from an HTTP response. */ + private void updateJSON(HttpResponse response) { + Result result = response.body(); lastResult = Optional.of(result); } + /** + * Executes the HTTP request to fetch Limelight results and stores the latest Result. + * + *

If the request is interrupted or fails, {@code lastResult} is cleared. + */ @Override public void run() { try { updateJSON(client.send(dumpRequest, handler)); } catch (InterruptedException e) { lastResult = Optional.empty(); - // background thread canceled - Thread.currentThread().interrupt(); + Thread.currentThread().interrupt(); // preserve interrupt status } catch (Exception e) { lastResult = Optional.empty(); } } + /** + * Returns the most recent fetched result. + * + * @return an {@link Optional} containing the latest {@link Result}, or empty if unavailable + */ public Optional getMostRecent() { return lastResult; } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java b/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java index 1cb97cc6..3f65236a 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java @@ -6,7 +6,24 @@ import org.json.JSONException; import org.json.JSONObject; +/** + * Utility class for safely extracting values from JSONObjects returned by the Limelight. + * + *

All methods handle missing keys or invalid types gracefully by returning {@link Optional}. + * This avoids throwing unchecked {@link JSONException}s during normal processing. + */ class JSONHelper { + + /** + * Converts an integer value from a JSON object to a boolean. + * + *

The key must be present and contain 0 (false) or 1 (true). Returns {@link Optional#empty()} + * if the key is missing or the value is not an integer. + * + * @param obj the JSON object to read from + * @param key the key corresponding to the boolean-as-int + * @return {@link Optional} containing true if value is 1, false if 0, or empty if missing/invalid + */ static Optional getBooleanFromInt(JSONObject obj, String key) { if (!obj.has(key)) { return Optional.empty(); @@ -18,6 +35,14 @@ static Optional getBooleanFromInt(JSONObject obj, String key) { } } + /** + * Returns a function that retrieves a nested JSONObject by key. + * + *

The returned function returns {@link Optional#empty()} if the key is missing or not a JSON object. + * + * @param key the key to extract + * @return a function that extracts an Optional<JSONObject> from a parent JSONObject + */ static Function> getJSONObject(String key) { return (j) -> { if (!j.has(key)) { @@ -31,16 +56,30 @@ static Function> getJSONObject(String key) { }; } + /** + * Returns the "root" JSONObject for the Limelight results. + * + *

Older versions of the Limelight returned a root node called "Results". This method + * normalizes that so downstream code can always work with a consistent root object. + * + * @param json the raw JSON response from the Limelight + * @return the root JSONObject to extract fields from + */ static JSONObject getRoot(JSONObject json) { if (json.has("Results")) { - // This JSON was provided by an older version of the limelight code, - // which had a "Results" root node. return json.getJSONObject("Results"); } else { return json; } } + /** + * Retrieves a long value from a JSONObject by key. + * + * @param obj the JSON object to read from + * @param key the key corresponding to the long value + * @return Optional containing the long value, or empty if missing or invalid + */ static Optional getLong(JSONObject obj, String key) { if (!obj.has(key)) { return Optional.empty(); @@ -52,6 +91,13 @@ static Optional getLong(JSONObject obj, String key) { } } + /** + * Retrieves a double value from a JSONObject by key. + * + * @param obj the JSON object to read from + * @param key the key corresponding to the double value + * @return Optional containing the double value, or empty if missing or invalid + */ static Optional getDouble(JSONObject obj, String key) { if (!obj.has(key)) { return Optional.empty(); @@ -63,6 +109,13 @@ static Optional getDouble(JSONObject obj, String key) { } } + /** + * Retrieves a JSONArray from a JSONObject by key. + * + * @param obj the JSON object to read from + * @param key the key corresponding to the JSON array + * @return Optional containing the JSONArray, or empty if missing or invalid + */ static Optional getArr(JSONObject obj, String key) { if (!obj.has(key)) { return Optional.empty(); diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java b/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java index 7caae133..22cecda2 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java @@ -9,45 +9,64 @@ import java.util.Set; import org.json.JSONObject; +/** + * Interface for interacting with a Limelight vision camera. + * + *

Provides methods for retrieving locational data, working with field maps, and accessing + * raw JSON output. Some legacy methods are marked {@link Deprecated} in favor of + * {@link LocationalData}-based APIs. + */ public interface Limelight { /** - * Gets the limelight with the default name. + * Gets the Limelight with the default name. * - * @return the {@link Limelight} object for interfacing with the limelight + * @return the {@link Limelight} object for interfacing with the default camera */ static Limelight getDefaultLimelight() { return RestLimelight.getDefaultLimelight(); } /** - * @deprecated use methods in {@link LocationalData} that return a {@link BotPoseEstimate}. + * Returns the timestamp of the most recent capture, in seconds. + * + * @deprecated Use methods in {@link LocationalData} that return a {@link BotPoseEstimate}. + * @return an {@link OptionalDouble} containing the timestamp if available */ @Deprecated OptionalDouble getTimestamp(); /** - * Returns {@code true} if the limelight has identified a target. + * Returns true if the Limelight currently has a valid target. * - * @deprecated use {@link LocationalData#hasTarget()} + * @deprecated Use {@link LocationalData#hasTarget()} instead. + * @return true if a target is detected */ @Deprecated boolean hasTarget(); - /** Gets an object for getting locational data. */ + /** Gets an object for retrieving locational data from the Limelight. */ LocationalData getLocationalData(); + /** + * Sets the field map for the Limelight from an input stream. Optionally, this can upload the + * map to the Limelight. + * + * @param stream the input stream containing the field map + * @param updateLimelight whether to update the Limelight with this map + * @throws IOException if reading from the stream fails + */ void setFieldMap(InputStream stream, boolean updateLimelight) throws IOException; /** - * Sets the field map for the limelight with a file in the deploy directory. Additionally, this - * may also upload the field map to the Limelight if desired. This will likely be a slow - * operation, and should not be regularly called. + * Sets the field map for the Limelight using a file in the deploy directory. + * + *

This method opens the file and delegates to {@link #setFieldMap(InputStream, boolean)}. + * Uploading the map to the Limelight can be slow and should not be called frequently. * - * @param filepath The path to the file from the deploy directory (using UNIX file seperators) - * @param updateLimelight If the limelight should be updated with this field map - * @throws IOException If the given filepath does not exist in the deploy directory or could not - * be read + * @param filepath path to the file relative to the deploy directory (use UNIX file separators) + * @param updateLimelight whether to update the Limelight with this map + * @throws IOException if the file does not exist or cannot be read */ default void setFieldMap(String filepath, boolean updateLimelight) throws IOException { File file = new File(Filesystem.getDeployDirectory(), filepath); @@ -57,23 +76,30 @@ default void setFieldMap(String filepath, boolean updateLimelight) throws IOExce } /** - * Gets the locations of the given AprilTags. + * Returns the 3D poses of the specified visible AprilTags. * - * @deprecated use {@link LocationalData#getVisibleAprilTagPoses()} + * @deprecated Use {@link LocationalData#getVisibleAprilTagPoses()} instead. + * @param visibleTags a set of AprilTag IDs to locate + * @return a list of {@link Pose3d} objects representing each tag's position */ @Deprecated List getLocatedAprilTags(Set visibleTags); /** - * @deprecated use {@link LocationalData#getCaptureLatency()} + * Returns the capture latency for the most recent Limelight frame, in seconds. + * + * @deprecated Use {@link LocationalData#getCaptureLatency()} instead. + * @return an {@link OptionalDouble} containing the latency if available */ @Deprecated OptionalDouble getCaptureLatency(); /** - * Gets the most recent JSON from the Limelight. Does not work for all implementations. + * Returns the most recent raw JSON dump from the Limelight. * - * @deprecated use {@link LocationalData#isValid()}. + * @deprecated Use {@link LocationalData#isValid()} instead. Not all Limelight implementations + * support this method. + * @return an {@link Optional} containing the JSON object if available */ @Deprecated Optional getJsonDump(); diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java index aa83e253..fe058e9d 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java @@ -5,30 +5,62 @@ import java.net.URISyntaxException; import java.net.http.HttpRequest; +/** + * A client for communicating with a Limelight camera over HTTP. + * + *

Provides functionality for building HTTP requests to the Limelight's REST API. + */ final class LimelightClient { + + /** The default HTTP port used by Limelight cameras. */ static final int DEFAULT_PORT = 5807; + + /** The hostname or IP address of the Limelight. */ private final String hostname; + + /** The port number to use for HTTP connections. */ private final int port; + /** + * Exception thrown when an HTTP request could not be constructed. + * + *

Wraps a {@link URISyntaxException} to provide additional context about the failure. + */ static class HttpRequestException extends IOException { public HttpRequestException(String message, URISyntaxException e) { super(message, e); } } + /** + * Constructs a LimelightClient using the default port. + * + * @param hostname the hostname or IP address of the Limelight + */ LimelightClient(String hostname) { this(hostname, DEFAULT_PORT); } + /** + * Constructs a LimelightClient with a specific port. + * + * @param hostname the hostname or IP address of the Limelight + * @param port the HTTP port to use + */ LimelightClient(String hostname, int port) { this.hostname = hostname; this.port = port; } /** - * Creates an HttpRequest builder for the limelight with the given path. + * Creates an {@link HttpRequest.Builder} for the Limelight with the given path. + * + *

The URI is automatically constructed using the hostname and port configured for this + * client. * - * @throws HttpRequestException If the request could not be created + * @param path the HTTP path (e.g., "/results" or "/upload-fieldmap") + * @return an {@link HttpRequest.Builder} ready for further configuration + * @throws HttpRequestException if the URI is invalid or cannot be created */ HttpRequest.Builder newRequestBuilder(String path) throws HttpRequestException { try { diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java index 5b5901f6..31bc3d9c 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java @@ -30,98 +30,248 @@ /** * LimelightHelpers provides static methods and classes for interfacing with Limelight vision - * cameras in FRC. This library supports all Limelight features including AprilTag tracking, Neural - * Networks, and standard color/retroreflective tracking. + * cameras in FRC (FIRST Robotics Competition). This library supports all Limelight features + * including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + * + *

This helper class facilitates communication with Limelight cameras via NetworkTables and + * HTTP requests, providing convenient methods for retrieving vision data, controlling camera + * settings, and parsing results. + * + *

Key features include: + *

    + *
  • AprilTag/Fiducial marker detection and pose estimation
    • + *
  • Retroreflective target tracking
    • + *
  • Neural network classification and detection
    • + *
  • Barcode/QR code reading
    • + *
  • Robot localization (MegaTag, MegaTag2)
    • + *
  • IMU data integration
    • + *
  • Camera configuration and control
    • + *
* - *

Coped from github.com/LimelightVision/limelightlib-wpijava + *

Copied from + * github.com/LimelightVision/limelightlib-wpijava + * + * @version 1.11 + * @see Limelight Documentation + * @author Limelight Vision */ class LimelightHelpers { + /** + * Cache for DoubleArrayEntry objects to avoid recreating NetworkTables entries. + * This improves performance by reusing entries across multiple calls. + * Key format: "tableName/entryName" + */ private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); - /** Represents a Color/Retroreflective Target Result extracted from JSON Output */ + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output. + * Contains pose information and targeting data for retroreflective vision targets + * detected by color or retroreflective vision pipelines. + * + *

This class provides access to: + *

    + *
  • Camera pose relative to target
    • + *
  • Robot pose on field and relative to target
    • + *
  • Target pose relative to camera and robot
    • + *
  • Targeting metrics (area, offsets, skew)
    • + *
+ */ public static class LimelightTarget_Retro { + /** + * Camera pose in target space (6-element array: x, y, z, roll, pitch, yaw). + * Positions in meters, angles in degrees. + */ @JsonProperty("t6c_ts") private double[] cameraPose_TargetSpace; + /** + * Robot pose in field space (6-element array: x, y, z, roll, pitch, yaw). + * Positions in meters, angles in degrees. + */ @JsonProperty("t6r_fs") private double[] robotPose_FieldSpace; + /** + * Robot pose in target space (6-element array: x, y, z, roll, pitch, yaw). + * Positions in meters, angles in degrees. + */ @JsonProperty("t6r_ts") private double[] robotPose_TargetSpace; + /** + * Target pose in camera space (6-element array: x, y, z, roll, pitch, yaw). + * Positions in meters, angles in degrees. + */ @JsonProperty("t6t_cs") private double[] targetPose_CameraSpace; + /** + * Target pose in robot space (6-element array: x, y, z, roll, pitch, yaw). + * Positions in meters, angles in degrees. + */ @JsonProperty("t6t_rs") private double[] targetPose_RobotSpace; + /** + * Gets the 3D camera pose in target space. + * This represents where the camera is positioned relative to the detected target. + * + * @return Pose3d representing camera position and orientation relative to the target + */ public Pose3d getCameraPose_TargetSpace() { return toPose3D(cameraPose_TargetSpace); } + /** + * Gets the 3D robot pose in field space. + * This is the robot's estimated position on the field based on target detection. + * + * @return Pose3d representing robot position and orientation on the field + */ public Pose3d getRobotPose_FieldSpace() { return toPose3D(robotPose_FieldSpace); } + /** + * Gets the 3D robot pose in target space. + * This represents where the robot is positioned relative to the detected target. + * + * @return Pose3d representing robot position and orientation relative to the target + */ public Pose3d getRobotPose_TargetSpace() { return toPose3D(robotPose_TargetSpace); } + /** + * Gets the 3D target pose in camera space. + * This represents where the target is positioned relative to the camera. + * + * @return Pose3d representing target position and orientation relative to the camera + */ public Pose3d getTargetPose_CameraSpace() { return toPose3D(targetPose_CameraSpace); } + /** + * Gets the 3D target pose in robot space. + * This represents where the target is positioned relative to the robot's center. + * + * @return Pose3d representing target position and orientation relative to the robot + */ public Pose3d getTargetPose_RobotSpace() { return toPose3D(targetPose_RobotSpace); } + /** + * Gets the 2D camera pose in target space (ignores z, roll, pitch). + * Useful for planar navigation calculations. + * + * @return Pose2d representing camera position and yaw relative to the target + */ public Pose2d getCameraPose_TargetSpace2D() { return toPose2D(cameraPose_TargetSpace); } + /** + * Gets the 2D robot pose in field space (ignores z, roll, pitch). + * Useful for planar navigation and field positioning. + * + * @return Pose2d representing robot position and yaw on the field + */ public Pose2d getRobotPose_FieldSpace2D() { return toPose2D(robotPose_FieldSpace); } + /** + * Gets the 2D robot pose in target space (ignores z, roll, pitch). + * Useful for planar approach calculations. + * + * @return Pose2d representing robot position and yaw relative to the target + */ public Pose2d getRobotPose_TargetSpace2D() { return toPose2D(robotPose_TargetSpace); } + /** + * Gets the 2D target pose in camera space (ignores z, roll, pitch). + * Useful for planar targeting calculations. + * + * @return Pose2d representing target position and yaw relative to the camera + */ public Pose2d getTargetPose_CameraSpace2D() { return toPose2D(targetPose_CameraSpace); } + /** + * Gets the 2D target pose in robot space (ignores z, roll, pitch). + * Useful for planar targeting and robot-relative positioning. + * + * @return Pose2d representing target position and yaw relative to the robot + */ public Pose2d getTargetPose_RobotSpace2D() { return toPose2D(targetPose_RobotSpace); } + /** + * Target area as percentage of image (0-100). + * Larger values indicate target is closer or larger in frame. + */ @JsonProperty("ta") public double ta; + /** + * Horizontal offset from crosshair to target center in degrees. + * Positive values indicate target is to the right of crosshair. + */ @JsonProperty("tx") public double tx; + /** + * Vertical offset from crosshair to target center in degrees. + * Positive values indicate target is above crosshair. + */ @JsonProperty("ty") public double ty; + /** + * Horizontal offset from crosshair to target center in pixels. + */ @JsonProperty("txp") public double tx_pixels; + /** + * Vertical offset from crosshair to target center in pixels. + */ @JsonProperty("typ") public double ty_pixels; + /** + * Horizontal offset from principal point (no crosshair adjustment) in degrees. + * More accurate than tx for calibrated cameras. + */ @JsonProperty("tx_nocross") public double tx_nocrosshair; + /** + * Vertical offset from principal point (no crosshair adjustment) in degrees. + * More accurate than ty for calibrated cameras. + */ @JsonProperty("ty_nocross") public double ty_nocrosshair; + /** + * Target skew/rotation in degrees (-90 to 0). + * Indicates the rotation of the target relative to camera orientation. + */ @JsonProperty("ts") public double ts; + /** + * Constructs a new LimelightTarget_Retro with default values. + * Initializes all pose arrays to 6-element arrays of zeros. + */ public LimelightTarget_Retro() { cameraPose_TargetSpace = new double[6]; robotPose_FieldSpace = new double[6]; @@ -131,94 +281,236 @@ public LimelightTarget_Retro() { } } - /** Represents an AprilTag/Fiducial Target Result extracted from JSON Output */ + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output. + * Contains pose information and targeting data for AprilTag fiducial markers. + * + *

AprilTags are visual fiducial markers used extensively in FRC for robot + * localization and pose estimation. This class provides comprehensive data about + * detected AprilTags including their ID, family, and various pose transformations. + * + *

This class provides access to: + *

    + *
  • Fiducial ID and family information
    • + *
  • Camera pose relative to tag
    • + *
  • Robot pose on field and relative to tag
    • + *
  • Tag pose relative to camera and robot
    • + *
  • Targeting metrics (area, offsets, skew)
    • + *
+ */ public static class LimelightTarget_Fiducial { + /** + * Fiducial marker ID number. + * For FRC AprilTags, this corresponds to specific field locations. + */ @JsonProperty("fID") public double fiducialID; + /** + * Fiducial family name (e.g., "tag36h11", "tag16h5"). + * Different tag families have different error correction and detection properties. + */ @JsonProperty("fam") public String fiducialFamily; + /** + * Camera pose in target space (6-element array: x, y, z, roll, pitch, yaw). + * Positions in meters, angles in degrees. + */ @JsonProperty("t6c_ts") private double[] cameraPose_TargetSpace; + /** + * Robot pose in field space (6-element array: x, y, z, roll, pitch, yaw). + * Positions in meters, angles in degrees. + * This is the robot's estimated global position based on this tag detection. + */ @JsonProperty("t6r_fs") private double[] robotPose_FieldSpace; + /** + * Robot pose in target space (6-element array: x, y, z, roll, pitch, yaw). + * Positions in meters, angles in degrees. + */ @JsonProperty("t6r_ts") private double[] robotPose_TargetSpace; + /** + * Target pose in camera space (6-element array: x, y, z, roll, pitch, yaw). + * Positions in meters, angles in degrees. + */ @JsonProperty("t6t_cs") private double[] targetPose_CameraSpace; + /** + * Target pose in robot space (6-element array: x, y, z, roll, pitch, yaw). + * Positions in meters, angles in degrees. + */ @JsonProperty("t6t_rs") private double[] targetPose_RobotSpace; + /** + * Gets the 3D camera pose in target space. + * This represents where the camera is positioned relative to the detected AprilTag. + * + * @return Pose3d representing camera position and orientation relative to the AprilTag + */ public Pose3d getCameraPose_TargetSpace() { return toPose3D(cameraPose_TargetSpace); } + /** + * Gets the 3D robot pose in field space. + * This is the robot's estimated global position based on the AprilTag detection + * and the known field layout. + * + * @return Pose3d representing robot position and orientation on the field + */ public Pose3d getRobotPose_FieldSpace() { return toPose3D(robotPose_FieldSpace); } + /** + * Gets the 3D robot pose in target space. + * This represents where the robot is positioned relative to the detected AprilTag. + * + * @return Pose3d representing robot position and orientation relative to the AprilTag + */ public Pose3d getRobotPose_TargetSpace() { return toPose3D(robotPose_TargetSpace); } + /** + * Gets the 3D target pose in camera space. + * This represents where the AprilTag is positioned relative to the camera. + * Useful for direct targeting calculations. + * + * @return Pose3d representing AprilTag position and orientation relative to the camera + */ public Pose3d getTargetPose_CameraSpace() { return toPose3D(targetPose_CameraSpace); } + /** + * Gets the 3D target pose in robot space. + * This represents where the AprilTag is positioned relative to the robot's center. + * Useful for robot-centric navigation and targeting. + * + * @return Pose3d representing AprilTag position and orientation relative to the robot + */ public Pose3d getTargetPose_RobotSpace() { return toPose3D(targetPose_RobotSpace); } + /** + * Gets the 2D camera pose in target space (ignores z, roll, pitch). + * Useful for planar navigation calculations relative to the tag. + * + * @return Pose2d representing camera position and yaw relative to the AprilTag + */ public Pose2d getCameraPose_TargetSpace2D() { return toPose2D(cameraPose_TargetSpace); } + /** + * Gets the 2D robot pose in field space (ignores z, roll, pitch). + * This is commonly used with WPILib's pose estimator for robot localization. + * + * @return Pose2d representing robot position and yaw on the field + */ public Pose2d getRobotPose_FieldSpace2D() { return toPose2D(robotPose_FieldSpace); } + /** + * Gets the 2D robot pose in target space (ignores z, roll, pitch). + * Useful for planar approach calculations to the tag. + * + * @return Pose2d representing robot position and yaw relative to the AprilTag + */ public Pose2d getRobotPose_TargetSpace2D() { return toPose2D(robotPose_TargetSpace); } + /** + * Gets the 2D target pose in camera space (ignores z, roll, pitch). + * Useful for planar targeting calculations. + * + * @return Pose2d representing AprilTag position and yaw relative to the camera + */ public Pose2d getTargetPose_CameraSpace2D() { return toPose2D(targetPose_CameraSpace); } + /** + * Gets the 2D target pose in robot space (ignores z, roll, pitch). + * Useful for planar targeting and robot-relative positioning. + * + * @return Pose2d representing AprilTag position and yaw relative to the robot + */ public Pose2d getTargetPose_RobotSpace2D() { return toPose2D(targetPose_RobotSpace); } + /** + * Target area as percentage of image (0-100). + * Larger values indicate tag is closer or larger in frame. + */ @JsonProperty("ta") public double ta; + /** + * Horizontal offset from crosshair to tag center in degrees. + * Positive values indicate tag is to the right of crosshair. + */ @JsonProperty("tx") public double tx; + /** + * Vertical offset from crosshair to tag center in degrees. + * Positive values indicate tag is above crosshair. + */ @JsonProperty("ty") public double ty; + /** + * Horizontal offset from crosshair to tag center in pixels. + */ @JsonProperty("txp") public double tx_pixels; + /** + * Vertical offset from crosshair to tag center in pixels. + */ @JsonProperty("typ") public double ty_pixels; + /** + * Horizontal offset from principal point (no crosshair adjustment) in degrees. + * More accurate than tx for calibrated cameras. Should be preferred for precision work. + */ @JsonProperty("tx_nocross") public double tx_nocrosshair; + /** + * Vertical offset from principal point (no crosshair adjustment) in degrees. + * More accurate than ty for calibrated cameras. Should be preferred for precision work. + */ @JsonProperty("ty_nocross") public double ty_nocrosshair; + /** + * Target skew/rotation in degrees (-90 to 0). + * Indicates the rotation of the tag relative to camera orientation. + */ @JsonProperty("ts") public double ts; + /** + * Constructs a new LimelightTarget_Fiducial with default values. + * Initializes all pose arrays to 6-element arrays of zeros. + */ public LimelightTarget_Fiducial() { cameraPose_TargetSpace = new double[6]; robotPose_FieldSpace = new double[6]; @@ -228,203 +520,495 @@ public LimelightTarget_Fiducial() { } } - /** Represents a Barcode Target Result extracted from JSON Output */ + /** + * Represents a Barcode Target Result extracted from JSON Output. + * Supports various barcode formats including QR codes, DataMatrix, Code128, and others. + * + *

Limelight can detect and decode various 1D and 2D barcodes, which can be useful + * for game piece identification, autonomous mode selection, or other data encoding needs. + * + *

Supported barcode families include: + *

    + *
  • QR Code
    • + *
  • DataMatrix
    • + *
  • Code 128
    • + *
  • Code 39
    • + *
  • EAN/UPC
    • + *
  • And others
    • + *
+ */ public static class LimelightTarget_Barcode { - /** Barcode family type (e.g. "QR", "DataMatrix", etc.) */ + /** + * Barcode family type (e.g. "QR", "DataMatrix", "Code128"). + * Indicates the encoding format of the detected barcode. + */ @JsonProperty("fam") public String family; - /** Gets the decoded data content of the barcode */ + /** + * Decoded data content of the barcode as a string. + * This is the actual information encoded in the barcode. + */ @JsonProperty("data") public String data; + /** + * Horizontal offset from crosshair to barcode center in pixels. + */ @JsonProperty("txp") public double tx_pixels; + /** + * Vertical offset from crosshair to barcode center in pixels. + */ @JsonProperty("typ") public double ty_pixels; + /** + * Horizontal offset from crosshair to barcode center in degrees. + * Positive values indicate barcode is to the right of crosshair. + */ @JsonProperty("tx") public double tx; + /** + * Vertical offset from crosshair to barcode center in degrees. + * Positive values indicate barcode is above crosshair. + */ @JsonProperty("ty") public double ty; + /** + * Horizontal offset from principal point (no crosshair adjustment) in degrees. + */ @JsonProperty("tx_nocross") public double tx_nocrosshair; + /** + * Vertical offset from principal point (no crosshair adjustment) in degrees. + */ @JsonProperty("ty_nocross") public double ty_nocrosshair; + /** + * Target area as percentage of image (0-100). + * Larger values indicate barcode is closer or larger in frame. + */ @JsonProperty("ta") public double ta; + /** + * Corner points of the barcode bounding box in image coordinates. + * Array format: [[x0, y0], [x1, y1], [x2, y2], [x3, y3]] + * Useful for visualization or precise barcode localization. + */ @JsonProperty("pts") public double[][] corners; + /** + * Constructs a new LimelightTarget_Barcode with default values. + */ public LimelightTarget_Barcode() {} + /** + * Gets the barcode family type. + * + * @return String representing the barcode format (e.g., "QR", "DataMatrix", "Code128") + */ public String getFamily() { return family; } } - /** Represents a Neural Classifier Pipeline Result extracted from JSON Output */ + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output. + * Used for neural network classification tasks (identifying what an object is). + * + *

Neural classifiers categorize entire images or regions into predefined classes. + * Unlike detectors, classifiers don't provide bounding boxes but instead classify + * the primary object in view. + * + *

Common FRC use cases: + *

    + *
  • Game piece identification (cone vs cube)
    • + *
  • Color classification
    • + *
  • Orientation detection
    • + *
  • State recognition
    • + *
+ */ public static class LimelightTarget_Classifier { + /** + * Name of the detected class. + * This is the human-readable label for the classification result. + */ @JsonProperty("class") public String className; + /** + * Numeric ID of the detected class. + * Useful for switch statements and numerical comparisons. + */ @JsonProperty("classID") public double classID; + /** + * Confidence score of the classification (0-100). + * Higher values indicate higher confidence in the classification result. + */ @JsonProperty("conf") public double confidence; + /** + * Zone number if using zone-based classification. + * Zones allow different classification behavior in different areas of the image. + */ @JsonProperty("zone") public double zone; + /** + * Horizontal offset from crosshair to classification region center in degrees. + */ @JsonProperty("tx") public double tx; + /** + * Horizontal offset from crosshair to classification region center in pixels. + */ @JsonProperty("txp") public double tx_pixels; + /** + * Vertical offset from crosshair to classification region center in degrees. + */ @JsonProperty("ty") public double ty; + /** + * Vertical offset from crosshair to classification region center in pixels. + */ @JsonProperty("typ") public double ty_pixels; + /** + * Constructs a new LimelightTarget_Classifier with default values. + */ public LimelightTarget_Classifier() {} } - /** Represents a Neural Detector Pipeline Result extracted from JSON Output */ + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output. + * Used for neural network object detection tasks (finding and classifying objects). + * + *

Neural detectors locate objects within an image and classify them. Unlike + * classifiers, detectors can find multiple objects and provide bounding box information. + * + *

Common FRC use cases: + *

    + *
  • Game piece detection and localization
    • + *
  • Multiple object tracking
    • + *
  • Robot detection
    • + *
  • Custom target identification
    • + *
+ */ public static class LimelightTarget_Detector { + /** + * Name of the detected class. + * This is the human-readable label for what was detected. + */ @JsonProperty("class") public String className; + /** + * Numeric ID of the detected class. + * Useful for switch statements and numerical comparisons. + */ @JsonProperty("classID") public double classID; + /** + * Confidence score of the detection (0-100). + * Higher values indicate higher confidence that the object is present. + */ @JsonProperty("conf") public double confidence; + /** + * Target area as percentage of image (0-100). + * Size of the detection bounding box. + */ @JsonProperty("ta") public double ta; + /** + * Horizontal offset from crosshair to detection center in degrees. + * Positive values indicate detection is to the right of crosshair. + */ @JsonProperty("tx") public double tx; + /** + * Vertical offset from crosshair to detection center in degrees. + * Positive values indicate detection is above crosshair. + */ @JsonProperty("ty") public double ty; + /** + * Horizontal offset from crosshair to detection center in pixels. + */ @JsonProperty("txp") public double tx_pixels; + /** + * Vertical offset from crosshair to detection center in pixels. + */ @JsonProperty("typ") public double ty_pixels; + /** + * Horizontal offset from principal point (no crosshair adjustment) in degrees. + */ @JsonProperty("tx_nocross") public double tx_nocrosshair; + /** + * Vertical offset from principal point (no crosshair adjustment) in degrees. + */ @JsonProperty("ty_nocross") public double ty_nocrosshair; + /** + * Constructs a new LimelightTarget_Detector with default values. + */ public LimelightTarget_Detector() {} } - /** Limelight Results object, parsed from a Limelight's JSON results output. */ + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + * Contains all targeting data, pose estimates, and metadata from a Limelight camera. + * + *

This is the primary result container that aggregates all detection types: + * retroreflective targets, AprilTags, neural detections, classifications, and barcodes. + * It also includes robot pose estimates and timing information. + * + *

Use {@link #getLatestResults(String)} to obtain this object from NetworkTables. + */ public static class LimelightResults { + /** + * Error message if JSON parsing failed, null otherwise. + * Check this field to ensure results are valid before use. + */ public String error; + /** + * Current pipeline index (0-9). + * Indicates which vision pipeline is currently active. + */ @JsonProperty("pID") public double pipelineID; + /** + * Pipeline processing latency in milliseconds. + * Time taken for the vision pipeline to process the image. + */ @JsonProperty("tl") public double latency_pipeline; + /** + * Image capture latency in milliseconds. + * Time between when the frame was captured and when processing began. + */ @JsonProperty("cl") public double latency_capture; + /** + * JSON parsing latency in milliseconds (calculated locally). + * Time taken to deserialize the JSON on the RoboRIO. + */ public double latency_jsonParse; + /** + * Timestamp when Limelight published the results (in seconds). + * Unix timestamp from the Limelight's internal clock. + */ @JsonProperty("ts") public double timestamp_LIMELIGHT_publish; + /** + * Timestamp when the RoboRIO FPGA captured the image (in seconds). + * FPGA timestamp synchronized with robot code timing. + */ @JsonProperty("ts_rio") public double timestamp_RIOFPGA_capture; + /** + * Whether valid target(s) are detected. + * True if at least one target meets the pipeline's criteria. + */ @JsonProperty("v") @JsonFormat(shape = Shape.NUMBER) public boolean valid; + /** + * Robot pose in field space (6-element array: x, y, z, roll, pitch, yaw). + * Positions in meters, angles in degrees. + * Uses Limelight's internal coordinate system. + */ @JsonProperty("botpose") public double[] botpose; + /** + * Robot pose in WPILib red alliance coordinate system. + * Origin at red alliance driver station wall. + */ @JsonProperty("botpose_wpired") public double[] botpose_wpired; + /** + * Robot pose in WPILib blue alliance coordinate system. + * Origin at blue alliance driver station wall. Recommended for most use cases. + */ @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + /** + * Number of AprilTags used for pose estimation. + * Higher counts generally indicate more reliable pose estimates. + */ @JsonProperty("botpose_tagcount") public double botpose_tagcount; + /** + * Distance in meters between the two farthest AprilTags used for pose. + * Larger spans generally improve pose accuracy. + */ @JsonProperty("botpose_span") public double botpose_span; + /** + * Average distance to AprilTags used for pose estimation in meters. + * Closer tags generally provide more accurate pose estimates. + */ @JsonProperty("botpose_avgdist") public double botpose_avgdist; + /** + * Average area of AprilTags used for pose estimation (0-100). + * Larger areas indicate tags are closer and provide better resolution. + */ @JsonProperty("botpose_avgarea") public double botpose_avgarea; + /** + * Camera pose in robot space (6-element array: x, y, z, roll, pitch, yaw). + * Describes where the camera is mounted relative to robot center. + */ @JsonProperty("t6c_rs") public double[] camerapose_robotspace; + /** + * Gets the 3D robot pose from the botpose array. + * Uses Limelight's internal coordinate system. + * + * @return Pose3d representing robot position and orientation + */ public Pose3d getBotPose3d() { return toPose3D(botpose); } + /** + * Gets the 3D robot pose in WPILib red alliance coordinates. + * Not recommended - use blue alliance for consistency. + * + * @return Pose3d in red alliance coordinate system + */ public Pose3d getBotPose3d_wpiRed() { return toPose3D(botpose_wpired); } + /** + * Gets the 3D robot pose in WPILib blue alliance coordinates. + * Recommended for most applications. + * + * @return Pose3d in blue alliance coordinate system + */ public Pose3d getBotPose3d_wpiBlue() { return toPose3D(botpose_wpiblue); } + /** + * Gets the 2D robot pose from the botpose array. + * Uses Limelight's internal coordinate system. + * + * @return Pose2d representing robot position and yaw + */ public Pose2d getBotPose2d() { return toPose2D(botpose); } + /** + * Gets the 2D robot pose in WPILib red alliance coordinates. + * Not recommended - use blue alliance for consistency. + * + * @return Pose2d in red alliance coordinate system + */ public Pose2d getBotPose2d_wpiRed() { return toPose2D(botpose_wpired); } + /** + * Gets the 2D robot pose in WPILib blue alliance coordinates. + * Recommended for use with WPILib pose estimators. + * + * @return Pose2d in blue alliance coordinate system + */ public Pose2d getBotPose2d_wpiBlue() { return toPose2D(botpose_wpiblue); } + /** + * Array of retroreflective target results. + * Contains data for all detected retroreflective/color targets. + */ @JsonProperty("Retro") public LimelightTarget_Retro[] targets_Retro; + /** + * Array of AprilTag/fiducial target results. + * Contains data for all detected AprilTags. + */ @JsonProperty("Fiducial") public LimelightTarget_Fiducial[] targets_Fiducials; + /** + * Array of neural classifier results. + * Contains classification results if a classifier pipeline is active. + */ @JsonProperty("Classifier") public LimelightTarget_Classifier[] targets_Classifier; + /** + * Array of neural detector results. + * Contains detection results if a detector pipeline is active. + */ @JsonProperty("Detector") public LimelightTarget_Detector[] targets_Detector; + /** + * Array of barcode detection results. + * Contains decoded barcode data. + */ @JsonProperty("Barcode") public LimelightTarget_Barcode[] targets_Barcode; + /** + * Constructs a new LimelightResults with default values. + * Initializes all arrays to appropriate default sizes. + */ public LimelightResults() { botpose = new double[6]; botpose_wpired = new double[6]; @@ -438,16 +1022,69 @@ public LimelightResults() { } } - /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */ + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + * Provides quick access to basic AprilTag detection data without full JSON parsing. + * + *

RawFiducials are lightweight representations of AprilTag detections available + * directly from NetworkTables. They're faster to access than full JSON parsing + * and useful when you only need basic tag information. + */ public static class RawFiducial { + /** + * AprilTag ID number. + * Corresponds to specific field locations in FRC. + */ public int id = 0; + + /** + * Horizontal offset from principal point in normalized coordinates (-1 to 1). + * More accurate than pixel-based offsets for calibrated cameras. + */ public double txnc = 0; + + /** + * Vertical offset from principal point in normalized coordinates (-1 to 1). + * More accurate than pixel-based offsets for calibrated cameras. + */ public double tync = 0; + + /** + * Target area as percentage of image (0-100). + * Indicates size of tag in camera view. + */ public double ta = 0; + + /** + * Distance from camera to tag in meters. + * Based on tag's known size and camera calibration. + */ public double distToCamera = 0; + + /** + * Distance from robot center to tag in meters. + * Accounts for camera offset from robot center. + */ public double distToRobot = 0; + + /** + * Pose ambiguity (0 = unambiguous, higher = more ambiguous). + * AprilTag pose estimation can have multiple solutions; this indicates confidence. + * Values above 0.2 are generally considered ambiguous. + */ public double ambiguity = 0; + /** + * Constructs a RawFiducial with specified detection parameters. + * + * @param id AprilTag ID number + * @param txnc Horizontal offset from principal point (normalized -1 to 1) + * @param tync Vertical offset from principal point (normalized -1 to 1) + * @param ta Target area percentage (0-100) + * @param distToCamera Distance from camera to tag in meters + * @param distToRobot Distance from robot center to tag in meters + * @param ambiguity Pose ambiguity value (0 = certain, higher = ambiguous) + */ public RawFiducial( int id, double txnc, @@ -466,21 +1103,78 @@ public RawFiducial( } } - /** Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. */ + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + * Provides quick access to neural detector data including bounding box corners. + * + *

RawDetections are lightweight representations of neural network detections available + * directly from NetworkTables. They include bounding box corner coordinates for + * visualization and precise object localization. + */ public static class RawDetection { + /** + * Detected class ID. + * Numeric identifier for the detected object class. + */ public int classId = 0; + + /** + * Horizontal offset from principal point in normalized coordinates (-1 to 1). + */ public double txnc = 0; + + /** + * Vertical offset from principal point in normalized coordinates (-1 to 1). + */ public double tync = 0; + + /** + * Target area as percentage of image (0-100). + * Size of the detection bounding box. + */ public double ta = 0; + + /** X coordinate of corner 0 of bounding box (normalized -1 to 1) */ public double corner0_X = 0; + + /** Y coordinate of corner 0 of bounding box (normalized -1 to 1) */ public double corner0_Y = 0; + + /** X coordinate of corner 1 of bounding box (normalized -1 to 1) */ public double corner1_X = 0; + + /** Y coordinate of corner 1 of bounding box (normalized -1 to 1) */ public double corner1_Y = 0; + + /** X coordinate of corner 2 of bounding box (normalized -1 to 1) */ public double corner2_X = 0; + + /** Y coordinate of corner 2 of bounding box (normalized -1 to 1) */ public double corner2_Y = 0; + + /** X coordinate of corner 3 of bounding box (normalized -1 to 1) */ public double corner3_X = 0; + + /** Y coordinate of corner 3 of bounding box (normalized -1 to 1) */ public double corner3_Y = 0; + /** + * Constructs a RawDetection with specified detection parameters. + * Corner coordinates define the bounding box of the detected object. + * + * @param classId Detected class ID + * @param txnc Horizontal offset from principal point (normalized) + * @param tync Vertical offset from principal point (normalized) + * @param ta Target area percentage + * @param corner0_X X coordinate of bounding box corner 0 + * @param corner0_Y Y coordinate of bounding box corner 0 + * @param corner1_X X coordinate of bounding box corner 1 + * @param corner1_Y Y coordinate of bounding box corner 1 + * @param corner2_X X coordinate of bounding box corner 2 + * @param corner2_Y Y coordinate of bounding box corner 2 + * @param corner3_X X coordinate of bounding box corner 3 + * @param corner3_Y Y coordinate of bounding box corner 3 + */ public RawDetection( int classId, double txnc, @@ -509,20 +1203,82 @@ public RawDetection( } } - /** Represents a 3D Pose Estimate. */ + /** + * Represents a 3D Pose Estimate with metadata about the quality and source of the estimate. + * Used primarily with vision-based localization systems like MegaTag and MegaTag2. + * + *

PoseEstimate combines a robot pose with quality metrics that help determine how + * reliable the estimate is. Use these metrics with WPILib's pose estimator to + * dynamically adjust standard deviations based on detection quality. + * + *

Quality indicators include: + *

    + *
  • Tag count - more tags generally means better accuracy
    • + *
  • Tag span - wider spacing between tags improves accuracy
    • + *
  • Average distance - closer tags provide better resolution
    • + *
  • Average area - larger tags in view provide better data
    • + *
  • Ambiguity data for each tag
    • + *
+ */ public static class PoseEstimate { + /** + * The estimated 2D robot pose on the field. + * Use with WPILib's pose estimator via addVisionMeasurement(). + */ public Pose2d pose; + + /** + * Timestamp of the pose estimate in seconds (FPGA timestamp). + * Synchronized with robot code timing for accurate pose integration. + */ public double timestampSeconds; + + /** + * Total latency of the pose estimate in milliseconds. + * Includes capture and processing time. + */ public double latency; + + /** + * Number of AprilTags used to compute this pose estimate. + * More tags generally provide more accurate and stable estimates. + */ public int tagCount; + + /** + * Distance between the farthest AprilTags used in meters. + * Larger spans reduce angular error and improve accuracy. + */ public double tagSpan; + + /** + * Average distance to all AprilTags used in meters. + * Closer tags provide better pixel resolution and accuracy. + */ public double avgTagDist; + + /** + * Average area of AprilTags used as percentage of image (0-100). + * Larger areas mean better resolution and more accurate pose data. + */ public double avgTagArea; + /** + * Raw fiducial data for all tags used in pose estimation. + * Includes individual tag metrics like ambiguity and distance. + */ public RawFiducial[] rawFiducials; + + /** + * Whether this estimate was computed using the MegaTag2 algorithm. + * MegaTag2 uses robot orientation data for improved accuracy. + */ public boolean isMegaTag2; - /** Instantiates a PoseEstimate object with default values */ + /** + * Constructs a PoseEstimate object with default values. + * All numeric fields initialized to 0, pose to origin, arrays to empty. + */ public PoseEstimate() { this.pose = new Pose2d(); this.timestampSeconds = 0; @@ -535,6 +1291,19 @@ public PoseEstimate() { this.isMegaTag2 = false; } + /** + * Constructs a PoseEstimate with all specified parameters. + * + * @param pose The estimated 2D robot pose + * @param timestampSeconds FPGA timestamp in seconds + * @param latency Total latency in milliseconds + * @param tagCount Number of AprilTags used + * @param tagSpan Distance between farthest tags in meters + * @param avgTagDist Average distance to tags in meters + * @param avgTagArea Average tag area as percentage + * @param rawFiducials Array of raw fiducial data for quality assessment + * @param isMegaTag2 Whether MegaTag2 algorithm was used + */ public PoseEstimate( Pose2d pose, double timestampSeconds, @@ -558,21 +1327,63 @@ public PoseEstimate( } } - /** Encapsulates the state of an internal Limelight IMU. */ + /** + * Encapsulates the state of an internal Limelight IMU (Inertial Measurement Unit). + * Contains orientation, angular velocity, and acceleration data. + * + *

Limelight 3 and newer models include a built-in IMU that can provide robot + * orientation data. This is particularly useful for MegaTag2 localization which + * requires robot orientation input for optimal accuracy. + * + *

IMU data includes: + *

    + *
  • Orientation angles (roll, pitch, yaw)
    • + *
  • Angular velocities (gyroscope data)
    • + *
  • Linear accelerations (accelerometer data)
    • + *
+ */ public static class IMUData { + /** Robot yaw angle in degrees (heading, rotation around Z axis) */ public double robotYaw = 0.0; + + /** Roll angle in degrees (rotation around X axis) */ public double Roll = 0.0; + + /** Pitch angle in degrees (rotation around Y axis) */ public double Pitch = 0.0; + + /** Yaw angle in degrees (rotation around Z axis) */ public double Yaw = 0.0; + + /** Angular velocity around X axis in degrees per second */ public double gyroX = 0.0; + + /** Angular velocity around Y axis in degrees per second */ public double gyroY = 0.0; + + /** Angular velocity around Z axis in degrees per second */ public double gyroZ = 0.0; + + /** Linear acceleration along X axis in G's (1G = 9.81 m/s²) */ public double accelX = 0.0; + + /** Linear acceleration along Y axis in G's */ public double accelY = 0.0; + + /** Linear acceleration along Z axis in G's */ public double accelZ = 0.0; + /** + * Constructs an IMUData object with all values initialized to 0. + */ public IMUData() {} + /** + * Constructs an IMUData object from a double array. + * Expected array format: [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ] + * + * @param imuData Array of IMU values (must be at least 10 elements) + */ public IMUData(double[] imuData) { if (imuData != null && imuData.length >= 10) { this.robotYaw = imuData[0]; @@ -589,11 +1400,26 @@ public IMUData(double[] imuData) { } } + /** + * Jackson ObjectMapper instance for JSON deserialization. + * Lazily initialized on first use. + */ private static ObjectMapper mapper; - /** Print JSON Parse time to the console in milliseconds */ + /** + * When enabled, prints JSON parsing time to console in milliseconds. + * Useful for performance debugging and optimization. + * Set to true to enable performance profiling. + */ static boolean profileJSON = false; + /** + * Sanitizes the Limelight name by returning "limelight" if the input is null or empty. + * This ensures a valid NetworkTables name even when no specific name is provided. + * + * @param name The Limelight name to sanitize + * @return The sanitized name, or "limelight" if input was null/empty + */ static final String sanitizeName(String name) { if (name == null || name.isEmpty()) { return "limelight"; @@ -602,9 +1428,11 @@ static final String sanitizeName(String name) { } /** - * Takes a 6-length array of pose data and converts it to a Pose3d object. Array format: [x, y, z, - * roll, pitch, yaw] where angles are in degrees. - * + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where: + * - x, y, z are in meters + * - roll, pitch, yaw are in degrees + * * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] * @return Pose3d object representing the pose, or empty Pose3d if invalid data */ @@ -622,10 +1450,12 @@ public static Pose3d toPose3D(double[] inData) { } /** - * Takes a 6-length array of pose data and converts it to a Pose2d object. Uses only x, y, and yaw - * components, ignoring z, roll, and pitch. Array format: [x, y, z, roll, pitch, yaw] where angles - * are in degrees. - * + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where: + * - x, y are in meters + * - yaw is in degrees + * * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] * @return Pose2d object representing the pose, or empty Pose2d if invalid data */ @@ -642,7 +1472,8 @@ public static Pose2d toPose2D(double[] inData) { /** * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. * Translation components are in meters, rotation components are in degrees. - * + * Useful for sending pose data to the Limelight or storing in NetworkTables. + * * @param pose The Pose3d object to convert * @return A 6-element array containing [x, y, z, roll, pitch, yaw] */ @@ -659,9 +1490,9 @@ public static double[] pose3dToArray(Pose3d pose) { /** * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. Note: z, roll, and - * pitch will be 0 since Pose2d only contains x, y, and yaw. - * + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * * @param pose The Pose2d object to convert * @return A 6-element array containing [x, y, 0, 0, 0, yaw] */ @@ -676,6 +1507,14 @@ public static double[] pose2dToArray(Pose2d pose) { return result; } + /** + * Extracts a single element from a double array at the specified position. + * Returns 0 if the array is too short or position is out of bounds. + * + * @param inData The source array + * @param position The index to extract (0-based) + * @return The value at the position, or 0 if position is out of bounds + */ private static double extractArrayEntry(double[] inData, int position) { if (inData.length < position + 1) { return 0; @@ -683,6 +1522,27 @@ private static double extractArrayEntry(double[] inData, int position) { return inData[position]; } + /** + * Internal method to retrieve a bot pose estimate from NetworkTables. + * Extracts pose data, latency, tag information, and raw fiducial data from + * a timestamped NetworkTables entry. + * + *

The pose array format is: + *

    + *
  • [0-5]: x, y, z, roll, pitch, yaw
    • + *
  • [6]: latency (ms)
    • + *
  • [7]: tag count
    • + *
  • [8]: tag span (m)
    • + *
  • [9]: average tag distance (m)
    • + *
  • [10]: average tag area (%)
    • + *
  • [11+]: raw fiducial data (7 values per tag)
    • + *
+ * + * @param limelightName Name/identifier of the Limelight + * @param entryName NetworkTables entry name to read from (e.g., "botpose_wpiblue") + * @param isMegaTag2 Whether this is a MegaTag2 estimate + * @return PoseEstimate object, or null if no data available + */ private static PoseEstimate getBotPoseEstimate( String limelightName, String entryName, boolean isMegaTag2) { DoubleArrayEntry poseEntry = @@ -712,7 +1572,7 @@ private static PoseEstimate getBotPoseEstimate( int expectedTotalVals = 11 + valsPerFiducial * tagCount; if (poseArray.length != expectedTotalVals) { - // Don't populate fiducials + // Don't populate fiducials if array length doesn't match expected format } else { for (int i = 0; i < tagCount; i++) { int baseIndex = 11 + (i * valsPerFiducial); @@ -741,9 +1601,10 @@ private static PoseEstimate getBotPoseEstimate( /** * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. - * + * Provides quick access to basic tag data without full JSON parsing. + * * @param limelightName Name/identifier of the Limelight - * @return Array of RawFiducial objects containing detection details + * @return Array of RawFiducial objects containing detection details, or empty array if none */ public static RawFiducial[] getRawFiducials(String limelightName) { var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); @@ -773,10 +1634,11 @@ public static RawFiducial[] getRawFiducials(String limelightName) { } /** - * Gets the latest raw neural detector results from NetworkTables - * + * Gets the latest raw neural detector results from NetworkTables. + * Provides quick access to detection data including bounding box corners. + * * @param limelightName Name/identifier of the Limelight - * @return Array of RawDetection objects containing detection details + * @return Array of RawDetection objects containing detection details, or empty array if none */ public static RawDetection[] getRawDetections(String limelightName) { var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); @@ -814,10 +1676,12 @@ public static RawDetection[] getRawDetections(String limelightName) { } /** - * Prints detailed information about a PoseEstimate to standard output. Includes timestamp, - * latency, tag count, tag span, average tag distance, average tag area, and detailed information - * about each detected fiducial. - * + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + *

Useful for debugging and understanding pose estimate quality. + * * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." */ public static void printPoseEstimate(PoseEstimate pose) { @@ -856,22 +1720,58 @@ public static void printPoseEstimate(PoseEstimate pose) { } } + /** + * Validates whether a PoseEstimate contains usable data. + * Checks that the pose is non-null and contains raw fiducial data. + * + * @param pose The PoseEstimate to validate + * @return True if the pose is non-null and contains raw fiducial data + */ public static Boolean validPoseEstimate(PoseEstimate pose) { return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; } + /** + * Gets the NetworkTable instance for a Limelight camera. + * All Limelight data is published to a table named after the camera. + * + * @param tableName Name/identifier of the Limelight + * @return NetworkTable instance for the specified Limelight + */ public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } + /** + * Flushes NetworkTables to ensure all queued updates are sent immediately. + * Call this after setting multiple values to ensure they're sent together + * and processed by the Limelight in the same update cycle. + * + *

Particularly important when using SetRobotOrientation for MegaTag2. + */ public static void Flush() { NetworkTableInstance.getDefault().flush(); } + /** + * Gets a specific NetworkTables entry for a Limelight camera. + * + * @param tableName Name/identifier of the Limelight + * @param entryName Name of the NetworkTables entry + * @return NetworkTableEntry for the specified entry + */ public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { return getLimelightNTTable(tableName).getEntry(entryName); } + /** + * Gets a DoubleArrayEntry for a Limelight, using a cache to avoid recreating entries. + * This improves performance by reusing entry objects across multiple calls. + * + * @param tableName Name/identifier of the Limelight + * @param entryName Name of the double array entry + * @return Cached or newly created DoubleArrayEntry + */ public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { String key = tableName + "/" + entryName; return doubleArrayEntries.computeIfAbsent( @@ -882,30 +1782,80 @@ public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, St }); } + /** + * Gets a double value from NetworkTables for a Limelight entry. + * + * @param tableName Name/identifier of the Limelight + * @param entryName Name of the entry + * @return Double value, or 0.0 if not found + */ public static double getLimelightNTDouble(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); } + /** + * Sets a double value in NetworkTables for a Limelight entry. + * + * @param tableName Name/identifier of the Limelight + * @param entryName Name of the entry + * @param val Value to set + */ public static void setLimelightNTDouble(String tableName, String entryName, double val) { getLimelightNTTableEntry(tableName, entryName).setDouble(val); } + /** + * Sets a double array value in NetworkTables for a Limelight entry. + * + * @param tableName Name/identifier of the Limelight + * @param entryName Name of the entry + * @param val Array of values to set + */ public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); } + /** + * Gets a double array from NetworkTables for a Limelight entry. + * + * @param tableName Name/identifier of the Limelight + * @param entryName Name of the entry + * @return Double array, or empty array if not found + */ public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); } + /** + * Gets a string value from NetworkTables for a Limelight entry. + * + * @param tableName Name/identifier of the Limelight + * @param entryName Name of the entry + * @return String value, or empty string if not found + */ public static String getLimelightNTString(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getString(""); } + /** + * Gets a string array from NetworkTables for a Limelight entry. + * + * @param tableName Name/identifier of the Limelight + * @param entryName Name of the entry + * @return String array, or empty array if not found + */ public static String[] getLimelightNTStringArray(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); } + /** + * Constructs a URL for making HTTP requests to a Limelight camera. + * Limelights expose an HTTP API on port 5807 for certain operations. + * + * @param tableName Name/identifier of the Limelight + * @param request The HTTP request path (e.g., "capturesnapshot") + * @return URL object for the request, or null if malformed + */ public static URL getLimelightURLString(String tableName, String request) { String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; URL url; @@ -919,11 +1869,12 @@ public static URL getLimelightURLString(String tableName, String request) { } ///// + ///// Basic Target Information Getters ///// /** * Does the Limelight have a valid target? - * + * * @param limelightName Name of the Limelight camera ("" for default) * @return True if a valid target is present, false otherwise */ @@ -933,9 +1884,10 @@ public static boolean getTV(String limelightName) { /** * Gets the horizontal offset from the crosshair to the target in degrees. - * + * Positive values indicate target is to the right of crosshair. + * * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees + * @return Horizontal offset angle in degrees (typically -29.8 to 29.8) */ public static double getTX(String limelightName) { return getLimelightNTDouble(limelightName, "tx"); @@ -943,33 +1895,34 @@ public static double getTX(String limelightName) { /** * Gets the vertical offset from the crosshair to the target in degrees. - * + * Positive values indicate target is above crosshair. + * * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees + * @return Vertical offset angle in degrees (typically -24.85 to 24.85) */ public static double getTY(String limelightName) { return getLimelightNTDouble(limelightName, "ty"); } /** - * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the - * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable - * crosshair functionality. - * + * Gets the horizontal offset from the principal pixel/point to the target in degrees. + * This is the most accurate 2d metric if you are using a calibrated camera and + * you don't need adjustable crosshair functionality. + * * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees + * @return Horizontal offset angle in degrees from principal point */ public static double getTXNC(String limelightName) { return getLimelightNTDouble(limelightName, "txnc"); } /** - * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the - * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable - * crosshair functionality. - * + * Gets the vertical offset from the principal pixel/point to the target in degrees. + * This is the most accurate 2d metric if you are using a calibrated camera and + * you don't need adjustable crosshair functionality. + * * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees + * @return Vertical offset angle in degrees from principal point */ public static double getTYNC(String limelightName) { return getLimelightNTDouble(limelightName, "tync"); @@ -977,7 +1930,8 @@ public static double getTYNC(String limelightName) { /** * Gets the target area as a percentage of the image (0-100%). - * + * Larger values indicate target is closer or larger in frame. + * * @param limelightName Name of the Limelight camera ("" for default) * @return Target area percentage (0-100) */ @@ -986,21 +1940,22 @@ public static double getTA(String limelightName) { } /** - * T2D is an array that contains several targeting metrcis - * + * T2D is an array that contains several targeting metrics in one convenient package. + * Array format: [targetValid, targetCount, targetLatency, captureLatency, tx, ty, + * txnc, tync, ta, tid, targetClassIndexDetector, targetClassIndexClassifier, + * targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, + * targetVerticalExtentPixels, targetSkewDegrees] + * * @param limelightName Name of the Limelight camera - * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, - * txnc, tync, ta, tid, targetClassIndexDetector, targetClassIndexClassifier, - * targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, - * targetVerticalExtentPixels, targetSkewDegrees] + * @return 17-element array containing comprehensive targeting data */ public static double[] getT2DArray(String limelightName) { return getLimelightNTDoubleArray(limelightName, "t2d"); } /** - * Gets the number of targets currently detected. - * + * Gets the number of targets currently detected by the active pipeline. + * * @param limelightName Name of the Limelight camera * @return Number of detected targets */ @@ -1013,8 +1968,8 @@ public static int getTargetCount(String limelightName) { } /** - * Gets the classifier class index from the currently running neural classifier pipeline - * + * Gets the classifier class index from the currently running neural classifier pipeline. + * * @param limelightName Name of the Limelight camera * @return Class index from classifier pipeline */ @@ -1027,9 +1982,9 @@ public static int getClassifierClassIndex(String limelightName) { } /** - * Gets the detector class index from the primary result of the currently running neural detector - * pipeline. - * + * Gets the detector class index from the primary result of the currently running + * neural detector pipeline. + * * @param limelightName Name of the Limelight camera * @return Class index from detector pipeline */ @@ -1043,9 +1998,9 @@ public static int getDetectorClassIndex(String limelightName) { /** * Gets the current neural classifier result class name. - * + * * @param limelightName Name of the Limelight camera - * @return Class name string from classifier pipeline + * @return Class name string from classifier pipeline (e.g., "cone", "cube") */ public static String getClassifierClass(String limelightName) { return getLimelightNTString(limelightName, "tcclass"); @@ -1053,9 +2008,9 @@ public static String getClassifierClass(String limelightName) { /** * Gets the primary neural detector result class name. - * + * * @param limelightName Name of the Limelight camera - * @return Class name string from detector pipeline + * @return Class name string from detector pipeline (e.g., "cone", "cube") */ public static String getDetectorClass(String limelightName) { return getLimelightNTString(limelightName, "tdclass"); @@ -1063,7 +2018,8 @@ public static String getDetectorClass(String limelightName) { /** * Gets the pipeline's processing latency contribution. - * + * This is the time spent processing the image through the vision pipeline. + * * @param limelightName Name of the Limelight camera * @return Pipeline latency in milliseconds */ @@ -1073,7 +2029,8 @@ public static double getLatency_Pipeline(String limelightName) { /** * Gets the capture latency. - * + * This is the time between when the frame was captured and when processing began. + * * @param limelightName Name of the Limelight camera * @return Capture latency in milliseconds */ @@ -1083,7 +2040,7 @@ public static double getLatency_Capture(String limelightName) { /** * Gets the active pipeline index. - * + * * @param limelightName Name of the Limelight camera * @return Current pipeline index (0-9) */ @@ -1092,18 +2049,19 @@ public static double getCurrentPipelineIndex(String limelightName) { } /** - * Gets the current pipeline type. - * + * Gets the current pipeline type string. + * * @param limelightName Name of the Limelight camera - * @return Pipeline type string (e.g. "retro", "apriltag", etc) + * @return Pipeline type string (e.g. "retro", "apriltag", "neural_detector") */ public static String getCurrentPipelineType(String limelightName) { return getLimelightNTString(limelightName, "getpipetype"); } /** - * Gets the full JSON results dump. - * + * Gets the full JSON results dump from the Limelight. + * Contains all targeting data in JSON format for advanced parsing. + * * @param limelightName Name of the Limelight camera * @return JSON string containing all current results */ @@ -1111,11 +2069,16 @@ public static String getJSONDump(String limelightName) { return getLimelightNTString(limelightName, "json"); } + ///// + ///// Pose Getters (Raw Arrays) + ///// + /** - * Switch to getBotPose - * - * @param limelightName - * @return + * Gets robot pose array in Limelight coordinate system. + * + * @deprecated Use {@link #getBotPose(String)} instead + * @param limelightName Name of the Limelight camera + * @return 6-element array [x, y, z, roll, pitch, yaw] */ @Deprecated public static double[] getBotpose(String limelightName) { @@ -1123,10 +2086,11 @@ public static double[] getBotpose(String limelightName) { } /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return + * Gets robot pose array in WPILib red alliance coordinates. + * + * @deprecated Use {@link #getBotPose_wpiRed(String)} instead + * @param limelightName Name of the Limelight camera + * @return 6-element array [x, y, z, roll, pitch, yaw] */ @Deprecated public static double[] getBotpose_wpiRed(String limelightName) { @@ -1134,346 +2098,989 @@ public static double[] getBotpose_wpiRed(String limelightName) { } /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return + * Gets robot pose array in WPILib blue alliance coordinates. + * + * @deprecated Use {@link #getBotPose_wpiBlue(String)} instead + * @param limelightName Name of the Limelight camera + * @return 6-element array [x, y, z, roll, pitch, yaw] */ @Deprecated public static double[] getBotpose_wpiBlue(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); } + /** + * Gets robot pose array in Limelight coordinate system. + * + * @param limelightName Name of the Limelight camera + * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees + */ public static double[] getBotPose(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose"); } + /** + * Gets robot pose array in WPILib red alliance coordinate system. + * Origin at red alliance driver station wall. + * + * @param limelightName Name of the Limelight camera + * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees + */ public static double[] getBotPose_wpiRed(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); } + /** + * Gets robot pose array in WPILib blue alliance coordinate system. + * Origin at blue alliance driver station wall. Recommended for most use cases. + * + * @param limelightName Name of the Limelight camera + * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees + */ public static double[] getBotPose_wpiBlue(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); } + /** + * Gets robot pose in target space as an array. + * Represents where the robot is relative to the detected target. + * + * @param limelightName Name of the Limelight camera + * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees + */ public static double[] getBotPose_TargetSpace(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); } + /** + * Gets camera pose in target space as an array. + * Represents where the camera is relative to the detected target. + * + * @param limelightName Name of the Limelight camera + * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees + */ public static double[] getCameraPose_TargetSpace(String limelightName) { return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); } + /** + * Gets target pose in camera space as an array. + * Represents where the target is relative to the camera. + * + * @param limelightName Name of the Limelight camera + * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees + */ public static double[] getTargetPose_CameraSpace(String limelightName) { return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); } + /** + * Gets target pose in robot space as an array. + * Represents where the target is relative to the robot center. + * + * @param limelightName Name of the Limelight camera + * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees + */ public static double[] getTargetPose_RobotSpace(String limelightName) { return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); } + /** + * Gets the RGB color values detected by the color pipeline. + * Useful for color-based target identification. + * + * @param limelightName Name of the Limelight camera + * @return Array of color values [R, G, B] (0-255) + */ public static double[] getTargetColor(String limelightName) { return getLimelightNTDoubleArray(limelightName, "tc"); } + /** + * Gets the current pipeline's fiducial ID of the primary target. + * + * @param limelightName Name of the Limelight camera + * @return Fiducial/AprilTag ID number + */ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } + /** + * Gets the neural network classifier class ID string. + * + * @param limelightName Name of the Limelight camera + * @return Class name string from the neural classifier + */ public static String getNeuralClassID(String limelightName) { return getLimelightNTString(limelightName, "tclass"); } + /** + * Gets raw barcode data as a string array. + * Each element contains the decoded string from a detected barcode. + * + * @param limelightName Name of the Limelight camera + * @return Array of decoded barcode strings + */ public static String[] getRawBarcodeData(String limelightName) { return getLimelightNTStringArray(limelightName, "rawbarcodes"); } ///// + ///// Pose Getters (Pose3d Objects) ///// + /** + * Gets the robot's 3D pose in field space using Limelight's internal coordinate system. + * + *

This method retrieves the robot's position and orientation as calculated by the Limelight + * based on AprilTag detections and the known field layout. The pose is in Limelight's native + * coordinate system. + * + *

Note: For WPILib integration, prefer {@link #getBotPose3d_wpiBlue(String)} which + * uses WPILib's standard blue alliance coordinate system. + * + * @param limelightName Name/identifier of the Limelight camera (use "" for default "limelight") + * @return Pose3d object representing robot position (meters) and orientation (radians) + */ public static Pose3d getBotPose3d(String limelightName) { + // Retrieve the 6-element pose array from NetworkTables double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + // Convert array format [x, y, z, roll, pitch, yaw] to Pose3d object return toPose3D(poseArray); } /** - * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. - * - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Red Alliance field - * space + * Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * + *

Not Recommended: Red alliance coordinates can be confusing as field orientation + * changes between alliances. Use {@link #getBotPose3d_wpiBlue(String)} instead for consistency. + * + *

In the red alliance coordinate system: + *

    + *
  • Origin (0,0) is at the red alliance driver station wall
    • + *
  • +X extends toward the blue alliance wall
    • + *
  • +Y extends to the left when facing the blue alliance wall
    • + *
  • Yaw of 0° means robot is facing the blue alliance wall
    • + *
+ * + * @param limelightName Name/identifier of the Limelight camera + * @return Pose3d in red alliance field space (meters and radians) */ public static Pose3d getBotPose3d_wpiRed(String limelightName) { + // Retrieve red alliance pose from NetworkTables double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); return toPose3D(poseArray); } /** - * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * + *

Recommended: This is the standard coordinate system for WPILib pose estimation + * and should be used with {@code SwerveDrivePoseEstimator.addVisionMeasurement()} or + * {@code DifferentialDrivePoseEstimator.addVisionMeasurement()}. + * + *

In the blue alliance coordinate system: + *

    + *
  • Origin (0,0) is at the blue alliance driver station wall
    • + *
  • +X extends toward the red alliance wall
    • + *
  • +Y extends to the left when facing the red alliance wall
    • + *
  • Yaw of 0° means robot is facing the red alliance wall
    • + *
+ * + *

Example usage: + * 

{@code
+   * Pose3d robotPose = LimelightHelpers.getBotPose3d_wpiBlue("limelight");
+   * // Convert to 2D for odometry: robotPose.toPose2d()
+   * }
* - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Blue Alliance field - * space + * @param limelightName Name/identifier of the Limelight camera + * @return Pose3d in blue alliance field space (meters and radians) */ public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + // Retrieve blue alliance pose from NetworkTables double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); return toPose3D(poseArray); } /** - * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * Gets the robot's 3D pose relative to the currently tracked target's coordinate system. + * + *

This provides the robot's position and orientation with respect to the detected target + * (AprilTag or other fiducial). Useful for target-relative navigation where you want to + * approach or align with a specific target. + * + *

In target space: + *

    + *
  • Origin (0,0,0) is at the target's center
    • + *
  • Coordinate axes depend on target orientation
    • + *
  • For AprilTags: +Z extends out from tag face, +X is right, +Y is down
    • + *
+ * + *

Example usage: + * 

{@code
+   * // Get pose relative to detected AprilTag
+   * Pose3d robotToTag = LimelightHelpers.getBotPose3d_TargetSpace("limelight");
+   * double distanceToTag = robotToTag.getTranslation().getNorm();
+   * }
* - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation relative to the target + * @param limelightName Name/identifier of the Limelight camera + * @return Pose3d representing robot position/orientation relative to the target */ public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + // Retrieve target-space pose from NetworkTables double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); return toPose3D(poseArray); } /** - * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * Gets the camera's 3D pose relative to the currently tracked target's coordinate system. + * + *

This provides the camera's position and orientation with respect to the detected target. + * Different from {@link #getBotPose3d_TargetSpace(String)} in that it gives the camera + * position rather than the robot center position. + * + *

Useful for: + *

    + *
  • Verifying camera calibration and mounting
    • + *
  • Direct camera-to-target distance calculations
    • + *
  • Debugging vision system setup
    • + *
* - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the target + * @param limelightName Name/identifier of the Limelight camera + * @return Pose3d representing camera position/orientation relative to the target */ public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + // Retrieve camera-to-target pose from NetworkTables double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); return toPose3D(poseArray); } /** - * Gets the target's 3D pose with respect to the camera's coordinate system. + * Gets the target's 3D pose relative to the camera's coordinate system. + * + *

This provides where the target is located from the camera's perspective. Useful for + * direct targeting calculations where you want to point the camera (and therefore robot) + * at a specific target. + * + *

In camera space: + *

    + *
  • Origin (0,0,0) is at the camera's optical center
    • + *
  • +Z extends forward (into the scene)
    • + *
  • +X extends right, +Y extends down (standard camera coordinates)
    • + *
+ * + *

Example usage: + * 

{@code
+   * Pose3d targetPose = LimelightHelpers.getTargetPose3d_CameraSpace("limelight");
+   * double targetDistance = targetPose.getTranslation().getZ(); // Forward distance
+   * double targetAngle = Math.atan2(targetPose.getTranslation().getX(), 
+   *                                   targetPose.getTranslation().getZ());
+   * }
* - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the camera + * @param limelightName Name/identifier of the Limelight camera + * @return Pose3d representing target position/orientation relative to the camera */ public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + // Retrieve target-in-camera-space pose from NetworkTables double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); return toPose3D(poseArray); } /** - * Gets the target's 3D pose with respect to the robot's coordinate system. + * Gets the target's 3D pose relative to the robot's coordinate system. + * + *

This provides where the target is located from the robot center's perspective, + * accounting for the camera's mounting position on the robot. This is the most useful + * target pose for robot-centric navigation and control. + * + *

In robot space: + *

    + *
  • Origin (0,0,0) is at the robot's center (as configured)
    • + *
  • +X extends forward from robot front
    • + *
  • +Y extends left, +Z extends up
    • + *
+ * + *

Example usage: + * 

{@code
+   * Pose3d targetPose = LimelightHelpers.getTargetPose3d_RobotSpace("limelight");
+   * Translation3d targetTranslation = targetPose.getTranslation();
+   * 
+   * // Calculate angle to target for turret control
+   * double angleToTarget = Math.atan2(targetTranslation.getY(), 
+   *                                     targetTranslation.getX());
+   * // Calculate distance for shooter velocity
+   * double distanceToTarget = targetTranslation.getNorm();
+   * }
* - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the robot + * @param limelightName Name/identifier of the Limelight camera + * @return Pose3d representing target position/orientation relative to robot center */ public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + // Retrieve target-in-robot-space pose from NetworkTables double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); return toPose3D(poseArray); } /** - * Gets the camera's 3D pose with respect to the robot's coordinate system. + * Gets the camera's 3D pose relative to the robot's coordinate system. + * + *

This represents where the camera is mounted on the robot. This value should match + * your camera's physical mounting position and is used internally for transforming + * between camera space and robot space. + * + *

Useful for: + *

    + *
  • Verifying camera mounting configuration
    • + *
  • Debugging coordinate transformations
    • + *
  • Custom pose estimation calculations
    • + *
+ * + *

Note: This can be configured via {@link #setCameraPose_RobotSpace(String, double, + * double, double, double, double, double)} or through the Limelight web interface. * - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the robot + * @param limelightName Name/identifier of the Limelight camera + * @return Pose3d representing camera mounting position/orientation relative to robot center */ public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + // Retrieve camera mounting pose from NetworkTables double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); return toPose3D(poseArray); } + ///// + ///// Pose Getters (Pose2d Objects) - For Odometry Integration + ///// + /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * Gets the robot's 2D pose in WPILib blue alliance coordinates. + * + *

This is a convenience method that extracts just the x, y, and yaw from the full 3D pose, + * making it ready for direct use with WPILib's 2D odometry and pose estimation systems. + * + *

Recommended for: Direct integration with WPILib pose estimators + * + *

Example usage: + * 

{@code
+   * // Simple approach - just get the pose
+   * Pose2d visionPose = LimelightHelpers.getBotPose2d_wpiBlue("limelight");
+   * }
* - * @param limelightName - * @return + * @param limelightName Name/identifier of the Limelight camera + * @return Pose2d in blue alliance coordinates (meters and radians) */ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - + // Get the raw pose array (includes 3D data) double[] result = getBotPose_wpiBlue(limelightName); + // Convert to 2D, discarding z, roll, pitch return toPose2D(result); } /** - * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Gets the MegaTag1 2D pose estimate with timestamp and quality metrics for WPILib blue alliance. + * + *

MegaTag1 uses multiple AprilTags simultaneously to compute a more accurate robot + * pose. This method returns not just the pose, but also timing and quality information needed + * for optimal pose estimator integration. + * + *

The returned {@link PoseEstimate} includes: + *

    + *
  • Robot pose (Pose2d)
    • + *
  • FPGA timestamp for proper temporal alignment
    • + *
  • Total latency (capture + processing)
    • + *
  • Number of tags used (more is better)
    • + *
  • Tag span (wider spacing is better)
    • + *
  • Average tag distance and area (quality metrics)
    • + *
+ * + *

Example usage with pose estimator: + * 

{@code
+   * PoseEstimate estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
+   * 
+   * if (LimelightHelpers.validPoseEstimate(estimate)) {
+   *     // Adjust standard deviations based on tag count and distance
+   *     Matrix stdDevs = VecBuilder.fill(
+   *         0.7 / estimate.tagCount,  // X std dev
+   *         0.7 / estimate.tagCount,  // Y std dev  
+   *         9999999                    // Theta std dev (trust gyro more)
+   *     );
+   *     
+   *     poseEstimator.addVisionMeasurement(
+   *         estimate.pose,
+   *         estimate.timestampSeconds,
+   *         stdDevs
+   *     );
+   * }
+   * }
* - * @param limelightName - * @return + * @param limelightName Name/identifier of the Limelight camera + * @return PoseEstimate with pose, timestamp, and quality metrics, or null if no data available */ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + // Get pose estimate from the standard botpose_wpiblue entry return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); } /** - * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. Make sure you are calling - * setRobotOrientation() before calling this method. + * Gets the MegaTag2 2D pose estimate with timestamp and quality metrics for WPILib blue alliance. + * + *

MegaTag2 is an advanced localization algorithm that incorporates robot orientation + * data (from your gyro/IMU) to improve pose accuracy. This can significantly reduce pose jumps + * and improve overall reliability. + * + *

CRITICAL: You MUST call {@link #SetRobotOrientation(String, double, double, double, + * double, double, double)} or {@link #SetRobotOrientation_NoFlush(String, double, double, double, + * double, double, double)} before calling this method in your periodic loop. MegaTag2 requires + * current robot orientation to function correctly. + * + *

Advantages of MegaTag2: + *

    + *
  • More stable pose estimates with less jumping
    • + *
  • Better disambiguation of pose solutions
    • + *
  • Improved accuracy, especially at longer distances
    • + *
  • Better handling of partial tag visibility
    • + *
+ * + *

Example usage: + * 

{@code
+   * // In your subsystem's periodic() method:
+   * 
+   * // 1. Update robot orientation (required for MegaTag2)
+   * LimelightHelpers.SetRobotOrientation(
+   *     "limelight",
+   *     m_gyro.getYaw(),           // Robot yaw
+   *     m_gyro.getRate(),          // Yaw rate (optional but helpful)
+   *     0, 0, 0, 0                 // Pitch and roll (usually not needed)
+   * );
+   * 
+   * // 2. Get MegaTag2 pose estimate
+   * PoseEstimate estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
+   * 
+   * // 3. Use with pose estimator
+   * if (LimelightHelpers.validPoseEstimate(estimate)) {
+   *     // Calculate dynamic standard deviations based on quality
+   *     double xyStdDev = 0.5;
+   *     if (estimate.tagCount >= 2) xyStdDev = 0.3;
+   *     if (estimate.avgTagDist > 4.0) xyStdDev *= 2;
+   *     
+   *     poseEstimator.addVisionMeasurement(
+   *         estimate.pose,
+   *         estimate.timestampSeconds,
+   *         VecBuilder.fill(xyStdDev, xyStdDev, 9999999)
+   *     );
+   * }
+   * }
* - * @param limelightName - * @return + * @param limelightName Name/identifier of the Limelight camera + * @return PoseEstimate with pose, timestamp, and quality metrics, or null if no data available + * @see #SetRobotOrientation(String, double, double, double, double, double, double) */ public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + // Get pose estimate from the MegaTag2 (ORB = Orientation-based Robot Localization) entry return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); } /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) - * - * @param limelightName - * @return + * Gets the robot's 2D pose in WPILib red alliance coordinates. + * + *

Not Recommended: Use {@link #getBotPose2d_wpiBlue(String)} instead for consistency. + * + * @param limelightName Name/identifier of the Limelight camera + * @return Pose2d in red alliance coordinates (meters and radians) */ public static Pose2d getBotPose2d_wpiRed(String limelightName) { - + // Get raw red alliance pose array double[] result = getBotPose_wpiRed(limelightName); + // Convert to 2D return toPose2D(result); } + /** + * Gets a basic pose estimate in Limelight's internal coordinate system. + * + *

For most FRC applications, use {@link #getBotPoseEstimate_wpiBlue(String)} instead + * which provides the pose in WPILib's standard blue alliance coordinates. + * + * @param limelightName Name/identifier of the Limelight camera + * @return PoseEstimate in Limelight coordinates, or null if no data available + */ public static PoseEstimate getBotPoseEstimate(String limelightName) { + // Get pose estimate from the standard botpose entry (Limelight coordinates) return getBotPoseEstimate(limelightName, "botpose", false); } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when - * you are on the RED alliance + * Gets the MegaTag1 pose estimate for red alliance coordinates. + * + *

Not Recommended: Use {@link #getBotPoseEstimate_wpiBlue(String)} instead. * - * @param limelightName - * @return + * @param limelightName Name/identifier of the Limelight camera + * @return PoseEstimate in red alliance coordinates, or null if no data available */ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + // Get pose estimate from red alliance entry return getBotPoseEstimate(limelightName, "botpose_wpired", false); } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when - * you are on the RED alliance + * Gets the MegaTag2 pose estimate for red alliance coordinates. + * + *

Not Recommended: Use {@link #getBotPoseEstimate_wpiBlue_MegaTag2(String)} instead. + * + *

CRITICAL: You MUST call {@link #SetRobotOrientation(String, double, double, double, + * double, double, double)} before using this method. * - * @param limelightName - * @return + * @param limelightName Name/identifier of the Limelight camera + * @return PoseEstimate in red alliance coordinates, or null if no data available */ public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + // Get MegaTag2 pose estimate from red alliance ORB entry return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); } /** - * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) + * Gets the robot's 2D pose in Limelight's internal coordinate system. + * + *

For WPILib integration, prefer {@link #getBotPose2d_wpiBlue(String)} instead. * - * @param limelightName - * @return + * @param limelightName Name/identifier of the Limelight camera + * @return Pose2d in Limelight coordinates (meters and radians) */ public static Pose2d getBotPose2d(String limelightName) { - + // Get raw pose array in Limelight coordinates double[] result = getBotPose(limelightName); + // Convert to 2D return toPose2D(result); } + ///// + ///// IMU Data + ///// + /** - * Gets the current IMU data from NetworkTables. IMU data is formatted as [robotYaw, Roll, Pitch, - * Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. Returns all zeros if data is invalid or - * unavailable. + * Gets the current IMU (Inertial Measurement Unit) data from the Limelight. + * + *

Limelight 3 and newer models include a built-in 6-axis IMU that provides orientation + * and motion data. This IMU can be used for robot localization, particularly with MegaTag2. + * + *

The IMU provides: + *

    + *
  • Orientation: Roll, pitch, and yaw angles
    • + *
  • Gyroscope: Angular velocities around X, Y, Z axes
    • + *
  • Accelerometer: Linear accelerations along X, Y, Z axes
    • + *
+ * + *

Coordinate System: + *

    + *
  • X: Right (from camera perspective)
    • + *
  • Y: Down
    • + *
  • Z: Forward (into scene)
    • + *
+ * + *

Units: + *

    + *
  • Angles: Degrees
    • + *
  • Angular velocities: Degrees per second
    • + *
  • Accelerations: G's (1G = 9.81 m/s²)
    • + *
+ * + *

Example usage: + * 

{@code
+   * IMUData imu = LimelightHelpers.getIMUData("limelight");
+   * 
+   * // Get robot heading
+   * double heading = imu.robotYaw;
+   * 
+   * // Get angular velocity for velocity tracking
+   * double turnRate = imu.gyroZ;
+   * 
+   * // Detect high acceleration events
+   * double totalAccel = Math.sqrt(
+   *     imu.accelX * imu.accelX + 
+   *     imu.accelY * imu.accelY + 
+   *     imu.accelZ * imu.accelZ
+   * );
+   * }
+ * + *

Note: Returns an IMUData object with all zeros if data is invalid or unavailable. + * Always check if your Limelight model supports IMU before relying on this data. * - * @param limelightName Name/identifier of the Limelight - * @return IMUData object containing all current IMU data + * @param limelightName Name/identifier of the Limelight camera + * @return IMUData object containing all current IMU measurements, or all zeros if unavailable */ public static IMUData getIMUData(String limelightName) { + // Retrieve IMU array from NetworkTables + // Format: [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ] double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + + // Validate array length (should be 10 elements) if (imuData == null || imuData.length < 10) { - return new IMUData(); // Returns object with all zeros + // Return zeroed IMUData if data is missing or invalid + return new IMUData(); } + + // Parse array into IMUData object return new IMUData(imuData); } ///// + ///// Camera Control Methods ///// + /** + * Sets the active vision pipeline. + * + *

Limelight supports up to 10 pipelines (0-9), each with different vision processing + * settings. Use this to switch between different tracking modes during a match. + * + *

Common pipeline uses: + *

    + *
  • Pipeline 0: AprilTag tracking for localization
    • + *
  • Pipeline 1: Retroreflective target tracking
    • + *
  • Pipeline 2: Neural network detector for game pieces
    • + *
+ * + *

Example usage: + * 

{@code
+   * // Switch to AprilTag pipeline during auto
+   * LimelightHelpers.setPipelineIndex("limelight", 0);
+   * 
+   * // Switch to game piece detector during teleop
+   * LimelightHelpers.setPipelineIndex("limelight", 2);
+   * }
+ * + * @param limelightName Name of the Limelight camera + * @param pipelineIndex Pipeline index to activate (0-9) + */ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + /** + * Sets the priority AprilTag ID for tracking. + * + *

When multiple AprilTags are visible, setting a priority ID tells the Limelight to + * prefer that specific tag for targeting calculations. The primary target will be the + * priority tag if it's visible, otherwise it falls back to normal sorting. + * + *

Use cases: + *

    + *
  • Targeting a specific scoring location
    • + *
  • Prioritizing closer/more relevant field elements
    • + *
  • Improving localization with strategically placed tags
    • + *
+ * + *

Example usage: + * 

{@code
+   * // Target tag #4 (e.g., speaker center tag)
+   * LimelightHelpers.setPriorityTagID("limelight", 4);
+   * 
+   * // Clear priority (set to -1 or use any invalid ID)
+   * LimelightHelpers.setPriorityTagID("limelight", -1);
+   * }
+ * + * @param limelightName Name of the Limelight camera + * @param ID AprilTag ID to prioritize, or -1 to clear priority + */ public static void setPriorityTagID(String limelightName, int ID) { setLimelightNTDouble(limelightName, "priorityid", ID); } + ///// + ///// LED Control Methods + ///// + /** - * Sets LED mode to be controlled by the current pipeline. + * Sets LED mode to be controlled by the current pipeline's configuration. + * + *

Each pipeline can have its own LED mode setting configured in the Limelight web interface. + * This is the default and recommended mode - let each pipeline control its own LEDs based on + * what tracking mode is active. + * + *

Example: Your AprilTag pipeline might have LEDs off, while your retroreflective + * pipeline has them on. * * @param limelightName Name of the Limelight camera */ public static void setLEDMode_PipelineControl(String limelightName) { + // ledMode 0 = pipeline control setLimelightNTDouble(limelightName, "ledMode", 0); } + /** + * Forces the LEDs off regardless of pipeline settings. + * + *

Useful for conserving power, reducing visual distractions, or when LEDs aren't needed + * (e.g., when using AprilTags or neural networks that don't require illumination). + * + *

Example usage: + * 

{@code
+   * // Turn off LEDs during auto when using AprilTags
+   * LimelightHelpers.setLEDMode_ForceOff("limelight");
+   * }
+ * + * @param limelightName Name of the Limelight camera + */ public static void setLEDMode_ForceOff(String limelightName) { + // ledMode 1 = force off setLimelightNTDouble(limelightName, "ledMode", 1); } + /** + * Forces the LEDs to blink regardless of pipeline settings. + * + *

Creates a blinking pattern useful for visual feedback or signaling. Can be used to + * indicate robot states, warn drivers, or debug vision system operation. + * + *

Example usage: + * 

{@code
+   * // Blink LEDs when target is acquired
+   * if (LimelightHelpers.getTV("limelight")) {
+   *     LimelightHelpers.setLEDMode_ForceBlink("limelight");
+   * }
+   * }
+ * + * @param limelightName Name of the Limelight camera + */ public static void setLEDMode_ForceBlink(String limelightName) { + // ledMode 2 = force blink setLimelightNTDouble(limelightName, "ledMode", 2); } + /** + * Forces the LEDs on at full brightness regardless of pipeline settings. + * + *

Ensures LEDs are always on, overriding pipeline configuration. Necessary for retroreflective + * tracking where consistent illumination is critical for reliable target detection. + * + *

Example usage: + * 

{@code
+   * // Ensure LEDs are on for retroreflective tape tracking
+   * LimelightHelpers.setLEDMode_ForceOn("limelight");
+   * }
+ * + * @param limelightName Name of the Limelight camera + */ public static void setLEDMode_ForceOn(String limelightName) { + // ledMode 3 = force on setLimelightNTDouble(limelightName, "ledMode", 3); } + ///// + ///// Stream Control Methods + ///// + /** * Enables standard side-by-side stream mode. + * + *

Displays both the primary and secondary camera streams (or processing views) side by side. + * This is the default streaming mode and is useful for seeing both the raw camera feed and + * the processed output simultaneously. + * + *

Stream modes: + *

    + *
  • Standard: Side-by-side view of both streams
    • + *
  • PiP Main: Secondary stream in corner of primary
    • + *
  • PiP Secondary: Primary stream in corner of secondary
    • + *
* * @param limelightName Name of the Limelight camera */ public static void setStreamMode_Standard(String limelightName) { + // stream 0 = standard side-by-side setLimelightNTDouble(limelightName, "stream", 0); } /** - * Enables Picture-in-Picture mode with secondary stream in the corner. + * Enables Picture-in-Picture mode with the secondary stream displayed in the corner. + * + *

Shows the primary stream in full view with the secondary stream as a small overlay + * in the corner. Useful when you want to focus on one view but still monitor the other. * * @param limelightName Name of the Limelight camera */ public static void setStreamMode_PiPMain(String limelightName) { + // stream 1 = PiP with secondary in corner setLimelightNTDouble(limelightName, "stream", 1); } /** - * Enables Picture-in-Picture mode with primary stream in the corner. + * Enables Picture-in-Picture mode with the primary stream displayed in the corner. + * + *

Shows the secondary stream in full view with the primary stream as a small overlay + * in the corner. Useful for monitoring processed output while viewing raw camera feed. * * @param limelightName Name of the Limelight camera */ public static void setStreamMode_PiPSecondary(String limelightName) { + // stream 2 = PiP with primary in corner setLimelightNTDouble(limelightName, "stream", 2); } + ///// + ///// Advanced Camera Configuration + ///// + /** - * Sets the crop window for the camera. The crop window in the UI must be completely open. + * Sets a dynamic crop window for the camera to limit the processing region. + * + *

Cropping restricts vision processing to a specific region of the camera's field of view. + * This can significantly improve performance by reducing the area that needs to be analyzed, + * and can help eliminate false detections from irrelevant parts of the image. + * + *

IMPORTANT: The crop window in the Limelight web interface must be set to fully + * open (uncropped) for this dynamic cropping to work. This method overrides the UI setting. + * + *

Coordinate System: + *

    + *
  • -1.0 = left/bottom edge of image
    • + *
  • 0.0 = center of image
    • + *
  • +1.0 = right/top edge of image
    • + *
+ * + *

Example usage: + * 

{@code
+   * // Crop to center 50% of image (horizontally and vertically)
+   * LimelightHelpers.setCropWindow("limelight", -0.25, 0.25, -0.25, 0.25);
+   * 
+   * // Crop to upper half of image (useful for high targets)
+   * LimelightHelpers.setCropWindow("limelight", -1.0, 1.0, 0.0, 1.0);
+   * 
+   * // Crop to right side (useful for side-mounted camera)
+   * LimelightHelpers.setCropWindow("limelight", 0.0, 1.0, -1.0, 1.0);
+   * 
+   * // Reset to full image
+   * LimelightHelpers.setCropWindow("limelight", -1.0, 1.0, -1.0, 1.0);
+   * }
* * @param limelightName Name of the Limelight camera - * @param cropXMin Minimum X value (-1 to 1) - * @param cropXMax Maximum X value (-1 to 1) - * @param cropYMin Minimum Y value (-1 to 1) - * @param cropYMax Maximum Y value (-1 to 1) + * @param cropXMin Minimum X value - left edge of crop region (-1 to 1) + * @param cropXMax Maximum X value - right edge of crop region (-1 to 1) + * @param cropYMin Minimum Y value - bottom edge of crop region (-1 to 1) + * @param cropYMax Maximum Y value - top edge of crop region (-1 to 1) */ public static void setCropWindow( String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + // Build array with crop boundaries double[] entries = new double[4]; entries[0] = cropXMin; entries[1] = cropXMax; entries[2] = cropYMin; entries[3] = cropYMax; + // Send to NetworkTables setLimelightNTDoubleArray(limelightName, "crop", entries); } - /** Sets 3D offset point for easy 3D targeting. */ + /** + * Sets a 3D offset point for fiducial (AprilTag) tracking. + * + *

This allows you to track a point offset from the center of the AprilTag. Instead of + * getting the robot's pose relative to the tag center, you can get the pose relative to + * a point of interest near the tag (e.g., a scoring position). + * + *

Use cases: + *

    + *
  • Tracking a scoring position that's offset from the tag
    • + *
  • Targeting a specific game element near a tag
    • + *
  • Accounting for tag placement relative to field features
    • + *
+ * + *

Coordinate System: Offsets are relative to the tag's coordinate system: + *

    + *
  • +X extends right from tag
    • + *
  • +Y extends down from tag
    • + *
  • +Z extends out from tag face (toward camera)
    • + *
+ * + *

Example usage: + * 

{@code
+   * // Track a point 0.5m to the right and 0.3m forward from the tag
+   * LimelightHelpers.setFiducial3DOffset("limelight", 0.5, 0.0, 0.3);
+   * 
+   * // Reset to tag center
+   * LimelightHelpers.setFiducial3DOffset("limelight", 0.0, 0.0, 0.0);
+   * }
+ * + * @param limelightName Name of the Limelight camera + * @param offsetX X offset in meters (right is positive) + * @param offsetY Y offset in meters (down is positive) + * @param offsetZ Z offset in meters (away from tag is positive) + */ public static void setFiducial3DOffset( String limelightName, double offsetX, double offsetY, double offsetZ) { + // Build offset array [x, y, z] double[] entries = new double[3]; entries[0] = offsetX; entries[1] = offsetY; entries[2] = offsetZ; + // Send to NetworkTables setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); } + ///// + ///// MegaTag2 Configuration + ///// + /** - * Sets robot orientation values used by MegaTag2 localization algorithm. + * Sets robot orientation values for the MegaTag2 localization algorithm. + * + *

CRITICAL for MegaTag2: This method MUST be called periodically (in your subsystem's + * periodic() or in your main robot periodic()) before retrieving MegaTag2 pose estimates. + * MegaTag2 uses robot orientation to improve pose accuracy and disambiguate solutions. + * + *

This method automatically flushes NetworkTables to ensure the orientation data is + * sent immediately. For better performance when setting multiple values, use + * {@link #SetRobotOrientation_NoFlush(String, double, double, double, double, double, double)} + * and call {@link #Flush()} once after all updates. + * + *

Coordinate System: All angles follow standard FRC conventions: + *

    + *
  • Yaw: 0° = facing red alliance wall, increases counterclockwise
    • + *
  • Pitch: 0° = level, positive = nose up
    • + *
  • Roll: 0° = level, positive = right side down
    • + *
+ * + *

Parameter Importance: + *

    + *
  • yaw: REQUIRED - This is the most critical parameter
    • + *
  • yawRate: Helpful but optional - improves motion prediction
    • + *
  • pitch, pitchRate, roll, rollRate: Usually unnecessary for ground robots
    • + *
+ * + *

Example usage (minimal - just yaw): + * 

{@code
+   * // In your vision subsystem's periodic():
+   * public void periodic() {
+   *     // Update robot orientation for MegaTag2
+   *     LimelightHelpers.SetRobotOrientation(
+   *         "limelight",
+   *         m_gyro.getYaw(),  // Robot heading from gyro
+   *         0, 0, 0, 0, 0     // Other parameters can be zero
+   *     );
+   *     
+   *     // Now safe to use MegaTag2
+   *     PoseEstimate pose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
+   * }
+   * }
+ * + *

Example usage (with yaw rate): + * 

{@code
+   * LimelightHelpers.SetRobotOrientation(
+   *     "limelight",
+   *     m_gyro.getYaw(),      // Current heading
+   *     m_gyro.getRate(),     // Angular velocity (helps with moving robots)
+   *     0, 0, 0, 0            // Pitch and roll usually not needed
+   * );
+   * }
* - * @param limelightName Name/identifier of the Limelight - * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC - * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second - * @param pitch (Unnecessary) Robot pitch in degrees - * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second - * @param roll (Unnecessary) Robot roll in degrees - * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + * @param limelightName Name/identifier of the Limelight camera + * @param yaw Robot yaw angle in degrees (0 = facing red alliance wall) + * @param yawRate Angular velocity of yaw in degrees per second (optional, can be 0) + * @param pitch Robot pitch angle in degrees (optional, usually 0 for ground robots) + * @param pitchRate Angular velocity of pitch in degrees per second (optional, usually 0) + * @param roll Robot roll angle in degrees (optional, usually 0 for ground robots) + * @param rollRate Angular velocity of roll in degrees per second (optional, usually 0) */ public static void SetRobotOrientation( String limelightName, @@ -1483,10 +3090,37 @@ public static void SetRobotOrientation( double pitchRate, double roll, double rollRate) { + // Call internal method with flush enabled SetRobotOrientation_INTERNAL( limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); } + /** + * Sets robot orientation for MegaTag2 without automatically flushing NetworkTables. + * + *

Use this variant when setting multiple NetworkTables values in sequence to improve + * performance. You must manually call {@link #Flush()} after all updates to ensure + * the data is sent to the Limelight. + * + *

Example usage: + * 

{@code
+   * // Set multiple values efficiently
+   * LimelightHelpers.SetRobotOrientation_NoFlush("limelight", yaw, yawRate, 0, 0, 0, 0);
+   * LimelightHelpers.setPipelineIndex("limelight", 0);
+   * LimelightHelpers.setPriorityTagID("limelight", 4);
+   * // Flush once after all updates
+   * LimelightHelpers.Flush();
+   * }
+ * + * @param limelightName Name/identifier of the Limelight camera + * @param yaw Robot yaw angle in degrees + * @param yawRate Angular velocity of yaw in degrees per second + * @param pitch Robot pitch angle in degrees + * @param pitchRate Angular velocity of pitch in degrees per second + * @param roll Robot roll angle in degrees + * @param rollRate Angular velocity of roll in degrees per second + * @see #SetRobotOrientation(String, double, double, double, double, double, double) + */ public static void SetRobotOrientation_NoFlush( String limelightName, double yaw, @@ -1495,10 +3129,26 @@ public static void SetRobotOrientation_NoFlush( double pitchRate, double roll, double rollRate) { + // Call internal method with flush disabled SetRobotOrientation_INTERNAL( limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); } + /** + * Internal implementation for setting robot orientation. + * + *

Handles packing orientation data into an array and optionally flushing NetworkTables. + * This method is private and called by the public SetRobotOrientation methods. + * + * @param limelightName Name/identifier of the Limelight camera + * @param yaw Robot yaw angle in degrees + * @param yawRate Angular velocity of yaw in degrees per second + * @param pitch Robot pitch angle in degrees + * @param pitchRate Angular velocity of pitch in degrees per second + * @param roll Robot roll angle in degrees + * @param rollRate Angular velocity of roll in degrees per second + * @param flush Whether to flush NetworkTables after setting the value + */ private static void SetRobotOrientation_INTERNAL( String limelightName, double yaw, @@ -1509,6 +3159,8 @@ private static void SetRobotOrientation_INTERNAL( double rollRate, boolean flush) { + // Pack orientation data into array format expected by Limelight + // Array order: [yaw, yawRate, pitch, pitchRate, roll, rollRate] double[] entries = new double[6]; entries[0] = yaw; entries[1] = yawRate; @@ -1516,7 +3168,11 @@ private static void SetRobotOrientation_INTERNAL( entries[3] = pitchRate; entries[4] = roll; entries[5] = rollRate; + + // Send to NetworkTables under the robot_orientation_set key setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + + // Flush if requested to ensure immediate transmission if (flush) { Flush(); } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java b/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java index fa74f822..5de2205c 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java @@ -7,55 +7,77 @@ import java.util.Set; /** - * Get positional data from limelight + * Interface for retrieving positional and vision data from a Limelight camera. + * + *

This includes robot pose estimates, visible AprilTags, and latency measurements. * * @see Limelight */ public interface LocationalData { - /** Returns {@code true} if the limelight has identified a target. */ + /** + * Returns {@code true} if the Limelight has identified at least one target in the current frame. + * + * @return {@code true} if a target is detected + */ boolean hasTarget(); - /** Returns {@code true} if the limelight has provided a valid response. */ + /** + * Returns {@code true} if the Limelight has provided a valid response for this frame. + * + * @return {@code true} if the data is valid + */ boolean isValid(); /** - * Gets the position of the robot with the center of the field as the origin. + * Gets the robot's 3D position relative to the center of the field. * - * @return The position of the robot + * @return the robot's pose, if available */ Optional getBotpose(); - /** Gets the estimated position of the robot with the center of the field as the origin. */ + /** + * Gets an estimated robot pose relative to the center of the field, including visible AprilTags. + * + * @return the estimated robot pose + */ Optional getBotPoseEstimate(); /** - * Gets the position of the robot with the blue driverstation as the origin + * Gets the robot's 3D position relative to the blue driver station as origin. * - * @return The position of the robot + * @return the robot's pose, if available */ Optional getBotposeBlue(); - /** Gets the estimated position of the robot with the blue driverstation as the origin. */ + /** + * Gets an estimated robot pose relative to the blue driver station as origin. + * + * @return the estimated robot pose + */ Optional getBotPoseEstimateBlue(); /** - * Gets the position of the robot with the red driverstation as the origin + * Gets the robot's 3D position relative to the red driver station as origin. * - * @return The position of the robot + * @return the robot's pose, if available */ Optional getBotposeRed(); - /** Gets the estimated position of the robot with the red driverstation as the origin. */ + /** + * Gets an estimated robot pose relative to the red driver station as origin. + * + * @return the estimated robot pose + */ Optional getBotPoseEstimateRed(); /** * Capture latency in milliseconds. * - *

Per the Limelight docs, this is the time between the end of the exposure of the middle row - * to the beginning of the tracking loop. + *

This is the time between the end of the exposure of the middle row and the beginning of + * the tracking loop. * - * @deprecated Use {@link #getBotPoseEstimateBlue()} or {@link #getBotPoseEstimateRed()} + * @deprecated Use {@link #getBotPoseEstimateBlue()} or {@link #getBotPoseEstimateRed()} instead */ @Deprecated OptionalDouble getCaptureLatency(); @@ -63,38 +85,48 @@ public interface LocationalData { /** * Targeting latency in milliseconds. * - *

Per the Limelight docs, this is the time consumed by the tracking loop this frame. + *

This is the time taken by the tracking loop for this frame. * - * @deprecated Use {@link #getBotPoseEstimateBlue()} or {@link #getBotPoseEstimateRed()} + * @deprecated Use {@link #getBotPoseEstimateBlue()} or {@link #getBotPoseEstimateRed()} instead */ @Deprecated OptionalDouble getTargetingLatency(); /** - * @deprecated use methods that return a {@link BotPoseEstimate}. + * @deprecated Use methods that return a {@link BotPoseEstimate} instead */ @Deprecated OptionalDouble getTimestamp(); + /** + * Returns the sum of capture latency and targeting latency in milliseconds, if both are available. + * + * @return the total latency in milliseconds + * @deprecated Use {@link #getBotPoseEstimateBlue()} or {@link #getBotPoseEstimateRed()} instead + */ @Deprecated default OptionalDouble lastMSDelay() { - OptionalDouble a = getCaptureLatency(); - OptionalDouble b = getTargetingLatency(); - if (a.isPresent() && b.isPresent()) { - return OptionalDouble.of(a.getAsDouble() + b.getAsDouble()); + OptionalDouble capture = getCaptureLatency(); + OptionalDouble targeting = getTargetingLatency(); + if (capture.isPresent() && targeting.isPresent()) { + return OptionalDouble.of(capture.getAsDouble() + targeting.getAsDouble()); } return OptionalDouble.empty(); } /** - * Gets the set of all visible tags + * Gets the IDs of all visible AprilTags in the current frame. * - * @return The visible tags - * @deprecated use {@link #getVisibleAprilTagPoses()} + * @return the set of visible tag IDs + * @deprecated use {@link #getVisibleAprilTagPoses()} instead */ @Deprecated Set getVisibleTags(); - /** Gets the visible AprilTags as a map from ID to position. */ + /** + * Returns a map of visible AprilTags, keyed by their ID, with values as their 3D positions. + * + * @return a map of visible AprilTags + */ Map getVisibleAprilTagPoses(); } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/NetworkTablesLimelight.java b/limelight/src/main/java/com/team2813/lib2813/limelight/NetworkTablesLimelight.java index 9579935a..1269e44d 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/NetworkTablesLimelight.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/NetworkTablesLimelight.java @@ -9,17 +9,26 @@ import java.io.IOException; import java.io.InputStream; import java.util.*; -import java.util.Arrays; -import java.util.Optional; -import java.util.OptionalDouble; import java.util.stream.Collectors; import org.json.JSONObject; +/** + * Implementation of {@link Limelight} that retrieves data from the NetworkTables interface of a + * Limelight camera. + */ class NetworkTablesLimelight implements Limelight { + + /** Sentinel array of zeros used to detect uninitialized poses. */ private static final double[] ZEROS = new double[6]; + private final String limelightName; private final AprilTagMapPoseHelper aprilTagMapPoseHelper; + /** + * Constructs a NetworkTablesLimelight instance. + * + * @param limelightName The hostname or NetworkTables name of the Limelight. + */ NetworkTablesLimelight(String limelightName) { this.limelightName = limelightName; aprilTagMapPoseHelper = new AprilTagMapPoseHelper(new LimelightClient(limelightName)); @@ -37,8 +46,7 @@ public boolean hasTarget() { @Override public void setFieldMap(InputStream stream, boolean updateLimelight) throws IOException { - // The updateLimelight assumes we have the hostname of the limelight, which we don't. For now, - // this won't update the limelight. + // For NT-based Limelight, we do not support uploading field maps over the network yet. aprilTagMapPoseHelper.setFieldMap(stream, false); } @@ -49,6 +57,7 @@ public List getLocatedAprilTags(Set visibleTags) { @Override public Optional getJsonDump() { + // This implementation does not provide raw JSON dumps return Optional.empty(); } @@ -59,9 +68,11 @@ public OptionalDouble getCaptureLatency() { @Override public LocationalData getLocationalData() { - LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(limelightName); + LimelightResults results = LimelightHelpers.getLatestResults(limelightName); + if (results.error == null && results.valid) { Map aprilTags = getVisibleAprilTagPoses(results); + var poseEstimate = toBotPoseEstimate(LimelightHelpers.getBotPoseEstimate(limelightName), aprilTags.keySet()); var redPoseEstimate = @@ -70,9 +81,11 @@ public LocationalData getLocationalData() { var bluePoseEstimate = toBotPoseEstimate( LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName), aprilTags.keySet()); + return new NTLocationalData( results, poseEstimate, redPoseEstimate, bluePoseEstimate, aprilTags); } + return StubLocationalData.INVALID; } @@ -94,7 +107,9 @@ private Map getVisibleAprilTagPoses(LimelightResults results) { return unmodifiableMap(map); } + /** Inner class implementing {@link LocationalData} backed by NetworkTables results. */ private class NTLocationalData implements LocationalData { + private final LimelightResults results; private final Optional poseEstimate; private final Optional redPoseEstimate; diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java b/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java index 1002184b..3cc1c51a 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java @@ -4,17 +4,40 @@ import java.util.OptionalDouble; import java.util.OptionalLong; +/** + * Utility class for working with {@link Optional} values. + *

+ * Provides methods to convert boxed {@link Optional} types to their corresponding primitive + * {@code Optional} types. This class is non-instantiable. + *

+ */ final class Optionals { - // make class non-instantiable - private Optionals() { - throw new AssertionError("cannot create Optionals instance"); - } - static OptionalLong unboxLong(Optional val) { - return val.map(OptionalLong::of).orElseGet(OptionalLong::empty); - } + /** + * Private constructor to prevent instantiation. + * Invoking this constructor will always throw an {@link AssertionError}. + */ + private Optionals() { + throw new AssertionError("cannot create Optionals instance"); + } - static OptionalDouble unboxDouble(Optional val) { - return val.map(OptionalDouble::of).orElseGet(OptionalDouble::empty); - } + /** + * Converts an {@link Optional} of {@link Long} to an {@link OptionalLong}. + * + * @param val the {@code Optional} to convert + * @return an {@code OptionalLong} containing the value if present, or an empty {@code OptionalLong} if not + */ + static OptionalLong unboxLong(Optional val) { + return val.map(OptionalLong::of).orElseGet(OptionalLong::empty); + } + + /** + * Converts an {@link Optional} of {@link Double} to an {@link OptionalDouble}. + * + * @param val the {@code Optional} to convert + * @return an {@code OptionalDouble} containing the value if present, or an empty {@code OptionalDouble} if not + */ + static OptionalDouble unboxDouble(Optional val) { + return val.map(OptionalDouble::of).orElseGet(OptionalDouble::empty); + } } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java b/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java index e104b672..1d534b78 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java @@ -19,24 +19,51 @@ import org.json.JSONArray; import org.json.JSONObject; +/** + * REST-based implementation of the Limelight interface for communicating with a Limelight vision camera. + * This class manages a connection to a Limelight device over HTTP and provides methods for retrieving + * vision targeting data, robot pose estimates, and AprilTag information. + * + *

Instances are managed through static factory methods and are cached based on their network address. + * The class uses a scheduled executor service to periodically collect data from the Limelight device. + */ class RestLimelight implements Limelight { + /** Map of cached Limelight instances keyed by their network address. */ private static final Map limelights = new HashMap<>(); + + /** Helper for managing AprilTag field map poses. */ private final AprilTagMapPoseHelper aprilTagMapPoseHelper; + + /** Thread for collecting data from the Limelight. */ private final DataCollection collectionThread; + + /** Shared executor service for all Limelight instances. */ private static final ScheduledExecutorService executor = Executors.newScheduledThreadPool(2); + /** Default network address for the Limelight device. */ static final String DEFAULT_ADDRESS = "limelight.local"; + /** Future representing the scheduled data collection task. */ private ScheduledFuture thread; + /** Whether the data collection thread has been started. */ boolean started = false; + /** + * Creates a new RestLimelight instance with the specified network address. + * + * @param address the hostname or IP address of the Limelight device + */ RestLimelight(String address) { var limelightClient = new LimelightClient(address); collectionThread = new DataCollection(limelightClient); aprilTagMapPoseHelper = new AprilTagMapPoseHelper(limelightClient); } + /** + * Starts the periodic data collection thread if it hasn't been started already. + * The thread runs at a fixed rate of 40ms with an initial delay of 20ms. + */ void start() { if (!started) { thread = executor.scheduleAtFixedRate(collectionThread, 20, 40, TimeUnit.MILLISECONDS); @@ -44,19 +71,34 @@ void start() { } } + /** + * Manually runs one iteration of the data collection thread. + * This is primarily useful for testing purposes. + */ void runThread() { collectionThread.run(); } + /** + * {@inheritDoc} + */ @Override public Optional getJsonDump() { return collectionThread.getMostRecent().map(DataCollection.Result::json); } + /** + * Gets the capture latency of the most recent frame from the Limelight. + * + * @return an OptionalDouble containing the capture latency in milliseconds, or empty if unavailable + */ public OptionalDouble getCaptureLatency() { return getLocationalData().getCaptureLatency(); } + /** + * {@inheritDoc} + */ @Override public OptionalDouble getTimestamp() { return getLocationalData().getTimestamp(); @@ -66,33 +108,59 @@ public OptionalDouble getTimestamp() { * Sets the field map for the limelight. Additionally, this may also upload the field map to the * Limelight if desired. This will likely be a slow operation, and should not be regularly called. * - * @param stream The reader which - * @param updateLimelight If the limelight should be updated with this field map + * @param stream the input stream containing the field map data + * @param updateLimelight if true, uploads the field map to the Limelight device + * @throws IOException if an I/O error occurs while reading the stream or uploading to the device */ @Override public void setFieldMap(InputStream stream, boolean updateLimelight) throws IOException { aprilTagMapPoseHelper.setFieldMap(stream, updateLimelight); } + /** + * {@inheritDoc} + */ @Override public List getLocatedAprilTags(Set visibleTags) { return aprilTagMapPoseHelper.getVisibleTagPoses(visibleTags); } + /** + * Creates a negation function that inverts the boolean result of another function. + * + * @param the type of the function input + * @param fnc the function to negate + * @return a function that returns the opposite boolean value + */ private static Function not(Function fnc) { return (t) -> !fnc.apply(t); } + /** + * Checks if the Limelight currently has a valid target in view. + * + * @return true if a target is detected, false otherwise + */ public boolean hasTarget() { return getLocationalData().hasTarget(); } + /** + * Gets the most recent locational data from the Limelight. + * + * @return LocationalData containing pose estimates, tag information, and latency data, + * or a stub with valid defaults if no data is available + */ public LocationalData getLocationalData() { Optional locationalData = collectionThread.getMostRecent().map(RestLocationalData::new); return locationalData.orElse(StubLocationalData.VALID); } + /** + * Cleans up resources by canceling the data collection thread and waiting for termination. + * Logs any interruption errors to the DriverStation. + */ private void clean() { try { thread.cancel(true); @@ -115,9 +183,10 @@ public static Limelight getDefaultLimelight() { * Gets the limelight with the specified name. Calling with a blank {@code limelightName} is * equivalent to calling {@link #getDefaultLimelight()} * - * @param limelightAddress The hostname or ip address of the limelight + * @param limelightAddress the hostname or IP address of the limelight * @return the {@link Limelight} object for interfacing with the limelight - * @throws NullPointerException if {@code limelightName} is null + * @throws NullPointerException if {@code limelightAddress} is null + * @throws IllegalArgumentException if {@code limelightAddress} is empty */ public static Limelight getLimelight(String limelightAddress) { String addr = Objects.requireNonNull(limelightAddress, "limelightAddress shouldn't be null"); @@ -129,6 +198,10 @@ public static Limelight getLimelight(String limelightAddress) { return result; } + /** + * Clears all cached Limelight instances and cleans up their resources. + * This method should be called when shutting down or resetting the system. + */ static void eraseInstances() { for (RestLimelight limelight : limelights.values()) { limelight.clean(); @@ -136,25 +209,49 @@ static void eraseInstances() { limelights.clear(); } + /** + * Implementation of LocationalData that parses data from JSON responses received from the Limelight. + * This class provides access to robot pose estimates, AprilTag information, and timing data. + */ private class RestLocationalData implements LocationalData { + /** The root JSON object containing all Limelight data. */ private final JSONObject root; + + /** The timestamp when the response was received, in seconds. */ private final double responseTimestamp; + /** + * Creates a new RestLocationalData instance from a data collection result. + * + * @param result the result containing JSON data and timestamp + */ RestLocationalData(DataCollection.Result result) { this.root = getRoot(result.json()); this.responseTimestamp = result.responseTimestamp(); } + /** + * {@inheritDoc} + */ @Override public boolean isValid() { return getBooleanFromInt(root, "v").orElse(false); } + /** + * {@inheritDoc} + */ @Override public boolean hasTarget() { return getArr(root, "Fiducial").map(not(JSONArray::isEmpty)).orElse(false); } + /** + * Checks if a pose array is invalid or contains only zero values. + * + * @param arr the JSON array to validate + * @return true if the array is invalid (wrong length, no target, or all zeros), false otherwise + */ private boolean invalidArray(JSONArray arr) { boolean simple = arr.length() != 6 || !hasTarget(); if (simple) { @@ -170,6 +267,14 @@ private boolean invalidArray(JSONArray arr) { return true; } + /** + * Parses a JSON array into a Pose3d object. + * The array is expected to contain 6 elements: [x, y, z, roll, pitch, yaw] where + * rotation values are in degrees. + * + * @param arr the JSON array containing pose data + * @return an Optional containing the parsed Pose3d, or empty if the array is invalid + */ private Optional parseArr(JSONArray arr) { if (invalidArray(arr)) { return Optional.empty(); @@ -183,56 +288,88 @@ private Optional parseArr(JSONArray arr) { new Pose3d(arr.getDouble(0), arr.getDouble(1), arr.getDouble(2), rotation)); } + /** + * {@inheritDoc} + */ @Override public OptionalDouble getTimestamp() { return unboxDouble(getDouble(root, "ts")); } + /** + * {@inheritDoc} + */ @Override public OptionalDouble getCaptureLatency() { return unboxDouble(getDouble(root, "cl")); } + /** + * {@inheritDoc} + */ @Override public OptionalDouble getTargetingLatency() { return unboxDouble(getDouble(root, "tl")); } + /** + * {@inheritDoc} + */ @Override public Optional getBotpose() { return getArr(root, "botpose").flatMap(this::parseArr); } + /** + * {@inheritDoc} + */ @Override public Optional getBotPoseEstimate() { return getArr(root, "botpose").flatMap(this::parseArr).map(this::toBotPoseEstimate); } + /** + * {@inheritDoc} + */ @Override public Optional getBotposeBlue() { return getArr(root, "botpose_wpiblue").flatMap(this::parseArr); } + /** + * {@inheritDoc} + */ @Override public Optional getBotPoseEstimateBlue() { return getArr(root, "botpose_wpiblue").flatMap(this::parseArr).map(this::toBotPoseEstimate); } /** - * Gets the position of the robot with the red driverstation as the origin + * Gets the position of the robot with the red driverstation as the origin. * - * @return The position of the robot + * @return an Optional containing the robot's Pose3d in red alliance coordinates, or empty if unavailable */ @Override public Optional getBotposeRed() { return getArr(root, "botpose_wpired").flatMap(this::parseArr); } + /** + * {@inheritDoc} + */ @Override public Optional getBotPoseEstimateRed() { return getArr(root, "botpose_wpired").flatMap(this::parseArr).map(this::toBotPoseEstimate); } + /** + * Converts a Pose3d into a BotPoseEstimate with timestamp and visible tag information. + * The timestamp is calculated by subtracting the total latency from the response timestamp. + * + * @param pose the 3D pose to convert + * @return a BotPoseEstimate with the 2D pose projection, adjusted timestamp, and visible tags + * @see Chief Delphi Discussion + */ private BotPoseEstimate toBotPoseEstimate(Pose3d pose) { // See // https://www.chiefdelphi.com/t/timestamp-parameter-when-adding-limelight-vision-to-odometry @@ -242,11 +379,18 @@ private BotPoseEstimate toBotPoseEstimate(Pose3d pose) { pose.toPose2d(), timestampSeconds, getVisibleAprilTagPoses().keySet()); } - /** Gets the id of the targeted tag. */ + /** + * Gets the ID of the currently targeted AprilTag. + * + * @return an OptionalLong containing the tag ID, or empty if no tag is targeted + */ OptionalLong getTagID() { return unboxLong(getLong(root, "pID")); } + /** + * {@inheritDoc} + */ @Override public Set getVisibleTags() { return getArr(root, "Fiducial") @@ -264,6 +408,9 @@ public Set getVisibleTags() { .orElseGet(Set::of); } + /** + * {@inheritDoc} + */ @Override public Map getVisibleAprilTagPoses() { return getArr(root, "Fiducial") @@ -282,4 +429,4 @@ public Map getVisibleAprilTagPoses() { .orElse(emptyMap()); } } -} +} \ No newline at end of file diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java b/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java index a36d4075..36c709c6 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java @@ -3,84 +3,176 @@ import edu.wpi.first.math.geometry.Pose3d; import java.util.*; -/** Implementation of LocationalData where all optional values return empty values. */ +/** + * Stub implementation of LocationalData where all optional values return empty. + * + *

This class provides a null-object pattern implementation for LocationalData, + * useful as a fallback when real Limelight data is unavailable. It can represent + * either valid or invalid data states, but always returns empty optionals for all + * pose and timing information. + * + *

Two singleton instances are provided: + *

    + *
  • {@link #VALID} - represents valid but empty data
    • + *
  • {@link #INVALID} - represents invalid/disconnected state
    • + *
+ */ final class StubLocationalData implements LocationalData { + /** Singleton instance representing valid but empty locational data. */ static final StubLocationalData VALID = new StubLocationalData(true); + + /** Singleton instance representing invalid locational data. */ static final StubLocationalData INVALID = new StubLocationalData(false); + /** Whether this stub data should be considered valid. */ private final boolean valid; + /** + * Creates a new StubLocationalData instance. + * + * @param valid whether this stub data should report as valid + */ private StubLocationalData(boolean valid) { this.valid = valid; } + /** + * {@inheritDoc} + * + * @return the validity state set during construction + */ @Override public boolean isValid() { return valid; } + /** + * {@inheritDoc} + * + * @return always false, as stub data never has targets + */ @Override public boolean hasTarget() { return false; } + /** + * {@inheritDoc} + * + * @return always empty + */ @Override public Optional getBotpose() { return Optional.empty(); } + /** + * {@inheritDoc} + * + * @return always empty + */ @Override public Optional getBotPoseEstimate() { return Optional.empty(); } + /** + * {@inheritDoc} + * + * @return always empty + */ @Override public Optional getBotposeBlue() { return Optional.empty(); } + /** + * {@inheritDoc} + * + * @return always empty + */ @Override public Optional getBotPoseEstimateBlue() { return Optional.empty(); } + /** + * {@inheritDoc} + * + * @return always empty + */ @Override public Optional getBotposeRed() { return Optional.empty(); } + /** + * {@inheritDoc} + * + * @return always empty + */ @Override public Optional getBotPoseEstimateRed() { return Optional.empty(); } + /** + * {@inheritDoc} + * + * @return always empty + */ @Override public OptionalDouble getCaptureLatency() { return OptionalDouble.empty(); } + /** + * {@inheritDoc} + * + * @return always empty + */ @Override public OptionalDouble getTargetingLatency() { return OptionalDouble.empty(); } + /** + * {@inheritDoc} + * + * @return always empty + */ @Override public OptionalDouble getTimestamp() { return OptionalDouble.empty(); } + /** + * {@inheritDoc} + * + * @return always empty + */ @Override public OptionalDouble lastMSDelay() { return OptionalDouble.empty(); } + /** + * {@inheritDoc} + * + * @return always an empty, immutable set + */ @Override public Set getVisibleTags() { return Set.of(); } + /** + * {@inheritDoc} + * + * @return always an empty, immutable map + */ @Override public Map getVisibleAprilTagPoses() { return Collections.emptyMap(); } -} +} \ No newline at end of file diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java index afa73bef..23b0fe43 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java @@ -4,14 +4,39 @@ import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.Pose3d; +/** + * Represents a fiducial (AprilTag) in 3D space. + * + *

This class stores the fiducial's unique ID and its pose as a 4x4 transformation matrix. It + * can convert this transform into a {@link Pose3d} object for use with WPILib's geometry and + * odometry systems. + */ public class Fiducial { + + /** The unique identifier of this fiducial. */ private int id; + + /** + * The 4x4 transformation matrix representing the fiducial's pose in 3D space. + * + *

The matrix is stored in row-major order as a 16-element array. + */ private double[] transform; + /** + * Returns the 3D pose of this fiducial as a {@link Pose3d}. + * + * @return the fiducial's pose in 3D space + */ public Pose3d getPosition() { return new Pose3d(new Matrix<>(Nat.N4(), Nat.N4(), transform)); } + /** + * Returns the unique ID of this fiducial. + * + * @return the fiducial's ID + */ public int getId() { return id; } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java index cbe78e56..f555d9fc 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java @@ -9,16 +9,40 @@ import java.util.Map; import java.util.stream.Collectors; +/** + * Retrieves and stores fiducial (AprilTag) information from a JSON input stream. + * + *

The JSON is expected to represent a {@link FieldMap}, which contains an array of + * {@link Fiducial} objects. Each fiducial is mapped by its unique ID for easy lookup. + */ public class FiducialRetriever { + + /** Gson instance used for deserializing JSON. */ private static final Gson gson = new Gson(); + + /** Map of fiducial ID to {@link Fiducial} object. */ private final Map fiducialMap; + /** + * Constructs a new {@link FiducialRetriever} by reading fiducials from the given input stream. + * + * @param stream an {@link InputStream} containing a JSON representation of a {@link FieldMap} + * @throws com.google.gson.JsonSyntaxException if the JSON is invalid + * @throws java.io.UncheckedIOException if there is an error reading from the stream + */ public FiducialRetriever(InputStream stream) { FieldMap map = gson.fromJson(new InputStreamReader(stream, UTF_8), FieldMap.class); + // Convert array of fiducials to an unmodifiable map keyed by fiducial ID fiducialMap = - Arrays.stream(map.fiducials).collect(Collectors.toUnmodifiableMap(Fiducial::getId, f -> f)); + Arrays.stream(map.fiducials) + .collect(Collectors.toUnmodifiableMap(Fiducial::getId, f -> f)); } + /** + * Returns an unmodifiable map of fiducials keyed by their ID. + * + * @return a {@link Map} of fiducial ID to {@link Fiducial} + */ public Map getFiducialMap() { return fiducialMap; } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FieldMap.java b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FieldMap.java index 064a5de4..a85585bb 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FieldMap.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FieldMap.java @@ -1,5 +1,23 @@ package com.team2813.lib2813.limelight.apriltag_map; +/** + * Represents a collection of fiducials (AprilTags) in a field map. + * + *

This class is intended to be deserialized from JSON. The JSON should contain an array of + * fiducials under the key "fiducials". Each fiducial is represented by a {@link Fiducial} object. + * + *

Example JSON format: + * 

+ * {
+ *   "fiducials": [
+ *     { "id": 1, "transform": [ ...16 values... ] },
+ *     { "id": 2, "transform": [ ...16 values... ] }
+ *   ]
+ * }
+ *
+ */ class FieldMap { + + /** Array of fiducials present in the field map. */ public Fiducial[] fiducials; } diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java b/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java index 0ef52898..390d8e21 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java @@ -13,10 +13,44 @@ import org.json.JSONObject; import org.junit.rules.ExternalResource; +/** + * Mock HTTP server that simulates a Limelight camera for testing purposes. + * + *

This class extends JUnit's {@link ExternalResource} to provide automatic setup + * and teardown of a fake HTTP server that mimics the Limelight REST API. It listens + * on port 5807 and handles two endpoints: + *

    + *
  • {@code /results} - GET endpoint that returns vision data in JSON format
    • + *
  • {@code /upload-fieldmap} - POST endpoint that accepts field map uploads
    • + *
+ * + *

Usage example in a JUnit test: + * 

{@code
+ * @Rule
+ * public FakeLimelight fakeLimelight = new FakeLimelight();
+ * 
+ * @Test
+ * public void testLimelightConnection() {
+ *     JSONObject response = new JSONObject();
+ *     response.put("v", 1);
+ *     fakeLimelight.setResultsResponse(response);
+ *     // Test code here
+ * }
+ * }
+ */ public class FakeLimelight extends ExternalResource { + /** Logger for tracking fake Limelight activity. */ private static final Logger logger = Logger.getLogger("FakeLimelight"); + + /** The underlying HTTP server instance. */ HttpServer server; + /** + * Sets up the fake Limelight server before each test. + * Creates an HTTP server on port 5807 with endpoints for results and field map uploads. + * + * @throws Throwable if server creation or startup fails + */ @Override protected void before() throws Throwable { server = HttpServer.create(new InetSocketAddress(5807), 0); @@ -26,23 +60,51 @@ protected void before() throws Throwable { server.start(); } + /** + * Tears down the fake Limelight server after each test. + * Stops the server with a 2-second grace period and clears the server reference. + */ @Override protected void after() { server.stop(2); server = null; } + /** + * HTTP handler that simulates the Limelight's GET /results endpoint. + * + *

This handler responds to GET requests with a configurable JSON body + * representing vision data from the Limelight. It returns 405 Method Not Allowed + * for non-GET requests. + */ private static class FakeGet implements HttpHandler { + /** The JSON response body to return. */ private String body; + /** + * Sets the response body that will be returned by GET requests. + * + * @param body the JSON string to return + */ public void setBody(String body) { this.body = body; } + /** + * Gets the current response body. + * + * @return the JSON string that will be returned + */ public String getBody() { return body; } + /** + * Handles incoming HTTP requests to the /results endpoint. + * + * @param exchange the HTTP exchange containing request and response + * @throws IOException if an I/O error occurs during response writing + */ @Override public void handle(HttpExchange exchange) throws IOException { logger.info("Request for results received"); @@ -58,9 +120,24 @@ public void handle(HttpExchange exchange) throws IOException { } } + /** + * HTTP handler that simulates the Limelight's POST /upload-fieldmap endpoint. + * + *

This handler accepts POST requests with field map data and stores the + * uploaded content for verification in tests. It returns 405 Method Not Allowed + * for non-POST requests. + */ private static class FakeFieldMap implements HttpHandler { + /** The most recently uploaded field map content. */ private String post; + /** + * Handles incoming HTTP requests to the /upload-fieldmap endpoint. + * Reads and stores the POST body content for later retrieval. + * + * @param exchange the HTTP exchange containing request and response + * @throws IOException if an I/O error occurs during request reading + */ @Override public void handle(HttpExchange exchange) throws IOException { logger.info("Request for field map received"); @@ -77,22 +154,45 @@ public void handle(HttpExchange exchange) throws IOException { } } + /** Handler for the /results GET endpoint. */ private final FakeGet resultsResponse = new FakeGet(); + + /** Handler for the /upload-fieldmap POST endpoint. */ private final FakeFieldMap fieldMapResponse = new FakeFieldMap(); + /** + * Resets the fake Limelight state by clearing the results response body. + * Useful for ensuring a clean state between test cases. + */ public void reset() { resultsResponse.setBody(""); } + /** + * Sets the JSON response that will be returned by the /results endpoint. + * + * @param response the JSONObject containing vision data to return + */ public void setResultsResponse(JSONObject response) { resultsResponse.setBody(response.toString()); } + /** + * Gets the most recently uploaded field map content. + * + * @return the field map data as a string, or null if no field map has been uploaded + */ public String getFieldMap() { return fieldMapResponse.post; } + /** + * Gets the current results response as a JSONObject. + * + * @return a JSONObject parsed from the current response body + * @throws org.json.JSONException if the response body is not valid JSON + */ public JSONObject getResultsResponse() { return new JSONObject(resultsResponse.getBody()); } -} +} \ No newline at end of file diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/LegacyRestLimelightTest.java b/limelight/src/test/java/com/team2813/lib2813/limelight/LegacyRestLimelightTest.java index 7ec943b4..aa12e2dc 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/LegacyRestLimelightTest.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/LegacyRestLimelightTest.java @@ -2,11 +2,38 @@ import org.json.JSONObject; +/** + * Test suite for verifying backward compatibility with legacy Limelight JSON format. + * + *

This class extends {@link RestLimelightTest} to ensure that the RestLimelight + * implementation continues to work with older JSON responses that include a "Results" + * root object. This legacy format was used in earlier versions of the Limelight API, + * and maintaining compatibility ensures that existing systems continue to function + * correctly. + * + *

The key difference between this test and the parent class is that all JSON + * test data files in the resources directory contain the legacy "Results" root + * object structure, which this test specifically validates. + * + * @see RestLimelightTest + */ public class LegacyRestLimelightTest extends RestLimelightTest { + + /** + * Sets the JSON response for the fake Limelight using the legacy format. + * + *

This override ensures that JSON objects with the legacy "Results" root + * structure are properly handled. All JSON files in the test resources directory + * use this legacy format, making this test class essential for verifying + * backward compatibility. + * + * @param json the JSONObject containing legacy-formatted Limelight data with + * a "Results" root object + */ @Override protected void setJson(JSONObject json) { // All json files in resources has the "Results" root json object, which is now legacy. // Therefore, this json object is legacy and this test will make sure legacy still works fakeLimelight.setResultsResponse(json); } -} +} \ No newline at end of file diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java b/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java index 8e472fc2..8a32bb37 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java @@ -26,14 +26,48 @@ import org.json.JSONObject; import org.junit.Test; +/** + * Abstract base class for Limelight implementation testing. + * + *

This class provides a comprehensive test suite for validating Limelight implementations + * against various JSON response scenarios. It tests all aspects of the Limelight interface + * including pose estimation, target detection, AprilTag visibility, and latency measurements. + * + *

Subclasses must implement two abstract methods to adapt the tests to their specific + * Limelight implementation: + *

    + *
  • {@link #createLimelight()} - creates the Limelight instance to test
    • + *
  • {@link #setJson(JSONObject)} - configures the test environment with JSON data
    • + *
+ * + *

Test coverage includes: + *

    + *
  • Empty/missing data scenarios
    • + *
  • Invalid data handling
    • + *
  • Robot pose estimation in multiple coordinate systems (field, blue alliance, red alliance)
    • + *
  • AprilTag detection and position reporting
    • + *
  • Latency measurements (capture and targeting)
    • + *
  • Field map integration
    • + *
+ */ abstract class LimelightTestCase { + /** + * Tests that an uninitialized Limelight returns empty values. + * Verifies that capture latency is not present when no data has been received. + */ @Test public final void emptyValues() { Limelight limelight = createLimelight(); assertWithMessage("JSON should be empty").that(limelight.getCaptureLatency()).isEmpty(); } + /** + * Tests handling of invalid/malformed JSON data. + * Verifies that the Limelight correctly identifies invalid data and returns empty optionals. + * + * @throws Exception if test resources cannot be loaded + */ @Test public final void invalidDataTest() throws Exception { JSONObject obj = readJSON("InvalidDataTest.json"); @@ -50,6 +84,12 @@ public final void invalidDataTest() throws Exception { assertThat(actualTargetingLatency).isEmpty(); } + /** + * Tests scenario where no target is detected but latency data is present. + * Validates that valid latency measurements can be obtained even without a target. + * + * @throws Exception if test resources cannot be loaded + */ @Test public final void absentTest1() throws Exception { JSONObject obj = readJSON("AbsentTest1.json"); @@ -70,6 +110,12 @@ public final void absentTest1() throws Exception { assertThat(locationalData.getBotPoseEstimateRed()).isEmpty(); } + /** + * Tests another scenario where no target is detected but latency data differs. + * Verifies consistent behavior with different latency values. + * + * @throws Exception if test resources cannot be loaded + */ @Test public final void absentTest2() throws Exception { JSONObject obj = readJSON("AbsentTest2.json"); @@ -90,6 +136,13 @@ public final void absentTest2() throws Exception { assertThat(locationalData.getBotPoseEstimateRed()).isEmpty(); } + /** + * Tests scenario with a detected target and full pose estimation data. + * Validates robot pose in field coordinates, blue alliance coordinates, and red alliance coordinates. + * Also verifies that pose estimates include valid timestamps. + * + * @throws Exception if test resources cannot be loaded + */ @Test public final void presentTest1() throws Exception { JSONObject obj = readJSON("PresentTest1.json"); @@ -133,6 +186,12 @@ public final void presentTest1() throws Exception { assertThat(redEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate); } + /** + * Tests another scenario with a detected target and different pose values. + * Ensures consistent pose estimation behavior across different robot positions. + * + * @throws Exception if test resources cannot be loaded + */ @Test public final void presentTest2() throws Exception { JSONObject obj = readJSON("PresentTest2.json"); @@ -171,6 +230,12 @@ public final void presentTest2() throws Exception { assertThat(blueEstimate.timestampSeconds()).isWithin(0.005).of(redEstimate.timestampSeconds()); } + /** + * Tests retrieval of robot pose in blue alliance coordinates. + * Validates the coordinate transformation from field coordinates to blue alliance origin. + * + * @throws Exception if test resources cannot be loaded + */ @Test public final void getBotposeBlue() throws Exception { JSONObject obj = readJSON("BotposeBlueRedTest.json"); @@ -187,6 +252,12 @@ public final void getBotposeBlue() throws Exception { assertThat(actualPose).isWithin(0.005).of(expectedPose); } + /** + * Tests retrieval of robot pose in red alliance coordinates. + * Validates the coordinate transformation from field coordinates to red alliance origin. + * + * @throws Exception if test resources cannot be loaded + */ @Test public final void getBotposeRed() throws Exception { JSONObject obj = readJSON("BotposeBlueRedTest.json"); @@ -204,6 +275,12 @@ public final void getBotposeRed() throws Exception { assertThat(actualPose).isWithin(0.005).of(expectedPose); } + /** + * Tests detection and reporting of visible AprilTag IDs. + * Verifies that the Limelight correctly identifies which tags are in view. + * + * @throws Exception if test resources cannot be loaded + */ @Test public final void getVisibleTags() throws Exception { JSONObject obj = readJSON("BotposeBlueRedTest.json"); @@ -213,6 +290,13 @@ public final void getVisibleTags() throws Exception { assertThat(limelight.getLocationalData().getVisibleTags()).containsExactly(20); } + /** + * Tests retrieval of visible AprilTag positions on the field. + * Validates that tag positions are correctly loaded from the field map and + * that pose estimates include the set of visible tags. + * + * @throws Exception if test resources cannot be loaded + */ @Test public final void getVisibleAprilTagPoses() throws Exception { JSONObject obj = readJSON("BotposeBlueRedTest.json"); @@ -239,6 +323,12 @@ public final void getVisibleAprilTagPoses() throws Exception { assertThat(locationalData.getBotPoseEstimateRed().get().visibleAprilTags()).isEqualTo(tags); } + /** + * Tests retrieval of AprilTag locations via the getLocatedAprilTags method. + * Validates that tag positions can be queried by their IDs after field map upload. + * + * @throws Exception if test resources cannot be loaded + */ @Test public final void visibleTagLocation() throws Exception { JSONObject obj = readJSON("BotposeBlueRedTest.json"); @@ -253,10 +343,29 @@ public final void visibleTagLocation() throws Exception { assertThat(pose.getTranslation()).isWithin(0.005).of(new Translation3d(-3.87, 0.72, 0.31)); } + /** + * Creates a Limelight instance for testing. + * Subclasses must implement this to provide their specific Limelight implementation. + * + * @return a Limelight instance to test + */ protected abstract Limelight createLimelight(); + /** + * Configures the test environment with JSON data. + * Subclasses must implement this to inject test data into their Limelight implementation. + * + * @param json the JSONObject containing Limelight response data + */ protected abstract void setJson(JSONObject json); + /** + * Uploads the FRC 2025 Round 2 field map to the Limelight. + * This configures AprilTag positions for testing. + * + * @param limelight the Limelight instance to configure + * @throws IOException if the field map resource cannot be loaded + */ private void uploadFieldMap(Limelight limelight) throws IOException { boolean updateLimelight = false; try (var stream = getClass().getResourceAsStream("frc2025r2.fmap")) { @@ -264,6 +373,13 @@ private void uploadFieldMap(Limelight limelight) throws IOException { } } + /** + * Reads a JSON test file from the classpath resources. + * + * @param fileName the name of the JSON file to read + * @return a JSONObject parsed from the file contents + * @throws IOException if the file cannot be found or read + */ private JSONObject readJSON(String fileName) throws IOException { try (InputStream is = getClass().getResourceAsStream(fileName)) { if (is == null) { @@ -276,6 +392,12 @@ private JSONObject readJSON(String fileName) throws IOException { } } + /** + * Asserts that the Limelight has a valid target detected. + * Checks both the top-level hasTarget() method and the LocationalData hasTarget() method. + * + * @param limelight the Limelight to check + */ private void assertHasTarget(Limelight limelight) { assertWithMessage("Should have target").that(limelight.hasTarget()).isTrue(); assertWithMessage("Should have target") @@ -283,9 +405,17 @@ private void assertHasTarget(Limelight limelight) { .isTrue(); } + /** + * Asserts that an OptionalDouble value is approximately equal to an expected value. + * If the optional is empty, the assertion fails. + * + * @param expected the expected double value + * @param actual the OptionalDouble to check + * @param delta the maximum acceptable difference between expected and actual + */ protected static void assertAlmostEqual(double expected, OptionalDouble actual, double delta) { actual.ifPresentOrElse( d -> assertEquals(expected, d, delta), () -> assertEquals(OptionalDouble.of(expected), actual)); } -} +} \ No newline at end of file diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java b/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java index a975c261..6b0e5942 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java @@ -7,11 +7,31 @@ import org.json.JSONObject; import org.junit.After; +/** + * Unit test class for {@link NetworkTablesLimelight}. + *

+ * This test framework simulates Limelight JSON data being written into NetworkTables and + * validates how the {@link Limelight} implementation handles pose estimation entries. + *

+ */ public class NetworkTablesLimelightTest extends LimelightTestCase { + + /** Name of the Limelight NetworkTable. */ private static final String TABLE_NAME = "limelight"; + + /** Half a millisecond in microseconds. */ private static final long ONE_HALF_MS_IN_MICROS = 500; + + /** One second in microseconds. */ private static final long ONE_SECOND_IN_MICROS = 1_000_000; + + /** Simulated timestamp in microseconds, advanced between test steps. */ private static long fakeTimestampMicros = 15 * ONE_SECOND_IN_MICROS; + + /** + * List of NetworkTables entries used for storing bot pose estimates. These cover both red/blue + * alliances and orb-based estimates. + */ private static final List BOT_POSE_ESTIMATE_ENTRIES = List.of( "botpose", @@ -20,6 +40,13 @@ public class NetworkTablesLimelightTest extends LimelightTestCase { "botpose_wpiblue", "botpose_orb_wpiblue"); + /** + * Resets Limelight NetworkTable entries after each test. + *

+ * Clears the JSON entry and all bot pose estimate entries, advancing the fake timestamp + * by two half-millisecond steps. + *

+ */ @After public void resetNetworkTables() { getJsonNTEntry().setString(""); @@ -31,11 +58,25 @@ public void resetNetworkTables() { fakeTimestampMicros += ONE_HALF_MS_IN_MICROS; } + /** + * Creates a new {@link NetworkTablesLimelight} instance for testing. + * + * @return a Limelight instance connected to the {@value #TABLE_NAME} table + */ @Override protected Limelight createLimelight() { return new NetworkTablesLimelight(TABLE_NAME); } + /** + * Sets the Limelight JSON data in NetworkTables. + *

+ * Copies JSON pose estimation arrays into their corresponding NetworkTable entries + * when latency values are available. + *

+ * + * @param json the simulated Limelight JSON response + */ @Override protected void setJson(JSONObject json) { JSONObject resultsJson = json.getJSONObject("Results"); @@ -50,10 +91,22 @@ protected void setJson(JSONObject json) { } } + /** + * Returns the JSON {@link NetworkTableEntry} for Limelight. + * + * @return the NetworkTables entry holding Limelight JSON + */ private static NetworkTableEntry getJsonNTEntry() { return LimelightHelpers.getLimelightNTTableEntry(TABLE_NAME, "json"); } + /** + * Writes a bot pose estimate array into the specified NetworkTables entry. + * + * @param resultsJson the Limelight "Results" JSON object + * @param entryName the entry name to populate + * @param latencyMillis the latency value in milliseconds + */ private static void setBotPoseEstimate( JSONObject resultsJson, String entryName, double latencyMillis) { double[] estimate_array = getBotPoseEstimateArray(resultsJson, entryName, latencyMillis); @@ -62,12 +115,36 @@ private static void setBotPoseEstimate( tableEntry.set(estimate_array, fakeTimestampMicros); } + /** + * Clears a bot pose estimate entry by setting it to an empty array. + * + * @param entryName the NetworkTables entry to clear + * @param timestampMicros the timestamp in microseconds + */ private static void clearBotPoseEstimate(String entryName, long timestampMicros) { DoubleArrayEntry tableEntry = LimelightHelpers.getLimelightDoubleArrayEntry(TABLE_NAME, entryName); tableEntry.set(new double[0], timestampMicros); } + /** + * Builds a bot pose estimate array from Limelight JSON data. + *

+ * The array is structured as: + *

    + *
  • Indices 0–5: pose data from Limelight
    • + *
  • Index 6: latency in milliseconds
    • + *
  • Index 7: tag count
    • + *
  • Index 8: tag ID
    • + *
+ * Additional indices are reserved for multiple tags. + *

+ * + * @param resultsJson the Limelight "Results" JSON object + * @param entryName the entry to extract + * @param latencyMillis the latency in milliseconds + * @return a double array representing the bot pose estimate, or an empty array if not present + */ private static double[] getBotPoseEstimateArray( JSONObject resultsJson, String entryName, double latencyMillis) { if (!resultsJson.has(entryName)) { diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java b/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java index b1e5e1d7..b2479a2d 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java @@ -16,28 +16,60 @@ import org.junit.ClassRule; import org.junit.Test; +/** + * Unit tests for {@link RestLimelight}. + *

+ * This test suite uses a {@link FakeLimelight} HTTP server to simulate REST responses from a + * physical Limelight device. It validates Limelight instance management, HTTP endpoint behavior, + * and field map uploads. + *

+ */ public class RestLimelightTest extends LimelightTestCase { + + /** + * Fake Limelight instance running as an embedded HTTP server. + *

+ * Used to simulate responses from a real Limelight device without requiring hardware. + *

+ */ @ClassRule public static final FakeLimelight fakeLimelight = new FakeLimelight(); + /** + * Resets any cached {@link RestLimelight} instances after each test. + */ @After public void resetLimelights() { RestLimelight.eraseInstances(); } + /** + * Resets the {@link FakeLimelight} after each test, ensuring no test state leaks + * between test methods. + */ @After public void resetFakeLimelight() { fakeLimelight.reset(); } + /** + * Verifies that calls to obtain the default {@link RestLimelight} return the same instance, + * and that the default instance is equal to one retrieved by name. + */ @Test public void equality() { Limelight a = RestLimelight.getDefaultLimelight(); Limelight b = RestLimelight.getDefaultLimelight(); assertEquals("Default limelight call returned different values", a, b); + Limelight c = RestLimelight.getLimelight(RestLimelight.DEFAULT_ADDRESS); assertEquals("Default limelights not equal to limelights named \"limelight\" (default)", a, c); } + /** + * Ensures the {@link FakeLimelight} responds correctly to REST requests. + * + * @throws Exception if the HTTP client request fails + */ @Test public void fakeWorks() throws Exception { fakeLimelight.setResultsResponse(new JSONObject().put("v", 1)); @@ -49,16 +81,25 @@ public void fakeWorks() throws Exception { assertEquals("{\"v\":1}", response.body()); } + /** + * Verifies that uploading a field map to a {@link RestLimelight} updates + * the {@link FakeLimelight} server as expected. + * + * @throws Exception if reading the field map resource or uploading fails + */ @Test public void setFieldMapWorks() throws Exception { Limelight limelight = createLimelight(); String resourceName = "frc2025r2.fmap"; + + // Upload field map from resources try (InputStream is = getClass().getResourceAsStream(resourceName)) { limelight.setFieldMap(is, true); } String fieldMap = fakeLimelight.getFieldMap(); assertNotNull(fieldMap); + // Verify expected field map content matches String expectedFieldMap; try (InputStream is = getClass().getResourceAsStream(resourceName)) { expectedFieldMap = new String(is.readAllBytes(), StandardCharsets.UTF_8); @@ -66,6 +107,11 @@ public void setFieldMapWorks() throws Exception { assertEquals(expectedFieldMap, fieldMap); } + /** + * Creates a new {@link RestLimelight} instance for testing. + * + * @return a {@code RestLimelight} bound to "localhost" + */ @Override protected Limelight createLimelight() { RestLimelight limelight = new RestLimelight("localhost"); @@ -73,14 +119,18 @@ protected Limelight createLimelight() { return limelight; } + /** + * Sets the Limelight JSON results response in the {@link FakeLimelight}. + *

+ * Unlike older schemas, the new Limelight JSON schema does not include a {@code "Results"} object + * at the root. Instead, all fields are inlined. Since resource test files still contain + * {@code "Results"}, this method extracts and forwards that sub-object. + *

+ * + * @param json the full Limelight JSON, expected to contain a {@code "Results"} object + */ @Override protected void setJson(JSONObject json) { - // limelight json schema has been updated to not json object "Results" in the root, and we want - // to test the new version, which does not. - // The new version just has all json that was in "Results" in the root, so the json object in - // "Results" will essentially be the new schema. - // Since we know that all the json objects in the resources folder have the "Results" json - // object, this should never fail. fakeLimelight.setResultsResponse(json.getJSONObject("Results")); } } diff --git a/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/CommandTester.java b/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/CommandTester.java index 42bc7741..5b6554e7 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/CommandTester.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/CommandTester.java @@ -3,12 +3,18 @@ import edu.wpi.first.wpilibj2.command.Command; /** - * Allows tests to run commands. + * Utility interface for running WPILib {@link Command} objects inside tests. * - *

Tests can get an instance by using {@link WPILibExtension}. + *

Provides a way to schedule and execute commands to completion within a test context. + * An implementation is provided via {@link WPILibExtension}. */ public interface CommandTester { - /** Schedules the provided command and runs it until it completes. */ + /** + * Schedules the provided {@link Command} and repeatedly runs it + * until the command reports it has finished. + * + * @param command the command to schedule and execute + */ void runUntilComplete(Command command); } diff --git a/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java b/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java index 262b70a5..7afc6359 100644 --- a/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java +++ b/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java @@ -13,11 +13,17 @@ import org.junit.jupiter.api.extension.ParameterResolver; /** - * JUnit Jupiter extension for testing code that depends on WPILib. + * JUnit Jupiter extension for testing robot code that depends on WPILib. * - *

Also provides a {@link CommandTester} for tests. + *

This extension: + *

    + *
  • Initializes the WPILib {@link HAL} and driver station simulation before tests run.
    • + *
  • Configures the {@link CommandScheduler} to ensure a clean environment.
    • + *
  • Provides a {@link CommandTester} parameter for tests, which allows running + * WPILib commands until completion.
    • + *
* - *

Example use: + *

Example usage: * * 

{@code
  * @ExtendWith(WPILibExtension.class)
@@ -26,7 +32,6 @@
  *   @Test
  *   public void initiallyNotInAir() {
  *     var flight = new FlightSubsystem();
- *
  *     assertThat(flight.inAir()).isFalse();
  *   }
  *
@@ -35,6 +40,7 @@
  *     var flight = new FlightSubsystem();
  *     Command takeOff = flight.createTakeOffCommandCommand();
  *
+ *     // Run the command under test
  *     commandTester.runUntilComplete(takeOff);
  *
  *     assertThat(flight.inAir()).isTrue();
@@ -49,6 +55,16 @@ public final class WPILibExtension
         BeforeAllCallback,
         ParameterResolver {
 
+  /**
+   * Initializes WPILib components before all tests.
+   * 

+   * Sets up the HAL, enables the driver station simulation,
+   * and resets the {@link CommandScheduler}.
+   * 

+   *
+   * @param context the JUnit extension context
+   * @throws IllegalStateException if the HAL cannot be initialized
+   */
   @Override
   public void beforeAll(ExtensionContext context) {
     // See https://www.chiefdelphi.com/t/driverstation-getalliance-in-gradle-test/
@@ -62,12 +78,25 @@ public void beforeAll(ExtensionContext context) {
     CommandScheduler.getInstance().unregisterAllSubsystems();
   }
 
+  /**
+   * Cleans up after each test by canceling all commands
+   * and unregistering all subsystems.
+   *
+   * @param context the JUnit extension context
+   */
   @Override
   public void afterEach(ExtensionContext context) {
     CommandScheduler.getInstance().cancelAll();
     CommandScheduler.getInstance().unregisterAllSubsystems();
   }
 
+  /**
+   * Cleans up after all tests by canceling all commands,
+   * unregistering all subsystems, disabling the scheduler,
+   * and resetting the driver station simulation.
+   *
+   * @param context the JUnit extension context
+   */
   @Override
   public void afterAll(ExtensionContext context) {
     CommandScheduler.getInstance().cancelAll();
@@ -77,6 +106,14 @@ public void afterAll(ExtensionContext context) {
     DriverStationSim.notifyNewData();
   }
 
+  /**
+   * Checks whether this extension can provide a parameter of type {@link CommandTester}.
+   *
+   * @param parameterContext the parameter context
+   * @param extensionContext the extension context
+   * @return true if the parameter type is {@code CommandTester}
+   * @throws ParameterResolutionException if parameter resolution fails
+   */
   @Override
   public boolean supportsParameter(
       ParameterContext parameterContext, ExtensionContext extensionContext)
@@ -84,6 +121,17 @@ public boolean supportsParameter(
     return CommandTester.class.equals(parameterContext.getParameter().getType());
   }
 
+  /**
+   * Provides a {@link CommandTester} instance for parameter injection.
+   * 

+   * The tester schedules the given command and repeatedly runs
+   * the {@link CommandScheduler} until the command completes.
+   * 

+   *
+   * @param parameterContext the parameter context
+   * @param extensionContext the extension context
+   * @return a {@link CommandTester} that executes commands to completion
+   */
   @Override
   public CommandTester resolveParameter(
       ParameterContext parameterContext, ExtensionContext extensionContext) {
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java
index f3b33296..78125886 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java
@@ -9,19 +9,41 @@
 import javax.annotation.Nullable;
 
 /**
- * Truth Subject for making assertions about {@link Pose2d} values.
+ * A Truth {@link Subject} for making assertions about {@link Pose2d} values.
  *
- * 
See Writing your own custom subject to learn about
- * creating custom Truth subjects.
+ * 
This subject provides fluent assertions for comparing poses, including
+ * tolerance-based comparisons of translations and rotations.
+ *
+ * 
See Truth: Writing your own custom subject
+ * for more on extending Truth.
  */
 public final class Pose2dSubject extends Subject {
 
-  // User-defined entry point
+  /**
+   * Entry point for {@link Pose2d} assertions.
+   *
+   * 
Usage:
+   * 
{@code
+   * Pose2d actualPose = ...;
+   * Pose2d expectedPose = ...;
+   *
+   * Pose2dSubject.assertThat(actualPose)
+   *     .isWithin(0.01)
+   *     .of(expectedPose);
+   * }

+   *
+   * @param pose the pose under test (may be {@code null})
+   * @return a {@link Pose2dSubject} for making assertions
+   */
   public static Pose2dSubject assertThat(@Nullable Pose2d pose) {
     return assertAbout(pose2ds()).that(pose);
   }
 
-  // Static method for getting the subject factory (for use with assertAbout())
+  /**
+   * Factory for {@link Pose2dSubject}, used with {@link assertAbout}.
+   *
+   * @return a factory for creating {@link Pose2dSubject} instances
+   */
   public static Subject.Factory pose2ds() {
     return Pose2dSubject::new;
   }
@@ -33,8 +55,15 @@ private Pose2dSubject(FailureMetadata failureMetadata, @Nullable Pose2d subject)
     this.actual = subject;
   }
 
-  // User-defined test assertion SPI below this point
-
+  /**
+   * Returns a tolerant comparison assertion for the current pose.
+   * 

+   * The tolerance applies to both translation (x, y) and rotation (θ).
+   * 

+   *
+   * @param tolerance the maximum allowed difference in meters (translation) or radians (rotation)
+   * @return a {@link TolerantComparison} for comparing poses with a tolerance
+   */
   public TolerantComparison isWithin(double tolerance) {
     return new TolerantComparison() {
       @Override
@@ -45,20 +74,36 @@ public void of(Pose2d expected) {
     };
   }
 
+  /**
+   * Returns a {@link Translation2dSubject} for making assertions about the translation
+   * component of this pose.
+   *
+   * @return a subject for the pose's translation
+   */
   public Translation2dSubject translation() {
     return check("getTranslation()")
         .about(Translation2dSubject.translation2ds())
         .that(nonNullActualPose().getTranslation());
   }
 
+  /**
+   * Returns a {@link Rotation2dSubject} for making assertions about the rotation
+   * component of this pose.
+   *
+   * @return a subject for the pose's rotation
+   */
   public Rotation2dSubject rotation() {
     return check("getRotation()")
         .about(Rotation2dSubject.rotation2ds())
         .that(nonNullActualPose().getRotation());
   }
 
-  // Helper methods below this point
-
+  /**
+   * Ensures that the actual pose is not {@code null}.
+   *
+   * @return the non-null actual {@link Pose2d}
+   * @throws AssertionError if the pose under test is {@code null}
+   */
   private Pose2d nonNullActualPose() {
     if (actual == null) {
       failWithActual(simpleFact("expected a non-null Pose2d"));
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java
index 46990843..8cb8e468 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java
@@ -9,19 +9,41 @@
 import javax.annotation.Nullable;
 
 /**
- * Truth Subject for making assertions about {@link Pose3d} values.
+ * A Truth {@link Subject} for making assertions about {@link Pose3d} values.
  *
- * 
See Writing your own custom subject to learn about
- * creating custom Truth subjects.
+ * 
This subject provides fluent assertions for comparing 3D poses, including
+ * tolerance-based comparisons of translations and rotations.
+ *
+ * 
See Truth: Writing your own custom subject
+ * for more on extending Truth.
  */
 public final class Pose3dSubject extends Subject {
 
-  // User-defined entry point
+  /**
+   * Entry point for {@link Pose3d} assertions.
+   *
+   * 
Usage:
+   * 
{@code
+   * Pose3d actualPose = ...;
+   * Pose3d expectedPose = ...;
+   *
+   * Pose3dSubject.assertThat(actualPose)
+   *     .isWithin(0.01)
+   *     .of(expectedPose);
+   * }

+   *
+   * @param pose the pose under test (may be {@code null})
+   * @return a {@link Pose3dSubject} for making assertions
+   */
   public static Pose3dSubject assertThat(@Nullable Pose3d pose) {
     return assertAbout(pose3ds()).that(pose);
   }
 
-  // Static method for getting the subject factory (for use with assertAbout())
+  /**
+   * Factory for {@link Pose3dSubject}, used with {@link assertAbout}.
+   *
+   * @return a factory for creating {@link Pose3dSubject} instances
+   */
   public static Subject.Factory pose3ds() {
     return Pose3dSubject::new;
   }
@@ -33,8 +55,14 @@ private Pose3dSubject(FailureMetadata failureMetadata, @Nullable Pose3d subject)
     this.actual = subject;
   }
 
-  // User-defined test assertion SPI below this point
-
+  /**
+   * Returns a tolerant comparison assertion for the current 3D pose.
+   *
+   * 
The tolerance applies to both translation (x, y, z) and rotation (roll, pitch, yaw).
+   *
+   * @param tolerance the maximum allowed difference in meters (translation) or radians (rotation)
+   * @return a {@link TolerantComparison} for comparing poses with a tolerance
+   */
   public TolerantComparison isWithin(double tolerance) {
     return new TolerantComparison() {
       @Override
@@ -45,22 +73,36 @@ public void of(Pose3d expected) {
     };
   }
 
-  // Chained subjects methods below this point
-
+  /**
+   * Returns a {@link Translation3dSubject} for making assertions about the translation
+   * component of this pose.
+   *
+   * @return a subject for the pose's 3D translation
+   */
   public Translation3dSubject translation() {
     return check("getTranslation()")
         .about(Translation3dSubject.translation3ds())
         .that(nonNullActualPose().getTranslation());
   }
 
+  /**
+   * Returns a {@link Rotation3dSubject} for making assertions about the rotation
+   * component of this pose.
+   *
+   * @return a subject for the pose's 3D rotation
+   */
   public Rotation3dSubject rotation() {
     return check("getRotation()")
         .about(Rotation3dSubject.rotation3ds())
         .that(nonNullActualPose().getRotation());
   }
 
-  // Helper methods below this point
-
+  /**
+   * Ensures that the actual pose is not {@code null}.
+   *
+   * @return the non-null actual {@link Pose3d}
+   * @throws AssertionError if the pose under test is {@code null}
+   */
   private Pose3d nonNullActualPose() {
     if (actual == null) {
       failWithActual(simpleFact("expected a non-null Pose3d"));
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java
index d50e4f0a..e14bbce3 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java
@@ -9,14 +9,41 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import javax.annotation.Nullable;
 
-/** Truth Subject for making assertions about {@link Rotation2d} values. */
+/**
+ * A Truth {@link Subject} for making assertions about {@link Rotation2d} values.
+ *
+ * 
This subject provides fluent assertions for comparing 2D rotations,
+ * including tolerance-based comparisons and checks for zero rotation.
+ *
+ * 
Rotations are compared in radians.
+ */
 public final class Rotation2dSubject extends Subject {
-  // User-defined entry point
+
+  /**
+   * Entry point for {@link Rotation2d} assertions.
+   *
+   * 
Usage:
+   * 
{@code
+   * Rotation2d actual = new Rotation2d(Math.PI / 2);
+   * Rotation2d expected = new Rotation2d(Math.PI / 2 + 1e-3);
+   *
+   * Rotation2dSubject.assertThat(actual)
+   *     .isWithin(1e-2)
+   *     .of(expected);
+   * }

+   *
+   * @param rotation the rotation under test (may be {@code null})
+   * @return a {@link Rotation2dSubject} for making assertions
+   */
   public static Rotation2dSubject assertThat(@Nullable Rotation2d rotation) {
     return assertAbout(rotation2ds()).that(rotation);
   }
 
-  // Static method for getting the subject factory (for use with assertAbout())
+  /**
+   * Factory for {@link Rotation2dSubject}, used with {@link assertAbout}.
+   *
+   * @return a factory for creating {@link Rotation2dSubject} instances
+   */
   public static Factory rotation2ds() {
     return Rotation2dSubject::new;
   }
@@ -28,8 +55,14 @@ private Rotation2dSubject(FailureMetadata failureMetadata, @Nullable Rotation2d
     this.actual = subject;
   }
 
-  // User-defined test assertion SPI below this point
-
+  /**
+   * Returns a tolerant comparison assertion for this rotation.
+   *
+   * 
The tolerance is expressed in radians.
+   *
+   * @param tolerance the maximum allowed difference in radians
+   * @return a {@link TolerantComparison} for comparing rotations with a tolerance
+   */
   public TolerantComparison isWithin(double tolerance) {
     return new TolerantComparison() {
       @Override
@@ -39,20 +72,33 @@ public void of(Rotation2d expected) {
     };
   }
 
+  /**
+   * Asserts that this rotation is exactly equal to {@link Rotation2d#kZero}.
+   *
+   * 
Fails if the rotation under test is {@code null} or not equal to zero.
+   */
   public void isZero() {
     if (!Rotation2d.kZero.equals(actual)) {
       failWithActual(simpleFact("expected to be zero"));
     }
   }
 
-  // Chained subjects methods below this point
-
+  /**
+   * Returns a {@link DoubleSubject} for making assertions about the rotation’s
+   * raw radians value.
+   *
+   * @return a {@link DoubleSubject} for the rotation’s radians
+   */
   public DoubleSubject getRadians() {
     return check("getRadians()").that(nonNullActual().getRadians());
   }
 
-  // Helper methods below this point
-
+  /**
+   * Ensures that the actual rotation is not {@code null}.
+   *
+   * @return the non-null actual {@link Rotation2d}
+   * @throws AssertionError if the rotation under test is {@code null}
+   */
   private Rotation2d nonNullActual() {
     if (actual == null) {
       failWithActual(simpleFact("expected a non-null Rotation2d"));
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java
index fe78d230..952f345b 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java
@@ -9,14 +9,47 @@
 import edu.wpi.first.math.geometry.Rotation3d;
 import javax.annotation.Nullable;
 
-/** Truth Subject for making assertions about {@link Rotation3d} values. */
+/**
+ * A Truth {@link Subject} for making assertions about {@link Rotation3d} values.
+ *
+ * 
This subject provides fluent assertions for comparing 3D rotations,
+ * including tolerance-based comparisons of roll, pitch, and yaw angles,
+ * as well as checks for zero rotation.
+ *
+ * 
Rotations are expressed in radians:
+ * 
    
+ *   
  • {@link #x()} → roll (counterclockwise about X axis)
    • 
+ *   
  • {@link #y()} → pitch (counterclockwise about Y axis)
    • 
+ *   
  • {@link #z()} → yaw (counterclockwise about Z axis)
    • 
+ * 

+ */
 public final class Rotation3dSubject extends Subject {
-  // User-defined entry point
+
+  /**
+   * Entry point for {@link Rotation3d} assertions.
+   *
+   * 
Usage:
+   * 
{@code
+   * Rotation3d actual = new Rotation3d(Math.PI / 2, 0, 0);
+   * Rotation3d expected = new Rotation3d(Math.PI / 2 + 1e-3, 0, 0);
+   *
+   * Rotation3dSubject.assertThat(actual)
+   *     .isWithin(1e-2)
+   *     .of(expected);
+   * }

+   *
+   * @param rotation the rotation under test (may be {@code null})
+   * @return a {@link Rotation3dSubject} for making assertions
+   */
   public static Rotation3dSubject assertThat(@Nullable Rotation3d rotation) {
     return assertAbout(rotation3ds()).that(rotation);
   }
 
-  // Static method for getting the subject factory (for use with assertAbout())
+  /**
+   * Factory for {@link Rotation3dSubject}, used with {@link assertAbout}.
+   *
+   * @return a factory for creating {@link Rotation3dSubject} instances
+   */
   public static Factory rotation3ds() {
     return Rotation3dSubject::new;
   }
@@ -28,53 +61,70 @@ private Rotation3dSubject(FailureMetadata failureMetadata, @Nullable Rotation3d
     this.actual = subject;
   }
 
-  // User-defined test assertion SPI below this point
-
+  /**
+   * Returns a tolerant comparison assertion for this rotation.
+   *
+   * 
The tolerance is expressed in radians and applies to each of the
+   * roll (X), pitch (Y), and yaw (Z) components independently.
+   *
+   * @param tolerance the maximum allowed difference in radians
+   * @return a {@link TolerantComparison} for comparing rotations with a tolerance
+   */
   public TolerantComparison isWithin(double tolerance) {
     return new TolerantComparison() {
       @Override
       public void of(Rotation3d expected) {
-        x().isWithin(tolerance).of(expected.getX()); // roll, in radians
-        y().isWithin(tolerance).of(expected.getY()); // pitch, in radians
-        z().isWithin(tolerance).of(expected.getZ()); // yaw, in radians
+        x().isWithin(tolerance).of(expected.getX()); // roll
+        y().isWithin(tolerance).of(expected.getY()); // pitch
+        z().isWithin(tolerance).of(expected.getZ()); // yaw
       }
     };
   }
 
+  /**
+   * Asserts that this rotation is exactly equal to {@link Rotation3d#kZero}.
+   *
+   * 
Fails if the rotation under test is {@code null} or not equal to zero.
+   */
   public void isZero() {
     if (!Rotation3d.kZero.equals(actual)) {
       failWithActual(simpleFact("expected to be zero"));
     }
   }
 
-  // Chained subjects methods below this point
-
   /**
-   * Returns a subject that can be used to make assertions about the counterclockwise rotation angle
-   * around the X axis (roll) in radians.
+   * Returns a subject for making assertions about the roll (X-axis rotation, radians).
+   *
+   * @return a {@link DoubleSubject} for the roll component in radians
    */
   public DoubleSubject x() {
     return check("getX()").that(nonNullActual().getX());
   }
 
   /**
-   * Returns a subject that can be used to make assertions about the counterclockwise rotation angle
-   * around the Y axis (pitch) in radians.
+   * Returns a subject for making assertions about the pitch (Y-axis rotation, radians).
+   *
+   * @return a {@link DoubleSubject} for the pitch component in radians
    */
   public DoubleSubject y() {
     return check("getY()").that(nonNullActual().getY());
   }
 
   /**
-   * Returns a subject that can be used to make assertions about the counterclockwise rotation angle
-   * around the Z axis (yaw) in radians.
+   * Returns a subject for making assertions about the yaw (Z-axis rotation, radians).
+   *
+   * @return a {@link DoubleSubject} for the yaw component in radians
    */
   public DoubleSubject z() {
     return check("getZ()").that(nonNullActual().getZ());
   }
 
-  // Helper methods below this point
-
+  /**
+   * Ensures that the actual rotation is not {@code null}.
+   *
+   * @return the non-null actual {@link Rotation3d}
+   * @throws AssertionError if the rotation under test is {@code null}
+   */
   private Rotation3d nonNullActual() {
     if (actual == null) {
       failWithActual(simpleFact("expected a non-null Rotation3d"));
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/TolerantComparison.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/TolerantComparison.java
index cc2c2b38..e601265d 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/TolerantComparison.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/TolerantComparison.java
@@ -3,26 +3,45 @@
 import org.jspecify.annotations.Nullable;
 
 /**
- * A partially specified check about an approximate relationship to a {@code double} subject using a
- * tolerance.
+ * Represents a partially specified check on a subject of type {@code T} for approximate equality
+ * within a given tolerance. This class is intended to be used as part of a fluent assertion API,
+ * where the subject and tolerance are defined earlier in the chain, and the expected value is
+ * provided via {@link #of(Object)}.
+ *
+ * 
For example, in a fluent assertion style:
+ * 
{@code
+ * assertThat(actualValue).isWithin(tolerance).of(expectedValue);
+ * }

+ *
+ * 
Subclasses of this class implement the specific logic for comparing the subject with the
+ * expected value considering the tolerance.
+ *
+ * @param  the type of the value being compared (typically {@link Double})
  */
 public abstract class TolerantComparison {
 
-  // Prevent subclassing outside of this package
+  /**
+   * Package-private constructor to prevent subclassing outside of this package.
+   */
   TolerantComparison() {}
 
   /**
-   * Fails if the subject was expected to be within the tolerance of the given value but was not.
-   * The subject and tolerance are specified earlier in the fluent call chain.
+   * Fails the assertion if the subject is not within the tolerance of the given expected value.
+   * The subject and tolerance must have been specified earlier in the fluent call chain.
+   *
+   * @param expected the value the subject is expected to be approximately equal to
    */
   public abstract void of(T expected);
 
-  // Remaining code copied from DoubleSubject.TolerantDoubleComparison
-
   /**
+   * {@inheritDoc}
+   *
+   * 
This method is unsupported for {@code TolerantComparison}. Equality comparisons should be
+   * performed via {@link #of(Object)} rather than {@link Object#equals(Object)}.
+   *
    * @throws UnsupportedOperationException always
-   * @deprecated {@link Object#equals(Object)} is not supported on TolerantComparison. If you meant
-   *     to compare doubles, use {@link #of(T)} instead.
+   * @deprecated Use {@link #of(Object)} to compare values. {@link Object#equals(Object)} is not
+   *     supported.
    */
   @Deprecated
   @Override
@@ -32,8 +51,12 @@ public final boolean equals(@Nullable Object o) {
   }
 
   /**
+   * {@inheritDoc}
+   *
+   * 
This method is unsupported for {@code TolerantComparison}.
+   *
    * @throws UnsupportedOperationException always
-   * @deprecated {@link Object#hashCode()} is not supported on TolerantComparison
+   * @deprecated {@link Object#hashCode()} is not supported on {@code TolerantComparison}.
    */
   @Deprecated
   @Override
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java
index 41a51edd..5b32fada 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java
@@ -9,28 +9,63 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import javax.annotation.Nullable;
 
-/** Truth Subject for making assertions about {@link Translation2d} values. */
+/**
+ * A Truth {@link Subject} for making fluent assertions about {@link Translation2d} values.
+ *
+ * 
This class allows tests to verify the x and y components of a {@code Translation2d}, perform
+ * approximate comparisons within a tolerance, and check special properties like being zero.
+ *
+ * 
Example usage:
+ * 
{@code
+ * Translation2d translation = new Translation2d(1.0, 2.0);
+ * Translation2dSubject.assertThat(translation)
+ *     .isWithin(0.01).of(new Translation2d(1.0, 2.0));
+ * Translation2dSubject.assertThat(translation).x().isEqualTo(1.0);
+ * Translation2dSubject.assertThat(translation).y().isEqualTo(2.0);
+ * }

+ */
 public final class Translation2dSubject extends Subject {
 
-  // User-defined entry point
+  /**
+   * Entry point for assertions about a {@link Translation2d} instance.
+   *
+   * @param translation the translation to assert about (nullable)
+   * @return a {@link Translation2dSubject} for fluent assertions
+   */
   public static Translation2dSubject assertThat(@Nullable Translation2d translation) {
     return assertAbout(translation2ds()).that(translation);
   }
 
-  // Static method for getting the subject factory (for use with assertAbout())
+  /**
+   * Returns a Truth {@link Factory} for {@link Translation2dSubject}, used with
+   * {@link com.google.common.truth.Truth#assertAbout(Subject.Factory)}.
+   *
+   * @return a factory for creating {@link Translation2dSubject} instances
+   */
   public static Factory translation2ds() {
     return Translation2dSubject::new;
   }
 
   private final Translation2d actual;
 
+  /**
+   * Constructor used internally by Truth.
+   *
+   * @param failureMetadata metadata about the failure for reporting
+   * @param subject the actual {@link Translation2d} value (nullable)
+   */
   private Translation2dSubject(FailureMetadata failureMetadata, @Nullable Translation2d subject) {
     super(failureMetadata, subject);
     this.actual = subject;
   }
 
-  // User-defined test assertion SPI below this point
-
+  /**
+   * Returns a {@link TolerantComparison} to assert that this translation is within the given
+   * {@code tolerance} of an expected {@link Translation2d}.
+   *
+   * @param tolerance the allowed absolute difference in both x and y components
+   * @return a {@link TolerantComparison} for fluent approximate comparisons
+   */
   public TolerantComparison isWithin(double tolerance) {
     return new TolerantComparison() {
       @Override
@@ -41,24 +76,38 @@ public void of(Translation2d expected) {
     };
   }
 
+  /**
+   * Fails if the {@link Translation2d} is not the zero vector.
+   */
   public void isZero() {
     if (!Translation2d.kZero.equals(actual)) {
       failWithActual(simpleFact("expected to be zero"));
     }
   }
 
-  // Chained subjects methods below this point
-
+  /**
+   * Returns a {@link DoubleSubject} for assertions on the x-component of the translation.
+   *
+   * @return a {@link DoubleSubject} for the x value
+   */
   public DoubleSubject x() {
     return check("getX()").that(nonNullActual().getX());
   }
 
+  /**
+   * Returns a {@link DoubleSubject} for assertions on the y-component of the translation.
+   *
+   * @return a {@link DoubleSubject} for the y value
+   */
   public DoubleSubject y() {
     return check("getY()").that(nonNullActual().getY());
   }
 
-  // Helper methods below this point
-
+  /**
+   * Ensures the actual translation is non-null, failing the assertion if it is null.
+   *
+   * @return the non-null {@link Translation2d} instance
+   */
   private Translation2d nonNullActual() {
     if (actual == null) {
       failWithActual(simpleFact("expected a non-null Translation2d"));
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java
index 302e1227..cb278d0e 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java
@@ -9,28 +9,64 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import javax.annotation.Nullable;
 
-/** Truth Subject for making assertions about {@link Translation3d} values. */
+/**
+ * A Truth {@link Subject} for making fluent assertions about {@link Translation3d} values.
+ *
+ * 
This class allows tests to verify the x, y, and z components of a {@code Translation3d},
+ * perform approximate comparisons within a tolerance, and check special properties like being zero.
+ *
+ * 
Example usage:
+ * 
{@code
+ * Translation3d translation = new Translation3d(1.0, 2.0, 3.0);
+ * Translation3dSubject.assertThat(translation)
+ *     .isWithin(0.01).of(new Translation3d(1.0, 2.0, 3.0));
+ * Translation3dSubject.assertThat(translation).x().isEqualTo(1.0);
+ * Translation3dSubject.assertThat(translation).y().isEqualTo(2.0);
+ * Translation3dSubject.assertThat(translation).z().isEqualTo(3.0);
+ * }

+ */
 public final class Translation3dSubject extends Subject {
 
-  // User-defined entry point
+  /**
+   * Entry point for assertions about a {@link Translation3d} instance.
+   *
+   * @param translation the translation to assert about (nullable)
+   * @return a {@link Translation3dSubject} for fluent assertions
+   */
   public static Translation3dSubject assertThat(@Nullable Translation3d translation) {
     return assertAbout(translation3ds()).that(translation);
   }
 
-  // Static method for getting the subject factory (for use with assertAbout())
+  /**
+   * Returns a Truth {@link Factory} for {@link Translation3dSubject}, used with
+   * {@link com.google.common.truth.Truth#assertAbout(Subject.Factory)}.
+   *
+   * @return a factory for creating {@link Translation3dSubject} instances
+   */
   public static Factory translation3ds() {
     return Translation3dSubject::new;
   }
 
   private final Translation3d actual;
 
+  /**
+   * Constructor used internally by Truth.
+   *
+   * @param failureMetadata metadata about the failure for reporting
+   * @param subject the actual {@link Translation3d} value (nullable)
+   */
   private Translation3dSubject(FailureMetadata failureMetadata, @Nullable Translation3d subject) {
     super(failureMetadata, subject);
     this.actual = subject;
   }
 
-  // User-defined test assertion SPI below this point
-
+  /**
+   * Returns a {@link TolerantComparison} to assert that this translation is within the given
+   * {@code tolerance} of an expected {@link Translation3d}.
+   *
+   * @param tolerance the allowed absolute difference in x, y, and z components
+   * @return a {@link TolerantComparison} for fluent approximate comparisons
+   */
   public TolerantComparison isWithin(double tolerance) {
     return new TolerantComparison() {
       @Override
@@ -42,28 +78,47 @@ public void of(Translation3d expected) {
     };
   }
 
+  /**
+   * Fails if the {@link Translation3d} is not the zero vector.
+   */
   public void isZero() {
     if (!Translation3d.kZero.equals(actual)) {
       failWithActual(simpleFact("expected to be zero"));
     }
   }
 
-  // Chained subjects methods below this point
-
+  /**
+   * Returns a {@link DoubleSubject} for assertions on the x-component of the translation.
+   *
+   * @return a {@link DoubleSubject} for the x value
+   */
   public DoubleSubject x() {
     return check("getX()").that(nonNullActual().getX());
   }
 
+  /**
+   * Returns a {@link DoubleSubject} for assertions on the y-component of the translation.
+   *
+   * @return a {@link DoubleSubject} for the y value
+   */
   public DoubleSubject y() {
     return check("getY()").that(nonNullActual().getY());
   }
 
+  /**
+   * Returns a {@link DoubleSubject} for assertions on the z-component of the translation.
+   *
+   * @return a {@link DoubleSubject} for the z value
+   */
   public DoubleSubject z() {
     return check("getZ()").that(nonNullActual().getZ());
   }
 
-  // Helper methods below this point
-
+  /**
+   * Ensures the actual translation is non-null, failing the assertion if it is null.
+   *
+   * @return the non-null {@link Translation3d} instance
+   */
   private Translation3d nonNullActual() {
     if (actual == null) {
       failWithActual(simpleFact("expected a non-null Translation3d"));
diff --git a/testing/src/test/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtensionTest.java b/testing/src/test/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtensionTest.java
index 3fd1cfe5..e09494de 100644
--- a/testing/src/test/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtensionTest.java
+++ b/testing/src/test/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtensionTest.java
@@ -22,9 +22,21 @@
 import org.junit.platform.testkit.engine.EngineTestKit;
 import org.junit.platform.testkit.engine.Events;
 
-/** Tests for {@link WPILibExtension}. */
+/**
+ * Unit tests for the {@link WPILibExtension} JUnit 5 extension.
+ *
+ * 
This class ensures that WPILib-specific functionality (like {@link CommandScheduler} and
+ * {@link DriverStation}) behaves correctly when used with the extension. It verifies that:
+ *
+ * 
    
+ *   
  • Commands are not scheduled unexpectedly before or after tests
+ *   
  • {@link DriverStation} is properly enabled during tests
+ *   
  • {@link CommandTester} can execute and verify commands
+ * 

+ */
 public class WPILibExtensionTest {
 
+  /** A fake command used for scheduling tests. */
   static final Command FAKE_COMMAND = new Command() {};
 
   @BeforeAll
@@ -34,6 +46,12 @@ static void initializeHal() {
     }
   }
 
+  /**
+   * Sample test class demonstrating usage of {@link WPILibExtension} with JUnit 5.
+   *
+   * 
Uses {@link TestMethodOrder} to enforce ordering and {@link Tag} to mark it as special
+   * testkit-only.
+   */
   @ExtendWith(WPILibExtension.class)
   @Tag("ignore-outside-testkit")
   @TestMethodOrder(MethodOrderer.OrderAnnotation.class)
@@ -52,6 +70,7 @@ public static void verifyDriverStationEnabled() {
       assertThat(DriverStation.isEnabled()).isTrue();
     }
 
+    /** Verifies that FAKE_COMMAND is not scheduled before the first test, then schedules it. */
     @Test
     @Order(1)
     public void verifyFakeCommandNotScheduledBeforeTest() {
@@ -64,6 +83,7 @@ public void verifyFakeCommandNotScheduledBeforeTest() {
       assertThat(commandScheduler.isScheduled(FAKE_COMMAND));
     }
 
+    /** Verifies that FAKE_COMMAND is not scheduled before the second test, then schedules it. */
     @Test
     @Order(2)
     public void verifyFakeCommandNotScheduledAfterTest(CommandTester commandTester) {
@@ -76,6 +96,7 @@ public void verifyFakeCommandNotScheduledAfterTest(CommandTester commandTester)
       assertThat(commandScheduler.isScheduled(FAKE_COMMAND));
     }
 
+    /** Verifies that {@link CommandTester} runs and correctly verifies a command. */
     @Test
     @Order(3)
     public void verifyCommandTester(CommandTester commandTester) {
@@ -93,12 +114,13 @@ public static void verifyFakeCommandNotScheduledAfterAllTests() {
     }
   } // end SampleTest
 
+  /**
+   * Verifies that {@link WPILibExtension} executes SampleTest correctly and without failures.
+   */
   @Test
   void verifyExtension() {
-    // Arrange
     withDriverStationTemporarilyEnabled(
         () -> {
-          // Schedule FAKE_COMMAND
           CommandScheduler commandScheduler = CommandScheduler.getInstance();
           commandScheduler.enable();
           commandScheduler.schedule(FAKE_COMMAND);
@@ -107,14 +129,16 @@ void verifyExtension() {
           assertThat(isScheduled).isTrue();
         });
 
-    // Act
     EngineExecutionResults results =
         EngineTestKit.engine("junit-jupiter").selectors(selectClass(SampleTest.class)).execute();
 
-    // Assert
     assertHasNoFailures(results);
   }
 
+  /**
+   * Temporarily enables the driver station for the duration of {@code runnable}, restoring its
+   * previous state afterwards.
+   */
   private void withDriverStationTemporarilyEnabled(Runnable runnable) {
     assertThat(RobotState.isDisabled()).isTrue();
     DriverStationSim.setEnabled(true);
@@ -142,11 +166,17 @@ private void assertHasNoFailures(Events events) {
         });
   }
 
+  /**
+   * A test command used for verifying {@link CommandTester}.
+   *
+   * 
Tracks initialize() and execute() calls, and allows validation of expected behavior.
+   */
   private static class VerifiableCommand extends Command {
     private static final int EXPECTED_EXECUTION_COUNT = 4;
     private int initializedCount = 0;
     private int executionCount = 0;
 
+    /** Verifies that initialize and execute were called as expected. */
     void verify() {
       assertWithMessage("initialize() should be called").that(initializedCount).isGreaterThan(0);
       assertWithMessage("initialize() should not be called more than once")

From 30eaad4f82adc4d13e815fdd27f902a74ae0e0da Mon Sep 17 00:00:00 2001
From: codingduck106 <65737878+codingduck106@users.noreply.github.com>
Date: Thu, 2 Oct 2025 19:38:04 -0700
Subject: [PATCH 02/13] Update
 lib/src/main/java/com/team2813/lib2813/control/ControlMode.java

removed redundancies

Co-authored-by: Kevin Cooney 
---
 .../java/com/team2813/lib2813/control/ControlMode.java    | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java b/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java
index 29d68004..3d3d043a 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java
@@ -14,14 +14,6 @@
  * each mode corresponding to a specific {@link ControlType} from the SPARK API.
  * Additional vendor mappings can be added as needed.
  * 
- * 
Supported control modes:
- * 
    
- * 
  • DUTY_CYCLE: Open-loop duty cycle control (percentage output)
    • 
- * 
  • VELOCITY: Closed-loop velocity control with PID feedback
    • 
- * 
  • MOTION_MAGIC: Closed-loop position control with motion profiling
    • 
- * 
  • VOLTAGE: Open-loop voltage control
    • 
- * 

- * 
  * @author Team 2813
  * @since 1.0
  */

From f99a86446de7e0c2d8f46abfbe595134f60046f9 Mon Sep 17 00:00:00 2001
From: codingduck106 <65737878+codingduck106@users.noreply.github.com>
Date: Thu, 2 Oct 2025 19:38:26 -0700
Subject: [PATCH 03/13] Update
 lib/src/main/java/com/team2813/lib2813/control/ControlMode.java

update voltage documentation

Co-authored-by: Kevin Cooney 
---
 lib/src/main/java/com/team2813/lib2813/control/ControlMode.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java b/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java
index 3d3d043a..7fa2bd8e 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java
@@ -65,7 +65,7 @@ public enum ControlMode {
      * 
Controls the motor by applying a specific voltage directly to the motor terminals.
      * Unlike duty cycle control, voltage control compensates for battery voltage variations
      * to provide more consistent motor behavior. The demand value represents the desired
-     * voltage to apply to the motor.
+     * voltage (in Volts) to apply to the motor.
      * 
      * 
Maps to {@link ControlType#kVoltage} for SPARK controllers.
      */

From a959999febca7e614bb36b22d6aab6e92ed5fac7 Mon Sep 17 00:00:00 2001
From: vdikov <120233643+vdikov@users.noreply.github.com>
Date: Mon, 6 Oct 2025 06:10:47 +0200
Subject: [PATCH 04/13] Update WPILibExtension.java

javadoc paragraphs start with `
` and don't need a closing `
`
---
 .../lib2813/testing/junit/jupiter/WPILibExtension.java | 10 ++++------
 1 file changed, 4 insertions(+), 6 deletions(-)

diff --git a/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java b/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java
index 7afc6359..31ea18f7 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java
@@ -57,10 +57,9 @@ public final class WPILibExtension
 
   /**
    * Initializes WPILib components before all tests.
-   * 

-   * Sets up the HAL, enables the driver station simulation,
+   *
+   * 
Sets up the HAL, enables the driver station simulation,
    * and resets the {@link CommandScheduler}.
-   * 

    *
    * @param context the JUnit extension context
    * @throws IllegalStateException if the HAL cannot be initialized
@@ -123,10 +122,9 @@ public boolean supportsParameter(
 
   /**
    * Provides a {@link CommandTester} instance for parameter injection.
-   * 

-   * The tester schedules the given command and repeatedly runs
+   *
+   * 
The tester schedules the given command and repeatedly runs
    * the {@link CommandScheduler} until the command completes.
-   * 

    *
    * @param parameterContext the parameter context
    * @param extensionContext the extension context

From 74ff256a6714668191b04543dc60ebfc91b089a9 Mon Sep 17 00:00:00 2001
From: codingduck106 <65737878+codingduck106@users.noreply.github.com>
Date: Thu, 9 Oct 2025 18:57:39 -0700
Subject: [PATCH 05/13] Update
 lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java

Co-authored-by: Kevin Cooney 
---
 .../team2813/lib2813/preferences/PersistedConfiguration.java    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java b/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java
index 9a65c1f5..a78bfd83 100644
--- a/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java
+++ b/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java
@@ -100,7 +100,7 @@ public final class PersistedConfiguration {
   static Consumer errorReporter = DataLogManager::log;
 
   /**
-   * Construct a record instance, populating its components from Preferences.
+   * Constructs a record instance, populating its components from Preferences, using the provided instance to get default values.
    *
    * 
The provided instance supplies the default values. For each component:
    * 
    

From 24680376fdc93177c1af7dc4648beec313fb0c6e Mon Sep 17 00:00:00 2001
From: codingduck106 <65737878+codingduck106@users.noreply.github.com>
Date: Thu, 9 Oct 2025 18:59:22 -0700
Subject: [PATCH 06/13] Update
 lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java

Co-authored-by: Kevin Cooney 
---
 .../team2813/lib2813/preferences/PersistedConfiguration.java    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java b/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java
index a78bfd83..a96dcaf7 100644
--- a/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java
+++ b/lib/src/main/java/com/team2813/lib2813/preferences/PersistedConfiguration.java
@@ -247,7 +247,7 @@ private static  T createFromPreferences(
 
       Object componentValue = null;
       if (needComponentValue && configWithDefaults != null) {
-        // Reflection trick: get the field value from the default record instance
+        // Use reflection to get the field value from the default record instance
         Field defaultValueField = clazz.getDeclaredField(name);
         defaultValueField.setAccessible(true);
         componentValue = defaultValueField.get(configWithDefaults);

From acb038b9d85004bbcb37f4c838788ff4c15574d3 Mon Sep 17 00:00:00 2001
From: codingduck106 <65737878+codingduck106@users.noreply.github.com>
Date: Thu, 9 Oct 2025 18:59:53 -0700
Subject: [PATCH 07/13] Update
 lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java

Co-authored-by: Kevin Cooney 
---
 .../lib2813/subsystems/ParameterizedIntakeSubsystemTest.java    | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java b/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java
index 8cdc8f0b..48cc8ea8 100644
--- a/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java
+++ b/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java
@@ -88,7 +88,7 @@ public void initialState() {
   public void intakeItem(CommandTester commandTester) {
     try (var intake = new ConcreteParameterizedIntakeSubsystem(fakeMotor, params)) {
       Command command = intake.intakeItemCommand();
-      assertMotorIsStopped(); // motor should not yet be running
+      assertMotorIsStopped();
 
       commandTester.runUntilComplete(command); // executes command fully
 

From 836d60e73516610235c4a5a3d459ccfb891fec4a Mon Sep 17 00:00:00 2001
From: codingduck106 <65737878+codingduck106@users.noreply.github.com>
Date: Thu, 9 Oct 2025 19:13:08 -0700
Subject: [PATCH 08/13] Update
 lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java

Co-authored-by: Kevin Cooney 
---
 .../lib2813/subsystems/ParameterizedIntakeSubsystem.java        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java
index d6a5e74f..839c46a2 100644
--- a/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java
+++ b/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java
@@ -48,7 +48,7 @@ public interface Builder {
     /**
      * Constructs a Params record.
      *
-     * @param controlMode the motor control mode, cannot be null
+     * @param controlMode the motor control mode, cannot be {@code null}
      * @param intakeDemand the motor demand for intake, must be non-zero
      * @param outtakeDemand the motor demand for outtake, must be non-zero and opposite sign of intakeDemand
      * @throws IllegalArgumentException if validation fails

From 15d6e5fbed59bf81ff7938473141c27be12bd25b Mon Sep 17 00:00:00 2001
From: codingduck106 <65737878+codingduck106@users.noreply.github.com>
Date: Thu, 9 Oct 2025 19:14:27 -0700
Subject: [PATCH 09/13] Update
 lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java

Co-authored-by: Kevin Cooney 
---
 .../lib2813/subsystems/ParameterizedIntakeSubsystem.java        | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java
index 839c46a2..51630a17 100644
--- a/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java
+++ b/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java
@@ -84,7 +84,7 @@ protected ParameterizedIntakeSubsystem(PIDMotor intakeMotor, Params params) {
   /**
    * Returns a command that spins the intake wheels in the intake direction.
    *
-   * @return an {@link InstantCommand} that intakes a game piece
+   * @return an {@link Command} that intakes a game piece
    */
   public final Command intakeItemCommand() {
     return new InstantCommand(this::intakeGamePiece, this);

From 8753cb8f24381945314b40a58a6792874608b533 Mon Sep 17 00:00:00 2001
From: codingduck106 <65737878+codingduck106@users.noreply.github.com>
Date: Thu, 9 Oct 2025 19:14:58 -0700
Subject: [PATCH 10/13] Update
 lib/src/main/java/com/team2813/lib2813/util/FakePIDMotor.java

Co-authored-by: Kevin Cooney 
---
 lib/src/main/java/com/team2813/lib2813/util/FakePIDMotor.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/main/java/com/team2813/lib2813/util/FakePIDMotor.java b/lib/src/main/java/com/team2813/lib2813/util/FakePIDMotor.java
index be6c2432..5417b904 100644
--- a/lib/src/main/java/com/team2813/lib2813/util/FakePIDMotor.java
+++ b/lib/src/main/java/com/team2813/lib2813/util/FakePIDMotor.java
@@ -10,7 +10,7 @@
  * A fake implementation of {@link PIDMotor} used for testing.
  *
  * 
    This class simulates motor behavior by storing the most recent control mode and demand value.
- * Assertions are used (via Truth) to enforce correct expectations in tests.
+ * Assertions are used to enforce correct expectations in tests.
  *
  * 
    Not intended for production use.
  *

From 1d95b7c336e7345afc59bc7b40e0945b02682619 Mon Sep 17 00:00:00 2001
From: codingduck106 <65737878+codingduck106@users.noreply.github.com>
Date: Thu, 9 Oct 2025 19:15:26 -0700
Subject: [PATCH 11/13] Update
 lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java

Co-authored-by: Kevin Cooney 
---
 .../com/team2813/lib2813/preferences/IsolatedPreferences.java   | 2 --
 1 file changed, 2 deletions(-)

diff --git a/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java b/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java
index abbf6192..8011ee88 100644
--- a/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java
+++ b/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java
@@ -65,8 +65,6 @@ protected void after() {
 
     // Restore Preferences to the default NetworkTable instance
     Preferences.setNetworkTableInstance(NetworkTableInstance.getDefault());
-
-    // Close the temporary instance
     tempInstance.close();
   }
 }

From bbdf64c33aa1e771ae641c55fa88eaf89e6b6d02 Mon Sep 17 00:00:00 2001
From: codingduck106 <65737878+codingduck106@users.noreply.github.com>
Date: Thu, 9 Oct 2025 19:16:26 -0700
Subject: [PATCH 12/13] Update
 lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java

Co-authored-by: Kevin Cooney 
---
 .../com/team2813/lib2813/preferences/IsolatedPreferences.java | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java b/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java
index 8011ee88..1b508dba 100644
--- a/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java
+++ b/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java
@@ -44,12 +44,8 @@ public NetworkTable getPreferencesTable() {
   protected void before() {
     // Ensure the default instance is initialized
     NetworkTableInstance.getDefault();
-
-    // Create a temporary local NetworkTable instance for isolation
     tempInstance = NetworkTableInstance.create();
     tempInstance.startLocal();
-
-    // Redirect Preferences to use the temporary instance
     Preferences.setNetworkTableInstance(tempInstance);
   }
 

From 8a1a096e4217c17e654588000353627ac90e34e3 Mon Sep 17 00:00:00 2001
From: codingduck106 
Date: Thu, 9 Oct 2025 20:36:29 -0700
Subject: [PATCH 13/13] made formatting changes, updated InputValidationTest to
 match gradle,downgraded to jdk 21.

---
 .../team2813/lib2813/control/ControlMode.java |  178 +-
 .../lib2813/control/DeviceInformation.java    |  227 +-
 .../com/team2813/lib2813/control/Encoder.java |  237 +-
 .../team2813/lib2813/control/InvertType.java  |  451 ++--
 .../com/team2813/lib2813/control/Motor.java   |  198 +-
 .../team2813/lib2813/control/PIDMotor.java    |  227 +-
 .../control/encoders/CancoderWrapper.java     |  270 +--
 .../lib2813/control/imu/Pigeon2Wrapper.java   |  247 +-
 .../control/motors/SparkMaxWrapper.java       |  188 +-
 .../control/motors/TalonFXWrapper.java        |  226 +-
 .../lib2813/doc-files/dutycyclediagram.jpeg   |  Bin 0 -> 27170 bytes
 .../preferences/PersistedConfiguration.java   |  168 +-
 .../lib2813/subsystems/MotorSubsystem.java    |    9 +-
 .../ParameterizedIntakeSubsystem.java         |   14 +-
 .../subsystems/lightshow/Lightshow.java       |   36 +-
 .../subsystems/lightshow/QueueLightshow.java  |    9 +-
 .../lib2813/subsystems/lightshow/State.java   |    8 +-
 .../lib2813/util/BuildConstantsRecord.java    |   12 +-
 .../team2813/lib2813/util/ConfigUtils.java    |    6 +-
 .../lib2813/util/InvalidCanIdException.java   |    3 +-
 .../lib2813/util/ShuffleboardTabs.java        |    6 +-
 .../lib2813/control/InvertTypeTest.java       |    7 +-
 .../preferences/IsolatedPreferences.java      |    4 +-
 .../PersistedConfigurationTest.java           |  190 +-
 .../ParameterizedIntakeSubsystemTest.java     |    9 +-
 .../util/BuildConstantsPublisherTest.java     |   14 +-
 .../lib2813/util/ControlUtilsTest.java        |    4 +-
 .../lib2813/util/FakeShuffleboardTabs.java    |    6 +-
 .../lib2813/util/InputValidationTest.java     |   14 +-
 .../lib2813/limelight/BotPoseEstimate.java    |    6 +-
 .../lib2813/limelight/DataCollection.java     |    4 +-
 .../lib2813/limelight/JSONHelper.java         |    5 +-
 .../team2813/lib2813/limelight/Limelight.java |   10 +-
 .../lib2813/limelight/LimelightClient.java    |    3 +-
 .../lib2813/limelight/LimelightHelpers.java   | 1994 ++---------------
 .../lib2813/limelight/LocationalData.java     |    7 +-
 .../team2813/lib2813/limelight/Optionals.java |   57 +-
 .../lib2813/limelight/RestLimelight.java      |  147 +-
 .../lib2813/limelight/StubLocationalData.java |   50 +-
 .../limelight/apriltag_map/Fiducial.java      |    6 +-
 .../apriltag_map/FiducialRetriever.java       |    7 +-
 .../limelight/apriltag_map/FieldMap.java      |    1 +
 .../lib2813/limelight/FakeLimelight.java      |   70 +-
 .../limelight/LegacyRestLimelightTest.java    |   40 +-
 .../lib2813/limelight/LimelightTestCase.java  |  129 +-
 .../limelight/NetworkTablesLimelightTest.java |   44 +-
 .../lib2813/limelight/RestLimelightTest.java  |   33 +-
 .../testing/junit/jupiter/CommandTester.java  |    8 +-
 .../junit/jupiter/WPILibExtension.java        |   25 +-
 .../lib2813/testing/truth/Pose2dSubject.java  |   24 +-
 .../lib2813/testing/truth/Pose3dSubject.java  |   19 +-
 .../testing/truth/Rotation2dSubject.java      |   10 +-
 .../testing/truth/Rotation3dSubject.java      |   19 +-
 .../testing/truth/TolerantComparison.java     |    9 +-
 .../testing/truth/Translation2dSubject.java   |   13 +-
 .../testing/truth/Translation3dSubject.java   |   13 +-
 .../junit/jupiter/WPILibExtensionTest.java    |    4 +-
 57 files changed, 2074 insertions(+), 3651 deletions(-)
 create mode 100644 lib/src/main/java/com/team2813/lib2813/doc-files/dutycyclediagram.jpeg

diff --git a/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java b/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java
index 7fa2bd8e..73a37ab9 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/ControlMode.java
@@ -4,96 +4,102 @@
 
 /**
  * Enumeration defining standardized motor control modes for the team's control system.
- * 
- * 
    This enum provides a vendor-neutral abstraction over different motor controller
- * control modes while maintaining compatibility with specific hardware implementations.
- * Each control mode maps to the appropriate vendor-specific control type for seamless
- * integration with different motor controller families.
- * 
- * 
    The enum currently includes mappings for REV Robotics SPARK controllers, with
- * each mode corresponding to a specific {@link ControlType} from the SPARK API.
- * Additional vendor mappings can be added as needed.
- * 
+ *
+ * 
    This enum provides a vendor-neutral abstraction over different motor controller control modes
+ * while maintaining compatibility with specific hardware implementations. Each control mode maps to
+ * the appropriate vendor-specific control type for seamless integration with different motor
+ * controller families.
+ *
+ * 
    The enum currently includes mappings for REV Robotics SPARK controllers, with each mode
+ * corresponding to a specific {@link ControlType} from the SPARK API. Additional vendor mappings
+ * can be added as needed.
+ *
  * @author Team 2813
  * @since 1.0
  */
 public enum ControlMode {
-    
-    /**
-     * Open-loop duty cycle control mode.
-     * 
-     * 
    Controls the motor by setting the duty cycle (percentage of full power) directly.
-     * The motor output is proportional to the demand value, typically ranging from
-     * -1.0 (full reverse) to +1.0 (full forward). This is the most basic control mode
-     * and does not use feedback control.
-     * 
-     * 
    Maps to {@link ControlType#kDutyCycle} for SPARK controllers.
-     */
-    DUTY_CYCLE(ControlType.kDutyCycle),
-    
-    /**
-     * Closed-loop velocity control mode.
-     * 
-     * 
    Controls the motor to maintain a specific velocity using PID feedback control.
-     * The controller continuously adjusts the motor output to minimize the error between
-     * the demanded velocity and the actual measured velocity from the encoder. This mode
-     * is ideal for applications requiring consistent speed regardless of load variations.
-     * 
-     * 
    Maps to {@link ControlType#kVelocity} for SPARK controllers.
-     */
-    VELOCITY(ControlType.kVelocity),
-    
-    /**
-     * Closed-loop position control mode with motion profiling.
-     * 
-     * 
    Controls the motor to reach a specific position using advanced motion profiling
-     * algorithms. The controller generates smooth velocity and acceleration profiles to
-     * move the mechanism to the target position while respecting configured motion
-     * constraints (max velocity, max acceleration). This mode provides the smoothest
-     * and most controlled movement for precise positioning applications.
-     * 
-     * 
    Maps to {@link ControlType#kPosition} for SPARK controllers.
-     * 
-     * 
    Note: Despite the name "MOTION_MAGIC," this maps to position control
-     * type for SPARK controllers, as the motion profiling is handled internally.
-     */
-    MOTION_MAGIC(ControlType.kPosition),
-    
-    /**
-     * Open-loop voltage control mode.
-     * 
-     * 
    Controls the motor by applying a specific voltage directly to the motor terminals.
-     * Unlike duty cycle control, voltage control compensates for battery voltage variations
-     * to provide more consistent motor behavior. The demand value represents the desired
-     * voltage (in Volts) to apply to the motor.
-     * 
-     * 
    Maps to {@link ControlType#kVoltage} for SPARK controllers.
-     */
-    VOLTAGE(ControlType.kVoltage);
 
-    /** The corresponding SPARK controller control type for this mode */
-    private final ControlType sparkMode;
+  /**
+   * Open-loop duty cycle control mode.
+   *
+   * 
    Controls the motor by setting the duty cycle (percentage of time the motor is receiving a
+   * signal/the signal is on) directly. The motor output is proportional to the pulse width,
+   * typically ranging from -1.0 (full reverse) to +1.0 (full forward). This is the most basic
+   * control mode and does not use feedback control.
+   *
+   * 
    Maps to {@link ControlType#kDutyCycle} for SPARK controllers.
+   *
+   * 
    Here is a visualization of the duty cycle: 
     
    
+   * 
    
+   * 
+   */
+  DUTY_CYCLE(ControlType.kDutyCycle),
 
-    /**
-     * Creates a ControlMode with the specified SPARK controller mapping.
-     * 
-     * @param sparkMode the corresponding {@link ControlType} for SPARK controllers
-     */
-    ControlMode(ControlType sparkMode) {
-        this.sparkMode = sparkMode;
-    }
+  /**
+   * Closed-loop velocity control mode.
+   *
+   * 
    Controls the motor to maintain a specific velocity using PID feedback control. The
+   * controller continuously adjusts the motor output to minimize the error between the demanded
+   * velocity and the actual measured velocity from the encoder. This mode is ideal for applications
+   * requiring consistent speed regardless of load variations.
+   *
+   * 
    Maps to {@link ControlType#kVelocity} for SPARK controllers.
+   */
+  VELOCITY(ControlType.kVelocity),
 
-    /**
-     * Gets the corresponding SPARK controller control type for this mode.
-     * 
-     * 
    This method provides the mapping from the vendor-neutral ControlMode
-     * to the specific {@link ControlType} required by REV Robotics SPARK
-     * controllers. This abstraction allows the same control mode enum to be
-     * used across different motor controller implementations.
-     * 
-     * @return the corresponding {@link ControlType} for SPARK controllers
-     */
-    public ControlType getSparkMode() {
-        return sparkMode;
-    }
-}
\ No newline at end of file
+  /**
+   * Closed-loop position control mode with motion profiling.
+   *
+   * 
    Controls the motor to reach a specific position using advanced motion profiling algorithms.
+   * The controller generates smooth velocity and acceleration profiles to move the mechanism to the
+   * target position while respecting configured motion constraints (max velocity, max
+   * acceleration). This mode provides the smoothest and most controlled movement for precise
+   * positioning applications.
+   *
+   * 
    Maps to {@link ControlType#kPosition} for SPARK controllers.
+   *
+   * 
    Note: Despite the name "MOTION_MAGIC," this maps to position control type for SPARK
+   * controllers, as the motion profiling is handled internally.
+   */
+  MOTION_MAGIC(ControlType.kPosition),
+
+  /**
+   * Open-loop voltage control mode.
+   *
+   * 
    Controls the motor by applying a specific voltage directly to the motor terminals. Unlike
+   * duty cycle control, voltage control compensates for battery voltage variations to provide more
+   * consistent motor behavior. The demand value represents the desired voltage (in Volts) to apply
+   * to the motor.
+   *
+   * 
    Maps to {@link ControlType#kVoltage} for SPARK controllers.
+   */
+  VOLTAGE(ControlType.kVoltage);
+
+  /** The corresponding SPARK controller control type for this mode */
+  private final ControlType sparkMode;
+
+  /**
+   * Creates a ControlMode with the specified SPARK controller mapping.
+   *
+   * @param sparkMode the corresponding {@link ControlType} for SPARK controllers
+   */
+  ControlMode(ControlType sparkMode) {
+    this.sparkMode = sparkMode;
+  }
+
+  /**
+   * Gets the corresponding SPARK controller control type for this mode.
+   *
+   * 
    This method provides the mapping from the vendor-neutral ControlMode to the specific {@link
+   * ControlType} required by REV Robotics SPARK controllers. This abstraction allows the same
+   * control mode enum to be used across different motor controller implementations.
+   *
+   * 
    The last references to this method were removed a while ago.
+   *
+   * @return the corresponding {@link ControlType} for SPARK controllers
+   */
+  @Deprecated(forRemoval = true)
+  public ControlType getSparkMode() {
+    return sparkMode;
+  }
+}
diff --git a/lib/src/main/java/com/team2813/lib2813/control/DeviceInformation.java b/lib/src/main/java/com/team2813/lib2813/control/DeviceInformation.java
index 71439b56..809e398f 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/DeviceInformation.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/DeviceInformation.java
@@ -5,140 +5,119 @@
 
 /**
  * Immutable value class representing the identifying information for a CAN device.
- * 
- * 
    This class encapsulates the essential information needed to uniquely identify
- * a device on the CAN bus network: the CAN ID and the specific CAN bus the device
- * is connected to. It provides a standardized way to represent device identity
- * across the team's control system architecture.
- * 
- * 
    The class distinguishes between devices on the RoboRIO's built-in CAN bus
- * (represented by an empty Optional for the canbus) and devices on named CAN buses
- * such as CANivore or other CAN bus interfaces.
- * 
+ *
+ * 
    This class encapsulates the essential information needed to uniquely identify a device on the
+ * CAN bus network: the CAN ID and the specific CAN bus the device is connected to. It provides a
+ * standardized way to represent device identity across the team's control system architecture.
+ *
+ * 
    The class distinguishes between devices on the RoboRIO's built-in CAN bus (represented by an
+ * empty Optional for the canbus) and devices on named CAN buses such as CANivore or other CAN bus
+ * interfaces.
+ *
  * 
    Key features:
+ *
  * 
      
- * 
    • Immutable design for thread safety and reliable identity
      • 
- * 
    • Input validation for CAN ID range [0, 62]
      • 
- * 
    • Proper equals() and hashCode() implementation for use in collections
      • 
- * 
    • Optional-based canbus representation for null safety
      • 
+ *   
    • Immutable design for thread safety and reliable identity
+ *   
    • Input validation for CAN ID range [0, 62]
  * 
    
- * 
- * 
    This class is commonly used as a key in device registries and for comparing
- * device instances to determine if they represent the same physical hardware.
- * 
+ *
+ * 
    This class is commonly used as a key in device registries and for comparing device instances
+ * to determine if they represent the same physical hardware.
+ *
  * @author Team 2813
  * @since 1.0
  */
 public final class DeviceInformation {
-    
-    /** The CAN ID of the device, validated to be in range [0, 62] */
-    private int id;
-    
-    /** The CAN bus name, empty if on the RoboRIO's default CAN bus */
-    private Optional canbus;
 
-    /**
-     * Creates DeviceInformation for a device on the RoboRIO's default CAN bus.
-     * 
-     * 
    This constructor is used for devices connected directly to the RoboRIO's
-     * built-in CAN bus interface. The canbus will be represented as an empty
-     * Optional to indicate the default bus.
-     * 
-     * @param id the CAN ID of the device, must be in range [0, 62]
-     * @throws com.team2813.lib2813.util.InvalidCanIdException if the CAN ID is outside the valid range
-     */
-    public DeviceInformation(int id) {
-        this(id, null);
-    }
+  /** The CAN ID of the device, validated to be in range [0, 62] */
+  private int id;
 
-    /**
-     * Creates DeviceInformation with a specific CAN bus name.
-     * 
-     * 
    This constructor supports devices on named CAN buses such as CANivore
-     * devices or other CAN bus interfaces. If {@code canbus} is {@code null},
-     * this method behaves identically to {@link #DeviceInformation(int)}.
-     * 
-     * 
    Examples of valid canbus names:
-     * 
      
-     * 
    • {@code "canivore"} - For CANivore devices
      • 
-     * 
    • {@code "CANivore_12345"} - For CANivore with specific serial number
      • 
-     * 
    • {@code null} - For RoboRIO default CAN bus
      • 
-     * 
    
-     * 
-     * @param id the CAN ID of the device, must be in range [0, 62]
-     * @param canbus the CAN bus name, or {@code null} for the RoboRIO default bus
-     * @throws com.team2813.lib2813.util.InvalidCanIdException if the CAN ID is outside the valid range
-     */
-    public DeviceInformation(int id, String canbus) {
-        this.id = InputValidation.checkCanId(id);
-        this.canbus = Optional.ofNullable(canbus);
-    }
+  /** The CAN bus name, empty if on the RoboRIO's default CAN bus */
+  private Optional canbus;
 
-    /**
-     * Gets the CAN ID of this device.
-     * 
-     * 
    The CAN ID is guaranteed to be in the valid range [0, 62] due to
-     * validation performed during construction.
-     * 
-     * @return the CAN ID of the device
-     */
-    public int id() {
-        return id;
-    }
+  /**
+   * Creates DeviceInformation for a device on the RoboRIO's default CAN bus.
+   *
+   * 
    This constructor is used for devices connected directly to the RoboRIO's built-in CAN bus
+   * interface. The canbus will be represented as an empty Optional to indicate the default bus.
+   *
+   * @param id the CAN ID of the device, must be in range [0, 62]
+   * @throws com.team2813.lib2813.util.InvalidCanIdException if the CAN ID is outside the valid
+   *     range
+   */
+  public DeviceInformation(int id) {
+    this(id, null);
+  }
 
-    /**
-     * Returns the CAN bus that this device is connected to.
-     * 
-     * 
    The return value interpretation:
-     * 
      
-     * 
    • {@link Optional#empty()} - Device is on the RoboRIO's default CAN bus
      • 
-     * 
    • {@code Optional.of("busname")} - Device is on the named CAN bus
      • 
-     * 
    
-     * 
-     * 
    This Optional-based approach provides null safety and makes the
-     * distinction between default and named buses explicit in the API.
-     * 
-     * @return an Optional containing the CAN bus name, or empty if on the RoboRIO CAN bus
-     */
-    public Optional canbus() {
-        return canbus;
-    }
+  /**
+   * Creates DeviceInformation with a specific CAN bus name.
+   *
+   * 
    This constructor supports devices on named CAN buses such as CANivore devices or other CAN
+   * bus interfaces. If {@code canbus} is {@code null}, this method behaves identically to {@link
+   * #DeviceInformation(int)}.
+   *
+   * @param id the CAN ID of the device, must be in range [0, 62]
+   * @param canbus the CAN bus name, or {@code null} for the RoboRIO default bus
+   * @throws com.team2813.lib2813.util.InvalidCanIdException if the CAN ID is outside the valid
+   *     range
+   */
+  public DeviceInformation(int id, String canbus) {
+    this.id = InputValidation.checkCanId(id);
+    this.canbus = Optional.ofNullable(canbus);
+  }
 
-    /**
-     * Determines whether this DeviceInformation is equal to another object.
-     * 
-     * 
    Two DeviceInformation instances are considered equal if and only if
-     * they have the same CAN ID and are on the same CAN bus. This allows
-     * DeviceInformation objects to be used reliably as keys in hash-based
-     * collections and for device identity comparisons.
-     * 
-     * @param o the object to compare against
-     * @return {@code true} if the objects represent the same device,
-     *         {@code false} otherwise
-     */
-    @Override
-    public boolean equals(Object o) {
-        if (!(o instanceof DeviceInformation)) return false;
-        DeviceInformation other = (DeviceInformation) o;
-        return other.id == id && other.canbus.equals(canbus);
-    }
+  /**
+   * Gets the CAN ID of this device.
+   *
+   * 
    The CAN ID is guaranteed to be in the valid range [0, 62] due to validation performed during
+   * construction.
+   *
+   * @return the CAN ID of the device
+   */
+  public int id() {
+    return id;
+  }
 
-    /**
-     * Returns a hash code value for this DeviceInformation.
-     * 
-     * 
    The hash code is computed from both the CAN ID and the CAN bus
-     * information, ensuring that equal DeviceInformation objects have
-     * equal hash codes. This implementation supports the use of
-     * DeviceInformation objects in hash-based collections such as
-     * HashMap and HashSet.
-     * 
-     * 
    The hash code formula combines the ID and canbus hash using
-     * a prime multiplier for good distribution properties.
-     * 
-     * @return a hash code value for this object
-     */
-    @Override
-    public int hashCode() {
-        return id * 31 + canbus.hashCode();
-    }
-}
\ No newline at end of file
+  /**
+   * Returns the CAN bus that this device is connected to.
+   *
+   * 
    The return value interpretation:
+   *
+   * 
      
+   *   
    • {@link Optional#empty()} - Device is on the RoboRIO's default CAN bus
+   *   
    • {@code Optional.of("busname")} - Device is on the named CAN bus
+   * 
    
+   *
+   * @return an Optional containing the CAN bus name, or empty if on the RoboRIO CAN bus
+   */
+  public Optional canbus() {
+    return canbus;
+  }
+
+  /**
+   * Determines whether this DeviceInformation is equal to another object.
+   *
+   * 
    Two DeviceInformation instances are considered equal if and only if they have the same CAN
+   * ID and are on the same CAN bus. This allows DeviceInformation objects to be used reliably as
+   * keys in hash-based collections and for device identity comparisons.
+   *
+   * @param o the object to compare against
+   * @return {@code true} if the objects represent the same device, {@code false} otherwise
+   */
+  @Override
+  public boolean equals(Object o) {
+    if (!(o instanceof DeviceInformation)) return false;
+    DeviceInformation other = (DeviceInformation) o;
+    return other.id == id && other.canbus.equals(canbus);
+  }
+
+  /**
+   * Returns a hash code value for this DeviceInformation.
+   *
+   * @return a hash code value for this object
+   */
+  @Override
+  public int hashCode() {
+    return id * 31 + canbus.hashCode();
+  }
+}
diff --git a/lib/src/main/java/com/team2813/lib2813/control/Encoder.java b/lib/src/main/java/com/team2813/lib2813/control/Encoder.java
index f3c90897..c3eed3e1 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/Encoder.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/Encoder.java
@@ -6,140 +6,123 @@
 
 /**
  * Interface specifying a device that can perceive rotational positions and velocities.
- * 
- * 
    This interface defines the contract for encoder devices that provide angular
- * position and velocity feedback. It supports both legacy double-based methods
- * (deprecated) and modern type-safe unit-based methods using WPILib's units system.
- * 
- * 
    The interface provides a migration path from unsafe raw double values to
- * type-safe {@link Angle} and {@link AngularVelocity} measurements. New implementations
- * should focus on the unit-safe methods, while legacy methods are maintained for
- * backward compatibility but marked for removal.
- * 
- * 
    Key features:
- * 
      
- * 
    • Type-safe position and velocity measurements using WPILib units
      • 
- * 
    • Default implementations to ease migration from legacy methods
      • 
- * 
    • Backward compatibility with existing code through deprecated methods
      • 
- * 
    • Flexible unit conversion through the units system
      • 
- * 
    
- * 
- * 
    Common implementations include absolute encoders (CANcoder), relative encoders
- * (integrated motor encoders), and other rotational position sensing devices.
- * 
+ *
+ * 
    This interface defines the contract for encoder devices that provide angular position and
+ * velocity feedback. It supports both legacy double-based methods (deprecated) and modern type-safe
+ * unit-based methods using WPILib's units system.
+ *
+ * 
    The interface provides a migration path from unsafe raw double values to type-safe {@link
+ * Angle} and {@link AngularVelocity} measurements. New implementations should focus on the
+ * unit-safe methods, while legacy methods are maintained for backward compatibility but marked for
+ * removal.
+ *
+ * 
    Common implementations include absolute encoders (CANcoder), relative encoders (integrated
+ * motor encoders), and other rotational position sensing devices.
+ *
  * @author Team 2813
  * @since 1.0
  */
 public interface Encoder {
 
-    /**
-     * Gets the current position of the encoder as a raw double value.
-     * 
-     * 
    Warning: This method returns position without specifying units,
-     * making it unsafe for reliable position calculations. The actual units
-     * depend on the specific encoder implementation and configuration.
-     * 
-     * @return the position of the encoder as an unspecified double value
-     * @deprecated This method does not specify position in a specific measurement, so it is not safe
-     *             to use. Use {@link #getPositionMeasure()} instead for type safety
-     */
-    @Deprecated(forRemoval = true)
-    double position();
+  /**
+   * Gets the current position of the encoder as a raw double value.
+   *
+   * @return the position of the encoder as an unspecified double value
+   * @deprecated This method does not specify position in a specific measurement, so it is not safe
+   *     to use. Use {@link #getPositionMeasure()} instead for type safety
+   */
+  @Deprecated(forRemoval = true)
+  double position();
 
-    /**
-     * Gets the current position of the encoder using type-safe units.
-     * 
-     * 
    This method returns the encoder position as an {@link Angle} measurement,
-     * providing type safety and explicit unit handling. The returned angle can be
-     * easily converted to any angular unit (degrees, radians, rotations) using
-     * the WPILib units system.
-     * 
-     * 
    Example usage:
-     * 
    {@code
-     * Angle position = encoder.getPositionMeasure();
-     * double degrees = position.in(Units.Degrees);
-     * double rotations = position.in(Units.Rotations);
-     * }
    
-     * 
-     * @return the current position of the encoder as an {@link Angle} measurement
-     */
-    Angle getPositionMeasure();
+  /**
+   * Gets the current position of the encoder using type-safe units.
+   *
+   * 
    This method returns the encoder position as an {@link Angle} measurement, providing type
+   * safety and explicit unit handling. The returned angle can be easily converted to any angular
+   * unit (degrees, radians, rotations) using the WPILib units system.
+   *
+   * 
    Example usage:
+   *
+   * 
    {@code
+   * Angle position = encoder.getPositionMeasure();
+   * double degrees = position.in(Units.Degrees);
+   * double rotations = position.in(Units.Rotations);
+   * }
    
+   *
+   * @return the current position of the encoder as an {@link Angle} measurement
+   */
+  Angle getPositionMeasure();
 
-    /**
-     * Sets the encoder position to the specified raw double value.
-     * 
-     * 
    Warning: This method accepts position without specifying units,
-     * making it unsafe and ambiguous. The interpretation of the position value
-     * depends on the specific encoder implementation and configuration.
-     * 
-     * @param position the new position value as an unspecified double
-     * @deprecated This method does not specify a unit, so it is not safe to use.
-     *             Use {@link #setPosition(Angle)} instead for type safety
-     */
-    @Deprecated(forRemoval = true)
-    void setPosition(double position);
+  /**
+   * Sets the encoder position to the specified raw double value.
+   *
+   * 
    Warning: This method accepts position without specifying units, making it unsafe and
+   * ambiguous. The interpretation of the position value depends on the specific encoder
+   * implementation and configuration.
+   *
+   * @param position the new position value as an unspecified double
+   * @deprecated This method does not specify a unit, so it is not safe to use. Use {@link
+   *     #setPosition(Angle)} instead for type safety
+   */
+  @Deprecated(forRemoval = true)
+  void setPosition(double position);
 
-    /**
-     * Sets the encoder position using type-safe units.
-     * 
-     * 
    This method accepts any {@link Angle} measurement and converts it to
-     * the encoder's native units for setting the position. The type-safe approach
-     * eliminates unit confusion and provides clear, readable code.
-     * 
-     * 
    The default implementation converts the angle to radians and calls the
-     * legacy {@link #setPosition(double)} method. Implementations should override
-     * this method to provide direct unit-safe position setting when possible.
-     * 
-     * 
    Example usage:
-     * 
    {@code
-     * encoder.setPosition(Units.Degrees.of(90));
-     * encoder.setPosition(Units.Rotations.of(0.25));
-     * }
    
-     * 
-     * @param position the new position as an {@link Angle} measurement
-     */
-    default void setPosition(Angle position) {
-        setPosition(position.in(Units.Radians));
-    }
+  /**
+   * Sets the encoder position using type-safe units.
+   *
+   * 
    This method accepts any {@link Angle} measurement and converts it to the encoder's native
+   * units for setting the position. The type-safe approach eliminates unit confusion and provides
+   * clear, readable code.
+   *
+   * 
    The default implementation converts the angle to radians and calls the legacy {@link
+   * #setPosition(double)} method. Implementations should override this method to provide direct
+   * unit-safe position setting when possible.
+   *
+   * 
    Example usage:
+   *
+   * 
    {@code
+   * encoder.setPosition(Units.Degrees.of(90));
+   * encoder.setPosition(Units.Rotations.of(0.25));
+   * }
    
+   *
+   * @param position the new position as an {@link Angle} measurement
+   */
+  default void setPosition(Angle position) {
+    setPosition(position.in(Units.Radians));
+  }
 
-    /**
-     * Gets the current velocity of the encoder as a raw double value.
-     * 
-     * 
    Warning: This method returns velocity without specifying units,
-     * making it unsafe for reliable velocity calculations. The actual units
-     * depend on the specific encoder implementation and configuration (could be
-     * RPM, radians/sec, rotations/sec, etc.).
-     * 
-     * @return the velocity that the encoder perceives as an unspecified double value
-     * @deprecated This method does not specify velocity in a specific measurement, so it is not safe
-     *             to use. Use {@link #getVelocityMeasure()} instead for type safety
-     */
-    @Deprecated(forRemoval = true)
-    double getVelocity();
+  /**
+   * Gets the current velocity of the encoder as a raw double value.
+   *
+   * @return the velocity that the encoder perceives as an unspecified double value
+   * @deprecated This method does not specify velocity in a specific measurement, so it is not safe
+   *     to use. Use {@link #getVelocityMeasure()} instead for type safety
+   */
+  @Deprecated(forRemoval = true)
+  double getVelocity();
 
-    /**
-     * Gets the current velocity of the encoder using type-safe units.
-     * 
-     * 
    This method returns the encoder velocity as an {@link AngularVelocity}
-     * measurement, providing type safety and explicit unit handling. The returned
-     * velocity can be easily converted to any angular velocity unit using the
-     * WPILib units system.
-     * 
-     * 
    The default implementation assumes the legacy {@link #getVelocity()} method
-     * returns radians per second and wraps it in a type-safe measurement.
-     * Implementations should override this method to provide the correct units
-     * for their specific hardware.
-     * 
-     * 
    Example usage:
-     * 
    {@code
-     * AngularVelocity velocity = encoder.getVelocityMeasure();
-     * double rpm = velocity.in(Units.RPM);
-     * double radPerSec = velocity.in(Units.RadiansPerSecond);
-     * }
    
-     * 
-     * @return the current velocity as an {@link AngularVelocity} measurement
-     */
-    default AngularVelocity getVelocityMeasure() {
-        return Units.RadiansPerSecond.of(getVelocity());
-    }
-}
\ No newline at end of file
+  /**
+   * Gets the current velocity of the encoder using type-safe units.
+   *
+   * 
    This method returns the encoder velocity as an {@link AngularVelocity} measurement,
+   * providing type safety and explicit unit handling. The returned velocity can be easily converted
+   * to any angular velocity unit using the WPILib units system.
+   *
+   * 
    The default implementation assumes the legacy {@link #getVelocity()} method returns radians
+   * per second and wraps it in a type-safe measurement. Implementations should override this method
+   * to provide the correct units for their specific hardware.
+   *
+   * 
    Example usage:
+   *
+   * 
    {@code
+   * AngularVelocity velocity = encoder.getVelocityMeasure();
+   * double rpm = velocity.in(Units.RPM);
+   * double radPerSec = velocity.in(Units.RadiansPerSecond);
+   * }
    
+   *
+   * @return the current velocity as an {@link AngularVelocity} measurement
+   */
+  default AngularVelocity getVelocityMeasure() {
+    return Units.RadiansPerSecond.of(getVelocity());
+  }
+}
diff --git a/lib/src/main/java/com/team2813/lib2813/control/InvertType.java b/lib/src/main/java/com/team2813/lib2813/control/InvertType.java
index f4c3b6e8..d80c5771 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/InvertType.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/InvertType.java
@@ -7,236 +7,237 @@
 import java.util.stream.Stream;
 
 /**
- * Enumeration defining motor inversion types for standardized motor control across different vendors.
- * 
+ * Enumeration defining motor inversion types for standardized motor control across different
+ * vendors.
+ *
  * 
    This enum provides a vendor-neutral abstraction for motor inversion settings while maintaining
- * compatibility with specific hardware implementations from CTRE Phoenix and REV Robotics SPARK controllers.
- * It handles both absolute rotation directions and relative following behaviors for multi-motor systems.
- * 
+ * compatibility with specific hardware implementations from CTRE Phoenix and REV Robotics SPARK
+ * controllers. It handles both absolute rotation directions and relative following behaviors for
+ * multi-motor systems.
+ *
  * 
    The enum supports two categories of inversion:
+ *
  * 
      
- * 
    • Rotation Values: Absolute directional settings (CLOCKWISE, COUNTER_CLOCKWISE) that
- *     define the positive direction of motor rotation
      • 
- * 
    • Following Values: Relative settings (FOLLOW_MASTER, OPPOSE_MASTER) used for
- *     follower motors that reference a primary motor's direction
      • 
+ *   
    • Rotation Values: Absolute directional settings (CLOCKWISE, COUNTER_CLOCKWISE) that
+ *       define the positive direction of motor rotation
+ *   
    • Following Values: Relative settings (FOLLOW_MASTER, OPPOSE_MASTER) used for follower
+ *       motors that reference a primary motor's direction
  * 
    
- * 
- * 
    Each rotation value includes mappings to vendor-specific inversion settings, enabling
- * seamless integration with different motor controller families without code changes.
- * 
+ *
+ * 
    Each rotation value includes mappings to vendor-specific inversion settings, enabling seamless
+ * integration with different motor controller families without code changes.
+ *
  * @author Team 2813
  * @since 1.0
  */
 public enum InvertType {
-    
-    /**
-     * Clockwise rotation is considered positive.
-     * 
-     * 
    When this inversion type is applied, positive motor output values will cause
-     * the motor to rotate in the clockwise direction when viewed from the output shaft end.
-     * This is the "normal" or "non-inverted" direction for most motor controllers.
-     * 
-     * 
    Vendor mappings:
-     * 
      
-     * 
    • Phoenix 6: {@code InvertedValue.Clockwise_Positive}
      • 
-     * 
    • SPARK MAX: {@code true} (inverted = false)
      • 
-     * 
    
-     */
-    CLOCKWISE(InvertedValue.Clockwise_Positive, true),
-    
-    /**
-     * Counter-clockwise rotation is considered positive.
-     * 
-     * 
    When this inversion type is applied, positive motor output values will cause
-     * the motor to rotate in the counter-clockwise direction when viewed from the output
-     * shaft end. This effectively inverts the motor's output relative to the default.
-     * 
-     * 
    Vendor mappings:
-     * 
      
-     * 
    • Phoenix 6: {@code InvertedValue.CounterClockwise_Positive}
      • 
-     * 
    • SPARK MAX: {@code false} (inverted = true)
      • 
-     * 
    
-     */
-    COUNTER_CLOCKWISE(InvertedValue.CounterClockwise_Positive, false),
-    
-    /**
-     * Follower motor matches the master motor's direction.
-     * 
-     * 
    This inversion type is used for follower motors that should rotate in the
-     * same direction as their master motor. The follower will mirror the master's
-     * output without any inversion applied.
-     * 
-     * 
    Note: This value has no direct vendor mapping as it represents a
-     * relationship rather than an absolute direction. It is resolved at the follower
-     * configuration level.
-     */
-    FOLLOW_MASTER,
-    
-    /**
-     * Follower motor opposes the master motor's direction.
-     * 
-     * 
    This inversion type is used for follower motors that should rotate opposite
-     * to their master motor. The follower will mirror the master's output but with
-     * inversion applied, creating opposing rotation.
-     * 
-     * 
    This is commonly used in differential drive systems where left and right
-     * motors need to rotate in opposite directions for forward motion.
-     * 
-     * 
    Note: This value has no direct vendor mapping as it represents a
-     * relationship rather than an absolute direction. It is resolved at the follower
-     * configuration level.
-     */
-    OPPOSE_MASTER;
-
-    /**
-     * A set of all {@link InvertType}s that have phoenix and spark max invert mappings.
-     * 
-     * 
    This set contains only the rotation values (CLOCKWISE, COUNTER_CLOCKWISE) that
-     * can be directly mapped to vendor-specific inversion settings. Any InvertType not
-     * in this set is intended for motor following relationships and does not have direct
-     * vendor mappings.
-     * 
-     * 
    This set is used for validation to ensure only appropriate inversion types
-     * are used in contexts that require absolute rotation directions.
-     */
-    public static final Set rotationValues =
-        Collections.unmodifiableSet(EnumSet.of(CLOCKWISE, COUNTER_CLOCKWISE));
-
-    /** The corresponding Phoenix 6 InvertedValue, if applicable */
-    private final Optional phoenixInvert;
-    
-    /** The corresponding SPARK MAX inversion boolean, if applicable */
-    private final Optional sparkMaxInvert;
-
-    /**
-     * Creates an InvertType for following relationships (no vendor mappings).
-     * 
-     * 
    This constructor is used for FOLLOW_MASTER and OPPOSE_MASTER values
-     * that represent relationships rather than absolute directions.
-     */
-    InvertType() {
-        phoenixInvert = Optional.empty();
-        sparkMaxInvert = Optional.empty();
-    }
-
-    /**
-     * Creates an InvertType with vendor-specific mappings.
-     * 
-     * 
    This constructor is used for rotation values (CLOCKWISE, COUNTER_CLOCKWISE)
-     * that have direct mappings to vendor-specific inversion settings.
-     * 
-     * @param phoenixInvert the corresponding Phoenix 6 InvertedValue
-     * @param sparkMaxInvert the corresponding SPARK MAX inversion setting
-     */
-    InvertType(InvertedValue phoenixInvert, boolean sparkMaxInvert) {
-        this.phoenixInvert = Optional.of(phoenixInvert);
-        this.sparkMaxInvert = Optional.of(sparkMaxInvert);
-    }
-
-    /**
-     * Gets an {@link InvertType} from a Phoenix 6 {@link InvertedValue}.
-     * 
-     * 
    This method provides reverse lookup from vendor-specific Phoenix settings
-     * to the vendor-neutral InvertType representation. It enables conversion from
-     * Phoenix-specific code to the standardized enum.
-     * 
-     * @param v the {@link InvertedValue} to search for
-     * @return an Optional containing the corresponding {@link InvertType}, or
-     *         {@link Optional#empty()} if no mapping exists
-     */
-    public static Optional fromPhoenixInvert(InvertedValue v) {
-        return Optional.ofNullable(Maps.phoenixMap.get(v));
-    }
-
-    /**
-     * Gets an {@link InvertType} from a SPARK MAX inversion boolean.
-     * 
-     * 
    This method provides reverse lookup from vendor-specific SPARK MAX settings
-     * to the vendor-neutral InvertType representation. It enables conversion from
-     * SPARK MAX-specific code to the standardized enum.
-     * 
-     * @param v the SPARK MAX inversion boolean to search for
-     * @return an Optional containing the corresponding {@link InvertType}, or
-     *         {@link Optional#empty()} if no mapping exists
-     */
-    public static Optional fromSparkMaxInvert(boolean v) {
-        return Optional.ofNullable(Maps.sparkMaxMap.get(v));
-    }
-
-    /**
-     * Gets the Phoenix 6 InvertedValue for this InvertType.
-     * 
-     * 
    This method returns the vendor-specific Phoenix mapping for rotation values.
-     * Following values (FOLLOW_MASTER, OPPOSE_MASTER) will return an empty Optional
-     * as they don't have direct Phoenix mappings.
-     * 
-     * @return an Optional containing the Phoenix InvertedValue, or empty if not applicable
-     */
-    public Optional phoenixInvert() {
-        return phoenixInvert;
-    }
-
-    /**
-     * Forces retrieval of the Phoenix 6 InvertedValue.
-     * 
-     * 
    This internal method assumes the Phoenix mapping exists and throws if it doesn't.
-     * It's used internally for map construction and should not be called on following values.
-     * 
-     * @return the Phoenix InvertedValue
-     * @throws NoSuchElementException if no Phoenix mapping exists
-     */
-    private InvertedValue forcePhoenixInvert() {
-        return phoenixInvert.orElseThrow();
-    }
-
-    /**
-     * Gets the SPARK MAX inversion boolean for this InvertType.
-     * 
-     * 
    This method returns the vendor-specific SPARK MAX mapping for rotation values.
-     * Following values (FOLLOW_MASTER, OPPOSE_MASTER) will return an empty Optional
-     * as they don't have direct SPARK MAX mappings.
-     * 
-     * @return an Optional containing the SPARK MAX inversion boolean, or empty if not applicable
-     */
-    public Optional sparkMaxInvert() {
-        return sparkMaxInvert;
-    }
-
-    /**
-     * Forces retrieval of the SPARK MAX inversion boolean.
-     * 
-     * 
    This internal method assumes the SPARK MAX mapping exists and throws if it doesn't.
-     * It's used internally for map construction and should not be called on following values.
-     * 
-     * @return the SPARK MAX inversion boolean
-     * @throws NoSuchElementException if no SPARK MAX mapping exists
-     */
-    private boolean forceSparkMaxInvert() {
-        return sparkMaxInvert.orElseThrow();
-    }
-
-    /**
-     * Contains the reverse lookup maps for vendor-specific inversion values.
-     * 
-     * 
    This static nested class holds the maps used by {@link #fromPhoenixInvert(InvertedValue)}
-     * and {@link #fromSparkMaxInvert(boolean)}. The maps are constructed lazily in a static
-     * context to avoid initialization overhead when reverse lookup is not needed.
-     * 
-     * 
    The maps are built by filtering enum values to include only those with vendor
-     * mappings and creating reverse mappings from vendor values to InvertType.
-     */
-    private static final class Maps {
-        
-        /** Reverse lookup map from Phoenix InvertedValue to InvertType */
-        private static final Map phoenixMap =
-            Stream.of(InvertType.values())
-                .filter((j) -> j.phoenixInvert.isPresent())
-                .collect(toUnmodifiableMap(InvertType::forcePhoenixInvert, (j) -> j, (a, b) -> null));
-        
-        /** Reverse lookup map from SPARK MAX boolean to InvertType */
-        private static final Map sparkMaxMap =
-            Stream.of(InvertType.values())
-                .filter((j) -> j.sparkMaxInvert.isPresent())
-                .collect(toUnmodifiableMap(InvertType::forceSparkMaxInvert, (j) -> j, (a, b) -> null));
-    }
-}
\ No newline at end of file
+
+  /**
+   * Clockwise rotation is considered positive.
+   *
+   * 
    When this inversion type is applied, positive motor output values will cause the motor to
+   * rotate in the clockwise direction when viewed from the output shaft end. This is the "normal"
+   * or "non-inverted" direction for most motor controllers.
+   *
+   * 
    Vendor mappings:
+   *
+   * 
      
+   *   
    • Phoenix 6: {@code InvertedValue.Clockwise_Positive}
+   *   
    • SPARK MAX: {@code true} (inverted = false)
+   * 
    
+   */
+  CLOCKWISE(InvertedValue.Clockwise_Positive, true),
+
+  /**
+   * Counter-clockwise rotation is considered positive.
+   *
+   * 
    When this inversion type is applied, positive motor output values will cause the motor to
+   * rotate in the counter-clockwise direction when viewed from the output shaft end. This
+   * effectively inverts the motor's output relative to the default.
+   *
+   * 
    Vendor mappings:
+   *
+   * 
      
+   *   
    • Phoenix 6: {@code InvertedValue.CounterClockwise_Positive}
+   *   
    • SPARK MAX: {@code false} (inverted = true)
+   * 
    
+   */
+  COUNTER_CLOCKWISE(InvertedValue.CounterClockwise_Positive, false),
+
+  /**
+   * Follower motor matches the master motor's direction.
+   *
+   * 
    This inversion type is used for follower motors that should rotate in the same direction as
+   * their master motor. The follower will mirror the master's output without any inversion applied.
+   *
+   * 
    Note: This value has no direct vendor mapping as it represents a relationship rather
+   * than an absolute direction. It is resolved at the follower configuration level.
+   */
+  FOLLOW_MASTER,
+
+  /**
+   * Follower motor opposes the master motor's direction.
+   *
+   * 
    This inversion type is used for follower motors that should rotate opposite to their master
+   * motor. The follower will mirror the master's output but with inversion applied, creating
+   * opposing rotation.
+   *
+   * 
    This is commonly used in differential drive systems where left and right motors need to
+   * rotate in opposite directions for forward motion.
+   *
+   * 
    Note: This value has no direct vendor mapping as it represents a relationship rather
+   * than an absolute direction. It is resolved at the follower configuration level.
+   */
+  OPPOSE_MASTER;
+
+  /**
+   * A set of all {@link InvertType}s that have phoenix and spark max invert mappings.
+   *
+   * 
    This set contains only the rotation values (CLOCKWISE, COUNTER_CLOCKWISE) that can be
+   * directly mapped to vendor-specific inversion settings. Any InvertType not in this set is
+   * intended for motor following relationships and does not have direct vendor mappings.
+   *
+   * 
    This set is used for validation to ensure only appropriate inversion types are used in
+   * contexts that require absolute rotation directions.
+   */
+  public static final Set rotationValues =
+      Collections.unmodifiableSet(EnumSet.of(CLOCKWISE, COUNTER_CLOCKWISE));
+
+  /** The corresponding Phoenix 6 InvertedValue, if applicable */
+  private final Optional phoenixInvert;
+
+  /** The corresponding SPARK MAX inversion boolean, if applicable */
+  private final Optional sparkMaxInvert;
+
+  /**
+   * Creates an InvertType for following relationships (no vendor mappings).
+   *
+   * 
    This constructor is used for FOLLOW_MASTER and OPPOSE_MASTER values that represent
+   * relationships rather than absolute directions.
+   */
+  InvertType() {
+    phoenixInvert = Optional.empty();
+    sparkMaxInvert = Optional.empty();
+  }
+
+  /**
+   * Creates an InvertType with vendor-specific mappings.
+   *
+   * 
    This constructor is used for rotation values (CLOCKWISE, COUNTER_CLOCKWISE) that have direct
+   * mappings to vendor-specific inversion settings.
+   *
+   * @param phoenixInvert the corresponding Phoenix 6 InvertedValue
+   * @param sparkMaxInvert the corresponding SPARK MAX inversion setting
+   */
+  InvertType(InvertedValue phoenixInvert, boolean sparkMaxInvert) {
+    this.phoenixInvert = Optional.of(phoenixInvert);
+    this.sparkMaxInvert = Optional.of(sparkMaxInvert);
+  }
+
+  /**
+   * Gets an {@link InvertType} from a Phoenix 6 {@link InvertedValue}.
+   *
+   * 
    This method provides reverse lookup from vendor-specific Phoenix settings to the
+   * vendor-neutral InvertType representation. It enables conversion from Phoenix-specific code to
+   * the standardized enum.
+   *
+   * @param v the {@link InvertedValue} to search for
+   * @return an Optional containing the corresponding {@link InvertType}, or {@link
+   *     Optional#empty()} if no mapping exists
+   */
+  public static Optional fromPhoenixInvert(InvertedValue v) {
+    return Optional.ofNullable(Maps.phoenixMap.get(v));
+  }
+
+  /**
+   * Gets an {@link InvertType} from a SPARK MAX inversion boolean.
+   *
+   * 
    This method provides reverse lookup from vendor-specific SPARK MAX settings to the
+   * vendor-neutral InvertType representation. It enables conversion from SPARK MAX-specific code to
+   * the standardized enum.
+   *
+   * @param v the SPARK MAX inversion boolean to search for
+   * @return an Optional containing the corresponding {@link InvertType}, or {@link
+   *     Optional#empty()} if no mapping exists
+   */
+  public static Optional fromSparkMaxInvert(boolean v) {
+    return Optional.ofNullable(Maps.sparkMaxMap.get(v));
+  }
+
+  /**
+   * Gets the Phoenix 6 InvertedValue for this InvertType.
+   *
+   * 
    This method returns the vendor-specific Phoenix mapping for rotation values. Following
+   * values (FOLLOW_MASTER, OPPOSE_MASTER) will return an empty Optional as they don't have direct
+   * Phoenix mappings.
+   *
+   * @return an Optional containing the Phoenix InvertedValue, or empty if not applicable
+   */
+  public Optional phoenixInvert() {
+    return phoenixInvert;
+  }
+
+  /**
+   * Forces retrieval of the Phoenix 6 InvertedValue.
+   *
+   * 
    This internal method assumes the Phoenix mapping exists and throws if it doesn't. It's used
+   * internally for map construction and should not be called on following values.
+   *
+   * @return the Phoenix InvertedValue
+   * @throws NoSuchElementException if no Phoenix mapping exists
+   */
+  private InvertedValue forcePhoenixInvert() {
+    return phoenixInvert.orElseThrow();
+  }
+
+  /**
+   * Gets the SPARK MAX inversion boolean for this InvertType.
+   *
+   * 
    This method returns the vendor-specific SPARK MAX mapping for rotation values. Following
+   * values (FOLLOW_MASTER, OPPOSE_MASTER) will return an empty Optional as they don't have direct
+   * SPARK MAX mappings.
+   *
+   * @return an Optional containing the SPARK MAX inversion boolean, or empty if not applicable
+   */
+  public Optional sparkMaxInvert() {
+    return sparkMaxInvert;
+  }
+
+  /**
+   * Forces retrieval of the SPARK MAX inversion boolean.
+   *
+   * 
    This internal method assumes the SPARK MAX mapping exists and throws if it doesn't. It's
+   * used internally for map construction and should not be called on following values.
+   *
+   * @return the SPARK MAX inversion boolean
+   * @throws NoSuchElementException if no SPARK MAX mapping exists
+   */
+  private boolean forceSparkMaxInvert() {
+    return sparkMaxInvert.orElseThrow();
+  }
+
+  /**
+   * Contains the reverse lookup maps for vendor-specific inversion values.
+   *
+   * 
    This static nested class holds the maps used by {@link #fromPhoenixInvert(InvertedValue)}
+   * and {@link #fromSparkMaxInvert(boolean)}. The maps are constructed lazily in a static context
+   * to avoid initialization overhead when reverse lookup is not needed.
+   *
+   * 
    The maps are built by filtering enum values to include only those with vendor mappings and
+   * creating reverse mappings from vendor values to InvertType.
+   */
+  private static final class Maps {
+
+    /** Reverse lookup map from Phoenix InvertedValue to InvertType */
+    private static final Map phoenixMap =
+        Stream.of(InvertType.values())
+            .filter((j) -> j.phoenixInvert.isPresent())
+            .collect(toUnmodifiableMap(InvertType::forcePhoenixInvert, (j) -> j, (a, b) -> null));
+
+    /** Reverse lookup map from SPARK MAX boolean to InvertType */
+    private static final Map sparkMaxMap =
+        Stream.of(InvertType.values())
+            .filter((j) -> j.sparkMaxInvert.isPresent())
+            .collect(toUnmodifiableMap(InvertType::forceSparkMaxInvert, (j) -> j, (a, b) -> null));
+  }
+}
diff --git a/lib/src/main/java/com/team2813/lib2813/control/Motor.java b/lib/src/main/java/com/team2813/lib2813/control/Motor.java
index 9643869f..a5012538 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/Motor.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/Motor.java
@@ -4,111 +4,113 @@
 
 /**
  * Interface defining the fundamental contract for motor control devices.
- * 
- * 
    This interface provides a vendor-neutral abstraction for motor controllers,
- * enabling standardized motor control across different hardware implementations.
- * It defines the essential operations needed for motor control: setting output
- * values with various control modes and monitoring current draw.
- * 
- * 
    The interface supports multiple control modes through the {@link ControlMode}
- * enumeration, allowing the same code to work with different control strategies
- * such as duty cycle, voltage, velocity, and motion magic control. This abstraction
- * enables easy switching between control modes without changing the calling code.
- * 
+ *
+ * 
    This interface provides a vendor-neutral abstraction for motor controllers, enabling
+ * standardized motor control across different hardware implementations. It defines the essential
+ * operations needed for motor control: setting output values with various control modes and
+ * monitoring current draw.
+ *
+ * 
    The interface supports multiple control modes through the {@link ControlMode} enumeration,
+ * allowing the same code to work with different control strategies such as duty cycle, voltage,
+ * velocity, and motion magic control. This abstraction enables easy switching between control modes
+ * without changing the calling code.
+ *
  * 
    Key features:
+ *
  * 
      
- * 
    • Vendor-neutral motor control abstraction
      • 
- * 
    • Multiple control modes with consistent API
      • 
- * 
    • Feedforward support for advanced control
      • 
- * 
    • Type-safe current monitoring using WPILib units
      • 
+ *   
    • Vendor-neutral motor control abstraction
+ *   
    • Multiple control modes with consistent API
+ *   
    • Feedforward support for advanced control
+ *   
    • Type-safe current monitoring using WPILib units
  * 
    
- * 
- * 
    Common implementations include TalonFX controllers, SPARK MAX controllers,
- * and other motor control devices that support the specified control modes.
- * 
+ *
+ * 
    Common implementations include TalonFX controllers, SPARK MAX controllers, and other motor
+ * control devices that support the specified control modes.
+ *
  * @author Team 2813
  * @since 1.0
  */
 public interface Motor {
 
-    /**
-     * Sets the motor to run with a specified control mode and demand value.
-     * 
-     * 
    This method provides basic motor control without feedforward compensation.
-     * The interpretation of the demand value depends on the selected control mode:
-     * 
-     * 
      
-     * 
    • DUTY_CYCLE: Demand represents percentage output (-1.0 to +1.0)
      • 
-     * 
    • VOLTAGE: Demand represents voltage to apply (in volts)
      • 
-     * 
    • VELOCITY: Demand represents target velocity (units depend on implementation)
      • 
-     * 
    • MOTION_MAGIC: Demand represents target position (units depend on implementation)
      • 
-     * 
    
-     * 
-     * 
    This is equivalent to calling {@link #set(ControlMode, double, double)}
-     * with a feedforward value of 0.0.
-     * 
-     * @param mode the control mode to use for motor operation
-     * @param demand the demand value whose interpretation depends on the control mode
-     */
-    void set(ControlMode mode, double demand);
+  /**
+   * Sets the motor to run with a specified control mode and demand value.
+   *
+   * 
    This method provides basic motor control without feedforward compensation. The
+   * interpretation of the demand value depends on the selected control mode:
+   *
+   * 
      
+   *   
    • DUTY_CYCLE: Demand represents percentage output (-1.0 to +1.0)
+   *   
    • VOLTAGE: Demand represents voltage to apply (in volts)
+   *   
    • VELOCITY: Demand represents target velocity (units depend on implementation)
+   *   
    • MOTION_MAGIC: Demand represents target position (units depend on implementation)
+   * 
    
+   *
+   * 
    This is equivalent to calling {@link #set(ControlMode, double, double)} with a feedforward
+   * value of 0.0.
+   *
+   * @param mode the control mode to use for motor operation
+   * @param demand the demand value whose interpretation depends on the control mode
+   */
+  void set(ControlMode mode, double demand);
 
-    /**
-     * Sets the motor to run with a specified control mode, demand value, and feedforward.
-     * 
-     * 
    This method provides advanced motor control with feedforward compensation
-     * to improve control performance. Feedforward helps the controller anticipate
-     * the required output, reducing tracking error and improving response time.
-     * 
-     * 
    The demand value interpretation is the same as in {@link #set(ControlMode, double)}.
-     * The feedforward value provides additional output that is added to the
-     * closed-loop controller output:
-     * 
-     * 
      
-     * 
    • Velocity Control: Feedforward often represents the expected motor
-     *     voltage needed to maintain the target velocity
      • 
-     * 
    • Position Control: Feedforward can compensate for gravity, friction,
-     *     or other predictable loads
      • 
-     * 
    • Open-loop modes: Feedforward is typically ignored or added directly
-     *     to the output
      • 
-     * 
    
-     * 
-     * 
    Feedforward values are typically calculated based on system identification
-     * or theoretical models of the mechanism being controlled.
-     * 
-     * @param mode the control mode to use for motor operation
-     * @param demand the demand value whose interpretation depends on the control mode
-     * @param feedForward the feedforward compensation value to improve control performance
-     */
-    void set(ControlMode mode, double demand, double feedForward);
+  /**
+   * Sets the motor to run with a specified control mode, demand value, and feedforward.
+   *
+   * 
    This method provides advanced motor control with feedforward compensation to improve control
+   * performance. Feedforward helps the controller anticipate the required output, reducing tracking
+   * error and improving response time.
+   *
+   * 
    The demand value interpretation is the same as in {@link #set(ControlMode, double)}. The
+   * feedforward value provides additional output that is added to the closed-loop controller
+   * output:
+   *
+   * 
      
+   *   
    • Velocity Control: Feedforward often represents the expected motor voltage needed
+   *       to maintain the target velocity
+   *   
    • Position Control: Feedforward can compensate for gravity, friction, or other
+   *       predictable loads
+   *   
    • Open-loop modes: Feedforward is typically ignored or added directly to the output
+   * 
    
+   *
+   * 
    Feedforward values are typically calculated based on system identification or theoretical
+   * models of the mechanism being controlled.
+   *
+   * @param mode the control mode to use for motor operation
+   * @param demand the demand value whose interpretation depends on the control mode
+   * @param feedForward the feedforward compensation value to improve control performance
+   */
+  void set(ControlMode mode, double demand, double feedForward);
 
-    /**
-     * Gets the current that is currently being applied to the motor.
-     * 
-     * 
    This method returns the actual current flowing through the motor windings,
-     * which is useful for:
-     * 
      
-     * 
    • Monitoring motor load and detecting mechanical binding
      • 
-     * 
    • Implementing current-based control strategies
      • 
-     * 
    • Ensuring motors operate within safe current limits
      • 
-     * 
    • Diagnostic monitoring and fault detection
      • 
-     * 
    • Power consumption analysis
      • 
-     * 
    
-     * 
-     * 
    The returned current measurement uses WPILib's type-safe units system,
-     * allowing easy conversion between different current units (amps, milliamps)
-     * and providing type safety in calculations.
-     * 
-     * 
    Example usage:
-     * 
    {@code
-     * Current motorCurrent = motor.getAppliedCurrent();
-     * double amps = motorCurrent.in(Units.Amps);
-     * if (amps > 30.0) {
-     *     // Handle high current condition
-     * }
-     * }
    
-     * 
-     * @return the current applied current as a type-safe {@link Current} measurement
-     * @since 2.0.0
-     */
-    Current getAppliedCurrent();
-}
\ No newline at end of file
+  /**
+   * Gets the current that is currently being applied to the motor.
+   *
+   * 
    This method returns the actual current flowing through the motor windings, which is useful
+   * for:
+   *
+   * 
      
+   *   
    • Monitoring motor load and detecting mechanical binding
+   *   
    • Implementing current-based control strategies
+   *   
    • Ensuring motors operate within safe current limits
+   *   
    • Diagnostic monitoring and fault detection
+   *   
    • Power consumption analysis
+   * 
    
+   *
+   * 
    The returned current measurement uses WPILib's type-safe units system, allowing easy
+   * conversion between different current units (amps, milliamps) and providing type safety in
+   * calculations.
+   *
+   * 
    Example usage:
+   *
+   * 
    {@code
+   * Current motorCurrent = motor.getAppliedCurrent();
+   * double amps = motorCurrent.in(Units.Amps);
+   * if (amps > 30.0) {
+   *     // Handle high current condition
+   * }
+   * }
    
+   *
+   * @return the current applied current as a type-safe {@link Current} measurement
+   * @since 2.0.0
+   */
+  Current getAppliedCurrent();
+}
diff --git a/lib/src/main/java/com/team2813/lib2813/control/PIDMotor.java b/lib/src/main/java/com/team2813/lib2813/control/PIDMotor.java
index 94d0b225..14fe6792 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/PIDMotor.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/PIDMotor.java
@@ -1,133 +1,134 @@
 package com.team2813.lib2813.control;
 
 /**
- * Interface defining a motor controller with integrated encoder feedback and PID control capabilities.
- * 
- * 
    This interface combines the functionality of both {@link Motor} and {@link Encoder} interfaces,
- * representing motor controllers that have built-in position/velocity feedback and can perform
- * closed-loop PID control. This is the most common type of motor controller used in competitive
- * robotics, where precise control and position feedback are essential.
- * 
- * 
    The interface extends the basic motor control capabilities with PID configuration methods
- * that allow tuning of closed-loop control parameters. Multiple PID slots are supported to
- * enable different control parameters for different operating conditions (e.g., different
- * gains for velocity vs position control, or different gains for different mechanisms).
- * 
+ * Interface defining a motor controller with integrated encoder feedback and PID control
+ * capabilities.
+ *
+ * 
    This interface combines the functionality of both {@link Motor} and {@link Encoder}
+ * interfaces, representing motor controllers that have built-in position/velocity feedback and can
+ * perform closed-loop PID control. This is the most common type of motor controller used in
+ * competitive robotics, where precise control and position feedback are essential.
+ *
+ * 
    The interface extends the basic motor control capabilities with PID configuration methods that
+ * allow tuning of closed-loop control parameters. Multiple PID slots are supported to enable
+ * different control parameters for different operating conditions (e.g., different gains for
+ * velocity vs position control, or different gains for different mechanisms).
+ *
  * 
    Key features inherited and added:
+ *
  * 
      
- * 
    • From Motor: Control mode support, current monitoring, feedforward capability
      • 
- * 
    • From Encoder: Position and velocity feedback with type-safe units
      • 
- * 
    • Added: PID parameter configuration with multi-slot support
      • 
+ *   
    • From Motor: Control mode support, current monitoring, feedforward capability
+ *   
    • From Encoder: Position and velocity feedback with type-safe units
+ *   
    • Added: PID parameter configuration with multi-slot support
  * 
    
- * 
+ *
  * 
    Common implementations include:
+ *
  * 
      
- * 
    • TalonFX (Falcon 500) controllers with integrated encoders
      • 
- * 
    • SPARK MAX controllers with attached encoders
      • 
- * 
    • Other smart motor controllers with feedback and PID capabilities
      • 
+ *   
    • TalonFX controllers with integrated encoders
+ *   
    • SPARK MAX controllers with attached encoders
+ *   
    • Other smart motor controllers with feedback and PID capabilities
  * 
    
- * 
+ *
  * 
    This interface enables sophisticated control strategies such as:
+ *
  * 
      
- * 
    • Velocity control for flywheels and intake mechanisms
      • 
- * 
    • Position control for arms, elevators, and turrets
      • 
- * 
    • Motion magic for smooth, profiled movements
      • 
- * 
    • Cascaded control loops with multiple feedback sources
      • 
+ *   
    • Velocity control for flywheels and intake mechanisms
+ *   
    • Position control for arms, elevators, and turrets
+ *   
    • Motion magic for smooth, profiled movements
+ *   
    • Cascaded control loops with multiple feedback sources
  * 
    
- * 
+ *
  * @author Team 2813
  * @since 1.0
  */
 public interface PIDMotor extends Motor, Encoder {
 
-    /**
-     * Configures the PID and feedforward constants for a specific control slot.
-     * 
-     * 
    Motor controllers typically support multiple PID parameter sets (slots) that
-     * can be switched between during operation. This allows different control parameters
-     * for different operating conditions without reconfiguration overhead.
-     * 
-     * 
    Common slot usage patterns:
-     * 
      
-     * 
    • Slot 0: Velocity control parameters
      • 
-     * 
    • Slot 1: Position control parameters
      • 
-     * 
    • Slot 2: Motion magic parameters
      • 
-     * 
    • Slot 3: Alternative parameters for different mechanisms or conditions
      • 
-     * 
    
-     * 
-     * 
    PID parameter meanings:
-     * 
      
-     * 
    • P (Proportional): Responds to current error magnitude
      • 
-     * 
    • I (Integral): Eliminates steady-state error by accumulating past error
      • 
-     * 
    • D (Derivative): Reduces oscillation by responding to error rate of change
      • 
-     * 
    • F (Feedforward): Provides base output for known system behavior
      • 
-     * 
    
-     * 
-     * 
    Note: The interpretation of the 'f' parameter may vary by implementation.
-     * Some controllers use it for velocity feedforward (kV), while others use it for
-     * arbitrary feedforward (kF). Consult the specific implementation documentation.
-     * 
-     * @param slot the PID slot number to configure (typically 0-3, range depends on implementation)
-     * @param p the proportional gain coefficient
-     * @param i the integral gain coefficient  
-     * @param d the derivative gain coefficient
-     * @param f the feedforward gain coefficient
-     */
-    void configPIDF(int slot, double p, double i, double d, double f);
+  /**
+   * Configures the PID and feedforward constants for a specific control slot.
+   *
+   * 
    Motor controllers typically support multiple PID parameter sets (slots) that can be switched
+   * between during operation. This allows different control parameters for different operating
+   * conditions without reconfiguration overhead.
+   *
+   * 
    PID parameter meanings:
+   *
+   * 
      
+   *   
    • P (Proportional): Responds to current error magnitude
+   *   
    • I (Integral): Eliminates steady-state error by accumulating past error
+   *   
    • D (Derivative): Reduces oscillation by responding to error rate of change
+   *   
    • F (Feedforward): Provides base output for known system behavior
+   * 
    
+   *
+   * 
    Note: The interpretation of the 'f' parameter may vary by implementation. Some
+   * controllers use it for velocity feedforward (kV), while others use it for arbitrary feedforward
+   * (kF). Consult the specific implementation documentation.
+   *
+   * @param slot the PID slot number to configure (typically 0-3, range depends on implementation)
+   * @param p the proportional gain coefficient
+   * @param i the integral gain coefficient
+   * @param d the derivative gain coefficient
+   * @param f the feedforward gain coefficient
+   */
+  void configPIDF(int slot, double p, double i, double d, double f);
 
-    /**
-     * Configures the PID and feedforward constants for the default control slot (slot 0).
-     * 
-     * 
    This is a convenience method equivalent to calling {@link #configPIDF(int, double, double, double, double)}
-     * with slot 0. Most applications start with slot 0 for their primary control parameters.
-     * 
-     * 
    This method is ideal for simple applications that only need one set of PID parameters
-     * or for initial tuning before implementing more advanced multi-slot strategies.
-     * 
-     * @param p the proportional gain coefficient
-     * @param i the integral gain coefficient
-     * @param d the derivative gain coefficient
-     * @param f the feedforward gain coefficient
-     */
-    void configPIDF(double p, double i, double d, double f);
+  /**
+   * Configures the PID and feedforward constants for the default control slot (slot 0).
+   *
+   * 
    This is a convenience method equivalent to calling {@link #configPIDF(int, double, double,
+   * double, double)} with slot 0. Most applications start with slot 0 for their primary control
+   * parameters.
+   *
+   * 
    This method is ideal for simple applications that only need one set of PID parameters or for
+   * initial tuning before implementing more advanced multi-slot strategies.
+   *
+   * @param p the proportional gain coefficient
+   * @param i the integral gain coefficient
+   * @param d the derivative gain coefficient
+   * @param f the feedforward gain coefficient
+   */
+  void configPIDF(double p, double i, double d, double f);
 
-    /**
-     * Configures the PID constants for a specific control slot without feedforward.
-     * 
-     * 
    This method is equivalent to calling {@link #configPIDF(int, double, double, double, double)}
-     * with a feedforward value of 0. It's useful when feedforward is not needed or will be
-     * handled separately through the {@link #set(ControlMode, double, double)} method.
-     * 
-     * 
    Pure PID control without feedforward is common for:
-     * 
      
-     * 
    • Position control where the mechanism is balanced (no gravity/friction compensation needed)
      • 
-     * 
    • Systems where disturbances are unpredictable
      • 
-     * 
    • Initial tuning stages before adding feedforward refinement
      • 
-     * 
    
-     * 
-     * @param slot the PID slot number to configure
-     * @param p the proportional gain coefficient
-     * @param i the integral gain coefficient
-     * @param d the derivative gain coefficient
-     */
-    void configPID(int slot, double p, double i, double d);
+  /**
+   * Configures the PID constants for a specific control slot without feedforward.
+   *
+   * 
    This method is equivalent to calling {@link #configPIDF(int, double, double, double,
+   * double)} with a feedforward value of 0. It's useful when feedforward is not needed or will be
+   * handled separately through the {@link #set(ControlMode, double, double)} method.
+   *
+   * 
    Pure PID control without feedforward is common for:
+   *
+   * 
      
+   *   
    • Position control where the mechanism is balanced (no gravity/friction compensation
+   *       needed)
+   *   
    • Systems where disturbances are unpredictable
+   *   
    • Initial tuning stages before adding feedforward refinement
+   * 
    
+   *
+   * @param slot the PID slot number to configure
+   * @param p the proportional gain coefficient
+   * @param i the integral gain coefficient
+   * @param d the derivative gain coefficient
+   */
+  void configPID(int slot, double p, double i, double d);
 
-    /**
-     * Configures the PID constants for the default control slot (slot 0) without feedforward.
-     * 
-     * 
    This is a convenience method equivalent to calling {@link #configPID(int, double, double, double)}
-     * with slot 0. It provides the simplest interface for basic PID control configuration.
-     * 
-     * 
    This method is perfect for:
-     * 
      
-     * 
    • Simple mechanisms that don't require feedforward
      • 
-     * 
    • Initial testing and tuning of PID parameters
      • 
-     * 
    • Applications where feedforward is applied externally
      • 
-     * 
    
-     * 
-     * @param p the proportional gain coefficient
-     * @param i the integral gain coefficient
-     * @param d the derivative gain coefficient
-     */
-    void configPID(double p, double i, double d);
-}
\ No newline at end of file
+  /**
+   * Configures the PID constants for the default control slot (slot 0) without feedforward.
+   *
+   * 
    This is a convenience method equivalent to calling {@link #configPID(int, double, double,
+   * double)} with slot 0. It provides the simplest interface for basic PID control configuration.
+   *
+   * 
    This method is perfect for:
+   *
+   * 
      
+   *   
    • Simple mechanisms that don't require feedforward
+   *   
    • Initial testing and tuning of PID parameters
+   *   
    • Applications where feedforward is applied externally
+   * 
    
+   *
+   * @param p the proportional gain coefficient
+   * @param i the integral gain coefficient
+   * @param d the derivative gain coefficient
+   */
+  void configPID(double p, double i, double d);
+}
diff --git a/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java b/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java
index 64e0c03c..2aabc2e4 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/encoders/CancoderWrapper.java
@@ -7,155 +7,155 @@
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import java.util.function.Supplier;
 
 /**
  * Wrapper class for a CTRE Phoenix 6 CANcoder absolute encoder.
- * 
- * 
    This class provides a standardized interface for interacting with CANcoder devices
- * while maintaining compatibility with the team's control system architecture. It implements
- * the {@link Encoder} interface and supports both legacy double-based methods and modern
- * type-safe unit-based methods.
- * 
- * 
    The class includes deprecated methods for backward compatibility, but new code should
- * use the type-safe methods that work with WPILib's units system.
- * 
+ *
+ * 
    This class provides a standardized interface for interacting with CANcoder devices while
+ * maintaining compatibility with the team's control system architecture. It implements the {@link
+ * Encoder} interface and supports both legacy double-based methods and modern type-safe unit-based
+ * methods.
+ *
+ * 
    The class includes deprecated methods for backward compatibility, but new code should use the
+ * type-safe methods that work with WPILib's units system.
+ *
  * @author Team 2813
  * @since 1.0
  */
 public class CancoderWrapper implements Encoder {
-    
-    /** The underlying CTRE CANcoder hardware object */
-    private CANcoder cancoder;
-    
-    /** Device information containing ID and CAN bus details */
-    private DeviceInformation info;
 
-    /**
-     * Creates a new CancoderWrapper with the specified CAN ID and bus name.
-     * 
-     * @param id the CAN ID of the CANcoder device
-     * @param canbus the name of the CAN bus the device is connected to (e.g., "rio", "canivore")
-     */
-    public CancoderWrapper(int id, String canbus) {
-        cancoder = new CANcoder(id, canbus);
-        info = new DeviceInformation(id, canbus);
-    }
+  /** The underlying CTRE CANcoder hardware object */
+  private CANcoder cancoder;
 
-    /**
-     * Creates a new CancoderWrapper with the specified CAN ID on the default CAN bus.
-     * 
-     * @param id the CAN ID of the CANcoder device
-     */
-    public CancoderWrapper(int id) {
-        cancoder = new CANcoder(id);
-        info = new DeviceInformation(id);
-    }
+  /** Device information containing ID and CAN bus details */
+  private DeviceInformation info;
 
-    /**
-     * Gets the current position of the encoder as a raw double value.
-     * 
-     * @deprecated Use {@link #getPositionMeasure()} instead for type safety with units
-     * @return the current position in rotations as a double
-     */
-    @Deprecated
-    @Override
-    public double position() {
-        return cancoder.getPosition().getValueAsDouble();
-    }
+  /**
+   * Creates a new CancoderWrapper with the specified CAN ID and bus name.
+   *
+   * @param id the CAN ID of the CANcoder device
+   * @param canbus the name of the CAN bus the device is connected to (e.g., "rio", "canivore")
+   */
+  public CancoderWrapper(int id, String canbus) {
+    cancoder = new CANcoder(id, canbus);
+    info = new DeviceInformation(id, canbus);
+  }
 
-    /**
-     * Gets the current position of the encoder using type-safe units.
-     * 
-     * 
    This method returns the position as an {@link Angle} measurement in rotations,
-     * which can be converted to other angular units as needed.
-     * 
-     * @return the current position as an {@link Angle} measurement
-     */
-    @Override
-    public Angle getPositionMeasure() {
-        return Units.Rotations.of(cancoder.getPosition().getValueAsDouble());
-    }
+  /**
+   * Creates a new CancoderWrapper with the specified CAN ID on the default CAN bus.
+   *
+   * @param id the CAN ID of the CANcoder device
+   */
+  public CancoderWrapper(int id) {
+    cancoder = new CANcoder(id);
+    info = new DeviceInformation(id);
+  }
 
-    /**
-     * Sets the position of the encoder using a raw double value.
-     * 
-     * 
    This method uses {@link ConfigUtils#phoenix6Config(Runnable)} to ensure
-     * reliable configuration of the Phoenix 6 device.
-     * 
-     * @deprecated Use {@link #setPosition(Angle)} instead for type safety with units
-     * @param position the new position in rotations
-     */
-    @Deprecated
-    @Override
-    public void setPosition(double position) {
-        ConfigUtils.phoenix6Config(() -> cancoder.setPosition(position));
-    }
+  /**
+   * Gets the current position of the encoder as a raw double value.
+   *
+   * @deprecated Use {@link #getPositionMeasure()} instead for type safety with units
+   * @return the current position in rotations as a double
+   */
+  @Deprecated
+  @Override
+  public double position() {
+    return cancoder.getPosition().getValueAsDouble();
+  }
 
-    /**
-     * Sets the position of the encoder using type-safe units.
-     * 
-     * 
    This method accepts any {@link Angle} measurement and converts it to rotations
-     * internally. It uses {@link ConfigUtils#phoenix6Config(Runnable)} to ensure
-     * reliable configuration of the Phoenix 6 device.
-     * 
-     * @param position the new position as an {@link Angle} measurement
-     */
-    @Override
-    public void setPosition(Angle position) {
-        ConfigUtils.phoenix6Config(() -> cancoder.setPosition(position.in(Units.Rotations)));
-    }
+  /**
+   * Gets the current position of the encoder using type-safe units.
+   *
+   * 
    This method returns the position as an {@link Angle} measurement in rotations, which can be
+   * converted to other angular units as needed.
+   *
+   * @return the current position as an {@link Angle} measurement
+   */
+  @Override
+  public Angle getPositionMeasure() {
+    return Units.Rotations.of(cancoder.getPosition().getValueAsDouble());
+  }
 
-    /**
-     * Provides direct access to the underlying CANcoder hardware object.
-     * 
-     * 
    This method allows access to Phoenix-specific configuration and methods
-     * that are not exposed through the {@link Encoder} interface.
-     * 
-     * @return the underlying {@link CANcoder} hardware object
-     */
-    public CANcoder encoder() {
-        return cancoder;
-    }
+  /**
+   * Sets the position of the encoder using a raw double value.
+   *
+   * 
    This method uses {@link ConfigUtils#phoenix6Config(Supplier)} to ensure reliable
+   * configuration of the Phoenix 6 device.
+   *
+   * @deprecated Use {@link #setPosition(Angle)} instead for type safety with units
+   * @param position the new position in rotations
+   */
+  @Deprecated
+  @Override
+  public void setPosition(double position) {
+    ConfigUtils.phoenix6Config(() -> cancoder.setPosition(position));
+  }
 
-    /**
-     * Gets the current velocity of the encoder as a raw double value.
-     * 
-     * @deprecated Use {@link #getVelocityMeasure()} instead for type safety with units
-     * @return the current velocity in rotations per second as a double
-     */
-    @Deprecated
-    @Override
-    public double getVelocity() {
-        return cancoder.getVelocity().getValueAsDouble();
-    }
+  /**
+   * Sets the position of the encoder using type-safe units.
+   *
+   * 
    This method accepts any {@link Angle} measurement and converts it to rotations internally.
+   * It uses {@link ConfigUtils#phoenix6Config(Supplier)} to ensure reliable configuration of the
+   * Phoenix 6 device.
+   *
+   * @param position the new position as an {@link Angle} measurement
+   */
+  @Override
+  public void setPosition(Angle position) {
+    ConfigUtils.phoenix6Config(() -> cancoder.setPosition(position.in(Units.Rotations)));
+  }
 
-    /**
-     * Gets the current velocity of the encoder using type-safe units.
-     * 
-     * 
    This method returns the velocity as an {@link AngularVelocity} measurement,
-     * which can be converted to other angular velocity units as needed.
-     * 
-     * @return the current velocity as an {@link AngularVelocity} measurement
-     */
-    @Override
-    public AngularVelocity getVelocityMeasure() {
-        return cancoder.getVelocity().getValue();
-    }
+  /**
+   * Provides direct access to the underlying CANcoder hardware object.
+   *
+   * 
    This method allows access to Phoenix-specific configuration and methods that are not exposed
+   * through the {@link Encoder} interface.
+   *
+   * @return the underlying {@link CANcoder} hardware object
+   */
+  public CANcoder encoder() {
+    return cancoder;
+  }
 
-    /**
-     * Determines whether this CancoderWrapper is equal to another object.
-     * 
-     * 
    Two CancoderWrapper instances are considered equal if they represent
-     * the same physical CANcoder device (same CAN ID and CAN bus).
-     * 
-     * @param obj the object to compare against
-     * @return {@code true} if the objects represent the same CANcoder device,
-     *         {@code false} otherwise
-     */
-    @Override
-    public boolean equals(Object obj) {
-        if (!(obj instanceof CancoderWrapper)) return false;
-        CancoderWrapper other = (CancoderWrapper) obj;
-        return info.equals(other.info);
-    }
-}
\ No newline at end of file
+  /**
+   * Gets the current velocity of the encoder as a raw double value.
+   *
+   * @deprecated Use {@link #getVelocityMeasure()} instead for type safety with units
+   * @return the current velocity in rotations per second as a double
+   */
+  @Deprecated
+  @Override
+  public double getVelocity() {
+    return cancoder.getVelocity().getValueAsDouble();
+  }
+
+  /**
+   * Gets the current velocity of the encoder using type-safe units.
+   *
+   * 
    This method returns the velocity as an {@link AngularVelocity} measurement, which can be
+   * converted to other angular velocity units as needed.
+   *
+   * @return the current velocity as an {@link AngularVelocity} measurement
+   */
+  @Override
+  public AngularVelocity getVelocityMeasure() {
+    return cancoder.getVelocity().getValue();
+  }
+
+  /**
+   * Determines whether this CancoderWrapper is equal to another object.
+   *
+   * 
    Two CancoderWrapper instances are considered equal if they represent the same physical
+   * CANcoder device (same CAN ID and CAN bus).
+   *
+   * @param obj the object to compare against
+   * @return {@code true} if the objects represent the same CANcoder device, {@code false} otherwise
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (!(obj instanceof CancoderWrapper)) return false;
+    CancoderWrapper other = (CancoderWrapper) obj;
+    return info.equals(other.info);
+  }
+}
diff --git a/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java b/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java
index aec32141..91c29c46 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/imu/Pigeon2Wrapper.java
@@ -3,144 +3,143 @@
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.team2813.lib2813.control.DeviceInformation;
 import com.team2813.lib2813.util.ConfigUtils;
+import java.util.function.Supplier;
 
 /**
  * Wrapper class for a CTRE Phoenix 6 Pigeon2 IMU (Inertial Measurement Unit).
- * 
- * 
    This class provides a standardized interface for interacting with Pigeon2 devices
- * while maintaining compatibility with the team's control system architecture. It includes
- * automatic reset detection and recovery functionality to handle power cycles and device
- * resets that can occur during competition.
- * 
- * 
    The wrapper maintains a cached heading value that is restored after device resets,
- * ensuring consistent heading readings across power cycles. The {@link #periodicResetCheck()}
- * method should be called periodically (typically in a subsystem's periodic method) to
- * monitor for and handle resets.
- * 
+ *
+ * 
    This class provides a standardized interface for interacting with Pigeon2 devices while
+ * maintaining compatibility with the team's control system architecture. It includes automatic
+ * reset detection and recovery functionality to handle power cycles and device resets that can
+ * occur during competition.
+ *
+ * 
    The wrapper maintains a cached heading value that is restored after reboot, ensuring
+ * consistent heading readings across each roborio bootup. The {@link #periodicResetCheck()} method
+ * should be called periodically (typically in a subsystem's periodic method) to monitor for and
+ * handle resets.
+ *
  * @author Team 2813
  * @since 1.0
  */
 public class Pigeon2Wrapper {
-    
-    /** The underlying CTRE Pigeon2 hardware object */
-    private Pigeon2 pigeon;
-    
-    /** Cached heading value used for reset recovery */
-    private double currentHeading = 0;
-    
-    /** Device information containing ID and CAN bus details */
-    private DeviceInformation info;
 
-    /**
-     * Creates a new Pigeon2Wrapper with the specified device number and CAN bus.
-     * 
-     * @param deviceNumber the CAN ID of the Pigeon2 device, must be in range [0,62]
-     * @param canbus the name of the CAN bus the device is connected to. This can be a 
-     *               SocketCAN interface (on Linux), a CANivore device name, or serial number
-     */
-    public Pigeon2Wrapper(int deviceNumber, String canbus) {
-        info = new DeviceInformation(deviceNumber, canbus);
-        pigeon = new Pigeon2(deviceNumber, canbus);
-    }
+  /** The underlying CTRE Pigeon2 hardware object */
+  private Pigeon2 pigeon;
 
-    /**
-     * Creates a new Pigeon2Wrapper with the specified device number on the default CAN bus.
-     * 
-     * @param deviceNumber the CAN ID of the Pigeon2 device, must be in range [0,62]
-     */
-    public Pigeon2Wrapper(int deviceNumber) {
-        info = new DeviceInformation(deviceNumber);
-        pigeon = new Pigeon2(deviceNumber);
-    }
+  /** Cached heading value used for reset recovery */
+  private double currentHeading = 0;
 
-    /**
-     * Provides direct access to the underlying Pigeon2 hardware object.
-     * 
-     * 
    This method allows access to Phoenix-specific configuration and methods
-     * that are not exposed through this wrapper class.
-     * 
-     * @return the underlying {@link Pigeon2} hardware object
-     */
-    public Pigeon2 getPigeon() {
-        return pigeon;
-    }
+  /** Device information containing ID and CAN bus details */
+  private DeviceInformation info;
 
-    /**
-     * Gets the current heading (yaw angle) of the IMU.
-     * 
-     * 
    This method returns the raw yaw value from the Pigeon2. The heading
-     * is typically measured in degrees, with the range and direction depending
-     * on the device configuration.
-     * 
-     * @return the current heading in degrees
-     */
-    public double getHeading() {
-        return pigeon.getYaw().getValueAsDouble();
-    }
+  /**
+   * Creates a new Pigeon2Wrapper with the specified device number and CAN bus.
+   *
+   * @param deviceNumber the CAN ID of the Pigeon2 device, must be in range [0,62]
+   * @param canbus the name of the CAN bus the device is connected to. This can be a SocketCAN
+   *     interface (on Linux), a CANivore device name, or serial number
+   */
+  public Pigeon2Wrapper(int deviceNumber, String canbus) {
+    info = new DeviceInformation(deviceNumber, canbus);
+    pigeon = new Pigeon2(deviceNumber, canbus);
+  }
 
-    /**
-     * Sets the current heading (yaw angle) of the IMU.
-     * 
-     * 
    This method updates both the hardware device and the cached heading value
-     * used for reset recovery. It uses {@link ConfigUtils#phoenix6Config(Runnable)}
-     * to ensure reliable configuration of the Phoenix 6 device.
-     * 
-     * @param angle the new heading angle in degrees
-     */
-    public void setHeading(double angle) {
-        ConfigUtils.phoenix6Config(() -> pigeon.setYaw(angle));
-        currentHeading = angle;
-    }
+  /**
+   * Creates a new Pigeon2Wrapper with the specified device number on the default CAN bus.
+   *
+   * @param deviceNumber the CAN ID of the Pigeon2 device, must be in range [0,62]
+   */
+  public Pigeon2Wrapper(int deviceNumber) {
+    info = new DeviceInformation(deviceNumber);
+    pigeon = new Pigeon2(deviceNumber);
+  }
 
-    /**
-     * Checks if a device reset has occurred and restores non-persistent settings if needed.
-     * 
-     * 
    This method should be called periodically (e.g., in a subsystem's periodic() method)
-     * to monitor for device resets that can occur due to power cycles, brownouts, or other
-     * electrical issues. When a reset is detected, the method automatically restores the
-     * cached heading value to maintain consistency.
-     * 
-     * 
    If no reset has occurred, the method updates the cached heading with the current
-     * hardware reading to ensure the cache stays synchronized.
-     * 
-     * 
    Implementation Note: This method should be called at least once per robot
-     * loop iteration for proper reset detection and recovery.
-     */
-    public void periodicResetCheck() {
-        if (!pigeon.hasResetOccurred()) {
-            currentHeading = getHeading();
-        } else {
-            setHeading(currentHeading);
-        }
-    }
+  /**
+   * Provides direct access to the underlying Pigeon2 hardware object.
+   *
+   * 
    This method allows access to Phoenix-specific configuration and methods that are not exposed
+   * through this wrapper class.
+   *
+   * @return the underlying {@link Pigeon2} hardware object
+   */
+  public Pigeon2 getPigeon() {
+    return pigeon;
+  }
+
+  /**
+   * Gets the current heading (yaw angle) of the IMU.
+   *
+   * 
    This method returns the raw yaw value from the Pigeon2. The heading is typically measured in
+   * degrees, with the range and direction depending on the device configuration.
+   *
+   * @return the current heading in degrees
+   */
+  public double getHeading() {
+    return pigeon.getYaw().getValueAsDouble();
+  }
 
-    /**
-     * Returns a hash code value for this Pigeon2Wrapper.
-     * 
-     * 
    The hash code is based on the device information (CAN ID and bus),
-     * ensuring that wrappers for the same physical device have the same hash code.
-     * 
-     * @return a hash code value for this object
-     */
-    public int hashCode() {
-        return info.hashCode();
+  /**
+   * Sets the current heading (yaw angle) of the IMU.
+   *
+   * 
    This method updates both the hardware device and the cached heading value used for reset
+   * recovery. It uses {@link ConfigUtils#phoenix6Config(Supplier)} to ensure reliable configuration
+   * of the Phoenix 6 device.
+   *
+   * @param angle the new heading angle in degrees
+   */
+  public void setHeading(double angle) {
+    ConfigUtils.phoenix6Config(() -> pigeon.setYaw(angle));
+    currentHeading = angle;
+  }
+
+  /**
+   * Checks if a device reset has occurred and restores non-persistent settings if needed.
+   *
+   * 
    This method should be called periodically (e.g., in a subsystem's periodic() method) to
+   * monitor for device resets that can occur due to power cycles, brownouts, or other electrical
+   * issues. When a reset is detected, the method automatically restores the cached heading value to
+   * maintain consistency.
+   *
+   * 
    If no reset has occurred, the method updates the cached heading with the current hardware
+   * reading to ensure the cache stays synchronized.
+   *
+   * 
    Implementation Note: This method should be called at least once per robot loop
+   * iteration for proper reset detection and recovery.
+   */
+  public void periodicResetCheck() {
+    if (!pigeon.hasResetOccurred()) {
+      currentHeading = getHeading();
+    } else {
+      setHeading(currentHeading);
     }
+  }
+
+  /**
+   * Returns a hash code value for this Pigeon2Wrapper.
+   *
+   * 
    The hash code is based on the device information (CAN ID and bus), ensuring that wrappers
+   * for the same physical device have the same hash code.
+   *
+   * @return a hash code value for this object
+   */
+  public int hashCode() {
+    return info.hashCode();
+  }
 
-    /**
-     * Determines whether this Pigeon2Wrapper is equal to another object.
-     * 
-     * 
    Two Pigeon2Wrapper instances are considered equal if they represent
-     * the same physical Pigeon2 device (same CAN ID and CAN bus).
-     * 
-     * @param other the object to compare against
-     * @return {@code true} if the objects represent the same Pigeon2 device,
-     *         {@code false} otherwise
-     */
-    public boolean equals(Object other) {
-        if (!(other instanceof Pigeon2Wrapper)) {
-            return false;
-        }
-        Pigeon2Wrapper o = (Pigeon2Wrapper) other;
-        return o.info.equals(info);
+  /**
+   * Determines whether this Pigeon2Wrapper is equal to another object.
+   *
+   * 
    Two Pigeon2Wrapper instances are considered equal if they represent the same physical
+   * Pigeon2 device (same CAN ID and CAN bus).
+   *
+   * @param other the object to compare against
+   * @return {@code true} if the objects represent the same Pigeon2 device, {@code false} otherwise
+   */
+  public boolean equals(Object other) {
+    if (!(other instanceof Pigeon2Wrapper)) {
+      return false;
     }
-}
\ No newline at end of file
+    Pigeon2Wrapper o = (Pigeon2Wrapper) other;
+    return o.info.equals(info);
+  }
+}
diff --git a/lib/src/main/java/com/team2813/lib2813/control/motors/SparkMaxWrapper.java b/lib/src/main/java/com/team2813/lib2813/control/motors/SparkMaxWrapper.java
index ac6cbe89..ba4d6e43 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/motors/SparkMaxWrapper.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/motors/SparkMaxWrapper.java
@@ -22,64 +22,66 @@
 
 /**
  * Wrapper class for REV Robotics SPARK MAX motor controller.
- * 
- * 
    This class provides a standardized interface for controlling SPARK MAX motor controllers
- * while maintaining compatibility with the team's control system architecture. It implements
- * the {@link PIDMotor} interface and supports both brushed and brushless motors with integrated
- * encoder feedback.
- * 
+ *
+ * 
    This class provides a standardized interface for controlling SPARK MAX motor controllers while
+ * maintaining compatibility with the team's control system architecture. It implements the {@link
+ * PIDMotor} interface and supports both brushed and brushless motors with integrated encoder
+ * feedback.
+ *
  * 
    Key features include:
+ *
  * 
      
- * 
    • Type-safe unit support using WPILib's units system
      • 
- * 
    • PID control configuration with multiple slot support
      • 
- * 
    • Follower motor management with independent inversion settings
      • 
- * 
    • Reliable configuration using {@link ConfigUtils} for REV devices
      • 
- * 
    • Support for various control modes (duty cycle, voltage)
      • 
+ *   
    • Type-safe unit support using WPILib's units system
+ *   
    • PID control configuration with multiple slot support
+ *   
    • Follower motor management with independent inversion settings
+ *   
    • Reliable configuration using {@link ConfigUtils} for REV devices
+ *   
    • Support for various control modes (duty cycle, voltage)
  * 
    
- * 
- * 
    The wrapper uses safe parameter reset mode and non-persistent parameters by default
- * for reliable operation during competition.
- * 
+ *
+ * 
    The wrapper uses safe parameter reset mode and non-persistent parameters by default for
+ * reliable operation during competition.
+ *
  * @author Team 2813
  * @since 1.0
  */
 public class SparkMaxWrapper implements PIDMotor {
-  
+
   /** List of follower SPARK MAX controllers managed by this wrapper */
   private final List followers = new ArrayList<>();
-  
+
   /** The primary SPARK MAX motor controller */
   private final SparkBase motor;
-  
+
   /** The integrated relative encoder from the SPARK MAX */
   private final RelativeEncoder encoder;
-  
+
   /** Whether the motor output is inverted */
   private final boolean inverted;
-  
+
   /** Configuration object for the SPARK MAX */
   private final SparkBaseConfig config;
-  
+
   /** Reset mode used for configuration operations */
   private final SparkBase.ResetMode resetMode;
-  
+
   /** Persistence mode used for configuration operations */
   private final SparkBase.PersistMode persistMode;
 
   /**
    * Creates a new SparkMaxWrapper to control a SPARK MAX motor controller.
-   * 
+   *
    * 
    The constructor initializes the motor controller with safe default settings:
+   *
    * 
      
-   * 
    • Reset mode: {@code kResetSafeParameters} for reliable startup
      • 
-   * 
    • Persist mode: {@code kNoPersistParameters} to prevent parameter corruption
      • 
-   * 
    • Alternate encoder configuration with specified inversion
      • 
+   *   
    • Reset mode: {@code kResetSafeParameters} for reliable startup
+   *   
    • Persist mode: {@code kNoPersistParameters} to prevent parameter corruption
+   *   
    • Alternate encoder configuration with specified inversion
    * 
    
-   * 
+   *
    * @param deviceId the CAN ID of the SPARK MAX controller
-   * @param type the motor type connected to the controller. Brushless motor wires must be 
-   *             connected to their matching colors and the hall sensor must be plugged in.
-   *             Brushed motors must be connected to the Red and Black terminals only
+   * @param type the motor type connected to the controller. Brushless motor wires must be connected
+   *     to their matching colors and the hall sensor must be plugged in. Brushed motors must be
+   *     connected to the Red and Black terminals only
    * @param inverted the inversion type for the motor output
    * @throws RuntimeException if the inversion type cannot be converted to a SPARK MAX setting
    */
@@ -95,10 +97,10 @@ public SparkMaxWrapper(int deviceId, SparkLowLevel.MotorType type, InvertType in
 
   /**
    * Sets the motor output using the specified control mode and demand value.
-   * 
-   * 
    This is a convenience method that calls {@link #set(ControlMode, double, double)}
-   * with a feedforward value of 0.
-   * 
+   *
+   * 
    This is a convenience method that calls {@link #set(ControlMode, double, double)} with a
+   * feedforward value of 0.
+   *
    * @param controlMode the control mode to use
    * @param demand the demand value (interpretation depends on control mode)
    */
@@ -109,16 +111,17 @@ public void set(ControlMode controlMode, double demand) {
 
   /**
    * Sets the motor output using the specified control mode, demand value, and feedforward.
-   * 
+   *
    * 
    Supported control modes:
+   *
    * 
      
-   * 
    • {@code VOLTAGE}: Sets motor voltage directly (demand in volts)
      • 
-   * 
    • {@code DUTY_CYCLE}: Sets motor duty cycle (demand as fraction from -1.0 to 1.0)
      • 
+   *   
    • {@code VOLTAGE}: Sets motor voltage directly (demand in volts)
+   *   
    • {@code DUTY_CYCLE}: Sets motor duty cycle (demand as fraction from -1.0 to 1.0)
    * 
    
-   * 
-   * 
    Note: The feedforward parameter is currently ignored but included for
-   * interface compatibility.
-   * 
+   *
+   * 
    Note: The feedforward parameter is currently ignored but included for interface
+   * compatibility.
+   *
    * @param controlMode the control mode to use
    * @param demand the demand value (interpretation depends on control mode)
    * @param feedForward the feedforward value (currently unused)
@@ -138,7 +141,7 @@ public void set(ControlMode controlMode, double demand, double feedForward) {
 
   /**
    * Gets the current position of the motor encoder as a raw double value.
-   * 
+   *
    * @deprecated Use {@link #getPositionMeasure()} instead for type safety with units
    * @return the current position in rotations as a double
    */
@@ -149,10 +152,10 @@ public double position() {
 
   /**
    * Gets the current position of the motor encoder using type-safe units.
-   * 
-   * 
    This method returns the position as an {@link Angle} measurement in rotations,
-   * which can be converted to other angular units as needed.
-   * 
+   *
+   * 
    This method returns the position as an {@link Angle} measurement in rotations, which can be
+   * converted to other angular units as needed.
+   *
    * @return the current position as an {@link Angle} measurement
    */
   @Override
@@ -162,10 +165,10 @@ public Angle getPositionMeasure() {
 
   /**
    * Gets the current applied current of the motor controller.
-   * 
-   * 
    This represents the actual current being drawn by the motor, which can be
-   * useful for monitoring motor load and detecting mechanical issues.
-   * 
+   *
+   * 
    This represents the actual current being drawn by the motor, which can be useful for
+   * monitoring motor load and detecting mechanical issues.
+   *
    * @return the current applied current as a {@link Current} measurement
    */
   @Override
@@ -175,7 +178,7 @@ public Current getAppliedCurrent() {
 
   /**
    * Sets the encoder position to the specified raw double value.
-   * 
+   *
    * @deprecated Use {@link #setPosition(Angle)} instead for type safety with units
    * @param position the new position in rotations
    */
@@ -186,10 +189,10 @@ public void setPosition(double position) {
 
   /**
    * Sets the encoder position using type-safe units.
-   * 
-   * 
    This method accepts any {@link Angle} measurement and converts it to rotations
-   * internally for the SPARK MAX encoder.
-   * 
+   *
+   * 
    This method accepts any {@link Angle} measurement and converts it to rotations internally
+   * for the SPARK MAX encoder.
+   *
    * @param position the new position as an {@link Angle} measurement
    */
   @Override
@@ -199,7 +202,7 @@ public void setPosition(Angle position) {
 
   /**
    * Gets the current velocity of the motor encoder as a raw double value.
-   * 
+   *
    * @deprecated Use {@link #getVelocityMeasure()} instead for type safety with units
    * @return the current velocity in RPM as a double
    */
@@ -210,11 +213,10 @@ public double getVelocity() {
 
   /**
    * Gets the current velocity of the motor encoder using type-safe units.
-   * 
-   * 
    This method returns the velocity as an {@link AngularVelocity} measurement
-   * in rotations per minute (RPM), which can be converted to other angular velocity
-   * units as needed.
-   * 
+   *
+   * 
    This method returns the velocity as an {@link AngularVelocity} measurement in rotations per
+   * minute (RPM), which can be converted to other angular velocity units as needed.
+   *
    * @return the current velocity as an {@link AngularVelocity} measurement
    */
   @Override
@@ -224,14 +226,13 @@ public AngularVelocity getVelocityMeasure() {
 
   /**
    * Configures the PIDF constants for a specific closed-loop slot.
-   * 
-   * 
    The SPARK MAX supports multiple PID slot configurations that can be switched
-   * between during operation. This allows for different control parameters for
-   * different operating conditions.
-   * 
+   *
+   * 
    The SPARK MAX supports multiple PID slot configurations that can be switched between during
+   * operation. This allows for different control parameters for different operating conditions.
+   *
    * @param slot the closed-loop slot to configure (0-3)
    * @param p the proportional gain
-   * @param i the integral gain  
+   * @param i the integral gain
    * @param d the derivative gain
    * @param f the feedforward gain
    * @throws RuntimeException if the slot number is invalid (not 0-3)
@@ -249,13 +250,13 @@ public void configPIDF(int slot, double p, double i, double d, double f) {
 
   /**
    * Configures the PIDF constants for the default closed-loop slot (slot 0).
-   * 
-   * 
    This is a convenience method that calls {@link #configPIDF(int, double, double, double, double)}
-   * with slot 0.
-   * 
+   *
+   * 
    This is a convenience method that calls {@link #configPIDF(int, double, double, double,
+   * double)} with slot 0.
+   *
    * @param p the proportional gain
    * @param i the integral gain
-   * @param d the derivative gain  
+   * @param d the derivative gain
    * @param f the feedforward gain
    */
   @Override
@@ -265,10 +266,10 @@ public void configPIDF(double p, double i, double d, double f) {
 
   /**
    * Configures the PID constants for a specific closed-loop slot.
-   * 
-   * 
    This is a convenience method that calls {@link #configPIDF(int, double, double, double, double)}
-   * with a feedforward value of 0.
-   * 
+   *
+   * 
    This is a convenience method that calls {@link #configPIDF(int, double, double, double,
+   * double)} with a feedforward value of 0.
+   *
    * @param slot the closed-loop slot to configure (0-3)
    * @param p the proportional gain
    * @param i the integral gain
@@ -282,10 +283,10 @@ public void configPID(int slot, double p, double i, double d) {
 
   /**
    * Configures the PID constants for the default closed-loop slot (slot 0).
-   * 
-   * 
    This is a convenience method that calls {@link #configPID(int, double, double, double)}
-   * with slot 0.
-   * 
+   *
+   * 
    This is a convenience method that calls {@link #configPID(int, double, double, double)} with
+   * slot 0.
+   *
    * @param p the proportional gain
    * @param i the integral gain
    * @param d the derivative gain
@@ -297,21 +298,22 @@ public void configPID(double p, double i, double d) {
 
   /**
    * Adds a follower SPARK MAX motor controller to this primary controller.
-   * 
-   * 
    Follower motors will automatically mirror the output of the primary motor
-   * but can have independent inversion settings. The follower relationship is
-   * configured at the hardware level for optimal performance.
-   * 
+   *
+   * 
    Follower motors will automatically mirror the output of the primary motor but can have
+   * independent inversion settings. The follower relationship is configured at the hardware level
+   * for optimal performance.
+   *
    * 
    Inversion behavior:
+   *
    * 
      
-   * 
    • {@code CLOCKWISE/COUNTER_CLOCKWISE}: Uses the specified absolute direction
      • 
-   * 
    • {@code FOLLOW_MASTER}: Matches the primary motor's inversion setting
      • 
-   * 
    • {@code OPPOSE_MASTER}: Inverts opposite to the primary motor's setting
      • 
+   *   
    • {@code CLOCKWISE/COUNTER_CLOCKWISE}: Uses the specified absolute direction
+   *   
    • {@code FOLLOW_MASTER}: Matches the primary motor's inversion setting
+   *   
    • {@code OPPOSE_MASTER}: Inverts opposite to the primary motor's setting
    * 
    
-   * 
-   * 
    The follower SPARK MAX object is retained in the followers list to prevent
-   * garbage collection and maintain the hardware relationship.
-   * 
+   *
+   * 
    The follower SPARK MAX object is retained in the followers list to prevent garbage
+   * collection and maintain the hardware relationship.
+   *
    * @param deviceId the CAN ID of the follower SPARK MAX
    * @param type the motor type connected to the follower controller
    * @param inverted the inversion type for the follower motor
@@ -331,4 +333,4 @@ public void addFollower(int deviceId, SparkLowLevel.MotorType type, InvertType i
                 new SparkMaxConfig().follow(motor).inverted(isInverted), resetMode, persistMode));
     followers.add(follower); // add to follower list so CANSparkMax follower object is preserved
   }
-}
\ No newline at end of file
+}
diff --git a/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java b/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java
index 38743f77..9d1eb47d 100644
--- a/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java
+++ b/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java
@@ -23,32 +23,32 @@
 import java.util.Objects;
 
 /**
- * Wrapper class for CTRE Phoenix 6 TalonFX motor controller (Falcon 500 motor).
- * 
- * 
    This class provides a standardized interface for controlling TalonFX motor controllers
- * while maintaining compatibility with the team's control system architecture. It implements
- * the {@link PIDMotor} interface and supports advanced control modes including velocity
- * control, motion magic, and position control with the integrated encoder.
- * 
+ * Wrapper class for CTRE Phoenix 6 TalonFX motor controller.
+ *
+ * 
    This class provides a standardized interface for controlling TalonFX motor controllers while
+ * maintaining compatibility with the team's control system architecture. It implements the {@link
+ * PIDMotor} interface and supports advanced control modes including velocity control, motion magic,
+ * and position control with the integrated encoder.
+ *
  * 
    Key features include:
+ *
  * 
      
- * 
    • Type-safe unit support using WPILib's units system
      • 
- * 
    • Advanced control modes: duty cycle, voltage, velocity, and motion magic
      • 
- * 
    • PID control configuration with multiple slot support
      • 
- * 
    • Follower motor management with strict and regular following modes
      • 
- * 
    • Built-in current limiting for motor protection (40A stator limit)
      • 
- * 
    • Input validation for CAN IDs and invert types
      • 
+ *   
    • Type-safe unit support using WPILib's units system
+ *   
    • Advanced control modes: duty cycle, voltage, velocity, and motion magic
+ *   
    • PID control configuration with multiple slot support
+ *   
    • Follower motor management with strict and regular following modes
+ *   
    • Built-in current limiting for motor protection (40A stator limit)
+ *   
    • Input validation for CAN IDs and invert types
  * 
    
- * 
- * 
    The wrapper automatically configures current limits (40A stator current limit with
- * supply current limiting enabled) and validates all input parameters to prevent
- * configuration errors.
- * 
+ *
+ * 
    The wrapper automatically configures current limits (40A stator current limit with supply
+ * current limiting enabled) and validates all input parameters to prevent configuration errors.
+ *
  * @author Team 2813
  * @since 1.0
  */
 public class TalonFXWrapper implements PIDMotor {
-  
+
   /** A list of follower TalonFX controllers to prevent garbage collection */
   private final List followers = new ArrayList<>();
 
@@ -62,18 +62,20 @@ public class TalonFXWrapper implements PIDMotor {
    * Creates a TalonFXWrapper on the specified CAN bus.
    *
    * 
    The constructor automatically configures:
+   *
    * 
      
-   * 
    • Motor output inversion based on the specified {@code invertType}
      • 
-   * 
    • Current limits: 40A stator current limit with supply current limiting enabled
      • 
-   * 
    • Input validation for CAN ID and invert type
      • 
+   *   
    • Motor output inversion based on the specified {@code invertType}
+   *   
    • Current limits: 40A stator current limit with supply current limiting enabled
+   *   
    • Input validation for CAN ID and invert type
    * 
    
    *
    * @param canID the CAN ID of the motor controller, must be in range [0, 62]
    * @param canbus the CAN bus that the motor is connected to (e.g., "rio", "canivore")
    * @param invertType the inversion type for motor output, must be a rotation value
    * @throws NullPointerException if either {@code invertType} or {@code canbus} are null
-   * @throws IllegalArgumentException if {@code invertType} is not in {@link InvertType#rotationValues}.
-   *         This exception is thrown when passed an {@link InvertType} that is for following motors
+   * @throws IllegalArgumentException if {@code invertType} is not in {@link
+   *     InvertType#rotationValues}. This exception is thrown when passed an {@link InvertType} that
+   *     is for following motors
    * @throws InvalidCanIdException if the CAN ID is invalid (outside range [0, 62])
    */
   public TalonFXWrapper(int canID, String canbus, InvertType invertType) {
@@ -100,14 +102,15 @@ public TalonFXWrapper(int canID, String canbus, InvertType invertType) {
   /**
    * Creates a TalonFXWrapper on the RoboRIO's default CAN bus.
    *
-   * 
    This is a convenience constructor that creates a TalonFX on the default "rio" CAN bus.
-   * The constructor automatically configures the same settings as the primary constructor.
+   * 
    This is a convenience constructor that creates a TalonFX on the default "rio" CAN bus. The
+   * constructor automatically configures the same settings as the primary constructor.
    *
    * @param canID the CAN ID of the motor controller, must be in range [0, 62]
    * @param invertType the inversion type for motor output, must be a rotation value
    * @throws NullPointerException if {@code invertType} is null
-   * @throws IllegalArgumentException if {@code invertType} is not in {@link InvertType#rotationValues}.
-   *         This exception is thrown when passed an {@link InvertType} that is for following motors
+   * @throws IllegalArgumentException if {@code invertType} is not in {@link
+   *     InvertType#rotationValues}. This exception is thrown when passed an {@link InvertType} that
+   *     is for following motors
    * @throws InvalidCanIdException if the CAN ID is invalid (outside range [0, 62])
    */
   public TalonFXWrapper(int canID, InvertType invertType) {
@@ -132,10 +135,10 @@ public TalonFXWrapper(int canID, InvertType invertType) {
 
   /**
    * Sets the motor output using the specified control mode and demand value.
-   * 
-   * 
    This is a convenience method that calls {@link #set(ControlMode, double, double)}
-   * with a feedforward value of 0.
-   * 
+   *
+   * 
    This is a convenience method that calls {@link #set(ControlMode, double, double)} with a
+   * feedforward value of 0.
+   *
    * @param controlMode the control mode to use
    * @param demand the demand value (interpretation depends on control mode)
    */
@@ -145,18 +148,19 @@ public void set(ControlMode controlMode, double demand) {
 
   /**
    * Sets the motor output using the specified control mode, demand value, and feedforward.
-   * 
+   *
    * 
    Supported control modes:
+   *
    * 
      
-   * 
    • {@code VELOCITY}: Velocity control in rotations per second with optional feedforward
      • 
-   * 
    • {@code MOTION_MAGIC}: Motion magic position control with trapezoidal motion profiling
      • 
-   * 
    • {@code VOLTAGE}: Direct voltage control (demand in volts)
      • 
-   * 
    • {@code DUTY_CYCLE} (default): Duty cycle control (demand as fraction from -1.0 to 1.0)
      • 
+   *   
    • {@code VELOCITY}: Velocity control in rotations per second with optional feedforward
+   *   
    • {@code MOTION_MAGIC}: Motion magic position control with trapezoidal motion profiling
+   *   
    • {@code VOLTAGE}: Direct voltage control (demand in volts)
+   *   
    • {@code DUTY_CYCLE} (default): Duty cycle control (demand as fraction from -1.0 to 1.0)
    * 
    
-   * 
-   * 
    Feedforward is supported for velocity and motion magic control modes to improve
-   * tracking performance and reduce steady-state error.
-   * 
+   *
+   * 
    Feedforward is supported for velocity and motion magic control modes to improve tracking
+   * performance and reduce steady-state error.
+   *
    * @param controlMode the control mode to use
    * @param demand the demand value (interpretation depends on control mode)
    * @param feedForward the feedforward value (used for velocity and motion magic modes)
@@ -187,7 +191,7 @@ public void set(ControlMode controlMode, double demand, double feedForward) {
 
   /**
    * Gets the current position of the integrated encoder as a raw double value.
-   * 
+   *
    * @deprecated Use {@link #getPositionMeasure()} instead for type safety with units
    * @return the current position in rotations as a double
    */
@@ -198,11 +202,11 @@ public double position() {
 
   /**
    * Gets the current position of the integrated encoder using type-safe units.
-   * 
-   * 
    This method returns the position as an {@link Angle} measurement in rotations,
-   * which can be converted to other angular units as needed. The TalonFX uses its
-   * integrated encoder for high-resolution position feedback.
-   * 
+   *
+   * 
    This method returns the position as an {@link Angle} measurement in rotations, which can be
+   * converted to other angular units as needed. The TalonFX uses its integrated encoder for
+   * high-resolution position feedback.
+   *
    * @return the current position as an {@link Angle} measurement
    */
   @Override
@@ -212,11 +216,11 @@ public Angle getPositionMeasure() {
 
   /**
    * Gets the current stator current of the motor controller.
-   * 
-   * 
    Stator current represents the current flowing through the motor windings and
-   * is useful for monitoring motor load, detecting mechanical binding, and ensuring
-   * the motor stays within safe operating limits.
-   * 
+   *
+   * 
    Stator current represents the current flowing through the motor windings and is useful for
+   * monitoring motor load, detecting mechanical binding, and ensuring the motor stays within safe
+   * operating limits.
+   *
    * @return the current stator current as a {@link Current} measurement
    */
   @Override
@@ -226,7 +230,7 @@ public Current getAppliedCurrent() {
 
   /**
    * Sets the encoder position to the specified raw double value.
-   * 
+   *
    * @deprecated Use {@link #setPosition(Angle)} instead for type safety with units
    * @param position the new position in rotations
    */
@@ -237,10 +241,10 @@ public void setPosition(double position) {
 
   /**
    * Sets the encoder position using type-safe units.
-   * 
-   * 
    This method accepts any {@link Angle} measurement and converts it to rotations
-   * internally for the TalonFX's integrated encoder.
-   * 
+   *
+   * 
    This method accepts any {@link Angle} measurement and converts it to rotations internally
+   * for the TalonFX's integrated encoder.
+   *
    * @param position the new position as an {@link Angle} measurement
    */
   @Override
@@ -250,7 +254,7 @@ public void setPosition(Angle position) {
 
   /**
    * Gets the current velocity of the integrated encoder as a raw double value.
-   * 
+   *
    * @deprecated Use {@link #getVelocityMeasure()} instead for type safety with units
    * @return the current velocity in rotations per second as a double
    */
@@ -261,11 +265,10 @@ public double getVelocity() {
 
   /**
    * Gets the current velocity of the integrated encoder using type-safe units.
-   * 
-   * 
    This method returns the velocity as an {@link AngularVelocity} measurement
-   * in rotations per second, which can be converted to other angular velocity
-   * units as needed.
-   * 
+   *
+   * 
    This method returns the velocity as an {@link AngularVelocity} measurement in rotations per
+   * second, which can be converted to other angular velocity units as needed.
+   *
    * @return the current velocity as an {@link AngularVelocity} measurement
    */
   @Override
@@ -275,11 +278,11 @@ public AngularVelocity getVelocityMeasure() {
 
   /**
    * Provides direct access to the underlying TalonFX hardware object.
-   * 
-   * 
    This method allows access to Phoenix-specific configuration and methods
-   * that are not exposed through the {@link PIDMotor} interface, such as
-   * advanced motion profiling parameters and diagnostic information.
-   * 
+   *
+   * 
    This method allows access to Phoenix-specific configuration and methods that are not exposed
+   * through the {@link PIDMotor} interface, such as advanced motion profiling parameters and
+   * diagnostic information.
+   *
    * @return the underlying {@link TalonFX} hardware object
    */
   public TalonFX motor() {
@@ -288,13 +291,14 @@ public TalonFX motor() {
 
   /**
    * Sets the neutral mode (brake/coast) of the motor controller.
-   * 
+   *
    * 
    This method allows runtime changes to the motor's neutral behavior:
+   *
    * 
      
-   * 
    • {@code NeutralModeValue.Brake}: Motor actively resists motion when disabled
      • 
-   * 
    • {@code NeutralModeValue.Coast}: Motor freewheels when disabled
      • 
+   *   
    • {@code NeutralModeValue.Brake}: Motor actively resists motion when disabled
+   *   
    • {@code NeutralModeValue.Coast}: Motor freewheels when disabled
    * 
    
-   * 
+   *
    * @param mode the neutral mode to set
    */
   public void setNeutralMode(NeutralModeValue mode) {
@@ -303,14 +307,14 @@ public void setNeutralMode(NeutralModeValue mode) {
 
   /**
    * Configures the PIDF constants for a specific closed-loop slot.
-   * 
-   * 
    The TalonFX supports multiple PID slot configurations that can be switched
-   * between during operation. This allows for different control parameters for
-   * different operating conditions (e.g., different PID gains for velocity vs position control).
-   * 
-   * 
    Note: The 'f' parameter represents velocity feedforward (kV) in Phoenix 6,
-   * not traditional feedforward gain.
-   * 
+   *
+   * 
    The TalonFX supports multiple PID slot configurations that can be switched between during
+   * operation. This allows for different control parameters for different operating conditions
+   * (e.g., different PID gains for velocity vs position control).
+   *
+   * 
    Note: The 'f' parameter represents velocity feedforward (kV) in Phoenix 6, not
+   * traditional feedforward gain.
+   *
    * @param slot the closed-loop slot to configure
    * @param p the proportional gain (kP)
    * @param i the integral gain (kI)
@@ -326,10 +330,10 @@ public void configPIDF(int slot, double p, double i, double d, double f) {
 
   /**
    * Configures the PIDF constants for the default closed-loop slot (slot 0).
-   * 
-   * 
    This is a convenience method that calls {@link #configPIDF(int, double, double, double, double)}
-   * with slot 0.
-   * 
+   *
+   * 
    This is a convenience method that calls {@link #configPIDF(int, double, double, double,
+   * double)} with slot 0.
+   *
    * @param p the proportional gain (kP)
    * @param i the integral gain (kI)
    * @param d the derivative gain (kD)
@@ -342,10 +346,10 @@ public void configPIDF(double p, double i, double d, double f) {
 
   /**
    * Configures the PID constants for a specific closed-loop slot.
-   * 
-   * 
    This is a convenience method that calls {@link #configPIDF(int, double, double, double, double)}
-   * with a velocity feedforward value of 0.
-   * 
+   *
+   * 
    This is a convenience method that calls {@link #configPIDF(int, double, double, double,
+   * double)} with a velocity feedforward value of 0.
+   *
    * @param slot the closed-loop slot to configure
    * @param p the proportional gain (kP)
    * @param i the integral gain (kI)
@@ -358,10 +362,10 @@ public void configPID(int slot, double p, double i, double d) {
 
   /**
    * Configures the PID constants for the default closed-loop slot (slot 0).
-   * 
-   * 
    This is a convenience method that calls {@link #configPID(int, double, double, double)}
-   * with slot 0.
-   * 
+   *
+   * 
    This is a convenience method that calls {@link #configPID(int, double, double, double)} with
+   * slot 0.
+   *
    * @param p the proportional gain (kP)
    * @param i the integral gain (kI)
    * @param d the derivative gain (kD)
@@ -373,22 +377,22 @@ public void configPID(double p, double i, double d) {
 
   /**
    * Adds a follower TalonFX motor controller on the specified CAN bus.
-   * 
-   * 
    Follower motors will automatically mirror the output of the primary motor
-   * but can have independent inversion settings. The wrapper supports both strict
-   * following and regular following modes based on the invert type:
-   * 
+   *
+   * 
    Follower motors will automatically mirror the output of the primary motor but can have
+   * independent inversion settings. The wrapper supports both strict following and regular
+   * following modes based on the invert type:
+   *
    * 
      
-   * 
    • Strict Following: Used when {@code invertType} is a rotation value 
-   *     (CLOCKWISE/COUNTER_CLOCKWISE). The follower uses hardware-level strict following
-   *     with the specified absolute inversion.
      • 
-   * 
    • Regular Following: Used when {@code invertType} is FOLLOW_MASTER or 
-   *     OPPOSE_MASTER. The follower uses regular following with relative inversion.
      • 
+   *   
    • Strict Following: Used when {@code invertType} is a rotation value
+   *       (CLOCKWISE/COUNTER_CLOCKWISE). The follower uses hardware-level strict following with the
+   *       specified absolute inversion.
+   *   
    • Regular Following: Used when {@code invertType} is FOLLOW_MASTER or OPPOSE_MASTER.
+   *       The follower uses regular following with relative inversion.
    * 
    
-   * 
-   * 
    The follower TalonFX object is retained in the followers list to prevent
-   * garbage collection and maintain the hardware relationship.
-   * 
+   *
+   * 
    The follower TalonFX object is retained in the followers list to prevent garbage collection
+   * and maintain the hardware relationship.
+   *
    * @param deviceNumber the CAN ID of the follower TalonFX, must be in range [0, 62]
    * @param canbus the CAN bus that the follower is connected to
    * @param invertType the inversion type for the follower motor
@@ -410,10 +414,10 @@ public void addFollower(int deviceNumber, String canbus, InvertType invertType)
 
   /**
    * Adds a follower TalonFX motor controller on the default CAN bus.
-   * 
-   * 
    This is a convenience method that calls {@link #addFollower(int, String, InvertType)}
-   * with the default CAN bus. See that method for detailed behavior documentation.
-   * 
+   *
+   * 
    This is a convenience method that calls {@link #addFollower(int, String, InvertType)} with
+   * the default CAN bus. See that method for detailed behavior documentation.
+   *
    * @param deviceNumber the CAN ID of the follower TalonFX, must be in range [0, 62]
    * @param invertType the inversion type for the follower motor
    * @throws InvalidCanIdException if the CAN ID is invalid (outside range [0, 62])
@@ -431,4 +435,4 @@ public void addFollower(int deviceNumber, InvertType invertType) {
     }
     followers.add(follower); // add to follower list so TalonFX follower object is preserved
   }
-}
\ No newline at end of file
+}
diff --git a/lib/src/main/java/com/team2813/lib2813/doc-files/dutycyclediagram.jpeg b/lib/src/main/java/com/team2813/lib2813/doc-files/dutycyclediagram.jpeg
new file mode 100644
index 0000000000000000000000000000000000000000..4d17426568e46560e6ba509adeced724c01fff1b
GIT binary patch
literal 27170
zcmeFZ2V7HG*Dii23L=6?uTfD!5wQVEi|Akw)QAW{5TYW|L_|S?ln97OjevmCt8^k#
zr3I;?B2_|>5_6&DxZDj*4y6oZL~iHofl!ot2`!^RC8xj8tv#rSyn#Qw{Fm{kxT
zCrc%(KO4(Vh?S3pjgN&{3&9|WWdmsKdV_!a!@>&Mxsii&6W3<&Lg7}3m4%Ism7Q$^
zXgqk^9sE7S&bMLPjzgLo`Sq@G>~s(~>=}~CDSa~Ut>DF0@~$J-?|N72CZ>
zMt1MMql!w($BwI<`bFzBLi>!4{v`uLl+k5l^BWd7Ev>9=?m0T$cgDCp@P6d;*!PK_
ze`wgV@QBFgFQSr?Q&L~1rDtTm&Mzn|DlU0bT3u6HSKsii@qJr+$LCIBS9cF-Xn5r7
z=-Bwgo%A3tqq5$|bbxC~5cHde^@9?BDB{*Z-%U{n4?%^{WTs
zW@7=uW8;J15OvL8Hi6auR15KuEv#_1x{23-(5|?}a9OoX+)w_cUYbsocA&w-?N{e{
zkFv}Ppkio;DBUF9fHoVy!RW?s!OwRpYuM%26@Tn27WaiaWW;Z)RKduQR5f`63QtK+
z4?7x0%!$6dHs#sk8gqdorPa)x+&ql*_Sk>wj+=Ovf8ES(dy@
z*VyfLhm5T*i&ljTn0_LzSl#e%YwJskygE6eph*myI3X3Hbro}}UQ9W7QQY)iHL(%<
z@=IV!Cg*WU1JAG8i;6yjXumaVt-NiWlFIJbmO}EVs?x7sfH)qO^?R=0gV$AtU|Bo6ZJ!Xbs%bOQBocbXEzK62E&Xj;PK5=(olnF(2D
z7@`nKn`kFX54F@P<84R82?drm8GU8fnUL#CRaC}JP-+t>alO?4p-U=!l}OI2gY&v6
z%ta498W5}tGuBP-+nrFFugr$+i9W`1{usVhX~N7UoyJSC&p72#%;0MI>PigN4e-r7
zrK%cwx%`Vg$F3Of6Rnn{oQ8*m>IdvhNL~}@Yj1W~Mj{^}Lf=$5Jk%7&u{-0RF?fx8
zX_x7lZg$y}D9%FJqmN_d6JMOFPlSCqyZ7C&H9Sa@x@#kOtbqwhQqY=jBF1V0tnVP2
z!s#93d2d!&x;zw}m#-kFd3)ePr^DR1owuns`WX#|^G8TvORHMgH4YXQQ_fH=F&=21
z2C4XKxi$8?>&m5XxLPRilMrls)Vr1x#~;|5G*B%tF%-Y#i*ASE`5$ZTdY`P!G`pAO
zQ;6}j`@W2A$X-}gkKOabx^{Nv0=+Ihzr22jEPg-8P+a1z#d`H`ret@%rhGUftZbNk
zOZkva%X{JDvX`~rkL7(*z5Y_%Ok{fw0I
zstl$G4?bZ?xGA
z5=UciGS2OGjGv}Hdb0Ba;r>^d<21QAiaqsAAMMl}IjV>zm#Rw@6^C#NOb$mO7ER0lv$e7D~?Sd;|1-D(o(qwDWT&9&P4z
zNRs49Qzy~D2W<9-_gt@s>Jbu)_zWj>4_cp03@qnM#H3XRrPdAE>bmq?zPV#2>ZC!)
zv*1jJ_;md4q6V7sBVR_e@-ZfqW^U$7y^YbDrEN~Z6rV>gG9ezcTqIkFcu0Js7p(fB
zy6`X)YT#sSnh=x!_~7m&u{5VwlY7g7=5W;h?zPvv^%eT3-;|WM-MVc*!}#EIbknXD
z?|Aw5UMO`k2JfdBbQ|8oG2C2jcX$u=bYzlqX2{#pA<6|v#DIBBJvY-#E@d-G^6>c;1vpB;61*_iT
zZ^IJilIfyDXwFJ_(;R<8b-6U+KyUO
z*gaW6Ra#4FA6m_+SKSN434$Tnnv8seEhPGu>xI~*P@>c$k*%i$yO>ZT@0cOMB6=^)
zZcbqcSNp-|!2Fj+%d5@rHguJ-FebCj$2(Yh-Okc8hJc(*-c~~7ndU$@EpfZ>?YmNL
zbI4USHr%Tc-1ug0b8V$MWyhQ>ruprP2VZ60E3Ukz*!;($yEjRA;yen!4&-Hcp#!no
zc7I-$w2C-1OQn5c+=S2dtT_*V<9!=1hV43hKHdj%k(oYp^gfuPipO6%NH|RK=O0po
zqyxjwR?@SU`ff8JgD{f)Lj^Uxgz@R>*$dn!uBtp^>*5ba7TzJS>@N3S)}RFKc~N4z
zkEZb}+t%ZvtlbOuM*MDEi~D>MMlQGw!wa!Iiou@nM)x
zq)E$gXs7nb!M+|HP0TK~W5*^r)ufij4HmG|Wn^R>nqyY&Z2!Kfx`fYe#o9Bw_9u1}
z->%_yKx<#n{-0V}*6Vq6<9ekYkzk1qJaR6*dmJq+dQ^2{t&;23LVVA4YkSITkfF*C}MdgV$LitiBL5Kg#{l|3_C
z+E<$0>q}k_YcRxC=t@g%laVd^vbQH;Nh7AC-~z`}H(tuU9E?6=n>)6Fm2D`VZ_@tW
z-5rr1jS^v#!&`zMVt1Ii523r$>e(*MIpH3vp)FD{@Ooh>j~q*fTV_T9?sI%>BFEK<
zfjkaqB!BUQZdvEUH-3XmXmdA3p+*elyO`k7@Ul0XkH2HkNn>Qepxk9J7Ct2Ws1V6x
z>si@=Jl-BYi>4!`n`AOb3-cNqS`|Rl^@z)x$S)~*~s-mfz7{zl)-pT2SLP5pJ&p9~>
zT>Um|`;HxG@VZkz+*#@OMfQvA=J-QyM)c>jQ$sMn7Mdo-awx!#%~tBm1xT8-MU9fUzl^A5f?sbw52IW4{EWp^33Te;dJ?8qUC7!|{f
zk|B>(i6LrWT$eHTYSODeb!{_Ub&-h4&s*A4T2bvOr8_q;s1Z3w_@$t@tjtcq;MGl6
zxskx*XX#%phWFi34fZO5`3`Jg9Hy8Xx9%i*?CI^N*y_Fs>SVK
zwq||@=+Xmj@znbhYgd3CF9=wDX|$w!?icw??*)}tsttINjTP|aYh*n#;w_G+=aY|u
zLbCFT$zz^Xk6W-iWhe&Qm9!^J=!UyASgyBP&MEu3>1Lw48l9FCQ{PBkj60k#<}+OA
z&d21ry=LGz#50A|X-WXx~
zBDkHXn?&1A@pBo3iCSU#)a>kXC&gx7zqR=wzG_(V3Avp=X5pa<4(5Oj~rI_8LE`l
zc$j{*Yo==Z17hdV3#xGrWZx=}J>>Jz>G6`3Or+rC%7UNq-N3AEK2=(cY94G8W;iVY
zPT8cVF&9`{&+%+IKsvEmYY*x4Kl_=4jQ6rM$wLBqT7SMS^823{Y+*}JS%)5r)j#`r
z8L>F}Ov#GHD#;&`{WmXj%T%{F91YkfYq?qJmod(#+6(!t<>WYLcHi7w;u&*h27OT(
zU0rESkACcCH;hbRLajk&ooF$gAe(%zFn>cr#^BA1Ir|o6+oN$uoQp$C2;@SK!%V0p
z-j)eHeY1}Y4I!?(fq@`M
zFd-t6p1HL6N^r5OhvFt_Zz0BaUzczR7glv`?pTjh_t37~f!l$rS^tJW7F#K1(ug1zNxPS#G79}kI0oD<_u&pX_z+@7RBlby3z333y)&bFCgch*?nD{FrH>i{&)fZtp5P%Y<@6nb5ewq|>PRH8D(7i2v(zag`hIX85EXS1pPh0h+`i
zHc?C90?Tg~o6(I2sE^7p3C50oOtf)Zxnj~p&y!cB5eK)moxbY-RrA=>2dMU$wn{4s
zKNH$*+kjo_$I_)f&semkWEf^$Z4i){f7XB&;D7!yRX$!mZ8PGp9z+JvTuR!lVSKkIERF4dJV3AV%0OymPlanZ(5z{!rYeHTt^mdf|Aq-I?(4}<
zMnhAlLjx}AB6Xi%2y#=pp8hE-%E2COB2}R!fO_b@{ogflJEth9CV0L*Ex7;a8g}Ls
zTK6|ij~%%`m)6dNY{ZuDpub__wHcP)!A!`bdhNr!#&dxSvUrgdukS9lbvN6u7q1Fs
zVjY%f08440u?2OPU6@el?gk2(?f_fcnu*SDriC`i&{wgGN6g+c
zp%8s01RKX78Hmk{3MMqukfEm~I4m{(V^=OJ>lm*quA?5GVDsvfF-9qN28B6;1vyZ^7F=%s6xZ1rsve4VJj9
zj;RlDOC0EzCEXF|;*)s&Oi)(EXC}l?d-nnRB?If@p$UYtY5WiM3)C=YpWGE}=Q*~5L;&~F6At|mPn}bN
z1Wx`W0Uq&T8Cn=@z4>^?$HFF$ChWZK40h@pmd;g*4ve2a>!$k~s1$z!)p9Zu+L?h3
z@<4dQS0zr-c$tun`U8Jiyue@N{JXq={@qyotCjFK?5tfbWe6@Hr+Y!Gi-R=csTYX(
zUQFm-?HXc1O~RSFEZm6>S*%L(1d=KLQ8MfIF2sL`7nFwzw($TkCZ>Q*+2V7N9)}%?
zu!he9J0US@8AbDtr+{U#8asasy)?^&Zma8nj{G!SA*C1Fly#uh8|;4r7OW(9%n-C9wD?4(hcl4y`@OTv$`aIxq9`MB!5}x_Y1E|nE~*RuO%$i
z!|4*?RW`OISiWpvMt_)3XL2Z8xeRc~&W)-Fzyjb8}A^+2aSReK~1ETPe=U`4$
zV3#d&ZI^)!7m5b|LEl?nY1(Kvz_a-fdTogbbw|9xcjFmn-0^gGP*EbP5Vk6)OS7F<
z6J;I8e4tuA&~)z41Hmyg1!C3I2#(HapwKX{>QQ8_A8c*|=Y*HZD>ZGtsJ-P@26Tg}|8||NLTRXQ||D0&i14bm^P1p;x*B?M}PJv2u9}$YpfH>>8KCm^~A!)uTtw
zQ5jwVt3Z*>2`&~2cGJ_D(AY;fnLmKK6TZMmW3aom|M-#s)`I2%ES@9=kR?saT^^Vt
zIEGsd6Jp_=21=`WEuO-+pxIUlhpqbI89=mif-N*o%Do}`jkYRZ$NaBqH!a%wUKR|{
z7&KXmOOU1AAQuk-Z?#s)Z)H$ze`h`OW;4-ZH4@~}s&122}aj?oc
zglD0hoOX~!`J2G~HcbWmPahB;jZ1B&^pau^%PXHM)m4@C+q)HG#2pi-6NbLjY8EGc
zFitEiC0TUOZ04J<*I)NwGBdMclG=((XA=L#s1Vi{k@$nao;#2ITPOeNND5$W;HwL~
zGVhsowgQ&v!YOMQo@n)1v&S6_hJJ0Kl`>D^1
z&&S&KJ-d3b)yzYNU(FO{=vvv2NPbjZ!*S-Kx7`88HL(2e?fP-93D(mLS`^Cl1|==E
z+oZSk_tj@@i{LuEOP@Ht<^CnZXPGJn(ic@<#(%LejLJEG>1+Nu_p$dqyj~0?jQKg-
zxt84I{JbZjAsjxh&WuG|v_(=ChF9ntgcpjL(2(4OjT|w;?zr;ai{AFsv;c{yt^R?w
z=XTSFN8_gnxPe6h^rrzP^d$MGkM{m93H<-{qHA%)X%Z6}S;vS;JiCSjC9B$F
zZ)YJ|=&e{G?egLM{>_E(dd*FPwF}bm`-m%Egx%_4`b^jY=<{Q*kmk=y(=$zpUip
z^$35tcj^hhui?M_vV~K^lKL?LRXv@NsR^O;lUqXMlvq?ffwkORDM?nY$8%X{y`*@6
zSi{3Q-as(09bj(b
zqOd=-0ymBS8>aB7>B&?*pxvHh2Y1k)627;HR4LSpDebLUP^djqSD$%1CYcsLS(H0a
z3Qd)kMakm#WYe?1Ooy+@CO5A6#U1IKWq7a-=FpWzWeJ~oxCIGj}7Q&
z_eTVyg{(QpbzVoOzSD@cEyBk;#dpI7XUdg(UZy7XW$tWrP>nr*2m1P)KAXd)?tGHs
zO~Ue%aag3^+NMqH^JKCfqY6$(vQEv5?Vm#kOk#dN`t|M3Wyr(-#BOop$mHd7Q!9d7hYCHpvL|_WwZQF?4}3l4
z)eEQED|a$F`**gL+gn8iN2g%!C?vqtHOr9Wxstj9+_!;$3F?HZ(2ouMIWMZ%uy)!$j(Cs
zO|?0DUXWsqC*>7fzKHL7biy^zXYfJPy476vR#}RVVr%P^!|F7<_3T>F%h{FkZWk`(
zYdevp<{ZeCb3$wT#HB(z`LlBu(+&sXUI)CcgzB=XJ9ek1>H;F(o!E&%_LV>+E%{&_usnaI?
zS>rRQ_i`@Yt=Ga}ulNQ#U+0f^F%u_nCI>hiba>8rxHMAr6`LEn+)mxgn8FY0I-3i{
zekDB<0oIX084R;V>}X-MYjE51Va!j)Z87Vatu13)PNV%a;QvQ`r#A!jMNpEnH7yun6v}N(QB3{Ji&_pNa$;aFCRi*qh4;#gwkb7S)
zOua%8ZZMO{BQruQ@tRIp&C-#+G%P;lBH$d>WqQhKq2@*Jz1Y$S)KPo))-4
z#_W8keju-0f5-&RiE-5{W5;#hT~0+#BnRpDHykh
z>^N4)ag!5)>*$QRWQ|Ff+G(YJNXC1*y3A(Pd6|EQ2hSvX3`@>guvaQ_Af6d&?&Tm6
zeH1?g?JCO2(Y5({z_MLl$mrU&4k61c!^~Yo|K0aHCeRv;F7#G7iJO9Ov9vqzfqqxhVk^f}-$?k%JG$+-
zbX(u}r=
z!R3r->eJJlmed5gfNaWK{B-^xOo|eHX^z+LddHp)r;QsvqD%x2CUA2D_f4-!gQfkJ
ziqcz;MXGnM$%?Se<73s_vly)^G69NsisoXD_+|2!dEE5E-AoHlu7?Pov|QxJEf9uS
zUFN!O*%kuRme;XK$F+%d?U$e=^QVT|zdX?%7uCHQHypRLSX?{cDa25jlTY(MDv7;7
zj`!PHtn}K)Qg@2!L
zK=6`d!z+Bo*RT(MU%XCc9Aj&_uqA$%?EufwEZ%LD&4clerEG(g9;NsUZx)?awBL#rxi#}m-}bEqjL%Ee(tD}NB{+**GlT{1F48rOa#hrpmv|CE
z%(P&+C!p_1)03yyYW7MzEs+RWxqGDTp+*P=IV=NQ;5B$2ioc`L7@+|50+rwqgsVk`
zGkT0;>to3$>GHbU%gVhr89Bl{hSYcyKFR%0`e`wX3M;`#aZ54p&Xr(EAJzw
z&+HZD^*Vp&*gKBiVB#{0a%c$2fyi!H@3!~J7>ALXtI;^@M#UU^P4a#fgZhxncLI~b
z8_bTf?YOB|B6xMvs`l~4V}jit9|zcI=H#-Wv;al8K=tbhqT$6;8^|H|?u_2B>t%WO
zj@wGKs&}FYP5v@#b!&#vIHvr~Ew!5oNim_mdbwhHA#ImqpIo)s@z$$bn@H5pkCA)t
zd-5N=G101_D>p!F1_%_nmvf^-e$wYc%DfO
z4T^AvHAA_$_*G}F<30H=cZ$rtNGRv`cQM@RR(uP^b#!BRFhNvfE5@*?oPK=fdWSvh
zG75bdP4g9Lc!Mw2JNZxmg2v@Y3SG!*qDq6mA`=p}$tAMszQ*a~JF9w)vHL9c_$Rm?
zTEdt4K+V2{TPA+soge}aZEt#6ZeZu!*{L$FCixm^4^?wmtb2GG2CdbWlbusAv+sSs
z-1ZHxbe*IOH)`ky^HyNeW>m>MD?@#8VOf;(L#k;0@!C}>bGysQyOf&xo?LtC-kfv8
zO?6IYW?N6aPYQ-mV|>#3!WD#6YDOL_>Y>CT5%<)*FTB+ckGV$MQ!ZhPZV7X##mdyR
z3N>pDY_IW9CBn)3;+Nh1DtCCa!1z6+KfbheSM2YpdonEl?1Fl&-c=@4E@!elP9`&<
zNAQ|{`TXeV(g>W#&NG6qsOXmii5G0k=^<`nZZc$Z-ICI?XxrD7X?L_oR-bPIk&u0i
zT`iKlo9WMvX2GT0>?qztFu@a%YeCN*>pI+Jo3;#pyIpE>E98o5kC!Z<>MD49sDys%
z6p=Dj{1_y0O?OzNQPTFUnjTqs2vLpmckVw<5i9EPiA$PZ5p(;bqG}QHG^B+8-M+i(
zSuMlJ04$$}2xj3t#gjx=)v2s_y>&vUDD-NX#J4ZH#l=@Ha71`Ct27Px0vZ7BiCA3J
z<=Z(oDly#;vRpPjrl}AWsLGW5+I|?t+1;Kr>vNY<+OGd5++LwJ!>{3#I9msVHC}1%
z#*_BoNpSBq;-ktP)d-5CmvT
zc$?x?1(Sb{s6eo0vc9&++S`C
z`@@URur)^G1B(i1CNwaTOohR~26TYLxJECI#5R8h%W24>mf!+`rWhOqoOr<8r89Kk
zq-;m~HDNqhH|F*JR?k63?+3!zBn|s+O@inP;LlsR8^cglg*|3Sziusfku&0b>G3Gy
zlKh>`AizoxfBCPJ2>V6<%xERpWc*h?l4z)H?j9II
z)Zru!W8&+UrK>^)+>?4*#ZE^ZRY%=!c~gJdS>k48YP%!lXpTx}eIxt*&bKXwyukobr
zpGn~2-@YOI_{rb{)Qo1zWKV4DBjYWLNT&S>wW=I_C>4B5u6hr`cxTGX7#{cr
z8ZJo|9}5hbA3*!;GHS^4=7blcg
ziaIk$)Df4jMjbDnJ~SG{svu<>Qn8j4{DRo0*DHkv-{&=m8JL}!_W8~tAyNwmqwq8E
zlnJpMM*IP9SLvQBXV_FeK0I`KA9BLf$W7i&-zL!B9LFD+=zXR$Qoh;!_rV8Yq`x
    D{-^v6TN5R(4nVwSJLwbe#!3vnBRZdOzDE8G(1I*?wtx&
zuU{VwpC*-ixU5*dnI?5LG2XTTSV`J~upWvg`==*49slEB`+Rq)iW0h1&~&yO>?~j#
z_`rUAu+~GUmAxAAHCLu`fKW_=6P~_`CMW;+kh4#AEb_j@)9IT`=;{P^k*5%>KynNL
z><D77@90{^4hMx-MAo4md6sq-Ur~Z|phTw>AoMQ$hF`hd&LaQ_D
zbLp?#k-~>|*Bq1>2CH#GSHtQ^cv|zkhL0Fe5xHs9j;zmGsvw$L?5MXFdFhZ1l6cOYtw%n6@S&86SncIj|~O
zZoLR9cw7-9OpTLigTELYCJel)d`-i>+#tXOh$n;JcK|<-Pkw(+Xw^*MeZf!%aeNdR
zNe#h{FQ9dKJ})2}2&?-TU`u#cfi7h`aSd=n?5QusTN#!<51Ei#EfezdIF$~9>o~`~
z;;Hh|jtRVg!N9vJMY{(GO7P_@V%t!q6+H$%;yTy-z4v&uuK)7A_!Z&=tWin`!T1Wh
zjGZ`Z{8x^|?_7!XU%<9`3bqJSZ?Us7j9|gV3c%db#=ct`B&>u1#sSyzYx|WtU@Y#z
zTpptaR-i_4spIPrf5-Kh&VQck@}o@Lh*WLBT3H%|e#hBJm#q^`4p2pk83=nZ
zcldH5J~bMwk4G6zgn3m=`vl;wZE-!Ou%d=c!5(?rfF)dWw^)Up<&qUZ1-icdN6V~c
zlJ^PN5gKq8hT;d~S9Su1$Y^DRWY%ar4m)o^<7bSZ*VKq@HHEIn23F20C)1iYvoKcu
zn9w$w@2?cQ|1hoY$59nrK+Nm_+ejjyef1;h5^@s{e!{@aje*&KR>oO!e)Edx1AG%q
z7f@n`;=c3#f_^8W+W$^OT`c=6O~v&XY~>1)f*QqK_NDfV(~$ETk&+YE91o2Dg*aC6
z%OUG%tipchex1G1hJnQ$Z`Mq%
zV11mgy;97xF1_Dg2J{EbP~-8IRKvC9a|}dV2cTw>TQoGl)(=cvve+<97Ho?==T_*P
zND%TR!bqm^U0nnP+9vFaI(o79H}I11zx5qrSxMmo2rr|-RvDuK+yX4v5qH@>EZ1t<
zvK<(a-uieXJ>42P!!rCWtqS~~^~#X+=>3Y?YG;OAn@rA?wlh9&d+nFtD0<)w^lG$h
zhj*6uThwyVXE~dAeXo~F5z3Oj%c$1(E9Y*PH>Yk8;CY7n7;p3=GX8WgCaVri5A08X
z17Ae|v{PWsb?|H2^6z@T1q9pimi)0H3SM7DrBIhC1Mi>#Ne)a5z+?aMMHsn7O5-@lb^cu2~U8h`H6zz0vx!?>(EZgbBrhaVF{guzmwDyk=Kn^B#W;^
zj{Y5P;qU5w{TCkvw+4#2T$nMB9Vel6iA~soi1-GSA1&Jt?B{bdz3#
zj-LmdvMQiJ+m5Vipgs74^>IH+1`yjP^}a{O{oe
z_o#0WMsJ><7qOtxACH$PL#etzb^`rC`(3jsSRuLytWTyl$Ep8E!Q0jR}ah3o#I
zf04z1(T<}Kyc>;305u8}Twn#I1SPjzY(255(0v2TiS>rD{tK7t#%G3;M|2Awa2fyH
z8{zeWws0nl@K|^JkpA-Bf$@L)vt!DaBY`cft!GLsMU!_v6!JgofB1Xum%5DzKIdhe
z(TQT!R2h{`XJzAaV{^{Xt`1S20QcS?M-PQ3f%@sPK=55l;_H+zj{1
z|yhn|@?vq7DY9Gy-mR#YMrP)s2y(8CDsqALl?y;4!
zbHS)0M2ul@$a=Y!)_xWt@qi~~jSC_xbaM^#XaYs7Sq`hhJ=
zk}$1KH`k#j{pu#Qv!#L|cRtvc6wOljCFrajmB;B3iUdJ7U80&@5@mv>?DT-OT7E`X
z#OOgt6CDX#$D^C>1wjZ8TmI#fz+6|Jvt?8Od!)@W!Llb#d@jOsnA0-4dadFmUVvK&Rru`K
zNmmJjbE-oMK8mgZRJ$YS_HW;gQhH3@4L=w15g-NdREYs6G6XC(WIcp!roq!*
z%}u$n)tysuE^S6DBi&bd;Vm03U~a9o@wPHVz;5;tF9hRTPrU)
zjI>dbW~;$zh~@)_IpdW6Ma$N1>zDcWBm+doo;UL1!yFH3r!sTHlQ4Ll%8tuL
zIpxYbV&YGz5TG0-~xipy<>S+fZe90VeX
z-nR>EXJDtaJ|9inb_#RDTw0q&JUZ?{oGe=fc_o;pIP@VP8oR~zzH!PjVQRSexhmq*
zamhDnW|)DRQV)WCg4vYP1zVK(StX?Gw#o?CHXiuEB&k^jj&R4q@b+u>a8trPW||{x6x8
z9miQO
z*gASJWBlD9xi*G+K^2yAAJy3tRDeOkrZ%ItE?=SmnQ-=wvW*Egy&+1ggtL~hEc_d|>z$FA^(
ziwLRkMJqpnuL66Q1&+!N^JETxx#3Vb0IKg>6;0?Y^%q$>Hqw@2^P&f8PDpTut9!6fi1k=#mWCjS&q9-HxykA0
zle)C{H(ZX~zOC!+sS*kAp3>%Sj+)9Htr|*{QM917vy$>QBc0vOK9B5nlmtG_JTm_V2@1xdYx7@i{VPKxFF9ofV;vs1q0s1}zO
z3ybHp?Kn0d%81fP)v)2OOKY}paCw=VY-e}3Btk~7T#l!~f7yCF;)+UE`nbr=a)o8!
zL9313SuZI}@_p26TKqZt>*LXHvVA?Dg9Xglc{D)+)7U^)FS0|;b?ziu(6;6ia`Sj=
zmvP?obfqyvm2GTNZZ;LajUHLKt&g^c;=FwB{!!eOj@w)MCbKqnX|w1{=L&FB!_2;H
zfSSVyw|ITbb`lj8wos^5MWJY$2VzfhxBED2>!ClM)*E0ik)|h_pm_;4S$LtzC?QYR
zLG}CXhGiweJ@e*loL3v)J{`HC&{-L=Z&PFXBfeyzUoMPgaIqxthQjUacCKhfz6FGT
z$E`anTffmFpYcRj-q=OggatWa^Kg$$&QSC<-lQWK(L=5iw?}SHUgx;Zo5o#3pCoCh
zO#qfqxdmn~+Cm`j1IEQ$P-b&+0
zw<&5hEiZ~Jis=1Z5nD10FDKS@g8PEluZgCyBfHjeyx%4YmJ`=D0*;S?6?V#H7ht{WC(;2
zeE7*L44)D#d8a+&z?<1;%NfAxjy3$G+799){^-AysSO8E8Dp69&NBRfUCxgpkba?o
zSW4n;7>zTU2?gXH8A_{G*g(Nn8F!{8k2@Im_f~OV^h>>-CY^U(PUph=%#OfqvAxCJ
zhqKiBgN|PcOS^$zE|nsX@B-GA2ilT57WW3##RM8$Nxbs(4n{HG3`tc1O*shKGgZEx
z@>}O(l&Rz6(jMGc@PMN88t?0K=62gPkU(~*^B;M*q$Ri(benM=8{20O?4MP%3?Aw?
z^f#dH_vKFUI)c+Pi>Q%~T2&xny&d5qG`+c;Vn4@4!ej9l{fd?rZ;uqOXyZ|AzKMo{
z`%o1LVQs|zJ|yg5?U3xJa228QUgQ2A1?et8W=Sk(88CY4Znr*Rto||qO4h|W#@PGY
zP)9NZkTnBqLSULz`4K3kovTQj_|?7P`N1HpHGe(wC(*S6dJHcho-w?EU>}BpTu4Rd
zpGtuLB#5N%CCZu{n?-_X(OzuLDGXZ?e#e^ecktPfutF1YhuSx$CLoui4}WEQX0ht4
z=cPXNp0V3aihSO8@SLk&+oDL$tXp~k(7g52koj@9|r-klPn_pU6c2mhBc<9~Ev{qJVUi^(2k8H*POS|gI;y?#hQIV+$2lUV9Ft{FXrcN#5
zMsS;@L8K9vO;~&>4@g6YzWKP!)(?O-?F4NCzV<|}#eE9{L4ZU>N9HL83Q^
zf~QRqjs!?Dru1nF^EZ*xQy^En1_(HJem(bmN^k@m6N9IPZGkh&K7dkC*@iECsbA4^
z@Q=;%KO{DDB>khV{M{tyzn;Zl_KAZ|kEVsm5~h~zM+!f?0nToy@y4r}I3_K5eEq^H0D&k{t$SyU>?C5bdn~
z8^Ey|OqWMp8c95}ePWf~4OEuKd;}W9&b{80s})Y!kjSO6=d+E%y{Xe
z@tL$w;INJyB$?3o0{&;Xpu!lR2=iw_ENE?Z{lJAho9=ni8?rP*X8=?@1=8Bq)U&#Qm`jJS)r2F}AL0FE!qER({lqUj-g|pk
zU%K8?y%0En&(4oK$?RH>QcTU?94Y@{4XlFumIyoYr8W4U8?gRm{&PNgQO`wCd5wFH
zWESwSg*%+bDwo11X1J`*EfwosazD#l>pkn--RG4Suy!q)W^q#HL3mI9t@#J<$iKM@
ztY;V(0uO%M`u4ln$Gg#|S&wAv7_D@7ljK?6#PMZa`9IO;t($?)`37uKH>~_8dbe$!
zkmm0S%d*WIpFdjpSY7pN<%)IN!)?z0lUjW6{ukcs`#(s@1Gk#|F?Da=|3Z3w{R0#G
zKV^H(|B1eD1uhT2Z2v(Nxbp5t_&zaUJy8ujZ|XoOFh*W~yP5vy*YtlZh1D{*|J5A_
zI?CoWMJC#Aiv%u!UkdCvux~uO|M{Ex55i~o-}*iPR%r%rOX_d$0lI2u{>`qJ`Zdvy
zUe9pWDRx`8u#JPw$*uPFJ^P1oKcfG&zBLBABm9rHv)6}nlNX*;esL>zY4-_+VirYi
z#@AgdlMN;;44TT7YZ+Z$JNHB%x3rC`)z(*P3u~ut4>N5Q;bRqjB<~;sZ5HhOkq9i!
zAO3Z%IDcqvhdL`-k$*
z?Gq9oPb}BwFN}!)u)YRZmpXl$35TyE+v!*2rm9EbTaa
z*7eiv(7Npu2H*PJY4zt<)<1ChO~d%u|8SL!EO6-`JP~fGKO9nF{jYJ?_kYaJz#|2-
zFV~~Q1aO%}YxP6mj)g7f|F8nrf!{ZSa18|R{f*v@W#igwcqCFu8j7~t!F70`@~~Yq>-BVjyKn7(b6zedAFHu
zN|V>RgI-$Yw9jx)!zOx}nFLv$;GubLM=Aa;W!u3+!H@rO>S}
z_oI2V{?OhJ+y8RkO8>{S{zv%waM9^uE43yq)!tjDd}MLUS&iUci_qRl+m6pfOWUBn
z+KLb5f0;|)|6v1mjr*=I0}f6gnH2~u7r!3@?&!&!|ATA)!^qz`oB!1Udr?fL-z>NP
z`P=`Gsd74MDck=etDn98f_WS;jxs=Tl#f4-s$nMbC(o`w59~}nP^Z+D?%Ph=Kfkj7
zgTkmAQA#pWV;eIZo9nLp;{!Gf6yAcW?1!K#`()j@?H^z57qW@1&snqKZu>lg$2_7-
zbr`=t+xr=%zFq&aPU7Mh&$w7+pDi0FUe7iWXi=$>3AIV%o69xjMR4ZX=XF9U&aUDyXm+h+B>(J2bcky*2{EBMpOx6)?kT+6;&1GPJ3h~mtVZS
z@X@0W6|-i{c%WR*Suul;6vbp_rKU)-~YkO54?8)SecWQW5VBVI{)*l{vVG1
z(d3QX7#pF*L?-{+;0JFCm%)This allows robot configuration values to be declared in record types
- * (which are immutable and self-documenting), but automatically backed by
- * NetworkTables Preferences so they can be updated live from Shuffleboard or
- * SmartDashboard. These persisted values will survive robot reboots and be
- * written to flash.
+ * 
    The Preference values can be updated in the Elastic. Updated values will be stored in the
+ * flash storage for the robot.
+ *
+ * 
    Example use:
+ *
+ * 
    {@code
+ * public final class Drive {
+ *
+ *   public record DriveConfiguration(
+ *       boolean addVisionMeasurements, long robotWeight,
+ *       DoubleSupplier powerMultiplier) {
+ *
+ *     public static DriveConfiguration fromPreferences() {
+ *       return PersistedConfiguration.fromPreferences("Drive", DriveConfiguration.class);
+ *     }
+ *   }
+ * }
+ * }
    
+ *
+ * 
    In the above example, {@code fromPreferences()} would return a record instance with the values
+ * populated the "Preferences" NetworkTables table. The keys would be:
  *
- * 
    Supported Component Types
    
- * Each record component (field) must be one of the following:
  * 
      
- *   
    • {@code boolean}, {@link BooleanSupplier}, or {@link Supplier}<{@link Boolean}>
      • 
- *   
    • {@code int}, {@link IntSupplier}, or {@link Supplier}<{@link Integer}>
      • 
- *   
    • {@code long}, {@link LongSupplier}, or {@link Supplier}<{@link Long}>
      • 
- *   
    • {@code double}, {@link DoubleSupplier}, or {@link Supplier}<{@link Double}>
      • 
- *   
    • {@code String} or {@link Supplier}<{@link String}>
      • 
- *   
    • Another {@code Record}, recursively following these rules
      • 
+ *   
    • {@code "Drive/addVisionMeasurements"}
+ *   
    • {@code "Drive/robotWeight"}
+ *   
    • {@code "Drive/powerMultiplier"}
  * 
    
  *
- * 
    Defaults
    
+ * 
    If no value is stored in Preferences for a key, the default value returned (and initialized in
+ * Preferences) would be the default value for the type of the record component. In the above
+ * example, if none of the above preference keys existed, preferences will be created with the
+ * following values:
+ *
  * 
      
- *   
    • If a record instance is provided, its values will be used as defaults for missing preferences.
      • 
- *   
    • If no defaults are provided, Java primitive defaults are used (e.g. {@code false}, {@code 0}, {@code 0.0}).
      • 
- *   
    • If a supplier is used (e.g. {@code DoubleSupplier}), the supplier will be invoked once to provide the default.
      • 
+ *   
    • {@code "Drive/addVisionMeasurements"}: {@code false}
+ *   
    • {@code "Drive/robotWeight"}: {@code 0}
+ *   
    • {@code "Drive/powerMultiplier"}: {@code 0.0}
  * 
    
  *
- * 
    Registry Enforcement
    
- * Each preference "namespace" (the {@code preferenceName} parameter) is bound to a single record class.
- * If code later attempts to bind the same namespace to a different record type, an exception is thrown.
- * This prevents accidental reuse of preference keys for incompatible types.
+ * 
    The caller could specify different default values by passing an instance of the record class:
  *
- * 
    Example Usage
    
  * 
    {@code
  * public final class Drive {
  *
- *   // Define a configuration record for drive parameters.
  *   public record DriveConfiguration(
- *       boolean addVisionMeasurements,
- *       long robotWeight,
+ *       boolean addVisionMeasurements, long robotWeight,
  *       DoubleSupplier maxAngularVelocity) {
  *
- *     // Load persisted configuration, with default values provided here.
  *     public static DriveConfiguration fromPreferences() {
- *       DriveConfiguration defaults =
- *           new DriveConfiguration(true, 2813, () -> 3.14);
- *       return PersistedConfiguration.fromPreferences("Drive", defaults);
+ *       DriveConfiguration defaultConfig = new DriveConfiguration(
+ *           true, 2813, () -> 3.14);
+ *       return PersistedConfiguration.fromPreferences("Drive", defaultConfig);
  *     }
  *   }
  * }
  * }
    
  *
- * 
    Preference keys created will be:
- * 
      
- *   
    • {@code "Drive/addVisionMeasurements"} (default: {@code true})
      • 
- *   
    • {@code "Drive/robotWeight"} (default: {@code 2813})
      • 
- *   
    • {@code "Drive/maxAngularVelocity"} (default: {@code 3.14})
      • 
- * 
    
+ * 
    In the above example, {@code fromPreferences()} would return a record instance with the values
+ * populated the "Preferences" NetworkTables table. The keys and default values would be:
  *
- * 
    Notes and Gotchas
    
  * 
      
- *   
    • Records are immutable; values are read from Preferences each time {@code fromPreferences} is called.
- *       If live updates are needed, use Supplier-based components instead of raw primitives.
      • 
- *   
    • Unsupported component types (e.g., {@code List}, {@code Map}, or custom classes) will log a warning,
- *       and the default value (if provided) will be used instead.
      • 
- *   
    • Suppliers must not return {@code null}. If they do, a warning is logged and the default supplier is used.
      • 
- *   
    • {@link Preferences} keys cannot contain the path separator character (usually {@code '/'});
- *       if they do, an {@link IllegalArgumentException} is thrown.
      • 
- *   
    • This class is {@code final} and cannot be instantiated—only static factory methods are provided.
      • 
+ *   
    • {@code "Drive/addVisionMeasurements"} (default value: {@code true})
+ *   
    • {@code "Drive/robotWeight"} (default value: {@code 2813})
+ *   
    • {@code "Drive/maxAngularVelocity"} (default value: {@code 3.14})
  * 
    
  *
- * @author Team 2813
+ * 
    For record classes with many component values of the same type, it is strongly recommended
+ * that a builder is provided to construct the record, to avoid callers passing the parameters in
+ * the wrong order. To make generation of a builder easier, consider using {@code @AutoBuilder}
+ * from Google Auto or {@code @Builder}
+ * from Project Lombok. Note that {@code PersistedConfiguration} will use the default record
+ * constructor to create record instances, so any parameter validation should be done in a custom
+ * constructor; see Custom
+ * Constructor in Java Records for details.
+ *
  * @since 2.0.0
  */
 public final class PersistedConfiguration {
@@ -100,20 +106,23 @@ public final class PersistedConfiguration {
   static Consumer errorReporter = DataLogManager::log;
 
   /**
-   * Constructs a record instance, populating its components from Preferences, using the provided instance to get default values.
+   * Constructs a record instance, populating its components from Preferences, using the provided
+   * instance to get default values.
    *
    * 
    The provided instance supplies the default values. For each component:
+   *
    * 
      
-   *   
    • If a corresponding preference already exists, that preference value is used.
      • 
+   *   
    • If a corresponding preference already exists, that preference value is used.
    *   
    • If no preference exists, the component value from {@code configWithDefaults} is stored
-   *       into Preferences and used as the returned value.
      • 
+   *       into Preferences and used as the returned value.
    * 
    
    *
    * @param preferenceName Subtable name under Preferences (must not contain '/')
    * @param configWithDefaults Instance containing default values for all record components
    * @return A new record instance populated with Preferences values
    * @throws IllegalArgumentException if {@code preferenceName} is empty or contains '/'
-   * @throws IllegalStateException if {@code preferenceName} was already registered to a different record class
+   * @throws IllegalStateException if {@code preferenceName} was already registered to a different
+   *     record class
    */
   public static  T fromPreferences(String preferenceName, T configWithDefaults) {
     @SuppressWarnings("unchecked")
@@ -122,11 +131,11 @@ public static  T fromPreferences(String preferenceName, T conf
   }
 
   /**
-   * Construct a record instance with values loaded from Preferences,
-   * using Java defaults if no explicit defaults are provided.
+   * Construct a record instance with values loaded from Preferences, using Java defaults if no
+   * explicit defaults are provided.
    *
-   * 
    For example, an {@code int} component defaults to {@code 0},
-   * and a {@code double} component defaults to {@code 0.0}.
+   * 
    For example, an {@code int} component defaults to {@code 0}, and a {@code double} component
+   * defaults to {@code 0.0}.
    *
    * @param preferenceName Subtable name under Preferences
    * @param recordClass Record type to instantiate
@@ -159,32 +168,28 @@ private static  T fromPreferences(
         throw new RuntimeException(e); // For unit tests
       }
       DriverStation.reportWarning(
-          String.format("Could not copy preferences into %s: %s",
-              recordClass.getSimpleName(), e),
+          String.format("Could not copy preferences into %s: %s", recordClass.getSimpleName(), e),
           e.getStackTrace());
       return configWithDefaults;
     }
   }
 
-  /**
-   * Validates that a preference name is legal (non-empty and does not contain path separators).
-   */
+  /** Validates that a preference name is legal (non-empty and does not contain path separators). */
   private static void validatePreferenceName(String name) {
     if (name.isEmpty()) {
       throw new IllegalArgumentException("name cannot be empty");
     }
     if (name.indexOf(PATH_SEPARATOR) >= 0) {
-      throw new IllegalArgumentException(
-          String.format("name cannot contain '%c'", PATH_SEPARATOR));
+      throw new IllegalArgumentException(String.format("name cannot contain '%c'", PATH_SEPARATOR));
     }
   }
 
   /**
    * Ensures the given preference namespace is not already registered to another record class.
    *
-   * 
    Each namespace is stored in a special NetworkTable registry entry. If the namespace
-   * is new, it is bound to the current record type. If it already exists and points to
-   * a different record type, this method throws an exception.
+   * 
    Each namespace is stored in a special NetworkTable registry entry. If the namespace is new,
+   * it is bound to the current record type. If it already exists and points to a different record
+   * type, this method throws an exception.
    */
   private static void verifyNotRegisteredToAnotherClass(
       NetworkTableInstance ntInstance, String name, Class recordClass) {
@@ -203,15 +208,14 @@ private static void verifyNotRegisteredToAnotherClass(
       if (!recordName.equals(registeredTo)) {
         throw new IllegalStateException(
             String.format(
-                "Preference with name '%s' already registered to %s",
-                name, registeredTo));
+                "Preference with name '%s' already registered to %s", name, registeredTo));
       }
     }
   }
 
   /**
-   * Core factory logic: instantiate a record using reflection,
-   * reading each component value from Preferences (or defaults).
+   * Core factory logic: instantiate a record using reflection, reading each component value from
+   * Preferences (or defaults).
    */
   private static  T createFromPreferences(
       String prefix, Class clazz, T configWithDefaults)
@@ -272,18 +276,14 @@ private static  T createFromPreferences(
     return constructor.newInstance(params);
   }
 
-  /**
-   * Functional interface mapping a record component into a preference-backed value.
-   */
+  /** Functional interface mapping a record component into a preference-backed value. */
   @FunctionalInterface
   private interface PreferenceFactory {
     Object create(
         RecordComponent component, String key, Object defaultValue, boolean initializePreference);
   }
 
-  /**
-   * Generic variant of {@link PreferenceFactory} with typed defaults.
-   */
+  /** Generic variant of {@link PreferenceFactory} with typed defaults. */
   @FunctionalInterface
   private interface GenericPreferenceFactory {
     T create(RecordComponent component, String key, T defaultValue, boolean initializePreference);
@@ -339,15 +339,17 @@ private static boolean booleanFactory(
     return Preferences.getBoolean(key, false);
   }
 
+  /** Gets a BooleanSupplier value from Preferences for the given component. */
   private static BooleanSupplier booleanSupplierFactory(
       RecordComponent component,
       String key,
       BooleanSupplier defaultValueSupplier,
       boolean initialize) {
     if (initialize) {
-      boolean defaultValue = defaultValueSupplier != null
-          ? defaultValueSupplier.getAsBoolean()
-          : false;
+      boolean defaultValue =
+          defaultValueSupplier != null
+              ? defaultValueSupplier.getAsBoolean()
+              : false; // ternary java operation for people who don't know
       Preferences.initBoolean(key, defaultValue);
     }
     return () -> Preferences.getBoolean(key, false);
@@ -440,7 +442,8 @@ private static Supplier supplierFactory(
         ((ParameterizedType) component.getGenericType()).getActualTypeArguments()[0];
     Type registeredType = SUPPLIER_TYPE_TO_REGISTERED_TYPE.get(supplierType);
     if (registeredType == null) {
-      warn("Cannot store '%s' in Preferences; type %s is unsupported",
+      warn(
+          "Cannot store '%s' in Preferences; type %s is unsupported",
           component.getName(), component.getGenericType());
       return defaultValueSupplier;
     }
@@ -451,8 +454,7 @@ private static Supplier supplierFactory(
       if (defaultValueSupplier != null) {
         defaultValue = defaultValueSupplier.get();
         if (defaultValue == null) {
-          warn("Cannot store '%s' in Preferences; default value is null",
-              component.getName());
+          warn("Cannot store '%s' in Preferences; default value is null", component.getName());
           return defaultValueSupplier;
         }
       }
@@ -477,9 +479,7 @@ private static void deleteLegacyKeys() {
     }
   }
 
-  /**
-   * Utility method for reporting warnings. Logs to the configured {@link #errorReporter}.
-   */
+  /** Utility method for reporting warnings. Logs to the configured {@link #errorReporter}. */
   private static void warn(String format, Object... args) {
     String message = String.format("WARNING: " + format, args);
     errorReporter.accept(message);
diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java
index 2510029d..552c117d 100644
--- a/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java
+++ b/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java
@@ -18,11 +18,12 @@
  * Abstract subsystem that manages a motor with PID control based on encoder measurements.
  *
  * 
    This class allows:
+ *
  * 
      
- *   
    • Setting a desired position via a {@link Supplier} setpoint.
      • 
- *   
    • Applying PID control to reach that setpoint using a {@link PIDController}.
      • 
- *   
    • Using different {@link ControlMode}s for manual or PID-driven motor control.
      • 
- *   
    • Flexible angle units via {@link AngleUnit} for calculations and measurements.
      • 
+ *   
    • Setting a desired position via a {@link Supplier} setpoint.
+ *   
    • Applying PID control to reach that setpoint using a {@link PIDController}.
+ *   
    • Using different {@link ControlMode}s for manual or PID-driven motor control.
+ *   
    • Flexible angle units via {@link AngleUnit} for calculations and measurements.
  * 
    
  *
  * @param  the type of {@link Supplier} used to provide dynamic setpoints.
diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java b/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java
index 51630a17..b8538a73 100644
--- a/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java
+++ b/lib/src/main/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystem.java
@@ -2,6 +2,7 @@
 
 import com.google.auto.value.AutoBuilder;
 import com.team2813.lib2813.control.ControlMode;
+import com.team2813.lib2813.control.Motor;
 import com.team2813.lib2813.control.PIDMotor;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -10,9 +11,9 @@
 /**
  * A subsystem for an intake mechanism with parameterized motor control.
  *
- * 
    This subsystem uses a {@link PIDMotor} and allows controlling the motor in intake and
- * outtake directions with preconfigured demand values. Commands are provided for common actions
- * such as intaking, outtaking, and stopping the motor.
+ * 
    This subsystem uses a {@link Motor} and allows controlling the motor in intake and outtake
+ * directions with preconfigured demand values. Commands are provided for common actions such as
+ * intaking, outtaking, and stopping the motor.
  */
 public abstract class ParameterizedIntakeSubsystem extends SubsystemBase implements AutoCloseable {
 
@@ -30,7 +31,9 @@ public record Params(ControlMode controlMode, double intakeDemand, double outtak
 
     /** Returns a builder for creating {@link Params}. Defaults the control mode to VOLTAGE. */
     public static Params.Builder builder() {
-      return new AutoBuilder_ParameterizedIntakeSubsystem_Params_Builder()
+      return new AutoBuilder_ParameterizedIntakeSubsystem_Params_Builder() // autogenerated class,
+          // ignore if it shows an
+          // error in the ide
           .setControlMode(ControlMode.VOLTAGE);
     }
 
@@ -50,7 +53,8 @@ public interface Builder {
      *
      * @param controlMode the motor control mode, cannot be {@code null}
      * @param intakeDemand the motor demand for intake, must be non-zero
-     * @param outtakeDemand the motor demand for outtake, must be non-zero and opposite sign of intakeDemand
+     * @param outtakeDemand the motor demand for outtake, must be non-zero and opposite sign of
+     *     intakeDemand
      * @throws IllegalArgumentException if validation fails
      */
     public Params {
diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/Lightshow.java b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/Lightshow.java
index b19cb3b2..d251b9d3 100644
--- a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/Lightshow.java
+++ b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/Lightshow.java
@@ -10,13 +10,13 @@
 /**
  * Abstract base class for robot lightshow subsystems.
  *
- * 
    A Lightshow subsystem manages a set of {@link State}s that determine the colors to be displayed
- * on LEDs or other lighting hardware. Implementations define how to actually apply the color via
- * {@link #useColor(Color)} and how to compute the active color in {@link #update()}.
+ * 
    A Lightshow subsystem manages a set of {@link State}s that determine the colors to be
+ * displayed on LEDs or other lighting hardware. Implementations define how to actually apply the
+ * color via {@link #useColor(Color)} and how to compute the active color in {@link #update()}.
  *
  * 
    States can be represented as enums implementing {@link State}, or as arbitrary {@link State}
  * instances.
- * 
+ *
  * 
    There is a built-in static {@link #off} state that always represents the lights being off.
  *
  * @author Team 2813
@@ -24,17 +24,18 @@
 public abstract class Lightshow extends SubsystemBase {
 
   /** A static state representing that the lights should be off (always applied). */
-  protected static final State off = new State() {
-    @Override
-    public Color color() {
-      return new Color(0, 0, 0);
-    }
+  protected static final State off =
+      new State() {
+        @Override
+        public Color color() {
+          return new Color(0, 0, 0);
+        }
 
-    @Override
-    public boolean apply() {
-      return true;
-    }
-  };
+        @Override
+        public boolean apply() {
+          return true;
+        }
+      };
 
   /** The set of active states that the lightshow considers when updating the color. */
   protected final Set states = new HashSet<>();
@@ -98,10 +99,9 @@ public final void addStates(Set states) {
   /**
    * Called periodically by the scheduler to update the lights.
    *
-   * 
    This implementation calls {@link #update()}, and applies the resulting color using
-   * {@link #useColor(Color)}. If {@link #update()} returns empty, the {@link #defaultState}'s
-   * color is used (if present). If neither provides a color, {@link #useColor(Color)} may not be
-   * called.
+   * 
    This implementation calls {@link #update()}, and applies the resulting color using {@link
+   * #useColor(Color)}. If {@link #update()} returns empty, the {@link #defaultState}'s color is
+   * used (if present). If neither provides a color, {@link #useColor(Color)} may not be called.
    */
   @Override
   public void periodic() {
diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/QueueLightshow.java b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/QueueLightshow.java
index 2b8a7b68..a375b4b3 100644
--- a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/QueueLightshow.java
+++ b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/QueueLightshow.java
@@ -10,14 +10,13 @@
  * A Lightshow implementation that manages a queue of active states.
  *
  * 
    Behavior:
+ *
  * 
      
- *   
    • All {@link State}s that return {@code true} for {@link State#apply()} are added to a
- *       queue.
      • 
+ *   
    • All {@link State}s that return {@code true} for {@link State#apply()} are added to a queue.
  *   
    • States are removed from the front of the queue if {@link State#apply()} returns {@code
- *       false}.
      • 
- *   
    • The last active state in the queue is used to determine the color to display.
      • 
+ *       false}.
+ *   
    • The last active state in the queue is used to determine the color to display.
  *   
    • If no states are active, the {@link Lightshow#off} state or {@link #defaultState} is used.
- *   
      • 
  * 
    
  *
  * @author Team 2813
diff --git a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/State.java b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/State.java
index ed2e0072..436c9c34 100644
--- a/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/State.java
+++ b/lib/src/main/java/com/team2813/lib2813/subsystems/lightshow/State.java
@@ -4,10 +4,10 @@
 
 /**
  * Represents a Lightshow state with a color and activation condition.
- * 
- * 
    Each {@link State} provides a color and a condition indicating whether it should currently
- * be applied.
- * 
+ *
+ * 
    Each {@link State} provides a color and a condition indicating whether it should currently be
+ * applied.
+ *
  * @author Team 2813
  */
 public interface State {
diff --git a/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsRecord.java b/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsRecord.java
index 325e753c..b65ec7f5 100644
--- a/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsRecord.java
+++ b/lib/src/main/java/com/team2813/lib2813/util/BuildConstantsRecord.java
@@ -34,8 +34,8 @@ record BuildConstantsRecord(
   /**
    * Constructs an instance from a class generated by the gversion Gradle plugin.
    *
-   * 
    Instructions for using the gversion Gradle plugin can be found
-   * 
+   * 
    Instructions for using the gversion Gradle plugin can be found 
    * in the WPILib documentation.
    *
    * @param buildConstantsClass Specially built class that contains the robot code build-time
@@ -82,12 +82,16 @@ static Optional fromGeneratedClass(Class buildConstants
     return Optional.empty();
   }
 
-  /** @return the git submit time formatted as {@code yyyy-MM-dd HH:mm:ss z}. */
+  /**
+   * @return the git submit time formatted as {@code yyyy-MM-dd HH:mm:ss z}.
+   */
   String gitSubmitTimeString() {
     return DATE_FORMATTER.format(gitSubmitTime);
   }
 
-  /** @return the build time formatted as {@code yyyy-MM-dd HH:mm:ss z}. */
+  /**
+   * @return the build time formatted as {@code yyyy-MM-dd HH:mm:ss z}.
+   */
   String buildTimeString() {
     return DATE_FORMATTER.format(buildTime);
   }
diff --git a/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java b/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java
index 9631e1ee..89812b5e 100644
--- a/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java
+++ b/lib/src/main/java/com/team2813/lib2813/util/ConfigUtils.java
@@ -27,7 +27,8 @@ private ConfigUtils() {
   /**
    * Applies a configuration to a REV device with retry attempts.
    *
-   * @param configMethod Supplier that applies a REV configuration and returns a {@link REVLibError}.
+   * @param configMethod Supplier that applies a REV configuration and returns a {@link
+   *     REVLibError}.
    */
   public static void revConfig(Supplier configMethod) {
     REVLibError errorCode = configMethod.get();
@@ -62,7 +63,8 @@ public static void phoenix6Config(Supplier configMethod) {
   /**
    * Applies a configuration to a CTRE device with retry attempts.
    *
-   * @param configMethod Supplier that applies a CTRE configuration and returns an {@link ErrorCode}.
+   * @param configMethod Supplier that applies a CTRE configuration and returns an {@link
+   *     ErrorCode}.
    */
   public static void ctreConfig(Supplier configMethod) {
     ErrorCode errorCode = configMethod.get();
diff --git a/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java b/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java
index 630dd3fc..fc272b82 100644
--- a/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java
+++ b/lib/src/main/java/com/team2813/lib2813/util/InvalidCanIdException.java
@@ -32,8 +32,7 @@ public InvalidCanIdException(int canId) {
     this.canId = canId;
     if (0 <= canId && canId <= 62) {
       throw new IllegalArgumentException(
-          String.format(
-              "%d is a valid CAN ID (it is between 0 and 62, inclusive)", canId));
+          String.format("%d is a valid CAN ID (it is between 0 and 62, inclusive)", canId));
     }
   }
 
diff --git a/lib/src/main/java/com/team2813/lib2813/util/ShuffleboardTabs.java b/lib/src/main/java/com/team2813/lib2813/util/ShuffleboardTabs.java
index e4b44b6b..87fb4e2b 100644
--- a/lib/src/main/java/com/team2813/lib2813/util/ShuffleboardTabs.java
+++ b/lib/src/main/java/com/team2813/lib2813/util/ShuffleboardTabs.java
@@ -7,9 +7,9 @@
  * Provides an abstraction over Shuffleboard tabs.
  *
  * 
    This interface acts as a wrapper around {@link Shuffleboard#getTab(String)} and {@link
- * Shuffleboard#selectTab(String)}, providing a seam for testing. By using this interface, code
- * can interact with Shuffleboard tabs without directly depending on the static Shuffleboard API,
- * which is useful for unit tests and mocking.
+ * Shuffleboard#selectTab(String)}, providing a seam for testing. By using this interface, code can
+ * interact with Shuffleboard tabs without directly depending on the static Shuffleboard API, which
+ * is useful for unit tests and mocking.
  */
 public interface ShuffleboardTabs {
 
diff --git a/lib/src/test/java/com/team2813/lib2813/control/InvertTypeTest.java b/lib/src/test/java/com/team2813/lib2813/control/InvertTypeTest.java
index 6352556f..bc92400b 100644
--- a/lib/src/test/java/com/team2813/lib2813/control/InvertTypeTest.java
+++ b/lib/src/test/java/com/team2813/lib2813/control/InvertTypeTest.java
@@ -6,12 +6,7 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import org.junit.Test;
 
-/**
- * Unit tests for {@link InvertType}.
- *
- * 
    These tests verify that invert mappings exist for all rotation-based invert types and that
- * conversion methods between Phoenix and Spark MAX inversions behave correctly.
- */
+/** Unit tests for {@link InvertType}. */
 public class InvertTypeTest {
 
   /**
diff --git a/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java b/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java
index 1b508dba..48bad686 100644
--- a/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java
+++ b/lib/src/test/java/com/team2813/lib2813/preferences/IsolatedPreferences.java
@@ -9,8 +9,8 @@
  * A JUnit {@link ExternalResource} that isolates {@link Preferences} changes made during a test.
  *
  * 
    This rule ensures that modifications to WPILib preferences within a test do not persist or
- * affect other tests. It accomplishes this by creating a temporary {@link NetworkTableInstance}
- * and associating it with {@link Preferences} for the duration of the test.
+ * affect other tests. It accomplishes this by creating a temporary {@link NetworkTableInstance} and
+ * associating it with {@link Preferences} for the duration of the test.
  *
  * 
    Usage example:
  *
diff --git a/lib/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java b/lib/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java
index a7004b7f..8d381c8a 100644
--- a/lib/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java
+++ b/lib/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java
@@ -12,7 +12,6 @@
 import edu.wpi.first.networktables.NetworkTableType;
 import edu.wpi.first.networktables.Topic;
 import edu.wpi.first.wpilibj.DataLogManager;
-import edu.wpi.first.wpilibj.Preferences;
 import java.util.HashSet;
 import java.util.Map;
 import java.util.Set;
@@ -24,14 +23,13 @@
 import org.junit.experimental.runners.Enclosed;
 import org.junit.rules.ErrorCollector;
 import org.junit.runner.RunWith;
-import org.junit.runners.Parameterized;
-import org.junit.runners.Parameterized.Parameters;
 
 /**
  * Unit tests for {@link PersistedConfiguration}.
  *
- * 
    This test class uses nested classes to test different types of preferences (boolean, int, long,
- * double, String, and record types). It verifies both default behavior and preference updates.
+ * 
    This test class uses nested classes to test different types of preferences (boolean, int,
+ * long, double, String, and record types). It verifies both default behavior and preference
+ * updates.
  *
  * 
    The Enclosed runner is used so we can have nested test classes.
  */
@@ -54,7 +52,6 @@ abstract static class PreferencesRegistryTestCase {
     /** The record class used for mapping preferences. */
     private final Class recordClass;
 
-    /** IsolatedPreferences ensures tests don't interfere with global Preferences. */
     @Rule public final IsolatedPreferences isolatedPreferences = new IsolatedPreferences();
 
     /** Allows collecting multiple test errors in a single test execution. */
@@ -81,8 +78,9 @@ protected PreferencesRegistryTestCase(String preferenceName, Class recordClas
     public final void setTestGlobals() {
       PersistedConfiguration.throwExceptions = true;
       PersistedConfiguration.errorReporter =
-          message -> errorCollector.addError(
-              new AssertionError("Unexpected warning: \"" + message + "\""));
+          message ->
+              errorCollector.addError(
+                  new AssertionError("Unexpected warning: \"" + message + "\""));
     }
 
     /** Resets global configuration after each test. */
@@ -150,44 +148,52 @@ private void collectKeys(NetworkTable table, Set result) {
       }
     }
 
-    /** Creates a record instance with default values configured for the test. */
+    /**
+     * Creates an instance of the record class with all values set to default values.
+     *
+     * 
    The values for the record components should all be unique, and none of them should be the
+     * Java default value for the type.
+     */
     protected abstract T createRecordWithConfiguredDefaults();
 
-    /** Asserts that the record has the configured defaults. */
+    /**
+     * Verifies that the given record has the same values as one created by {@link
+     * #createRecordWithConfiguredDefaults()}.
+     */
     protected abstract void assertHasConfiguredDefaults(T record);
 
-    /** Asserts that preferences contain the configured default values. */
+    /**
+     * Verifies that preferences have the same values as one created by {@link
+     * #createRecordWithConfiguredDefaults()}.
+     */
     protected abstract void assertPreferencesHaveConfiguredDefaults();
 
-    /** Asserts that the record has Java defaults (0, false, or empty for object types). */
+    /** Verifies that all values of the given record equal the Java default value for the type. */
     protected abstract void assertHasJavaDefaults(T record);
 
-    /** Asserts that the preferences contain Java default values. */
+    /** Verifies that all preferences have values equal to the Java default value of the type. */
     protected abstract void assertPreferencesHaveJavaDefaults();
 
-    /** Updates the preference values according to {@link ValuesKind}. */
+    /** Updates the values of all preferences. */
     protected abstract void updatePreferenceValues(ValuesKind kind);
 
     /**
-     * Asserts that the record contains values matching the updated preference values.
-     *
-     * @param kind type of values to compare against
-     * @param record the record to verify
+     * Verifies that all values of the given record equal the values returned by {@link
+     * #updatePreferenceValues(ValuesKind)}.
      */
     protected abstract void assertHasUpdatedValues(ValuesKind kind, T record);
 
     /**
-     * Asserts that all supplier fields in the record reflect updated preference values.
-     *
-     * @param record the record to verify
+     * Verifies that all suppliers contained in the given record have values equal the values
+     * returned by {@link #updatePreferenceValues(ValuesKind)}.
      */
     protected abstract void assertSuppliersHaveUpdatedValues(T record);
 
     /**
      * Verifies that the preference name maps to only one record type.
      *
-     * 
    If a preference is already registered with a different record type, an exception should
-     * be thrown.
+     * 
    If a preference is already registered with a different record type, an exception should be
+     * thrown.
      */
     @Test
     public void preferenceNameMapsToOnlyOneRecordType() {
@@ -198,6 +204,7 @@ public void preferenceNameMapsToOnlyOneRecordType() {
               IllegalStateException.class,
               () -> PersistedConfiguration.fromPreferences(preferenceName, UnrelatedRecord.class));
 
+      // Assert: Exception has expected message
       assertThat(exception)
           .hasMessageThat()
           .containsMatch("Preference with name '" + preferenceName + "' already registered");
@@ -220,12 +227,139 @@ public void preferenceNameMapsToOnlyOneRecordType() {
     // is omitted here but follows the same style as above: explain the test record,
     // default values, and the assertion logic.
 
-  }
+    @Test
+    public void withoutExistingPreferences_passingRecordInstance() {
+      // Arrange
+      T recordWithDefaults = createRecordWithConfiguredDefaults();
+
+      // Act
+      T recordWithPreferences =
+          PersistedConfiguration.fromPreferences(preferenceName, recordWithDefaults);
+
+      // Assert: Preferences injected
+      assertHasConfiguredDefaults(recordWithPreferences);
+      // Ensure the suppliers return the same value if called multiple times.
+      assertHasConfiguredDefaults(recordWithPreferences);
+
+      // Assert: Default values set
+      assertPreferencesHaveConfiguredDefaults();
+
+      // Arrange: Update preferences
+      updatePreferenceValues(ValuesKind.UPDATED_VALUES);
+      var preferenceValues = preferenceValues();
+
+      // Act
+      T newRecordWithPreferences =
+          PersistedConfiguration.fromPreferences(preferenceName, recordWithDefaults);
+
+      // Assert: Preferences injected
+      assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences);
+      assertSuppliersHaveUpdatedValues(recordWithPreferences);
+      assertHasNoChangesSince(preferenceValues);
+      // Ensure the suppliers return the same value if called multiple times.
+      assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences);
+    }
 
-  /** A record used for testing invalid/unrelated registration. */
-  record UnrelatedRecord(int team) {
-    UnrelatedRecord() {
-      this(2813);
+    @Test
+    public void withoutExistingPreferences_passingRecordClass() {
+      // Act
+      var recordWithPreferences =
+          PersistedConfiguration.fromPreferences(preferenceName, recordClass);
+
+      // Assert: Preferences injected
+      assertHasJavaDefaults(recordWithPreferences);
+
+      // Assert: Default values set
+      assertPreferencesHaveJavaDefaults();
+      // Ensure the suppliers return the same value if called multiple times.
+      assertPreferencesHaveJavaDefaults();
+
+      // Arrange: Update preferences
+      updatePreferenceValues(ValuesKind.UPDATED_VALUES);
+      var preferenceValues = preferenceValues();
+
+      // Act
+      var newRecordWithPreferences =
+          PersistedConfiguration.fromPreferences(preferenceName, recordClass);
+
+      // Assert: Preferences injected
+      assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences);
+      assertSuppliersHaveUpdatedValues(recordWithPreferences);
+      assertHasNoChangesSince(preferenceValues);
+      // Ensure the suppliers return the same value if called multiple times.
+      assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences);
     }
+
+    @Test
+    public void withExistingPreferences_passingRecordInstance() {
+      // Arrange
+      updatePreferenceValues(ValuesKind.INITIAL_VALUES);
+      var preferenceValues = preferenceValues();
+      T recordWithDefaults = createRecordWithConfiguredDefaults();
+
+      // Act
+      T recordWithPreferences =
+          PersistedConfiguration.fromPreferences(preferenceName, recordWithDefaults);
+
+      // Assert: Preferences injected
+      assertHasUpdatedValues(ValuesKind.INITIAL_VALUES, recordWithPreferences);
+      assertHasNoChangesSince(preferenceValues);
+      // Ensure the suppliers return the same value if called multiple times.
+      assertHasUpdatedValues(ValuesKind.INITIAL_VALUES, recordWithPreferences);
+
+      // Arrange: Update preferences
+      updatePreferenceValues(ValuesKind.UPDATED_VALUES);
+      preferenceValues = preferenceValues();
+
+      // Act
+      var newRecordWithPreferences =
+          PersistedConfiguration.fromPreferences(preferenceName, recordWithDefaults);
+
+      // Assert: Preferences injected
+      assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences);
+      assertSuppliersHaveUpdatedValues(recordWithPreferences);
+      assertHasNoChangesSince(preferenceValues);
+      // Ensure the suppliers return the same value if called multiple times.
+      assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences);
+    }
+
+    @Test
+    public void withExistingPreferences_passingRecordClass() {
+      // Arrange
+      updatePreferenceValues(ValuesKind.INITIAL_VALUES);
+      var preferenceValues = preferenceValues();
+
+      // Act
+      var recordWithPreferences =
+          PersistedConfiguration.fromPreferences(preferenceName, recordClass);
+
+      // Assert: Preferences injected
+      assertHasUpdatedValues(ValuesKind.INITIAL_VALUES, recordWithPreferences);
+      assertHasNoChangesSince(preferenceValues);
+      // Ensure the suppliers return the same value if called multiple times.
+      assertHasUpdatedValues(ValuesKind.INITIAL_VALUES, recordWithPreferences);
+
+      // Arrange: Update preferences
+      updatePreferenceValues(ValuesKind.UPDATED_VALUES);
+      preferenceValues = preferenceValues();
+
+      // Act
+      var newRecordWithPreferences =
+          PersistedConfiguration.fromPreferences(preferenceName, recordClass);
+
+      // Assert: Preferences injected
+      assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences);
+      assertSuppliersHaveUpdatedValues(recordWithPreferences);
+      assertHasNoChangesSince(preferenceValues);
+      // Ensure the suppliers return the same value if called multiple times.
+      assertHasUpdatedValues(ValuesKind.UPDATED_VALUES, newRecordWithPreferences);
+    }
+  }
+}
+
+/** A record used for testing invalid/unrelated registration. */
+record UnrelatedRecord(int team) {
+  UnrelatedRecord() {
+    this(2813);
   }
 }
diff --git a/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java b/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java
index 48cc8ea8..0627f1e6 100644
--- a/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java
+++ b/lib/src/test/java/com/team2813/lib2813/subsystems/ParameterizedIntakeSubsystemTest.java
@@ -23,11 +23,12 @@
  * subsystem behaves correctly regardless of control mode configuration.
  *
  * 
    The tests verify:
+ *
  * 
      
- *   
    • Initial motor state is stopped
      • 
- *   
    • Motor responds correctly to intake commands
      • 
- *   
    • Motor stops correctly after intake/outtake commands
      • 
- *   
    • Motor responds correctly to outtake commands
      • 
+ *   
    • Initial motor state is stopped
+ *   
    • Motor responds correctly to intake commands
+ *   
    • Motor stops correctly after intake/outtake commands
+ *   
    • Motor responds correctly to outtake commands
  * 
    
  */
 @ParameterizedClass
diff --git a/lib/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java b/lib/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java
index 3cf88c20..d31734ac 100644
--- a/lib/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java
+++ b/lib/src/test/java/com/team2813/lib2813/util/BuildConstantsPublisherTest.java
@@ -22,9 +22,9 @@
 /**
  * Unit tests for {@link BuildConstantsPublisher}.
  *
- * 
    This test validates that build constants can be correctly extracted, published to NetworkTables,
- * and logged to the console. It also includes a custom Truth {@link Subject} for verifying that
- * string representations of dates parse as {@link LocalDateTime}.
+ * 
    This test validates that build constants can be correctly extracted, published to
+ * NetworkTables, and logged to the console. It also includes a custom Truth {@link Subject} for
+ * verifying that string representations of dates parse as {@link LocalDateTime}.
  */
 public class BuildConstantsPublisherTest {
 
@@ -65,7 +65,9 @@ private DateTimeStringSubject(FailureMetadata metadata, String actual) {
       this.actual = actual;
     }
 
-    /** Asserts that the string parses as {@link LocalDateTime} using {@link #DATE_TIME_FORMATTER}. */
+    /**
+     * Asserts that the string parses as {@link LocalDateTime} using {@link #DATE_TIME_FORMATTER}.
+     */
     public void parsesAsLocalDateTime() {
       if (actual == null) {
         failWithActual(simpleFact("expected to parse as LocalDateTime, but was null"));
@@ -82,9 +84,7 @@ public void parsesAsLocalDateTime() {
     }
   }
 
-  /**
-   * Returns the string value of a key in a NetworkTable, or empty if the key is not present.
-   */
+  /** Returns the string value of a key in a NetworkTable, or empty if the key is not present. */
   private String getStringEntryOrEmpty(NetworkTable table, String key) {
     return table.getStringTopic(key).getEntry("").get();
   }
diff --git a/lib/src/test/java/com/team2813/lib2813/util/ControlUtilsTest.java b/lib/src/test/java/com/team2813/lib2813/util/ControlUtilsTest.java
index d4995a68..09b7f196 100644
--- a/lib/src/test/java/com/team2813/lib2813/util/ControlUtilsTest.java
+++ b/lib/src/test/java/com/team2813/lib2813/util/ControlUtilsTest.java
@@ -10,8 +10,8 @@
  * Unit tests for {@link ControlUtils}.
  *
  * 
    Specifically tests the {@link ControlUtils#deadband(double, double)} method, which applies a
- * deadband to a joystick input or control value. A deadband zeroes out values within a threshold and
- * scales values outside the threshold appropriately.
+ * deadband to a joystick input or control value. A deadband zeroes out values within a threshold
+ * and scales values outside the threshold appropriately.
  */
 public class ControlUtilsTest {
 
diff --git a/lib/src/test/java/com/team2813/lib2813/util/FakeShuffleboardTabs.java b/lib/src/test/java/com/team2813/lib2813/util/FakeShuffleboardTabs.java
index 8916a4ac..8e82c275 100644
--- a/lib/src/test/java/com/team2813/lib2813/util/FakeShuffleboardTabs.java
+++ b/lib/src/test/java/com/team2813/lib2813/util/FakeShuffleboardTabs.java
@@ -9,9 +9,9 @@
 /**
  * A fake implementation of {@link ShuffleboardTabs} for testing purposes.
  *
- * 
    Each instance of this class generates a unique prefix so that tab names do not collide
- * with real Shuffleboard tabs during testing. This allows multiple tests to run in parallel
- * without interfering with each other.
+ * 
    Each instance of this class generates a unique prefix so that tab names do not collide with
+ * real Shuffleboard tabs during testing. This allows multiple tests to run in parallel without
+ * interfering with each other.
  */
 public final class FakeShuffleboardTabs implements ShuffleboardTabs {
 
diff --git a/lib/src/test/java/com/team2813/lib2813/util/InputValidationTest.java b/lib/src/test/java/com/team2813/lib2813/util/InputValidationTest.java
index 9bef0465..b30c5d5e 100644
--- a/lib/src/test/java/com/team2813/lib2813/util/InputValidationTest.java
+++ b/lib/src/test/java/com/team2813/lib2813/util/InputValidationTest.java
@@ -22,14 +22,14 @@ public class InputValidationTest {
   /**
    * Tests for the {@link InputValidation#checkCanId(int)} method.
    *
-   * 
    CAN IDs are valid in the range [0, 62]. This class ensures that invalid IDs throw
-   * {@link InvalidCanIdException} and that valid IDs return unchanged.
+   * 
    CAN IDs are valid in the range [0, 62]. This class ensures that invalid IDs throw {@link
+   * InvalidCanIdException} and that valid IDs return unchanged.
    */
   public static class CheckCanIdTest {
 
     /**
-     * Verifies that {@link InputValidation#checkCanId(int)} throws
-     * {@link InvalidCanIdException} for IDs outside the valid range [0, 62].
+     * Verifies that {@link InputValidation#checkCanId(int)} throws {@link InvalidCanIdException}
+     * for IDs outside the valid range [0, 62].
      */
     @Test
     public void invalidCanId() {
@@ -41,13 +41,13 @@ public void invalidCanId() {
         // Verify that the exception contains the invalid ID
         assertThat(exception.getCanId()).isEqualTo(invalidCanId);
         // Verify that the exception message mentions invalid CAN ID
-        assertThat(exception).hasMessageThat().contains("is not a valid can id");
+        assertThat(exception).hasMessageThat().contains("is not a valid CAN ID");
       }
     }
 
     /**
-     * Verifies that {@link InputValidation#checkCanId(int)} returns the original ID for valid
-     * CAN IDs in the range [0, 62].
+     * Verifies that {@link InputValidation#checkCanId(int)} returns the original ID for valid CAN
+     * IDs in the range [0, 62].
      */
     @Test
     public void validCanID() {
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java b/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java
index 4e67befc..28b88757 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java
@@ -10,11 +10,11 @@
  * currently visible AprilTag IDs.
  *
  * @param pose The estimated position and orientation of the robot in 2D space.
- * @param timestampSeconds The timestamp of this estimate, in seconds, relative to the drivetrain clock.
+ * @param timestampSeconds The timestamp of this estimate, in seconds, relative to the drivetrain
+ *     clock.
  * @param visibleAprilTags The set of AprilTag IDs that were visible when this estimate was made.
  */
-public record BotPoseEstimate(
-    Pose2d pose, double timestampSeconds, Set visibleAprilTags) {
+public record BotPoseEstimate(Pose2d pose, double timestampSeconds, Set visibleAprilTags) {
 
   /**
    * @deprecated Use the constructor including {@code visibleAprilTags}. This constructor
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java b/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java
index 832cf3ed..e256d59c 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java
@@ -60,9 +60,7 @@ public DataCollection(LimelightClient limelightClient) {
    */
   record Result(JSONObject json, double responseTimestamp) {}
 
-  /**
-   * Custom body handler to parse the HTTP response into a {@link Result} with timestamp.
-   */
+  /** Custom body handler to parse the HTTP response into a {@link Result} with timestamp. */
   private static class JSONHandler implements BodyHandler {
 
     @Override
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java b/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java
index 3f65236a..0a0c99ca 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java
@@ -17,7 +17,7 @@ class JSONHelper {
   /**
    * Converts an integer value from a JSON object to a boolean.
    *
-   * 
    The key must be present and contain 0 (false) or 1 (true). Returns {@link Optional#empty()} 
+   * 
    The key must be present and contain 0 (false) or 1 (true). Returns {@link Optional#empty()}
    * if the key is missing or the value is not an integer.
    *
    * @param obj the JSON object to read from
@@ -38,7 +38,8 @@ static Optional getBooleanFromInt(JSONObject obj, String key) {
   /**
    * Returns a function that retrieves a nested JSONObject by key.
    *
-   * 
    The returned function returns {@link Optional#empty()} if the key is missing or not a JSON object.
+   * 
    The returned function returns {@link Optional#empty()} if the key is missing or not a JSON
+   * object.
    *
    * @param key the key to extract
    * @return a function that extracts an Optional<JSONObject> from a parent JSONObject
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java b/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java
index 22cecda2..c5d80529 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java
@@ -12,9 +12,9 @@
 /**
  * Interface for interacting with a Limelight vision camera.
  *
- * 
    Provides methods for retrieving locational data, working with field maps, and accessing
- * raw JSON output. Some legacy methods are marked {@link Deprecated} in favor of
- * {@link LocationalData}-based APIs.
+ * 
    Provides methods for retrieving locational data, working with field maps, and accessing raw
+ * JSON output. Some legacy methods are marked {@link Deprecated} in favor of {@link
+ * LocationalData}-based APIs.
  */
 public interface Limelight {
 
@@ -49,8 +49,8 @@ static Limelight getDefaultLimelight() {
   LocationalData getLocationalData();
 
   /**
-   * Sets the field map for the Limelight from an input stream. Optionally, this can upload the
-   * map to the Limelight.
+   * Sets the field map for the Limelight from an input stream. Optionally, this can upload the map
+   * to the Limelight.
    *
    * @param stream the input stream containing the field map
    * @param updateLimelight whether to update the Limelight with this map
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java
index fe058e9d..ca003b09 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java
@@ -55,8 +55,7 @@ public HttpRequestException(String message, URISyntaxException e) {
   /**
    * Creates an {@link HttpRequest.Builder} for the Limelight with the given path.
    *
-   * 
    The URI is automatically constructed using the hostname and port configured for this
-   * client.
+   * 
    The URI is automatically constructed using the hostname and port configured for this client.
    *
    * @param path the HTTP path (e.g., "/results" or "/upload-fieldmap")
    * @return an {@link HttpRequest.Builder} ready for further configuration
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java
index 31bc3d9c..5b5901f6 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java
@@ -30,248 +30,98 @@
 
 /**
  * LimelightHelpers provides static methods and classes for interfacing with Limelight vision
- * cameras in FRC (FIRST Robotics Competition). This library supports all Limelight features 
- * including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking.
- * 
- * 
    This helper class facilitates communication with Limelight cameras via NetworkTables and
- * HTTP requests, providing convenient methods for retrieving vision data, controlling camera
- * settings, and parsing results.
- * 
- * 
    Key features include:
- * 
      
- *   
    • AprilTag/Fiducial marker detection and pose estimation
      • 
- *   
    • Retroreflective target tracking
      • 
- *   
    • Neural network classification and detection
      • 
- *   
    • Barcode/QR code reading
      • 
- *   
    • Robot localization (MegaTag, MegaTag2)
      • 
- *   
    • IMU data integration
      • 
- *   
    • Camera configuration and control
      • 
- * 
    
+ * cameras in FRC. This library supports all Limelight features including AprilTag tracking, Neural
+ * Networks, and standard color/retroreflective tracking.
  *
- * 
    Copied from 
- * github.com/LimelightVision/limelightlib-wpijava
- * 
- * @version 1.11
- * @see Limelight Documentation
- * @author Limelight Vision
+ * 
    Coped from github.com/LimelightVision/limelightlib-wpijava
  */
 class LimelightHelpers {
 
-  /**
-   * Cache for DoubleArrayEntry objects to avoid recreating NetworkTables entries.
-   * This improves performance by reusing entries across multiple calls.
-   * Key format: "tableName/entryName"
-   */
   private static final Map doubleArrayEntries = new ConcurrentHashMap<>();
 
-  /**
-   * Represents a Color/Retroreflective Target Result extracted from JSON Output.
-   * Contains pose information and targeting data for retroreflective vision targets
-   * detected by color or retroreflective vision pipelines.
-   * 
-   * 
    This class provides access to:
-   * 
      
-   *   
    • Camera pose relative to target
      • 
-   *   
    • Robot pose on field and relative to target
      • 
-   *   
    • Target pose relative to camera and robot
      • 
-   *   
    • Targeting metrics (area, offsets, skew)
      • 
-   * 
    
-   */
+  /** Represents a Color/Retroreflective Target Result extracted from JSON Output */
   public static class LimelightTarget_Retro {
 
-    /** 
-     * Camera pose in target space (6-element array: x, y, z, roll, pitch, yaw).
-     * Positions in meters, angles in degrees.
-     */
     @JsonProperty("t6c_ts")
     private double[] cameraPose_TargetSpace;
 
-    /** 
-     * Robot pose in field space (6-element array: x, y, z, roll, pitch, yaw).
-     * Positions in meters, angles in degrees.
-     */
     @JsonProperty("t6r_fs")
     private double[] robotPose_FieldSpace;
 
-    /** 
-     * Robot pose in target space (6-element array: x, y, z, roll, pitch, yaw).
-     * Positions in meters, angles in degrees.
-     */
     @JsonProperty("t6r_ts")
     private double[] robotPose_TargetSpace;
 
-    /** 
-     * Target pose in camera space (6-element array: x, y, z, roll, pitch, yaw).
-     * Positions in meters, angles in degrees.
-     */
     @JsonProperty("t6t_cs")
     private double[] targetPose_CameraSpace;
 
-    /** 
-     * Target pose in robot space (6-element array: x, y, z, roll, pitch, yaw).
-     * Positions in meters, angles in degrees.
-     */
     @JsonProperty("t6t_rs")
     private double[] targetPose_RobotSpace;
 
-    /**
-     * Gets the 3D camera pose in target space.
-     * This represents where the camera is positioned relative to the detected target.
-     * 
-     * @return Pose3d representing camera position and orientation relative to the target
-     */
     public Pose3d getCameraPose_TargetSpace() {
       return toPose3D(cameraPose_TargetSpace);
     }
 
-    /**
-     * Gets the 3D robot pose in field space.
-     * This is the robot's estimated position on the field based on target detection.
-     * 
-     * @return Pose3d representing robot position and orientation on the field
-     */
     public Pose3d getRobotPose_FieldSpace() {
       return toPose3D(robotPose_FieldSpace);
     }
 
-    /**
-     * Gets the 3D robot pose in target space.
-     * This represents where the robot is positioned relative to the detected target.
-     * 
-     * @return Pose3d representing robot position and orientation relative to the target
-     */
     public Pose3d getRobotPose_TargetSpace() {
       return toPose3D(robotPose_TargetSpace);
     }
 
-    /**
-     * Gets the 3D target pose in camera space.
-     * This represents where the target is positioned relative to the camera.
-     * 
-     * @return Pose3d representing target position and orientation relative to the camera
-     */
     public Pose3d getTargetPose_CameraSpace() {
       return toPose3D(targetPose_CameraSpace);
     }
 
-    /**
-     * Gets the 3D target pose in robot space.
-     * This represents where the target is positioned relative to the robot's center.
-     * 
-     * @return Pose3d representing target position and orientation relative to the robot
-     */
     public Pose3d getTargetPose_RobotSpace() {
       return toPose3D(targetPose_RobotSpace);
     }
 
-    /**
-     * Gets the 2D camera pose in target space (ignores z, roll, pitch).
-     * Useful for planar navigation calculations.
-     * 
-     * @return Pose2d representing camera position and yaw relative to the target
-     */
     public Pose2d getCameraPose_TargetSpace2D() {
       return toPose2D(cameraPose_TargetSpace);
     }
 
-    /**
-     * Gets the 2D robot pose in field space (ignores z, roll, pitch).
-     * Useful for planar navigation and field positioning.
-     * 
-     * @return Pose2d representing robot position and yaw on the field
-     */
     public Pose2d getRobotPose_FieldSpace2D() {
       return toPose2D(robotPose_FieldSpace);
     }
 
-    /**
-     * Gets the 2D robot pose in target space (ignores z, roll, pitch).
-     * Useful for planar approach calculations.
-     * 
-     * @return Pose2d representing robot position and yaw relative to the target
-     */
     public Pose2d getRobotPose_TargetSpace2D() {
       return toPose2D(robotPose_TargetSpace);
     }
 
-    /**
-     * Gets the 2D target pose in camera space (ignores z, roll, pitch).
-     * Useful for planar targeting calculations.
-     * 
-     * @return Pose2d representing target position and yaw relative to the camera
-     */
     public Pose2d getTargetPose_CameraSpace2D() {
       return toPose2D(targetPose_CameraSpace);
     }
 
-    /**
-     * Gets the 2D target pose in robot space (ignores z, roll, pitch).
-     * Useful for planar targeting and robot-relative positioning.
-     * 
-     * @return Pose2d representing target position and yaw relative to the robot
-     */
     public Pose2d getTargetPose_RobotSpace2D() {
       return toPose2D(targetPose_RobotSpace);
     }
 
-    /** 
-     * Target area as percentage of image (0-100).
-     * Larger values indicate target is closer or larger in frame.
-     */
     @JsonProperty("ta")
     public double ta;
 
-    /** 
-     * Horizontal offset from crosshair to target center in degrees.
-     * Positive values indicate target is to the right of crosshair.
-     */
     @JsonProperty("tx")
     public double tx;
 
-    /** 
-     * Vertical offset from crosshair to target center in degrees.
-     * Positive values indicate target is above crosshair.
-     */
     @JsonProperty("ty")
     public double ty;
 
-    /** 
-     * Horizontal offset from crosshair to target center in pixels.
-     */
     @JsonProperty("txp")
     public double tx_pixels;
 
-    /** 
-     * Vertical offset from crosshair to target center in pixels.
-     */
     @JsonProperty("typ")
     public double ty_pixels;
 
-    /** 
-     * Horizontal offset from principal point (no crosshair adjustment) in degrees.
-     * More accurate than tx for calibrated cameras.
-     */
     @JsonProperty("tx_nocross")
     public double tx_nocrosshair;
 
-    /** 
-     * Vertical offset from principal point (no crosshair adjustment) in degrees.
-     * More accurate than ty for calibrated cameras.
-     */
     @JsonProperty("ty_nocross")
     public double ty_nocrosshair;
 
-    /** 
-     * Target skew/rotation in degrees (-90 to 0).
-     * Indicates the rotation of the target relative to camera orientation.
-     */
     @JsonProperty("ts")
     public double ts;
 
-    /**
-     * Constructs a new LimelightTarget_Retro with default values.
-     * Initializes all pose arrays to 6-element arrays of zeros.
-     */
     public LimelightTarget_Retro() {
       cameraPose_TargetSpace = new double[6];
       robotPose_FieldSpace = new double[6];
@@ -281,236 +131,94 @@ public LimelightTarget_Retro() {
     }
   }
 
-  /**
-   * Represents an AprilTag/Fiducial Target Result extracted from JSON Output.
-   * Contains pose information and targeting data for AprilTag fiducial markers.
-   * 
-   * 
    AprilTags are visual fiducial markers used extensively in FRC for robot
-   * localization and pose estimation. This class provides comprehensive data about
-   * detected AprilTags including their ID, family, and various pose transformations.
-   * 
-   * 
    This class provides access to:
-   * 
      
-   *   
    • Fiducial ID and family information
      • 
-   *   
    • Camera pose relative to tag
      • 
-   *   
    • Robot pose on field and relative to tag
      • 
-   *   
    • Tag pose relative to camera and robot
      • 
-   *   
    • Targeting metrics (area, offsets, skew)
      • 
-   * 
    
-   */
+  /** Represents an AprilTag/Fiducial Target Result extracted from JSON Output */
   public static class LimelightTarget_Fiducial {
 
-    /** 
-     * Fiducial marker ID number.
-     * For FRC AprilTags, this corresponds to specific field locations.
-     */
     @JsonProperty("fID")
     public double fiducialID;
 
-    /** 
-     * Fiducial family name (e.g., "tag36h11", "tag16h5").
-     * Different tag families have different error correction and detection properties.
-     */
     @JsonProperty("fam")
     public String fiducialFamily;
 
-    /** 
-     * Camera pose in target space (6-element array: x, y, z, roll, pitch, yaw).
-     * Positions in meters, angles in degrees.
-     */
     @JsonProperty("t6c_ts")
     private double[] cameraPose_TargetSpace;
 
-    /** 
-     * Robot pose in field space (6-element array: x, y, z, roll, pitch, yaw).
-     * Positions in meters, angles in degrees.
-     * This is the robot's estimated global position based on this tag detection.
-     */
     @JsonProperty("t6r_fs")
     private double[] robotPose_FieldSpace;
 
-    /** 
-     * Robot pose in target space (6-element array: x, y, z, roll, pitch, yaw).
-     * Positions in meters, angles in degrees.
-     */
     @JsonProperty("t6r_ts")
     private double[] robotPose_TargetSpace;
 
-    /** 
-     * Target pose in camera space (6-element array: x, y, z, roll, pitch, yaw).
-     * Positions in meters, angles in degrees.
-     */
     @JsonProperty("t6t_cs")
     private double[] targetPose_CameraSpace;
 
-    /** 
-     * Target pose in robot space (6-element array: x, y, z, roll, pitch, yaw).
-     * Positions in meters, angles in degrees.
-     */
     @JsonProperty("t6t_rs")
     private double[] targetPose_RobotSpace;
 
-    /**
-     * Gets the 3D camera pose in target space.
-     * This represents where the camera is positioned relative to the detected AprilTag.
-     * 
-     * @return Pose3d representing camera position and orientation relative to the AprilTag
-     */
     public Pose3d getCameraPose_TargetSpace() {
       return toPose3D(cameraPose_TargetSpace);
     }
 
-    /**
-     * Gets the 3D robot pose in field space.
-     * This is the robot's estimated global position based on the AprilTag detection
-     * and the known field layout.
-     * 
-     * @return Pose3d representing robot position and orientation on the field
-     */
     public Pose3d getRobotPose_FieldSpace() {
       return toPose3D(robotPose_FieldSpace);
     }
 
-    /**
-     * Gets the 3D robot pose in target space.
-     * This represents where the robot is positioned relative to the detected AprilTag.
-     * 
-     * @return Pose3d representing robot position and orientation relative to the AprilTag
-     */
     public Pose3d getRobotPose_TargetSpace() {
       return toPose3D(robotPose_TargetSpace);
     }
 
-    /**
-     * Gets the 3D target pose in camera space.
-     * This represents where the AprilTag is positioned relative to the camera.
-     * Useful for direct targeting calculations.
-     * 
-     * @return Pose3d representing AprilTag position and orientation relative to the camera
-     */
     public Pose3d getTargetPose_CameraSpace() {
       return toPose3D(targetPose_CameraSpace);
     }
 
-    /**
-     * Gets the 3D target pose in robot space.
-     * This represents where the AprilTag is positioned relative to the robot's center.
-     * Useful for robot-centric navigation and targeting.
-     * 
-     * @return Pose3d representing AprilTag position and orientation relative to the robot
-     */
     public Pose3d getTargetPose_RobotSpace() {
       return toPose3D(targetPose_RobotSpace);
     }
 
-    /**
-     * Gets the 2D camera pose in target space (ignores z, roll, pitch).
-     * Useful for planar navigation calculations relative to the tag.
-     * 
-     * @return Pose2d representing camera position and yaw relative to the AprilTag
-     */
     public Pose2d getCameraPose_TargetSpace2D() {
       return toPose2D(cameraPose_TargetSpace);
     }
 
-    /**
-     * Gets the 2D robot pose in field space (ignores z, roll, pitch).
-     * This is commonly used with WPILib's pose estimator for robot localization.
-     * 
-     * @return Pose2d representing robot position and yaw on the field
-     */
     public Pose2d getRobotPose_FieldSpace2D() {
       return toPose2D(robotPose_FieldSpace);
     }
 
-    /**
-     * Gets the 2D robot pose in target space (ignores z, roll, pitch).
-     * Useful for planar approach calculations to the tag.
-     * 
-     * @return Pose2d representing robot position and yaw relative to the AprilTag
-     */
     public Pose2d getRobotPose_TargetSpace2D() {
       return toPose2D(robotPose_TargetSpace);
     }
 
-    /**
-     * Gets the 2D target pose in camera space (ignores z, roll, pitch).
-     * Useful for planar targeting calculations.
-     * 
-     * @return Pose2d representing AprilTag position and yaw relative to the camera
-     */
     public Pose2d getTargetPose_CameraSpace2D() {
       return toPose2D(targetPose_CameraSpace);
     }
 
-    /**
-     * Gets the 2D target pose in robot space (ignores z, roll, pitch).
-     * Useful for planar targeting and robot-relative positioning.
-     * 
-     * @return Pose2d representing AprilTag position and yaw relative to the robot
-     */
     public Pose2d getTargetPose_RobotSpace2D() {
       return toPose2D(targetPose_RobotSpace);
     }
 
-    /** 
-     * Target area as percentage of image (0-100).
-     * Larger values indicate tag is closer or larger in frame.
-     */
     @JsonProperty("ta")
     public double ta;
 
-    /** 
-     * Horizontal offset from crosshair to tag center in degrees.
-     * Positive values indicate tag is to the right of crosshair.
-     */
     @JsonProperty("tx")
     public double tx;
 
-    /** 
-     * Vertical offset from crosshair to tag center in degrees.
-     * Positive values indicate tag is above crosshair.
-     */
     @JsonProperty("ty")
     public double ty;
 
-    /** 
-     * Horizontal offset from crosshair to tag center in pixels.
-     */
     @JsonProperty("txp")
     public double tx_pixels;
 
-    /** 
-     * Vertical offset from crosshair to tag center in pixels.
-     */
     @JsonProperty("typ")
     public double ty_pixels;
 
-    /** 
-     * Horizontal offset from principal point (no crosshair adjustment) in degrees.
-     * More accurate than tx for calibrated cameras. Should be preferred for precision work.
-     */
     @JsonProperty("tx_nocross")
     public double tx_nocrosshair;
 
-    /** 
-     * Vertical offset from principal point (no crosshair adjustment) in degrees.
-     * More accurate than ty for calibrated cameras. Should be preferred for precision work.
-     */
     @JsonProperty("ty_nocross")
     public double ty_nocrosshair;
 
-    /** 
-     * Target skew/rotation in degrees (-90 to 0).
-     * Indicates the rotation of the tag relative to camera orientation.
-     */
     @JsonProperty("ts")
     public double ts;
 
-    /**
-     * Constructs a new LimelightTarget_Fiducial with default values.
-     * Initializes all pose arrays to 6-element arrays of zeros.
-     */
     public LimelightTarget_Fiducial() {
       cameraPose_TargetSpace = new double[6];
       robotPose_FieldSpace = new double[6];
@@ -520,495 +228,203 @@ public LimelightTarget_Fiducial() {
     }
   }
 
-  /**
-   * Represents a Barcode Target Result extracted from JSON Output.
-   * Supports various barcode formats including QR codes, DataMatrix, Code128, and others.
-   * 
-   * 
    Limelight can detect and decode various 1D and 2D barcodes, which can be useful
-   * for game piece identification, autonomous mode selection, or other data encoding needs.
-   * 
-   * 
    Supported barcode families include:
-   * 
      
-   *   
    • QR Code
      • 
-   *   
    • DataMatrix
      • 
-   *   
    • Code 128
      • 
-   *   
    • Code 39
      • 
-   *   
    • EAN/UPC
      • 
-   *   
    • And others
      • 
-   * 
    
-   */
+  /** Represents a Barcode Target Result extracted from JSON Output */
   public static class LimelightTarget_Barcode {
 
-    /** 
-     * Barcode family type (e.g. "QR", "DataMatrix", "Code128").
-     * Indicates the encoding format of the detected barcode.
-     */
+    /** Barcode family type (e.g. "QR", "DataMatrix", etc.) */
     @JsonProperty("fam")
     public String family;
 
-    /** 
-     * Decoded data content of the barcode as a string.
-     * This is the actual information encoded in the barcode.
-     */
+    /** Gets the decoded data content of the barcode */
     @JsonProperty("data")
     public String data;
 
-    /** 
-     * Horizontal offset from crosshair to barcode center in pixels.
-     */
     @JsonProperty("txp")
     public double tx_pixels;
 
-    /** 
-     * Vertical offset from crosshair to barcode center in pixels.
-     */
     @JsonProperty("typ")
     public double ty_pixels;
 
-    /** 
-     * Horizontal offset from crosshair to barcode center in degrees.
-     * Positive values indicate barcode is to the right of crosshair.
-     */
     @JsonProperty("tx")
     public double tx;
 
-    /** 
-     * Vertical offset from crosshair to barcode center in degrees.
-     * Positive values indicate barcode is above crosshair.
-     */
     @JsonProperty("ty")
     public double ty;
 
-    /** 
-     * Horizontal offset from principal point (no crosshair adjustment) in degrees.
-     */
     @JsonProperty("tx_nocross")
     public double tx_nocrosshair;
 
-    /** 
-     * Vertical offset from principal point (no crosshair adjustment) in degrees.
-     */
     @JsonProperty("ty_nocross")
     public double ty_nocrosshair;
 
-    /** 
-     * Target area as percentage of image (0-100).
-     * Larger values indicate barcode is closer or larger in frame.
-     */
     @JsonProperty("ta")
     public double ta;
 
-    /** 
-     * Corner points of the barcode bounding box in image coordinates.
-     * Array format: [[x0, y0], [x1, y1], [x2, y2], [x3, y3]]
-     * Useful for visualization or precise barcode localization.
-     */
     @JsonProperty("pts")
     public double[][] corners;
 
-    /**
-     * Constructs a new LimelightTarget_Barcode with default values.
-     */
     public LimelightTarget_Barcode() {}
 
-    /**
-     * Gets the barcode family type.
-     * 
-     * @return String representing the barcode format (e.g., "QR", "DataMatrix", "Code128")
-     */
     public String getFamily() {
       return family;
     }
   }
 
-  /**
-   * Represents a Neural Classifier Pipeline Result extracted from JSON Output.
-   * Used for neural network classification tasks (identifying what an object is).
-   * 
-   * 
    Neural classifiers categorize entire images or regions into predefined classes.
-   * Unlike detectors, classifiers don't provide bounding boxes but instead classify
-   * the primary object in view.
-   * 
-   * 
    Common FRC use cases:
-   * 
      
-   *   
    • Game piece identification (cone vs cube)
      • 
-   *   
    • Color classification
      • 
-   *   
    • Orientation detection
      • 
-   *   
    • State recognition
      • 
-   * 
    
-   */
+  /** Represents a Neural Classifier Pipeline Result extracted from JSON Output */
   public static class LimelightTarget_Classifier {
 
-    /** 
-     * Name of the detected class.
-     * This is the human-readable label for the classification result.
-     */
     @JsonProperty("class")
     public String className;
 
-    /** 
-     * Numeric ID of the detected class.
-     * Useful for switch statements and numerical comparisons.
-     */
     @JsonProperty("classID")
     public double classID;
 
-    /** 
-     * Confidence score of the classification (0-100).
-     * Higher values indicate higher confidence in the classification result.
-     */
     @JsonProperty("conf")
     public double confidence;
 
-    /** 
-     * Zone number if using zone-based classification.
-     * Zones allow different classification behavior in different areas of the image.
-     */
     @JsonProperty("zone")
     public double zone;
 
-    /** 
-     * Horizontal offset from crosshair to classification region center in degrees.
-     */
     @JsonProperty("tx")
     public double tx;
 
-    /** 
-     * Horizontal offset from crosshair to classification region center in pixels.
-     */
     @JsonProperty("txp")
     public double tx_pixels;
 
-    /** 
-     * Vertical offset from crosshair to classification region center in degrees.
-     */
     @JsonProperty("ty")
     public double ty;
 
-    /** 
-     * Vertical offset from crosshair to classification region center in pixels.
-     */
     @JsonProperty("typ")
     public double ty_pixels;
 
-    /**
-     * Constructs a new LimelightTarget_Classifier with default values.
-     */
     public LimelightTarget_Classifier() {}
   }
 
-  /**
-   * Represents a Neural Detector Pipeline Result extracted from JSON Output.
-   * Used for neural network object detection tasks (finding and classifying objects).
-   * 
-   * 
    Neural detectors locate objects within an image and classify them. Unlike
-   * classifiers, detectors can find multiple objects and provide bounding box information.
-   * 
-   * 
    Common FRC use cases:
-   * 
      
-   *   
    • Game piece detection and localization
      • 
-   *   
    • Multiple object tracking
      • 
-   *   
    • Robot detection
      • 
-   *   
    • Custom target identification
      • 
-   * 
    
-   */
+  /** Represents a Neural Detector Pipeline Result extracted from JSON Output */
   public static class LimelightTarget_Detector {
 
-    /** 
-     * Name of the detected class.
-     * This is the human-readable label for what was detected.
-     */
     @JsonProperty("class")
     public String className;
 
-    /** 
-     * Numeric ID of the detected class.
-     * Useful for switch statements and numerical comparisons.
-     */
     @JsonProperty("classID")
     public double classID;
 
-    /** 
-     * Confidence score of the detection (0-100).
-     * Higher values indicate higher confidence that the object is present.
-     */
     @JsonProperty("conf")
     public double confidence;
 
-    /** 
-     * Target area as percentage of image (0-100).
-     * Size of the detection bounding box.
-     */
     @JsonProperty("ta")
     public double ta;
 
-    /** 
-     * Horizontal offset from crosshair to detection center in degrees.
-     * Positive values indicate detection is to the right of crosshair.
-     */
     @JsonProperty("tx")
     public double tx;
 
-    /** 
-     * Vertical offset from crosshair to detection center in degrees.
-     * Positive values indicate detection is above crosshair.
-     */
     @JsonProperty("ty")
     public double ty;
 
-    /** 
-     * Horizontal offset from crosshair to detection center in pixels.
-     */
     @JsonProperty("txp")
     public double tx_pixels;
 
-    /** 
-     * Vertical offset from crosshair to detection center in pixels.
-     */
     @JsonProperty("typ")
     public double ty_pixels;
 
-    /** 
-     * Horizontal offset from principal point (no crosshair adjustment) in degrees.
-     */
     @JsonProperty("tx_nocross")
     public double tx_nocrosshair;
 
-    /** 
-     * Vertical offset from principal point (no crosshair adjustment) in degrees.
-     */
     @JsonProperty("ty_nocross")
     public double ty_nocrosshair;
 
-    /**
-     * Constructs a new LimelightTarget_Detector with default values.
-     */
     public LimelightTarget_Detector() {}
   }
 
-  /**
-   * Limelight Results object, parsed from a Limelight's JSON results output.
-   * Contains all targeting data, pose estimates, and metadata from a Limelight camera.
-   * 
-   * 
    This is the primary result container that aggregates all detection types:
-   * retroreflective targets, AprilTags, neural detections, classifications, and barcodes.
-   * It also includes robot pose estimates and timing information.
-   * 
-   * 
    Use {@link #getLatestResults(String)} to obtain this object from NetworkTables.
-   */
+  /** Limelight Results object, parsed from a Limelight's JSON results output. */
   public static class LimelightResults {
 
-    /** 
-     * Error message if JSON parsing failed, null otherwise.
-     * Check this field to ensure results are valid before use.
-     */
     public String error;
 
-    /** 
-     * Current pipeline index (0-9).
-     * Indicates which vision pipeline is currently active.
-     */
     @JsonProperty("pID")
     public double pipelineID;
 
-    /** 
-     * Pipeline processing latency in milliseconds.
-     * Time taken for the vision pipeline to process the image.
-     */
     @JsonProperty("tl")
     public double latency_pipeline;
 
-    /** 
-     * Image capture latency in milliseconds.
-     * Time between when the frame was captured and when processing began.
-     */
     @JsonProperty("cl")
     public double latency_capture;
 
-    /** 
-     * JSON parsing latency in milliseconds (calculated locally).
-     * Time taken to deserialize the JSON on the RoboRIO.
-     */
     public double latency_jsonParse;
 
-    /** 
-     * Timestamp when Limelight published the results (in seconds).
-     * Unix timestamp from the Limelight's internal clock.
-     */
     @JsonProperty("ts")
     public double timestamp_LIMELIGHT_publish;
 
-    /** 
-     * Timestamp when the RoboRIO FPGA captured the image (in seconds).
-     * FPGA timestamp synchronized with robot code timing.
-     */
     @JsonProperty("ts_rio")
     public double timestamp_RIOFPGA_capture;
 
-    /** 
-     * Whether valid target(s) are detected.
-     * True if at least one target meets the pipeline's criteria.
-     */
     @JsonProperty("v")
     @JsonFormat(shape = Shape.NUMBER)
     public boolean valid;
 
-    /** 
-     * Robot pose in field space (6-element array: x, y, z, roll, pitch, yaw).
-     * Positions in meters, angles in degrees.
-     * Uses Limelight's internal coordinate system.
-     */
     @JsonProperty("botpose")
     public double[] botpose;
 
-    /** 
-     * Robot pose in WPILib red alliance coordinate system.
-     * Origin at red alliance driver station wall.
-     */
     @JsonProperty("botpose_wpired")
     public double[] botpose_wpired;
 
-    /** 
-     * Robot pose in WPILib blue alliance coordinate system.
-     * Origin at blue alliance driver station wall. Recommended for most use cases.
-     */
     @JsonProperty("botpose_wpiblue")
     public double[] botpose_wpiblue;
 
-    /** 
-     * Number of AprilTags used for pose estimation.
-     * Higher counts generally indicate more reliable pose estimates.
-     */
     @JsonProperty("botpose_tagcount")
     public double botpose_tagcount;
 
-    /** 
-     * Distance in meters between the two farthest AprilTags used for pose.
-     * Larger spans generally improve pose accuracy.
-     */
     @JsonProperty("botpose_span")
     public double botpose_span;
 
-    /** 
-     * Average distance to AprilTags used for pose estimation in meters.
-     * Closer tags generally provide more accurate pose estimates.
-     */
     @JsonProperty("botpose_avgdist")
     public double botpose_avgdist;
 
-    /** 
-     * Average area of AprilTags used for pose estimation (0-100).
-     * Larger areas indicate tags are closer and provide better resolution.
-     */
     @JsonProperty("botpose_avgarea")
     public double botpose_avgarea;
 
-    /** 
-     * Camera pose in robot space (6-element array: x, y, z, roll, pitch, yaw).
-     * Describes where the camera is mounted relative to robot center.
-     */
     @JsonProperty("t6c_rs")
     public double[] camerapose_robotspace;
 
-    /**
-     * Gets the 3D robot pose from the botpose array.
-     * Uses Limelight's internal coordinate system.
-     * 
-     * @return Pose3d representing robot position and orientation
-     */
     public Pose3d getBotPose3d() {
       return toPose3D(botpose);
     }
 
-    /**
-     * Gets the 3D robot pose in WPILib red alliance coordinates.
-     * Not recommended - use blue alliance for consistency.
-     * 
-     * @return Pose3d in red alliance coordinate system
-     */
     public Pose3d getBotPose3d_wpiRed() {
       return toPose3D(botpose_wpired);
     }
 
-    /**
-     * Gets the 3D robot pose in WPILib blue alliance coordinates.
-     * Recommended for most applications.
-     * 
-     * @return Pose3d in blue alliance coordinate system
-     */
     public Pose3d getBotPose3d_wpiBlue() {
       return toPose3D(botpose_wpiblue);
     }
 
-    /**
-     * Gets the 2D robot pose from the botpose array.
-     * Uses Limelight's internal coordinate system.
-     * 
-     * @return Pose2d representing robot position and yaw
-     */
     public Pose2d getBotPose2d() {
       return toPose2D(botpose);
     }
 
-    /**
-     * Gets the 2D robot pose in WPILib red alliance coordinates.
-     * Not recommended - use blue alliance for consistency.
-     * 
-     * @return Pose2d in red alliance coordinate system
-     */
     public Pose2d getBotPose2d_wpiRed() {
       return toPose2D(botpose_wpired);
     }
 
-    /**
-     * Gets the 2D robot pose in WPILib blue alliance coordinates.
-     * Recommended for use with WPILib pose estimators.
-     * 
-     * @return Pose2d in blue alliance coordinate system
-     */
     public Pose2d getBotPose2d_wpiBlue() {
       return toPose2D(botpose_wpiblue);
     }
 
-    /** 
-     * Array of retroreflective target results.
-     * Contains data for all detected retroreflective/color targets.
-     */
     @JsonProperty("Retro")
     public LimelightTarget_Retro[] targets_Retro;
 
-    /** 
-     * Array of AprilTag/fiducial target results.
-     * Contains data for all detected AprilTags.
-     */
     @JsonProperty("Fiducial")
     public LimelightTarget_Fiducial[] targets_Fiducials;
 
-    /** 
-     * Array of neural classifier results.
-     * Contains classification results if a classifier pipeline is active.
-     */
     @JsonProperty("Classifier")
     public LimelightTarget_Classifier[] targets_Classifier;
 
-    /** 
-     * Array of neural detector results.
-     * Contains detection results if a detector pipeline is active.
-     */
     @JsonProperty("Detector")
     public LimelightTarget_Detector[] targets_Detector;
 
-    /** 
-     * Array of barcode detection results.
-     * Contains decoded barcode data.
-     */
     @JsonProperty("Barcode")
     public LimelightTarget_Barcode[] targets_Barcode;
 
-    /**
-     * Constructs a new LimelightResults with default values.
-     * Initializes all arrays to appropriate default sizes.
-     */
     public LimelightResults() {
       botpose = new double[6];
       botpose_wpired = new double[6];
@@ -1022,69 +438,16 @@ public LimelightResults() {
     }
   }
 
-  /**
-   * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output.
-   * Provides quick access to basic AprilTag detection data without full JSON parsing.
-   * 
-   * 
    RawFiducials are lightweight representations of AprilTag detections available
-   * directly from NetworkTables. They're faster to access than full JSON parsing
-   * and useful when you only need basic tag information.
-   */
+  /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */
   public static class RawFiducial {
-    /** 
-     * AprilTag ID number.
-     * Corresponds to specific field locations in FRC.
-     */
     public int id = 0;
-    
-    /** 
-     * Horizontal offset from principal point in normalized coordinates (-1 to 1).
-     * More accurate than pixel-based offsets for calibrated cameras.
-     */
     public double txnc = 0;
-    
-    /** 
-     * Vertical offset from principal point in normalized coordinates (-1 to 1).
-     * More accurate than pixel-based offsets for calibrated cameras.
-     */
     public double tync = 0;
-    
-    /** 
-     * Target area as percentage of image (0-100).
-     * Indicates size of tag in camera view.
-     */
     public double ta = 0;
-    
-    /** 
-     * Distance from camera to tag in meters.
-     * Based on tag's known size and camera calibration.
-     */
     public double distToCamera = 0;
-    
-    /** 
-     * Distance from robot center to tag in meters.
-     * Accounts for camera offset from robot center.
-     */
     public double distToRobot = 0;
-    
-    /** 
-     * Pose ambiguity (0 = unambiguous, higher = more ambiguous).
-     * AprilTag pose estimation can have multiple solutions; this indicates confidence.
-     * Values above 0.2 are generally considered ambiguous.
-     */
     public double ambiguity = 0;
 
-    /**
-     * Constructs a RawFiducial with specified detection parameters.
-     * 
-     * @param id AprilTag ID number
-     * @param txnc Horizontal offset from principal point (normalized -1 to 1)
-     * @param tync Vertical offset from principal point (normalized -1 to 1)
-     * @param ta Target area percentage (0-100)
-     * @param distToCamera Distance from camera to tag in meters
-     * @param distToRobot Distance from robot center to tag in meters
-     * @param ambiguity Pose ambiguity value (0 = certain, higher = ambiguous)
-     */
     public RawFiducial(
         int id,
         double txnc,
@@ -1103,78 +466,21 @@ public RawFiducial(
     }
   }
 
-  /**
-   * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output.
-   * Provides quick access to neural detector data including bounding box corners.
-   * 
-   * 
    RawDetections are lightweight representations of neural network detections available
-   * directly from NetworkTables. They include bounding box corner coordinates for
-   * visualization and precise object localization.
-   */
+  /** Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. */
   public static class RawDetection {
-    /** 
-     * Detected class ID.
-     * Numeric identifier for the detected object class.
-     */
     public int classId = 0;
-    
-    /** 
-     * Horizontal offset from principal point in normalized coordinates (-1 to 1).
-     */
     public double txnc = 0;
-    
-    /** 
-     * Vertical offset from principal point in normalized coordinates (-1 to 1).
-     */
     public double tync = 0;
-    
-    /** 
-     * Target area as percentage of image (0-100).
-     * Size of the detection bounding box.
-     */
     public double ta = 0;
-    
-    /** X coordinate of corner 0 of bounding box (normalized -1 to 1) */
     public double corner0_X = 0;
-    
-    /** Y coordinate of corner 0 of bounding box (normalized -1 to 1) */
     public double corner0_Y = 0;
-    
-    /** X coordinate of corner 1 of bounding box (normalized -1 to 1) */
     public double corner1_X = 0;
-    
-    /** Y coordinate of corner 1 of bounding box (normalized -1 to 1) */
     public double corner1_Y = 0;
-    
-    /** X coordinate of corner 2 of bounding box (normalized -1 to 1) */
     public double corner2_X = 0;
-    
-    /** Y coordinate of corner 2 of bounding box (normalized -1 to 1) */
     public double corner2_Y = 0;
-    
-    /** X coordinate of corner 3 of bounding box (normalized -1 to 1) */
     public double corner3_X = 0;
-    
-    /** Y coordinate of corner 3 of bounding box (normalized -1 to 1) */
     public double corner3_Y = 0;
 
-    /**
-     * Constructs a RawDetection with specified detection parameters.
-     * Corner coordinates define the bounding box of the detected object.
-     * 
-     * @param classId Detected class ID
-     * @param txnc Horizontal offset from principal point (normalized)
-     * @param tync Vertical offset from principal point (normalized)
-     * @param ta Target area percentage
-     * @param corner0_X X coordinate of bounding box corner 0
-     * @param corner0_Y Y coordinate of bounding box corner 0
-     * @param corner1_X X coordinate of bounding box corner 1
-     * @param corner1_Y Y coordinate of bounding box corner 1
-     * @param corner2_X X coordinate of bounding box corner 2
-     * @param corner2_Y Y coordinate of bounding box corner 2
-     * @param corner3_X X coordinate of bounding box corner 3
-     * @param corner3_Y Y coordinate of bounding box corner 3
-     */
     public RawDetection(
         int classId,
         double txnc,
@@ -1203,82 +509,20 @@ public RawDetection(
     }
   }
 
-  /**
-   * Represents a 3D Pose Estimate with metadata about the quality and source of the estimate.
-   * Used primarily with vision-based localization systems like MegaTag and MegaTag2.
-   * 
-   * 
    PoseEstimate combines a robot pose with quality metrics that help determine how
-   * reliable the estimate is. Use these metrics with WPILib's pose estimator to
-   * dynamically adjust standard deviations based on detection quality.
-   * 
-   * 
    Quality indicators include:
-   * 
      
-   *   
    • Tag count - more tags generally means better accuracy
      • 
-   *   
    • Tag span - wider spacing between tags improves accuracy
      • 
-   *   
    • Average distance - closer tags provide better resolution
      • 
-   *   
    • Average area - larger tags in view provide better data
      • 
-   *   
    • Ambiguity data for each tag
      • 
-   * 
    
-   */
+  /** Represents a 3D Pose Estimate. */
   public static class PoseEstimate {
-    /** 
-     * The estimated 2D robot pose on the field.
-     * Use with WPILib's pose estimator via addVisionMeasurement().
-     */
     public Pose2d pose;
-    
-    /** 
-     * Timestamp of the pose estimate in seconds (FPGA timestamp).
-     * Synchronized with robot code timing for accurate pose integration.
-     */
     public double timestampSeconds;
-    
-    /** 
-     * Total latency of the pose estimate in milliseconds.
-     * Includes capture and processing time.
-     */
     public double latency;
-    
-    /** 
-     * Number of AprilTags used to compute this pose estimate.
-     * More tags generally provide more accurate and stable estimates.
-     */
     public int tagCount;
-    
-    /** 
-     * Distance between the farthest AprilTags used in meters.
-     * Larger spans reduce angular error and improve accuracy.
-     */
     public double tagSpan;
-    
-    /** 
-     * Average distance to all AprilTags used in meters.
-     * Closer tags provide better pixel resolution and accuracy.
-     */
     public double avgTagDist;
-    
-    /** 
-     * Average area of AprilTags used as percentage of image (0-100).
-     * Larger areas mean better resolution and more accurate pose data.
-     */
     public double avgTagArea;
 
-    /** 
-     * Raw fiducial data for all tags used in pose estimation.
-     * Includes individual tag metrics like ambiguity and distance.
-     */
     public RawFiducial[] rawFiducials;
-    
-    /** 
-     * Whether this estimate was computed using the MegaTag2 algorithm.
-     * MegaTag2 uses robot orientation data for improved accuracy.
-     */
     public boolean isMegaTag2;
 
-    /**
-     * Constructs a PoseEstimate object with default values.
-     * All numeric fields initialized to 0, pose to origin, arrays to empty.
-     */
+    /** Instantiates a PoseEstimate object with default values */
     public PoseEstimate() {
       this.pose = new Pose2d();
       this.timestampSeconds = 0;
@@ -1291,19 +535,6 @@ public PoseEstimate() {
       this.isMegaTag2 = false;
     }
 
-    /**
-     * Constructs a PoseEstimate with all specified parameters.
-     * 
-     * @param pose The estimated 2D robot pose
-     * @param timestampSeconds FPGA timestamp in seconds
-     * @param latency Total latency in milliseconds
-     * @param tagCount Number of AprilTags used
-     * @param tagSpan Distance between farthest tags in meters
-     * @param avgTagDist Average distance to tags in meters
-     * @param avgTagArea Average tag area as percentage
-     * @param rawFiducials Array of raw fiducial data for quality assessment
-     * @param isMegaTag2 Whether MegaTag2 algorithm was used
-     */
     public PoseEstimate(
         Pose2d pose,
         double timestampSeconds,
@@ -1327,63 +558,21 @@ public PoseEstimate(
     }
   }
 
-  /**
-   * Encapsulates the state of an internal Limelight IMU (Inertial Measurement Unit).
-   * Contains orientation, angular velocity, and acceleration data.
-   * 
-   * 
    Limelight 3 and newer models include a built-in IMU that can provide robot
-   * orientation data. This is particularly useful for MegaTag2 localization which
-   * requires robot orientation input for optimal accuracy.
-   * 
-   * 
    IMU data includes:
-   * 
      
-   *   
    • Orientation angles (roll, pitch, yaw)
      • 
-   *   
    • Angular velocities (gyroscope data)
      • 
-   *   
    • Linear accelerations (accelerometer data)
      • 
-   * 
    
-   */
+  /** Encapsulates the state of an internal Limelight IMU. */
   public static class IMUData {
-    /** Robot yaw angle in degrees (heading, rotation around Z axis) */
     public double robotYaw = 0.0;
-    
-    /** Roll angle in degrees (rotation around X axis) */
     public double Roll = 0.0;
-    
-    /** Pitch angle in degrees (rotation around Y axis) */
     public double Pitch = 0.0;
-    
-    /** Yaw angle in degrees (rotation around Z axis) */
     public double Yaw = 0.0;
-    
-    /** Angular velocity around X axis in degrees per second */
     public double gyroX = 0.0;
-    
-    /** Angular velocity around Y axis in degrees per second */
     public double gyroY = 0.0;
-    
-    /** Angular velocity around Z axis in degrees per second */
     public double gyroZ = 0.0;
-    
-    /** Linear acceleration along X axis in G's (1G = 9.81 m/s²) */
     public double accelX = 0.0;
-    
-    /** Linear acceleration along Y axis in G's */
     public double accelY = 0.0;
-    
-    /** Linear acceleration along Z axis in G's */
     public double accelZ = 0.0;
 
-    /**
-     * Constructs an IMUData object with all values initialized to 0.
-     */
     public IMUData() {}
 
-    /**
-     * Constructs an IMUData object from a double array.
-     * Expected array format: [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]
-     * 
-     * @param imuData Array of IMU values (must be at least 10 elements)
-     */
     public IMUData(double[] imuData) {
       if (imuData != null && imuData.length >= 10) {
         this.robotYaw = imuData[0];
@@ -1400,26 +589,11 @@ public IMUData(double[] imuData) {
     }
   }
 
-  /** 
-   * Jackson ObjectMapper instance for JSON deserialization.
-   * Lazily initialized on first use.
-   */
   private static ObjectMapper mapper;
 
-  /** 
-   * When enabled, prints JSON parsing time to console in milliseconds.
-   * Useful for performance debugging and optimization.
-   * Set to true to enable performance profiling.
-   */
+  /** Print JSON Parse time to the console in milliseconds */
   static boolean profileJSON = false;
 
-  /**
-   * Sanitizes the Limelight name by returning "limelight" if the input is null or empty.
-   * This ensures a valid NetworkTables name even when no specific name is provided.
-   * 
-   * @param name The Limelight name to sanitize
-   * @return The sanitized name, or "limelight" if input was null/empty
-   */
   static final String sanitizeName(String name) {
     if (name == null || name.isEmpty()) {
       return "limelight";
@@ -1428,11 +602,9 @@ static final String sanitizeName(String name) {
   }
 
   /**
-   * Takes a 6-length array of pose data and converts it to a Pose3d object.
-   * Array format: [x, y, z, roll, pitch, yaw] where:
-   * - x, y, z are in meters
-   * - roll, pitch, yaw are in degrees
-   * 
+   * Takes a 6-length array of pose data and converts it to a Pose3d object. Array format: [x, y, z,
+   * roll, pitch, yaw] where angles are in degrees.
+   *
    * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
    * @return Pose3d object representing the pose, or empty Pose3d if invalid data
    */
@@ -1450,12 +622,10 @@ public static Pose3d toPose3D(double[] inData) {
   }
 
   /**
-   * Takes a 6-length array of pose data and converts it to a Pose2d object.
-   * Uses only x, y, and yaw components, ignoring z, roll, and pitch.
-   * Array format: [x, y, z, roll, pitch, yaw] where:
-   * - x, y are in meters
-   * - yaw is in degrees
-   * 
+   * Takes a 6-length array of pose data and converts it to a Pose2d object. Uses only x, y, and yaw
+   * components, ignoring z, roll, and pitch. Array format: [x, y, z, roll, pitch, yaw] where angles
+   * are in degrees.
+   *
    * @param inData Array containing pose data [x, y, z, roll, pitch, yaw]
    * @return Pose2d object representing the pose, or empty Pose2d if invalid data
    */
@@ -1472,8 +642,7 @@ public static Pose2d toPose2D(double[] inData) {
   /**
    * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
    * Translation components are in meters, rotation components are in degrees.
-   * Useful for sending pose data to the Limelight or storing in NetworkTables.
-   * 
+   *
    * @param pose The Pose3d object to convert
    * @return A 6-element array containing [x, y, z, roll, pitch, yaw]
    */
@@ -1490,9 +659,9 @@ public static double[] pose3dToArray(Pose3d pose) {
 
   /**
    * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw].
-   * Translation components are in meters, rotation components are in degrees.
-   * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw.
-   * 
+   * Translation components are in meters, rotation components are in degrees. Note: z, roll, and
+   * pitch will be 0 since Pose2d only contains x, y, and yaw.
+   *
    * @param pose The Pose2d object to convert
    * @return A 6-element array containing [x, y, 0, 0, 0, yaw]
    */
@@ -1507,14 +676,6 @@ public static double[] pose2dToArray(Pose2d pose) {
     return result;
   }
 
-  /**
-   * Extracts a single element from a double array at the specified position.
-   * Returns 0 if the array is too short or position is out of bounds.
-   * 
-   * @param inData The source array
-   * @param position The index to extract (0-based)
-   * @return The value at the position, or 0 if position is out of bounds
-   */
   private static double extractArrayEntry(double[] inData, int position) {
     if (inData.length < position + 1) {
       return 0;
@@ -1522,27 +683,6 @@ private static double extractArrayEntry(double[] inData, int position) {
     return inData[position];
   }
 
-  /**
-   * Internal method to retrieve a bot pose estimate from NetworkTables.
-   * Extracts pose data, latency, tag information, and raw fiducial data from
-   * a timestamped NetworkTables entry.
-   * 
-   * 
    The pose array format is:
-   * 
      
-   *   
    • [0-5]: x, y, z, roll, pitch, yaw
      • 
-   *   
    • [6]: latency (ms)
      • 
-   *   
    • [7]: tag count
      • 
-   *   
    • [8]: tag span (m)
      • 
-   *   
    • [9]: average tag distance (m)
      • 
-   *   
    • [10]: average tag area (%)
      • 
-   *   
    • [11+]: raw fiducial data (7 values per tag)
      • 
-   * 
    
-   * 
-   * @param limelightName Name/identifier of the Limelight
-   * @param entryName NetworkTables entry name to read from (e.g., "botpose_wpiblue")
-   * @param isMegaTag2 Whether this is a MegaTag2 estimate
-   * @return PoseEstimate object, or null if no data available
-   */
   private static PoseEstimate getBotPoseEstimate(
       String limelightName, String entryName, boolean isMegaTag2) {
     DoubleArrayEntry poseEntry =
@@ -1572,7 +712,7 @@ private static PoseEstimate getBotPoseEstimate(
     int expectedTotalVals = 11 + valsPerFiducial * tagCount;
 
     if (poseArray.length != expectedTotalVals) {
-      // Don't populate fiducials if array length doesn't match expected format
+      // Don't populate fiducials
     } else {
       for (int i = 0; i < tagCount; i++) {
         int baseIndex = 11 + (i * valsPerFiducial);
@@ -1601,10 +741,9 @@ private static PoseEstimate getBotPoseEstimate(
 
   /**
    * Gets the latest raw fiducial/AprilTag detection results from NetworkTables.
-   * Provides quick access to basic tag data without full JSON parsing.
-   * 
+   *
    * @param limelightName Name/identifier of the Limelight
-   * @return Array of RawFiducial objects containing detection details, or empty array if none
+   * @return Array of RawFiducial objects containing detection details
    */
   public static RawFiducial[] getRawFiducials(String limelightName) {
     var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials");
@@ -1634,11 +773,10 @@ public static RawFiducial[] getRawFiducials(String limelightName) {
   }
 
   /**
-   * Gets the latest raw neural detector results from NetworkTables.
-   * Provides quick access to detection data including bounding box corners.
-   * 
+   * Gets the latest raw neural detector results from NetworkTables
+   *
    * @param limelightName Name/identifier of the Limelight
-   * @return Array of RawDetection objects containing detection details, or empty array if none
+   * @return Array of RawDetection objects containing detection details
    */
   public static RawDetection[] getRawDetections(String limelightName) {
     var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections");
@@ -1676,12 +814,10 @@ public static RawDetection[] getRawDetections(String limelightName) {
   }
 
   /**
-   * Prints detailed information about a PoseEstimate to standard output.
-   * Includes timestamp, latency, tag count, tag span, average tag distance,
-   * average tag area, and detailed information about each detected fiducial.
-   * 
-   * 
    Useful for debugging and understanding pose estimate quality.
-   * 
+   * Prints detailed information about a PoseEstimate to standard output. Includes timestamp,
+   * latency, tag count, tag span, average tag distance, average tag area, and detailed information
+   * about each detected fiducial.
+   *
    * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available."
    */
   public static void printPoseEstimate(PoseEstimate pose) {
@@ -1720,58 +856,22 @@ public static void printPoseEstimate(PoseEstimate pose) {
     }
   }
 
-  /**
-   * Validates whether a PoseEstimate contains usable data.
-   * Checks that the pose is non-null and contains raw fiducial data.
-   * 
-   * @param pose The PoseEstimate to validate
-   * @return True if the pose is non-null and contains raw fiducial data
-   */
   public static Boolean validPoseEstimate(PoseEstimate pose) {
     return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0;
   }
 
-  /**
-   * Gets the NetworkTable instance for a Limelight camera.
-   * All Limelight data is published to a table named after the camera.
-   * 
-   * @param tableName Name/identifier of the Limelight
-   * @return NetworkTable instance for the specified Limelight
-   */
   public static NetworkTable getLimelightNTTable(String tableName) {
     return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
   }
 
-  /**
-   * Flushes NetworkTables to ensure all queued updates are sent immediately.
-   * Call this after setting multiple values to ensure they're sent together
-   * and processed by the Limelight in the same update cycle.
-   * 
-   * 
    Particularly important when using SetRobotOrientation for MegaTag2.
-   */
   public static void Flush() {
     NetworkTableInstance.getDefault().flush();
   }
 
-  /**
-   * Gets a specific NetworkTables entry for a Limelight camera.
-   * 
-   * @param tableName Name/identifier of the Limelight
-   * @param entryName Name of the NetworkTables entry
-   * @return NetworkTableEntry for the specified entry
-   */
   public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) {
     return getLimelightNTTable(tableName).getEntry(entryName);
   }
 
-  /**
-   * Gets a DoubleArrayEntry for a Limelight, using a cache to avoid recreating entries.
-   * This improves performance by reusing entry objects across multiple calls.
-   * 
-   * @param tableName Name/identifier of the Limelight
-   * @param entryName Name of the double array entry
-   * @return Cached or newly created DoubleArrayEntry
-   */
   public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) {
     String key = tableName + "/" + entryName;
     return doubleArrayEntries.computeIfAbsent(
@@ -1782,80 +882,30 @@ public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, St
         });
   }
 
-  /**
-   * Gets a double value from NetworkTables for a Limelight entry.
-   * 
-   * @param tableName Name/identifier of the Limelight
-   * @param entryName Name of the entry
-   * @return Double value, or 0.0 if not found
-   */
   public static double getLimelightNTDouble(String tableName, String entryName) {
     return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0);
   }
 
-  /**
-   * Sets a double value in NetworkTables for a Limelight entry.
-   * 
-   * @param tableName Name/identifier of the Limelight
-   * @param entryName Name of the entry
-   * @param val Value to set
-   */
   public static void setLimelightNTDouble(String tableName, String entryName, double val) {
     getLimelightNTTableEntry(tableName, entryName).setDouble(val);
   }
 
-  /**
-   * Sets a double array value in NetworkTables for a Limelight entry.
-   * 
-   * @param tableName Name/identifier of the Limelight
-   * @param entryName Name of the entry
-   * @param val Array of values to set
-   */
   public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) {
     getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val);
   }
 
-  /**
-   * Gets a double array from NetworkTables for a Limelight entry.
-   * 
-   * @param tableName Name/identifier of the Limelight
-   * @param entryName Name of the entry
-   * @return Double array, or empty array if not found
-   */
   public static double[] getLimelightNTDoubleArray(String tableName, String entryName) {
     return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]);
   }
 
-  /**
-   * Gets a string value from NetworkTables for a Limelight entry.
-   * 
-   * @param tableName Name/identifier of the Limelight
-   * @param entryName Name of the entry
-   * @return String value, or empty string if not found
-   */
   public static String getLimelightNTString(String tableName, String entryName) {
     return getLimelightNTTableEntry(tableName, entryName).getString("");
   }
 
-  /**
-   * Gets a string array from NetworkTables for a Limelight entry.
-   * 
-   * @param tableName Name/identifier of the Limelight
-   * @param entryName Name of the entry
-   * @return String array, or empty array if not found
-   */
   public static String[] getLimelightNTStringArray(String tableName, String entryName) {
     return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]);
   }
 
-  /**
-   * Constructs a URL for making HTTP requests to a Limelight camera.
-   * Limelights expose an HTTP API on port 5807 for certain operations.
-   * 
-   * @param tableName Name/identifier of the Limelight
-   * @param request The HTTP request path (e.g., "capturesnapshot")
-   * @return URL object for the request, or null if malformed
-   */
   public static URL getLimelightURLString(String tableName, String request) {
     String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request;
     URL url;
@@ -1869,12 +919,11 @@ public static URL getLimelightURLString(String tableName, String request) {
   }
 
   /////
-  ///// Basic Target Information Getters
   /////
 
   /**
    * Does the Limelight have a valid target?
-   * 
+   *
    * @param limelightName Name of the Limelight camera ("" for default)
    * @return True if a valid target is present, false otherwise
    */
@@ -1884,10 +933,9 @@ public static boolean getTV(String limelightName) {
 
   /**
    * Gets the horizontal offset from the crosshair to the target in degrees.
-   * Positive values indicate target is to the right of crosshair.
-   * 
+   *
    * @param limelightName Name of the Limelight camera ("" for default)
-   * @return Horizontal offset angle in degrees (typically -29.8 to 29.8)
+   * @return Horizontal offset angle in degrees
    */
   public static double getTX(String limelightName) {
     return getLimelightNTDouble(limelightName, "tx");
@@ -1895,34 +943,33 @@ public static double getTX(String limelightName) {
 
   /**
    * Gets the vertical offset from the crosshair to the target in degrees.
-   * Positive values indicate target is above crosshair.
-   * 
+   *
    * @param limelightName Name of the Limelight camera ("" for default)
-   * @return Vertical offset angle in degrees (typically -24.85 to 24.85)
+   * @return Vertical offset angle in degrees
    */
   public static double getTY(String limelightName) {
     return getLimelightNTDouble(limelightName, "ty");
   }
 
   /**
-   * Gets the horizontal offset from the principal pixel/point to the target in degrees.
-   * This is the most accurate 2d metric if you are using a calibrated camera and
-   * you don't need adjustable crosshair functionality.
-   * 
+   * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the
+   * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable
+   * crosshair functionality.
+   *
    * @param limelightName Name of the Limelight camera ("" for default)
-   * @return Horizontal offset angle in degrees from principal point
+   * @return Horizontal offset angle in degrees
    */
   public static double getTXNC(String limelightName) {
     return getLimelightNTDouble(limelightName, "txnc");
   }
 
   /**
-   * Gets the vertical offset from the principal pixel/point to the target in degrees.
-   * This is the most accurate 2d metric if you are using a calibrated camera and
-   * you don't need adjustable crosshair functionality.
-   * 
+   * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the
+   * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable
+   * crosshair functionality.
+   *
    * @param limelightName Name of the Limelight camera ("" for default)
-   * @return Vertical offset angle in degrees from principal point
+   * @return Vertical offset angle in degrees
    */
   public static double getTYNC(String limelightName) {
     return getLimelightNTDouble(limelightName, "tync");
@@ -1930,8 +977,7 @@ public static double getTYNC(String limelightName) {
 
   /**
    * Gets the target area as a percentage of the image (0-100%).
-   * Larger values indicate target is closer or larger in frame.
-   * 
+   *
    * @param limelightName Name of the Limelight camera ("" for default)
    * @return Target area percentage (0-100)
    */
@@ -1940,22 +986,21 @@ public static double getTA(String limelightName) {
   }
 
   /**
-   * T2D is an array that contains several targeting metrics in one convenient package.
-   * Array format: [targetValid, targetCount, targetLatency, captureLatency, tx, ty,
-   * txnc, tync, ta, tid, targetClassIndexDetector, targetClassIndexClassifier,
-   * targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels,
-   * targetVerticalExtentPixels, targetSkewDegrees]
-   * 
+   * T2D is an array that contains several targeting metrcis
+   *
    * @param limelightName Name of the Limelight camera
-   * @return 17-element array containing comprehensive targeting data
+   * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty,
+   *     txnc, tync, ta, tid, targetClassIndexDetector, targetClassIndexClassifier,
+   *     targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels,
+   *     targetVerticalExtentPixels, targetSkewDegrees]
    */
   public static double[] getT2DArray(String limelightName) {
     return getLimelightNTDoubleArray(limelightName, "t2d");
   }
 
   /**
-   * Gets the number of targets currently detected by the active pipeline.
-   * 
+   * Gets the number of targets currently detected.
+   *
    * @param limelightName Name of the Limelight camera
    * @return Number of detected targets
    */
@@ -1968,8 +1013,8 @@ public static int getTargetCount(String limelightName) {
   }
 
   /**
-   * Gets the classifier class index from the currently running neural classifier pipeline.
-   * 
+   * Gets the classifier class index from the currently running neural classifier pipeline
+   *
    * @param limelightName Name of the Limelight camera
    * @return Class index from classifier pipeline
    */
@@ -1982,9 +1027,9 @@ public static int getClassifierClassIndex(String limelightName) {
   }
 
   /**
-   * Gets the detector class index from the primary result of the currently running
-   * neural detector pipeline.
-   * 
+   * Gets the detector class index from the primary result of the currently running neural detector
+   * pipeline.
+   *
    * @param limelightName Name of the Limelight camera
    * @return Class index from detector pipeline
    */
@@ -1998,9 +1043,9 @@ public static int getDetectorClassIndex(String limelightName) {
 
   /**
    * Gets the current neural classifier result class name.
-   * 
+   *
    * @param limelightName Name of the Limelight camera
-   * @return Class name string from classifier pipeline (e.g., "cone", "cube")
+   * @return Class name string from classifier pipeline
    */
   public static String getClassifierClass(String limelightName) {
     return getLimelightNTString(limelightName, "tcclass");
@@ -2008,9 +1053,9 @@ public static String getClassifierClass(String limelightName) {
 
   /**
    * Gets the primary neural detector result class name.
-   * 
+   *
    * @param limelightName Name of the Limelight camera
-   * @return Class name string from detector pipeline (e.g., "cone", "cube")
+   * @return Class name string from detector pipeline
    */
   public static String getDetectorClass(String limelightName) {
     return getLimelightNTString(limelightName, "tdclass");
@@ -2018,8 +1063,7 @@ public static String getDetectorClass(String limelightName) {
 
   /**
    * Gets the pipeline's processing latency contribution.
-   * This is the time spent processing the image through the vision pipeline.
-   * 
+   *
    * @param limelightName Name of the Limelight camera
    * @return Pipeline latency in milliseconds
    */
@@ -2029,8 +1073,7 @@ public static double getLatency_Pipeline(String limelightName) {
 
   /**
    * Gets the capture latency.
-   * This is the time between when the frame was captured and when processing began.
-   * 
+   *
    * @param limelightName Name of the Limelight camera
    * @return Capture latency in milliseconds
    */
@@ -2040,7 +1083,7 @@ public static double getLatency_Capture(String limelightName) {
 
   /**
    * Gets the active pipeline index.
-   * 
+   *
    * @param limelightName Name of the Limelight camera
    * @return Current pipeline index (0-9)
    */
@@ -2049,19 +1092,18 @@ public static double getCurrentPipelineIndex(String limelightName) {
   }
 
   /**
-   * Gets the current pipeline type string.
-   * 
+   * Gets the current pipeline type.
+   *
    * @param limelightName Name of the Limelight camera
-   * @return Pipeline type string (e.g. "retro", "apriltag", "neural_detector")
+   * @return Pipeline type string (e.g. "retro", "apriltag", etc)
    */
   public static String getCurrentPipelineType(String limelightName) {
     return getLimelightNTString(limelightName, "getpipetype");
   }
 
   /**
-   * Gets the full JSON results dump from the Limelight.
-   * Contains all targeting data in JSON format for advanced parsing.
-   * 
+   * Gets the full JSON results dump.
+   *
    * @param limelightName Name of the Limelight camera
    * @return JSON string containing all current results
    */
@@ -2069,16 +1111,11 @@ public static String getJSONDump(String limelightName) {
     return getLimelightNTString(limelightName, "json");
   }
 
-  /////
-  ///// Pose Getters (Raw Arrays)
-  /////
-
   /**
-   * Gets robot pose array in Limelight coordinate system.
-   * 
-   * @deprecated Use {@link #getBotPose(String)} instead
-   * @param limelightName Name of the Limelight camera
-   * @return 6-element array [x, y, z, roll, pitch, yaw]
+   * Switch to getBotPose
+   *
+   * @param limelightName
+   * @return
    */
   @Deprecated
   public static double[] getBotpose(String limelightName) {
@@ -2086,11 +1123,10 @@ public static double[] getBotpose(String limelightName) {
   }
 
   /**
-   * Gets robot pose array in WPILib red alliance coordinates.
-   * 
-   * @deprecated Use {@link #getBotPose_wpiRed(String)} instead
-   * @param limelightName Name of the Limelight camera
-   * @return 6-element array [x, y, z, roll, pitch, yaw]
+   * Switch to getBotPose_wpiRed
+   *
+   * @param limelightName
+   * @return
    */
   @Deprecated
   public static double[] getBotpose_wpiRed(String limelightName) {
@@ -2098,989 +1134,346 @@ public static double[] getBotpose_wpiRed(String limelightName) {
   }
 
   /**
-   * Gets robot pose array in WPILib blue alliance coordinates.
-   * 
-   * @deprecated Use {@link #getBotPose_wpiBlue(String)} instead
-   * @param limelightName Name of the Limelight camera
-   * @return 6-element array [x, y, z, roll, pitch, yaw]
+   * Switch to getBotPose_wpiBlue
+   *
+   * @param limelightName
+   * @return
    */
   @Deprecated
   public static double[] getBotpose_wpiBlue(String limelightName) {
     return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
   }
 
-  /**
-   * Gets robot pose array in Limelight coordinate system.
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees
-   */
   public static double[] getBotPose(String limelightName) {
     return getLimelightNTDoubleArray(limelightName, "botpose");
   }
 
-  /**
-   * Gets robot pose array in WPILib red alliance coordinate system.
-   * Origin at red alliance driver station wall.
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees
-   */
   public static double[] getBotPose_wpiRed(String limelightName) {
     return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
   }
 
-  /**
-   * Gets robot pose array in WPILib blue alliance coordinate system.
-   * Origin at blue alliance driver station wall. Recommended for most use cases.
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees
-   */
   public static double[] getBotPose_wpiBlue(String limelightName) {
     return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
   }
 
-  /**
-   * Gets robot pose in target space as an array.
-   * Represents where the robot is relative to the detected target.
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees
-   */
   public static double[] getBotPose_TargetSpace(String limelightName) {
     return getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
   }
 
-  /**
-   * Gets camera pose in target space as an array.
-   * Represents where the camera is relative to the detected target.
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees
-   */
   public static double[] getCameraPose_TargetSpace(String limelightName) {
     return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
   }
 
-  /**
-   * Gets target pose in camera space as an array.
-   * Represents where the target is relative to the camera.
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees
-   */
   public static double[] getTargetPose_CameraSpace(String limelightName) {
     return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
   }
 
-  /**
-   * Gets target pose in robot space as an array.
-   * Represents where the target is relative to the robot center.
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @return 6-element array [x, y, z, roll, pitch, yaw] in meters and degrees
-   */
   public static double[] getTargetPose_RobotSpace(String limelightName) {
     return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
   }
 
-  /**
-   * Gets the RGB color values detected by the color pipeline.
-   * Useful for color-based target identification.
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @return Array of color values [R, G, B] (0-255)
-   */
   public static double[] getTargetColor(String limelightName) {
     return getLimelightNTDoubleArray(limelightName, "tc");
   }
 
-  /**
-   * Gets the current pipeline's fiducial ID of the primary target.
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @return Fiducial/AprilTag ID number
-   */
   public static double getFiducialID(String limelightName) {
     return getLimelightNTDouble(limelightName, "tid");
   }
 
-  /**
-   * Gets the neural network classifier class ID string.
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @return Class name string from the neural classifier
-   */
   public static String getNeuralClassID(String limelightName) {
     return getLimelightNTString(limelightName, "tclass");
   }
 
-  /**
-   * Gets raw barcode data as a string array.
-   * Each element contains the decoded string from a detected barcode.
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @return Array of decoded barcode strings
-   */
   public static String[] getRawBarcodeData(String limelightName) {
     return getLimelightNTStringArray(limelightName, "rawbarcodes");
   }
 
   /////
-  ///// Pose Getters (Pose3d Objects)
   /////
 
-  /**
-   * Gets the robot's 3D pose in field space using Limelight's internal coordinate system.
-   * 
-   * 
    This method retrieves the robot's position and orientation as calculated by the Limelight
-   * based on AprilTag detections and the known field layout. The pose is in Limelight's native
-   * coordinate system.
-   * 
-   * 
    Note: For WPILib integration, prefer {@link #getBotPose3d_wpiBlue(String)} which
-   * uses WPILib's standard blue alliance coordinate system.
-   * 
-   * @param limelightName Name/identifier of the Limelight camera (use "" for default "limelight")
-   * @return Pose3d object representing robot position (meters) and orientation (radians)
-   */
   public static Pose3d getBotPose3d(String limelightName) {
-    // Retrieve the 6-element pose array from NetworkTables
     double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose");
-    // Convert array format [x, y, z, roll, pitch, yaw] to Pose3d object
     return toPose3D(poseArray);
   }
 
   /**
-   * Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System.
-   * 
-   * 
    Not Recommended: Red alliance coordinates can be confusing as field orientation
-   * changes between alliances. Use {@link #getBotPose3d_wpiBlue(String)} instead for consistency.
-   * 
-   * 
    In the red alliance coordinate system:
-   * 
      
-   *   
    • Origin (0,0) is at the red alliance driver station wall
      • 
-   *   
    • +X extends toward the blue alliance wall
      • 
-   *   
    • +Y extends to the left when facing the blue alliance wall
      • 
-   *   
    • Yaw of 0° means robot is facing the blue alliance wall
      • 
-   * 
    
-   * 
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return Pose3d in red alliance field space (meters and radians)
+   * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System.
+   *
+   * @param limelightName Name/identifier of the Limelight
+   * @return Pose3d object representing the robot's position and orientation in Red Alliance field
+   *     space
    */
   public static Pose3d getBotPose3d_wpiRed(String limelightName) {
-    // Retrieve red alliance pose from NetworkTables
     double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired");
     return toPose3D(poseArray);
   }
 
   /**
-   * Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System.
-   * 
-   * 
    Recommended: This is the standard coordinate system for WPILib pose estimation
-   * and should be used with {@code SwerveDrivePoseEstimator.addVisionMeasurement()} or
-   * {@code DifferentialDrivePoseEstimator.addVisionMeasurement()}.
-   * 
-   * 
    In the blue alliance coordinate system:
-   * 
      
-   *   
    • Origin (0,0) is at the blue alliance driver station wall
      • 
-   *   
    • +X extends toward the red alliance wall
      • 
-   *   
    • +Y extends to the left when facing the red alliance wall
      • 
-   *   
    • Yaw of 0° means robot is facing the red alliance wall
      • 
-   * 
    
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * Pose3d robotPose = LimelightHelpers.getBotPose3d_wpiBlue("limelight");
-   * // Convert to 2D for odometry: robotPose.toPose2d()
-   * }
    
+   * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System.
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return Pose3d in blue alliance field space (meters and radians)
+   * @param limelightName Name/identifier of the Limelight
+   * @return Pose3d object representing the robot's position and orientation in Blue Alliance field
+   *     space
    */
   public static Pose3d getBotPose3d_wpiBlue(String limelightName) {
-    // Retrieve blue alliance pose from NetworkTables
     double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
     return toPose3D(poseArray);
   }
 
   /**
-   * Gets the robot's 3D pose relative to the currently tracked target's coordinate system.
-   * 
-   * 
    This provides the robot's position and orientation with respect to the detected target
-   * (AprilTag or other fiducial). Useful for target-relative navigation where you want to
-   * approach or align with a specific target.
-   * 
-   * 
    In target space:
-   * 
      
-   *   
    • Origin (0,0,0) is at the target's center
      • 
-   *   
    • Coordinate axes depend on target orientation
      • 
-   *   
    • For AprilTags: +Z extends out from tag face, +X is right, +Y is down
      • 
-   * 
    
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * // Get pose relative to detected AprilTag
-   * Pose3d robotToTag = LimelightHelpers.getBotPose3d_TargetSpace("limelight");
-   * double distanceToTag = robotToTag.getTranslation().getNorm();
-   * }
    
+   * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system.
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return Pose3d representing robot position/orientation relative to the target
+   * @param limelightName Name/identifier of the Limelight
+   * @return Pose3d object representing the robot's position and orientation relative to the target
    */
   public static Pose3d getBotPose3d_TargetSpace(String limelightName) {
-    // Retrieve target-space pose from NetworkTables
     double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
     return toPose3D(poseArray);
   }
 
   /**
-   * Gets the camera's 3D pose relative to the currently tracked target's coordinate system.
-   * 
-   * 
    This provides the camera's position and orientation with respect to the detected target.
-   * Different from {@link #getBotPose3d_TargetSpace(String)} in that it gives the camera
-   * position rather than the robot center position.
-   * 
-   * 
    Useful for:
-   * 
      
-   *   
    • Verifying camera calibration and mounting
      • 
-   *   
    • Direct camera-to-target distance calculations
      • 
-   *   
    • Debugging vision system setup
      • 
-   * 
    
+   * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system.
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return Pose3d representing camera position/orientation relative to the target
+   * @param limelightName Name/identifier of the Limelight
+   * @return Pose3d object representing the camera's position and orientation relative to the target
    */
   public static Pose3d getCameraPose3d_TargetSpace(String limelightName) {
-    // Retrieve camera-to-target pose from NetworkTables
     double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
     return toPose3D(poseArray);
   }
 
   /**
-   * Gets the target's 3D pose relative to the camera's coordinate system.
-   * 
-   * 
    This provides where the target is located from the camera's perspective. Useful for
-   * direct targeting calculations where you want to point the camera (and therefore robot)
-   * at a specific target.
-   * 
-   * 
    In camera space:
-   * 
      
-   *   
    • Origin (0,0,0) is at the camera's optical center
      • 
-   *   
    • +Z extends forward (into the scene)
      • 
-   *   
    • +X extends right, +Y extends down (standard camera coordinates)
      • 
-   * 
    
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * Pose3d targetPose = LimelightHelpers.getTargetPose3d_CameraSpace("limelight");
-   * double targetDistance = targetPose.getTranslation().getZ(); // Forward distance
-   * double targetAngle = Math.atan2(targetPose.getTranslation().getX(), 
-   *                                   targetPose.getTranslation().getZ());
-   * }
    
+   * Gets the target's 3D pose with respect to the camera's coordinate system.
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return Pose3d representing target position/orientation relative to the camera
+   * @param limelightName Name/identifier of the Limelight
+   * @return Pose3d object representing the target's position and orientation relative to the camera
    */
   public static Pose3d getTargetPose3d_CameraSpace(String limelightName) {
-    // Retrieve target-in-camera-space pose from NetworkTables
     double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
     return toPose3D(poseArray);
   }
 
   /**
-   * Gets the target's 3D pose relative to the robot's coordinate system.
-   * 
-   * 
    This provides where the target is located from the robot center's perspective,
-   * accounting for the camera's mounting position on the robot. This is the most useful
-   * target pose for robot-centric navigation and control.
-   * 
-   * 
    In robot space:
-   * 
      
-   *   
    • Origin (0,0,0) is at the robot's center (as configured)
      • 
-   *   
    • +X extends forward from robot front
      • 
-   *   
    • +Y extends left, +Z extends up
      • 
-   * 
    
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * Pose3d targetPose = LimelightHelpers.getTargetPose3d_RobotSpace("limelight");
-   * Translation3d targetTranslation = targetPose.getTranslation();
-   * 
-   * // Calculate angle to target for turret control
-   * double angleToTarget = Math.atan2(targetTranslation.getY(), 
-   *                                     targetTranslation.getX());
-   * // Calculate distance for shooter velocity
-   * double distanceToTarget = targetTranslation.getNorm();
-   * }
    
+   * Gets the target's 3D pose with respect to the robot's coordinate system.
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return Pose3d representing target position/orientation relative to robot center
+   * @param limelightName Name/identifier of the Limelight
+   * @return Pose3d object representing the target's position and orientation relative to the robot
    */
   public static Pose3d getTargetPose3d_RobotSpace(String limelightName) {
-    // Retrieve target-in-robot-space pose from NetworkTables
     double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
     return toPose3D(poseArray);
   }
 
   /**
-   * Gets the camera's 3D pose relative to the robot's coordinate system.
-   * 
-   * 
    This represents where the camera is mounted on the robot. This value should match
-   * your camera's physical mounting position and is used internally for transforming
-   * between camera space and robot space.
-   * 
-   * 
    Useful for:
-   * 
      
-   *   
    • Verifying camera mounting configuration
      • 
-   *   
    • Debugging coordinate transformations
      • 
-   *   
    • Custom pose estimation calculations
      • 
-   * 
    
-   * 
-   * 
    Note: This can be configured via {@link #setCameraPose_RobotSpace(String, double, 
-   * double, double, double, double, double)} or through the Limelight web interface.
+   * Gets the camera's 3D pose with respect to the robot's coordinate system.
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return Pose3d representing camera mounting position/orientation relative to robot center
+   * @param limelightName Name/identifier of the Limelight
+   * @return Pose3d object representing the camera's position and orientation relative to the robot
    */
   public static Pose3d getCameraPose3d_RobotSpace(String limelightName) {
-    // Retrieve camera mounting pose from NetworkTables
     double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace");
     return toPose3D(poseArray);
   }
 
-  /////
-  ///// Pose Getters (Pose2d Objects) - For Odometry Integration
-  /////
-
   /**
-   * Gets the robot's 2D pose in WPILib blue alliance coordinates.
-   * 
-   * 
    This is a convenience method that extracts just the x, y, and yaw from the full 3D pose,
-   * making it ready for direct use with WPILib's 2D odometry and pose estimation systems.
-   * 
-   * 
    Recommended for: Direct integration with WPILib pose estimators
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * // Simple approach - just get the pose
-   * Pose2d visionPose = LimelightHelpers.getBotPose2d_wpiBlue("limelight");
-   * }
    
+   * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement)
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return Pose2d in blue alliance coordinates (meters and radians)
+   * @param limelightName
+   * @return
    */
   public static Pose2d getBotPose2d_wpiBlue(String limelightName) {
-    // Get the raw pose array (includes 3D data)
+
     double[] result = getBotPose_wpiBlue(limelightName);
-    // Convert to 2D, discarding z, roll, pitch
     return toPose2D(result);
   }
 
   /**
-   * Gets the MegaTag1 2D pose estimate with timestamp and quality metrics for WPILib blue alliance.
-   * 
-   * 
    MegaTag1 uses multiple AprilTags simultaneously to compute a more accurate robot
-   * pose. This method returns not just the pose, but also timing and quality information needed
-   * for optimal pose estimator integration.
-   * 
-   * 
    The returned {@link PoseEstimate} includes:
-   * 
      
-   *   
    • Robot pose (Pose2d)
      • 
-   *   
    • FPGA timestamp for proper temporal alignment
      • 
-   *   
    • Total latency (capture + processing)
      • 
-   *   
    • Number of tags used (more is better)
      • 
-   *   
    • Tag span (wider spacing is better)
      • 
-   *   
    • Average tag distance and area (quality metrics)
      • 
-   * 
    
-   * 
-   * 
    Example usage with pose estimator:
-   * 
    {@code
-   * PoseEstimate estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
-   * 
-   * if (LimelightHelpers.validPoseEstimate(estimate)) {
-   *     // Adjust standard deviations based on tag count and distance
-   *     Matrix stdDevs = VecBuilder.fill(
-   *         0.7 / estimate.tagCount,  // X std dev
-   *         0.7 / estimate.tagCount,  // Y std dev  
-   *         9999999                    // Theta std dev (trust gyro more)
-   *     );
-   *     
-   *     poseEstimator.addVisionMeasurement(
-   *         estimate.pose,
-   *         estimate.timestampSeconds,
-   *         stdDevs
-   *     );
-   * }
-   * }
    
+   * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator
+   * (addVisionMeasurement) in the WPILib Blue alliance coordinate system.
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return PoseEstimate with pose, timestamp, and quality metrics, or null if no data available
+   * @param limelightName
+   * @return
    */
   public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
-    // Get pose estimate from the standard botpose_wpiblue entry
     return getBotPoseEstimate(limelightName, "botpose_wpiblue", false);
   }
 
   /**
-   * Gets the MegaTag2 2D pose estimate with timestamp and quality metrics for WPILib blue alliance.
-   * 
-   * 
    MegaTag2 is an advanced localization algorithm that incorporates robot orientation
-   * data (from your gyro/IMU) to improve pose accuracy. This can significantly reduce pose jumps
-   * and improve overall reliability.
-   * 
-   * 
    CRITICAL: You MUST call {@link #SetRobotOrientation(String, double, double, double,
-   * double, double, double)} or {@link #SetRobotOrientation_NoFlush(String, double, double, double,
-   * double, double, double)} before calling this method in your periodic loop. MegaTag2 requires
-   * current robot orientation to function correctly.
-   * 
-   * 
    Advantages of MegaTag2:
-   * 
      
-   *   
    • More stable pose estimates with less jumping
      • 
-   *   
    • Better disambiguation of pose solutions
      • 
-   *   
    • Improved accuracy, especially at longer distances
      • 
-   *   
    • Better handling of partial tag visibility
      • 
-   * 
    
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * // In your subsystem's periodic() method:
-   * 
-   * // 1. Update robot orientation (required for MegaTag2)
-   * LimelightHelpers.SetRobotOrientation(
-   *     "limelight",
-   *     m_gyro.getYaw(),           // Robot yaw
-   *     m_gyro.getRate(),          // Yaw rate (optional but helpful)
-   *     0, 0, 0, 0                 // Pitch and roll (usually not needed)
-   * );
-   * 
-   * // 2. Get MegaTag2 pose estimate
-   * PoseEstimate estimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
-   * 
-   * // 3. Use with pose estimator
-   * if (LimelightHelpers.validPoseEstimate(estimate)) {
-   *     // Calculate dynamic standard deviations based on quality
-   *     double xyStdDev = 0.5;
-   *     if (estimate.tagCount >= 2) xyStdDev = 0.3;
-   *     if (estimate.avgTagDist > 4.0) xyStdDev *= 2;
-   *     
-   *     poseEstimator.addVisionMeasurement(
-   *         estimate.pose,
-   *         estimate.timestampSeconds,
-   *         VecBuilder.fill(xyStdDev, xyStdDev, 9999999)
-   *     );
-   * }
-   * }
    
+   * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator
+   * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. Make sure you are calling
+   * setRobotOrientation() before calling this method.
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return PoseEstimate with pose, timestamp, and quality metrics, or null if no data available
-   * @see #SetRobotOrientation(String, double, double, double, double, double, double)
+   * @param limelightName
+   * @return
    */
   public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) {
-    // Get pose estimate from the MegaTag2 (ORB = Orientation-based Robot Localization) entry
     return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true);
   }
 
   /**
-   * Gets the robot's 2D pose in WPILib red alliance coordinates.
-   * 
-   * 
    Not Recommended: Use {@link #getBotPose2d_wpiBlue(String)} instead for consistency.
-   * 
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return Pose2d in red alliance coordinates (meters and radians)
+   * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement)
+   *
+   * @param limelightName
+   * @return
    */
   public static Pose2d getBotPose2d_wpiRed(String limelightName) {
-    // Get raw red alliance pose array
+
     double[] result = getBotPose_wpiRed(limelightName);
-    // Convert to 2D
     return toPose2D(result);
   }
 
-  /**
-   * Gets a basic pose estimate in Limelight's internal coordinate system.
-   * 
-   * 
    For most FRC applications, use {@link #getBotPoseEstimate_wpiBlue(String)} instead
-   * which provides the pose in WPILib's standard blue alliance coordinates.
-   * 
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return PoseEstimate in Limelight coordinates, or null if no data available
-   */
   public static PoseEstimate getBotPoseEstimate(String limelightName) {
-    // Get pose estimate from the standard botpose entry (Limelight coordinates)
     return getBotPoseEstimate(limelightName, "botpose", false);
   }
 
   /**
-   * Gets the MegaTag1 pose estimate for red alliance coordinates.
-   * 
-   * 
    Not Recommended: Use {@link #getBotPoseEstimate_wpiBlue(String)} instead.
+   * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when
+   * you are on the RED alliance
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return PoseEstimate in red alliance coordinates, or null if no data available
+   * @param limelightName
+   * @return
    */
   public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
-    // Get pose estimate from red alliance entry
     return getBotPoseEstimate(limelightName, "botpose_wpired", false);
   }
 
   /**
-   * Gets the MegaTag2 pose estimate for red alliance coordinates.
-   * 
-   * 
    Not Recommended: Use {@link #getBotPoseEstimate_wpiBlue_MegaTag2(String)} instead.
-   * 
-   * 
    CRITICAL: You MUST call {@link #SetRobotOrientation(String, double, double, double,
-   * double, double, double)} before using this method.
+   * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when
+   * you are on the RED alliance
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return PoseEstimate in red alliance coordinates, or null if no data available
+   * @param limelightName
+   * @return
    */
   public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) {
-    // Get MegaTag2 pose estimate from red alliance ORB entry
     return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true);
   }
 
   /**
-   * Gets the robot's 2D pose in Limelight's internal coordinate system.
-   * 
-   * 
    For WPILib integration, prefer {@link #getBotPose2d_wpiBlue(String)} instead.
+   * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement)
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return Pose2d in Limelight coordinates (meters and radians)
+   * @param limelightName
+   * @return
    */
   public static Pose2d getBotPose2d(String limelightName) {
-    // Get raw pose array in Limelight coordinates
+
     double[] result = getBotPose(limelightName);
-    // Convert to 2D
     return toPose2D(result);
   }
 
-  /////
-  ///// IMU Data
-  /////
-
   /**
-   * Gets the current IMU (Inertial Measurement Unit) data from the Limelight.
-   * 
-   * 
    Limelight 3 and newer models include a built-in 6-axis IMU that provides orientation
-   * and motion data. This IMU can be used for robot localization, particularly with MegaTag2.
-   * 
-   * 
    The IMU provides:
-   * 
      
-   *   
    • Orientation: Roll, pitch, and yaw angles
      • 
-   *   
    • Gyroscope: Angular velocities around X, Y, Z axes
      • 
-   *   
    • Accelerometer: Linear accelerations along X, Y, Z axes
      • 
-   * 
    
-   * 
-   * 
    Coordinate System:
-   * 
      
-   *   
    • X: Right (from camera perspective)
      • 
-   *   
    • Y: Down
      • 
-   *   
    • Z: Forward (into scene)
      • 
-   * 
    
-   * 
-   * 
    Units:
-   * 
      
-   *   
    • Angles: Degrees
      • 
-   *   
    • Angular velocities: Degrees per second
      • 
-   *   
    • Accelerations: G's (1G = 9.81 m/s²)
      • 
-   * 
    
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * IMUData imu = LimelightHelpers.getIMUData("limelight");
-   * 
-   * // Get robot heading
-   * double heading = imu.robotYaw;
-   * 
-   * // Get angular velocity for velocity tracking
-   * double turnRate = imu.gyroZ;
-   * 
-   * // Detect high acceleration events
-   * double totalAccel = Math.sqrt(
-   *     imu.accelX * imu.accelX + 
-   *     imu.accelY * imu.accelY + 
-   *     imu.accelZ * imu.accelZ
-   * );
-   * }
    
-   * 
-   * 
    Note: Returns an IMUData object with all zeros if data is invalid or unavailable.
-   * Always check if your Limelight model supports IMU before relying on this data.
+   * Gets the current IMU data from NetworkTables. IMU data is formatted as [robotYaw, Roll, Pitch,
+   * Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. Returns all zeros if data is invalid or
+   * unavailable.
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @return IMUData object containing all current IMU measurements, or all zeros if unavailable
+   * @param limelightName Name/identifier of the Limelight
+   * @return IMUData object containing all current IMU data
    */
   public static IMUData getIMUData(String limelightName) {
-    // Retrieve IMU array from NetworkTables
-    // Format: [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]
     double[] imuData = getLimelightNTDoubleArray(limelightName, "imu");
-    
-    // Validate array length (should be 10 elements)
     if (imuData == null || imuData.length < 10) {
-      // Return zeroed IMUData if data is missing or invalid
-      return new IMUData();
+      return new IMUData(); // Returns object with all zeros
     }
-    
-    // Parse array into IMUData object
     return new IMUData(imuData);
   }
 
   /////
-  ///// Camera Control Methods
   /////
 
-  /**
-   * Sets the active vision pipeline.
-   * 
-   * 
    Limelight supports up to 10 pipelines (0-9), each with different vision processing
-   * settings. Use this to switch between different tracking modes during a match.
-   * 
-   * 
    Common pipeline uses:
-   * 
      
-   *   
    • Pipeline 0: AprilTag tracking for localization
      • 
-   *   
    • Pipeline 1: Retroreflective target tracking
      • 
-   *   
    • Pipeline 2: Neural network detector for game pieces
      • 
-   * 
    
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * // Switch to AprilTag pipeline during auto
-   * LimelightHelpers.setPipelineIndex("limelight", 0);
-   * 
-   * // Switch to game piece detector during teleop
-   * LimelightHelpers.setPipelineIndex("limelight", 2);
-   * }
    
-   *
-   * @param limelightName Name of the Limelight camera
-   * @param pipelineIndex Pipeline index to activate (0-9)
-   */
   public static void setPipelineIndex(String limelightName, int pipelineIndex) {
     setLimelightNTDouble(limelightName, "pipeline", pipelineIndex);
   }
 
-  /**
-   * Sets the priority AprilTag ID for tracking.
-   * 
-   * 
    When multiple AprilTags are visible, setting a priority ID tells the Limelight to
-   * prefer that specific tag for targeting calculations. The primary target will be the
-   * priority tag if it's visible, otherwise it falls back to normal sorting.
-   * 
-   * 
    Use cases:
-   * 
      
-   *   
    • Targeting a specific scoring location
      • 
-   *   
    • Prioritizing closer/more relevant field elements
      • 
-   *   
    • Improving localization with strategically placed tags
      • 
-   * 
    
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * // Target tag #4 (e.g., speaker center tag)
-   * LimelightHelpers.setPriorityTagID("limelight", 4);
-   * 
-   * // Clear priority (set to -1 or use any invalid ID)
-   * LimelightHelpers.setPriorityTagID("limelight", -1);
-   * }
    
-   *
-   * @param limelightName Name of the Limelight camera
-   * @param ID AprilTag ID to prioritize, or -1 to clear priority
-   */
   public static void setPriorityTagID(String limelightName, int ID) {
     setLimelightNTDouble(limelightName, "priorityid", ID);
   }
 
-  /////
-  ///// LED Control Methods
-  /////
-
   /**
-   * Sets LED mode to be controlled by the current pipeline's configuration.
-   * 
-   * 
    Each pipeline can have its own LED mode setting configured in the Limelight web interface.
-   * This is the default and recommended mode - let each pipeline control its own LEDs based on
-   * what tracking mode is active.
-   * 
-   * 
    Example: Your AprilTag pipeline might have LEDs off, while your retroreflective
-   * pipeline has them on.
+   * Sets LED mode to be controlled by the current pipeline.
    *
    * @param limelightName Name of the Limelight camera
    */
   public static void setLEDMode_PipelineControl(String limelightName) {
-    // ledMode 0 = pipeline control
     setLimelightNTDouble(limelightName, "ledMode", 0);
   }
 
-  /**
-   * Forces the LEDs off regardless of pipeline settings.
-   * 
-   * 
    Useful for conserving power, reducing visual distractions, or when LEDs aren't needed
-   * (e.g., when using AprilTags or neural networks that don't require illumination).
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * // Turn off LEDs during auto when using AprilTags
-   * LimelightHelpers.setLEDMode_ForceOff("limelight");
-   * }
    
-   *
-   * @param limelightName Name of the Limelight camera
-   */
   public static void setLEDMode_ForceOff(String limelightName) {
-    // ledMode 1 = force off
     setLimelightNTDouble(limelightName, "ledMode", 1);
   }
 
-  /**
-   * Forces the LEDs to blink regardless of pipeline settings.
-   * 
-   * 
    Creates a blinking pattern useful for visual feedback or signaling. Can be used to
-   * indicate robot states, warn drivers, or debug vision system operation.
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * // Blink LEDs when target is acquired
-   * if (LimelightHelpers.getTV("limelight")) {
-   *     LimelightHelpers.setLEDMode_ForceBlink("limelight");
-   * }
-   * }
    
-   *
-   * @param limelightName Name of the Limelight camera
-   */
   public static void setLEDMode_ForceBlink(String limelightName) {
-    // ledMode 2 = force blink
     setLimelightNTDouble(limelightName, "ledMode", 2);
   }
 
-  /**
-   * Forces the LEDs on at full brightness regardless of pipeline settings.
-   * 
-   * 
    Ensures LEDs are always on, overriding pipeline configuration. Necessary for retroreflective
-   * tracking where consistent illumination is critical for reliable target detection.
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * // Ensure LEDs are on for retroreflective tape tracking
-   * LimelightHelpers.setLEDMode_ForceOn("limelight");
-   * }
    
-   *
-   * @param limelightName Name of the Limelight camera
-   */
   public static void setLEDMode_ForceOn(String limelightName) {
-    // ledMode 3 = force on
     setLimelightNTDouble(limelightName, "ledMode", 3);
   }
 
-  /////
-  ///// Stream Control Methods
-  /////
-
   /**
    * Enables standard side-by-side stream mode.
-   * 
-   * 
    Displays both the primary and secondary camera streams (or processing views) side by side.
-   * This is the default streaming mode and is useful for seeing both the raw camera feed and
-   * the processed output simultaneously.
-   * 
-   * 
    Stream modes:
-   * 
      
-   *   
    • Standard: Side-by-side view of both streams
      • 
-   *   
    • PiP Main: Secondary stream in corner of primary
      • 
-   *   
    • PiP Secondary: Primary stream in corner of secondary
      • 
-   * 
    
    *
    * @param limelightName Name of the Limelight camera
    */
   public static void setStreamMode_Standard(String limelightName) {
-    // stream 0 = standard side-by-side
     setLimelightNTDouble(limelightName, "stream", 0);
   }
 
   /**
-   * Enables Picture-in-Picture mode with the secondary stream displayed in the corner.
-   * 
-   * 
    Shows the primary stream in full view with the secondary stream as a small overlay
-   * in the corner. Useful when you want to focus on one view but still monitor the other.
+   * Enables Picture-in-Picture mode with secondary stream in the corner.
    *
    * @param limelightName Name of the Limelight camera
    */
   public static void setStreamMode_PiPMain(String limelightName) {
-    // stream 1 = PiP with secondary in corner
     setLimelightNTDouble(limelightName, "stream", 1);
   }
 
   /**
-   * Enables Picture-in-Picture mode with the primary stream displayed in the corner.
-   * 
-   * 
    Shows the secondary stream in full view with the primary stream as a small overlay
-   * in the corner. Useful for monitoring processed output while viewing raw camera feed.
+   * Enables Picture-in-Picture mode with primary stream in the corner.
    *
    * @param limelightName Name of the Limelight camera
    */
   public static void setStreamMode_PiPSecondary(String limelightName) {
-    // stream 2 = PiP with primary in corner
     setLimelightNTDouble(limelightName, "stream", 2);
   }
 
-  /////
-  ///// Advanced Camera Configuration
-  /////
-
   /**
-   * Sets a dynamic crop window for the camera to limit the processing region.
-   * 
-   * 
    Cropping restricts vision processing to a specific region of the camera's field of view.
-   * This can significantly improve performance by reducing the area that needs to be analyzed,
-   * and can help eliminate false detections from irrelevant parts of the image.
-   * 
-   * 
    IMPORTANT: The crop window in the Limelight web interface must be set to fully
-   * open (uncropped) for this dynamic cropping to work. This method overrides the UI setting.
-   * 
-   * 
    Coordinate System:
-   * 
      
-   *   
    • -1.0 = left/bottom edge of image
      • 
-   *   
    • 0.0 = center of image
      • 
-   *   
    • +1.0 = right/top edge of image
      • 
-   * 
    
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * // Crop to center 50% of image (horizontally and vertically)
-   * LimelightHelpers.setCropWindow("limelight", -0.25, 0.25, -0.25, 0.25);
-   * 
-   * // Crop to upper half of image (useful for high targets)
-   * LimelightHelpers.setCropWindow("limelight", -1.0, 1.0, 0.0, 1.0);
-   * 
-   * // Crop to right side (useful for side-mounted camera)
-   * LimelightHelpers.setCropWindow("limelight", 0.0, 1.0, -1.0, 1.0);
-   * 
-   * // Reset to full image
-   * LimelightHelpers.setCropWindow("limelight", -1.0, 1.0, -1.0, 1.0);
-   * }
    
+   * Sets the crop window for the camera. The crop window in the UI must be completely open.
    *
    * @param limelightName Name of the Limelight camera
-   * @param cropXMin Minimum X value - left edge of crop region (-1 to 1)
-   * @param cropXMax Maximum X value - right edge of crop region (-1 to 1)
-   * @param cropYMin Minimum Y value - bottom edge of crop region (-1 to 1)
-   * @param cropYMax Maximum Y value - top edge of crop region (-1 to 1)
+   * @param cropXMin Minimum X value (-1 to 1)
+   * @param cropXMax Maximum X value (-1 to 1)
+   * @param cropYMin Minimum Y value (-1 to 1)
+   * @param cropYMax Maximum Y value (-1 to 1)
    */
   public static void setCropWindow(
       String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) {
-    // Build array with crop boundaries
     double[] entries = new double[4];
     entries[0] = cropXMin;
     entries[1] = cropXMax;
     entries[2] = cropYMin;
     entries[3] = cropYMax;
-    // Send to NetworkTables
     setLimelightNTDoubleArray(limelightName, "crop", entries);
   }
 
-  /**
-   * Sets a 3D offset point for fiducial (AprilTag) tracking.
-   * 
-   * 
    This allows you to track a point offset from the center of the AprilTag. Instead of
-   * getting the robot's pose relative to the tag center, you can get the pose relative to
-   * a point of interest near the tag (e.g., a scoring position).
-   * 
-   * 
    Use cases:
-   * 
      
-   *   
    • Tracking a scoring position that's offset from the tag
      • 
-   *   
    • Targeting a specific game element near a tag
      • 
-   *   
    • Accounting for tag placement relative to field features
      • 
-   * 
    
-   * 
-   * 
    Coordinate System: Offsets are relative to the tag's coordinate system:
-   * 
      
-   *   
    • +X extends right from tag
      • 
-   *   
    • +Y extends down from tag
      • 
-   *   
    • +Z extends out from tag face (toward camera)
      • 
-   * 
    
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * // Track a point 0.5m to the right and 0.3m forward from the tag
-   * LimelightHelpers.setFiducial3DOffset("limelight", 0.5, 0.0, 0.3);
-   * 
-   * // Reset to tag center
-   * LimelightHelpers.setFiducial3DOffset("limelight", 0.0, 0.0, 0.0);
-   * }
    
-   * 
-   * @param limelightName Name of the Limelight camera
-   * @param offsetX X offset in meters (right is positive)
-   * @param offsetY Y offset in meters (down is positive)
-   * @param offsetZ Z offset in meters (away from tag is positive)
-   */
+  /** Sets 3D offset point for easy 3D targeting. */
   public static void setFiducial3DOffset(
       String limelightName, double offsetX, double offsetY, double offsetZ) {
-    // Build offset array [x, y, z]
     double[] entries = new double[3];
     entries[0] = offsetX;
     entries[1] = offsetY;
     entries[2] = offsetZ;
-    // Send to NetworkTables
     setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries);
   }
 
-  /////
-  ///// MegaTag2 Configuration
-  /////
-
   /**
-   * Sets robot orientation values for the MegaTag2 localization algorithm.
-   * 
-   * 
    CRITICAL for MegaTag2: This method MUST be called periodically (in your subsystem's
-   * periodic() or in your main robot periodic()) before retrieving MegaTag2 pose estimates.
-   * MegaTag2 uses robot orientation to improve pose accuracy and disambiguate solutions.
-   * 
-   * 
    This method automatically flushes NetworkTables to ensure the orientation data is
-   * sent immediately. For better performance when setting multiple values, use
-   * {@link #SetRobotOrientation_NoFlush(String, double, double, double, double, double, double)}
-   * and call {@link #Flush()} once after all updates.
-   * 
-   * 
    Coordinate System: All angles follow standard FRC conventions:
-   * 
      
-   *   
    • Yaw: 0° = facing red alliance wall, increases counterclockwise
      • 
-   *   
    • Pitch: 0° = level, positive = nose up
      • 
-   *   
    • Roll: 0° = level, positive = right side down
      • 
-   * 
    
-   * 
-   * 
    Parameter Importance:
-   * 
      
-   *   
    • yaw: REQUIRED - This is the most critical parameter
      • 
-   *   
    • yawRate: Helpful but optional - improves motion prediction
      • 
-   *   
    • pitch, pitchRate, roll, rollRate: Usually unnecessary for ground robots
      • 
-   * 
    
-   * 
-   * 
    Example usage (minimal - just yaw):
-   * 
    {@code
-   * // In your vision subsystem's periodic():
-   * public void periodic() {
-   *     // Update robot orientation for MegaTag2
-   *     LimelightHelpers.SetRobotOrientation(
-   *         "limelight",
-   *         m_gyro.getYaw(),  // Robot heading from gyro
-   *         0, 0, 0, 0, 0     // Other parameters can be zero
-   *     );
-   *     
-   *     // Now safe to use MegaTag2
-   *     PoseEstimate pose = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
-   * }
-   * }
    
-   * 
-   * 
    Example usage (with yaw rate):
-   * 
    {@code
-   * LimelightHelpers.SetRobotOrientation(
-   *     "limelight",
-   *     m_gyro.getYaw(),      // Current heading
-   *     m_gyro.getRate(),     // Angular velocity (helps with moving robots)
-   *     0, 0, 0, 0            // Pitch and roll usually not needed
-   * );
-   * }
    
+   * Sets robot orientation values used by MegaTag2 localization algorithm.
    *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @param yaw Robot yaw angle in degrees (0 = facing red alliance wall)
-   * @param yawRate Angular velocity of yaw in degrees per second (optional, can be 0)
-   * @param pitch Robot pitch angle in degrees (optional, usually 0 for ground robots)
-   * @param pitchRate Angular velocity of pitch in degrees per second (optional, usually 0)
-   * @param roll Robot roll angle in degrees (optional, usually 0 for ground robots)
-   * @param rollRate Angular velocity of roll in degrees per second (optional, usually 0)
+   * @param limelightName Name/identifier of the Limelight
+   * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC
+   * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second
+   * @param pitch (Unnecessary) Robot pitch in degrees
+   * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second
+   * @param roll (Unnecessary) Robot roll in degrees
+   * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second
    */
   public static void SetRobotOrientation(
       String limelightName,
@@ -3090,37 +1483,10 @@ public static void SetRobotOrientation(
       double pitchRate,
       double roll,
       double rollRate) {
-    // Call internal method with flush enabled
     SetRobotOrientation_INTERNAL(
         limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true);
   }
 
-  /**
-   * Sets robot orientation for MegaTag2 without automatically flushing NetworkTables.
-   * 
-   * 
    Use this variant when setting multiple NetworkTables values in sequence to improve
-   * performance. You must manually call {@link #Flush()} after all updates to ensure
-   * the data is sent to the Limelight.
-   * 
-   * 
    Example usage:
-   * 
    {@code
-   * // Set multiple values efficiently
-   * LimelightHelpers.SetRobotOrientation_NoFlush("limelight", yaw, yawRate, 0, 0, 0, 0);
-   * LimelightHelpers.setPipelineIndex("limelight", 0);
-   * LimelightHelpers.setPriorityTagID("limelight", 4);
-   * // Flush once after all updates
-   * LimelightHelpers.Flush();
-   * }
    
-   *
-   * @param limelightName Name/identifier of the Limelight camera
-   * @param yaw Robot yaw angle in degrees
-   * @param yawRate Angular velocity of yaw in degrees per second
-   * @param pitch Robot pitch angle in degrees
-   * @param pitchRate Angular velocity of pitch in degrees per second
-   * @param roll Robot roll angle in degrees
-   * @param rollRate Angular velocity of roll in degrees per second
-   * @see #SetRobotOrientation(String, double, double, double, double, double, double)
-   */
   public static void SetRobotOrientation_NoFlush(
       String limelightName,
       double yaw,
@@ -3129,26 +1495,10 @@ public static void SetRobotOrientation_NoFlush(
       double pitchRate,
       double roll,
       double rollRate) {
-    // Call internal method with flush disabled
     SetRobotOrientation_INTERNAL(
         limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false);
   }
 
-  /**
-   * Internal implementation for setting robot orientation.
-   * 
-   * 
    Handles packing orientation data into an array and optionally flushing NetworkTables.
-   * This method is private and called by the public SetRobotOrientation methods.
-   * 
-   * @param limelightName Name/identifier of the Limelight camera
-   * @param yaw Robot yaw angle in degrees
-   * @param yawRate Angular velocity of yaw in degrees per second
-   * @param pitch Robot pitch angle in degrees
-   * @param pitchRate Angular velocity of pitch in degrees per second
-   * @param roll Robot roll angle in degrees
-   * @param rollRate Angular velocity of roll in degrees per second
-   * @param flush Whether to flush NetworkTables after setting the value
-   */
   private static void SetRobotOrientation_INTERNAL(
       String limelightName,
       double yaw,
@@ -3159,8 +1509,6 @@ private static void SetRobotOrientation_INTERNAL(
       double rollRate,
       boolean flush) {
 
-    // Pack orientation data into array format expected by Limelight
-    // Array order: [yaw, yawRate, pitch, pitchRate, roll, rollRate]
     double[] entries = new double[6];
     entries[0] = yaw;
     entries[1] = yawRate;
@@ -3168,11 +1516,7 @@ private static void SetRobotOrientation_INTERNAL(
     entries[3] = pitchRate;
     entries[4] = roll;
     entries[5] = rollRate;
-    
-    // Send to NetworkTables under the robot_orientation_set key
     setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries);
-    
-    // Flush if requested to ensure immediate transmission
     if (flush) {
       Flush();
     }
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java b/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java
index 5de2205c..b7ea4b7b 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java
@@ -74,8 +74,8 @@ public interface LocationalData {
   /**
    * Capture latency in milliseconds.
    *
-   * 
    This is the time between the end of the exposure of the middle row and the beginning of
-   * the tracking loop.
+   * 
    This is the time between the end of the exposure of the middle row and the beginning of the
+   * tracking loop.
    *
    * @deprecated Use {@link #getBotPoseEstimateBlue()} or {@link #getBotPoseEstimateRed()} instead
    */
@@ -99,7 +99,8 @@ public interface LocationalData {
   OptionalDouble getTimestamp();
 
   /**
-   * Returns the sum of capture latency and targeting latency in milliseconds, if both are available.
+   * Returns the sum of capture latency and targeting latency in milliseconds, if both are
+   * available.
    *
    * @return the total latency in milliseconds
    * @deprecated Use {@link #getBotPoseEstimateBlue()} or {@link #getBotPoseEstimateRed()} instead
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java b/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java
index 3cc1c51a..33c97f50 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java
@@ -6,38 +6,39 @@
 
 /**
  * Utility class for working with {@link Optional} values.
- * 
    
- * Provides methods to convert boxed {@link Optional} types to their corresponding primitive
+ *
+ * 
    Provides methods to convert boxed {@link Optional} types to their corresponding primitive
  * {@code Optional} types. This class is non-instantiable.
- * 
    
  */
 final class Optionals {
 
-    /**
-     * Private constructor to prevent instantiation.
-     * Invoking this constructor will always throw an {@link AssertionError}.
-     */
-    private Optionals() {
-        throw new AssertionError("cannot create Optionals instance");
-    }
+  /**
+   * Private constructor to prevent instantiation. Invoking this constructor will always throw an
+   * {@link AssertionError}.
+   */
+  private Optionals() {
+    throw new AssertionError("cannot create Optionals instance");
+  }
 
-    /**
-     * Converts an {@link Optional} of {@link Long} to an {@link OptionalLong}.
-     *
-     * @param val the {@code Optional} to convert
-     * @return an {@code OptionalLong} containing the value if present, or an empty {@code OptionalLong} if not
-     */
-    static OptionalLong unboxLong(Optional val) {
-        return val.map(OptionalLong::of).orElseGet(OptionalLong::empty);
-    }
+  /**
+   * Converts an {@link Optional} of {@link Long} to an {@link OptionalLong}.
+   *
+   * @param val the {@code Optional} to convert
+   * @return an {@code OptionalLong} containing the value if present, or an empty {@code
+   *     OptionalLong} if not
+   */
+  static OptionalLong unboxLong(Optional val) {
+    return val.map(OptionalLong::of).orElseGet(OptionalLong::empty);
+  }
 
-    /**
-     * Converts an {@link Optional} of {@link Double} to an {@link OptionalDouble}.
-     *
-     * @param val the {@code Optional} to convert
-     * @return an {@code OptionalDouble} containing the value if present, or an empty {@code OptionalDouble} if not
-     */
-    static OptionalDouble unboxDouble(Optional val) {
-        return val.map(OptionalDouble::of).orElseGet(OptionalDouble::empty);
-    }
+  /**
+   * Converts an {@link Optional} of {@link Double} to an {@link OptionalDouble}.
+   *
+   * @param val the {@code Optional} to convert
+   * @return an {@code OptionalDouble} containing the value if present, or an empty {@code
+   *     OptionalDouble} if not
+   */
+  static OptionalDouble unboxDouble(Optional val) {
+    return val.map(OptionalDouble::of).orElseGet(OptionalDouble::empty);
+  }
 }
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java b/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java
index 1d534b78..277501b0 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java
@@ -20,23 +20,24 @@
 import org.json.JSONObject;
 
 /**
- * REST-based implementation of the Limelight interface for communicating with a Limelight vision camera.
- * This class manages a connection to a Limelight device over HTTP and provides methods for retrieving
- * vision targeting data, robot pose estimates, and AprilTag information.
- * 
- * 
    Instances are managed through static factory methods and are cached based on their network address.
- * The class uses a scheduled executor service to periodically collect data from the Limelight device.
+ * REST-based implementation of the Limelight interface for communicating with a Limelight vision
+ * camera. This class manages a connection to a Limelight device over HTTP and provides methods for
+ * retrieving vision targeting data, robot pose estimates, and AprilTag information.
+ *
+ * 
    Instances are managed through static factory methods and are cached based on their network
+ * address. The class uses a scheduled executor service to periodically collect data from the
+ * Limelight device.
  */
 class RestLimelight implements Limelight {
   /** Map of cached Limelight instances keyed by their network address. */
   private static final Map limelights = new HashMap<>();
-  
+
   /** Helper for managing AprilTag field map poses. */
   private final AprilTagMapPoseHelper aprilTagMapPoseHelper;
-  
+
   /** Thread for collecting data from the Limelight. */
   private final DataCollection collectionThread;
-  
+
   /** Shared executor service for all Limelight instances. */
   private static final ScheduledExecutorService executor = Executors.newScheduledThreadPool(2);
 
@@ -51,7 +52,7 @@ class RestLimelight implements Limelight {
 
   /**
    * Creates a new RestLimelight instance with the specified network address.
-   * 
+   *
    * @param address the hostname or IP address of the Limelight device
    */
   RestLimelight(String address) {
@@ -61,8 +62,8 @@ class RestLimelight implements Limelight {
   }
 
   /**
-   * Starts the periodic data collection thread if it hasn't been started already.
-   * The thread runs at a fixed rate of 40ms with an initial delay of 20ms.
+   * Starts the periodic data collection thread if it hasn't been started already. The thread runs
+   * at a fixed rate of 40ms with an initial delay of 20ms.
    */
   void start() {
     if (!started) {
@@ -72,16 +73,14 @@ void start() {
   }
 
   /**
-   * Manually runs one iteration of the data collection thread.
-   * This is primarily useful for testing purposes.
+   * Manually runs one iteration of the data collection thread. This is primarily useful for testing
+   * purposes.
    */
   void runThread() {
     collectionThread.run();
   }
 
-  /**
-   * {@inheritDoc}
-   */
+  /** {@inheritDoc} */
   @Override
   public Optional getJsonDump() {
     return collectionThread.getMostRecent().map(DataCollection.Result::json);
@@ -89,16 +88,15 @@ public Optional getJsonDump() {
 
   /**
    * Gets the capture latency of the most recent frame from the Limelight.
-   * 
-   * @return an OptionalDouble containing the capture latency in milliseconds, or empty if unavailable
+   *
+   * @return an OptionalDouble containing the capture latency in milliseconds, or empty if
+   *     unavailable
    */
   public OptionalDouble getCaptureLatency() {
     return getLocationalData().getCaptureLatency();
   }
 
-  /**
-   * {@inheritDoc}
-   */
+  /** {@inheritDoc} */
   @Override
   public OptionalDouble getTimestamp() {
     return getLocationalData().getTimestamp();
@@ -117,9 +115,7 @@ public void setFieldMap(InputStream stream, boolean updateLimelight) throws IOEx
     aprilTagMapPoseHelper.setFieldMap(stream, updateLimelight);
   }
 
-  /**
-   * {@inheritDoc}
-   */
+  /** {@inheritDoc} */
   @Override
   public List getLocatedAprilTags(Set visibleTags) {
     return aprilTagMapPoseHelper.getVisibleTagPoses(visibleTags);
@@ -127,7 +123,7 @@ public List getLocatedAprilTags(Set visibleTags) {
 
   /**
    * Creates a negation function that inverts the boolean result of another function.
-   * 
+   *
    * @param  the type of the function input
    * @param fnc the function to negate
    * @return a function that returns the opposite boolean value
@@ -138,7 +134,7 @@ private static  Function not(Function fnc) {
 
   /**
    * Checks if the Limelight currently has a valid target in view.
-   * 
+   *
    * @return true if a target is detected, false otherwise
    */
   public boolean hasTarget() {
@@ -147,9 +143,9 @@ public boolean hasTarget() {
 
   /**
    * Gets the most recent locational data from the Limelight.
-   * 
-   * @return LocationalData containing pose estimates, tag information, and latency data,
-   *         or a stub with valid defaults if no data is available
+   *
+   * @return LocationalData containing pose estimates, tag information, and latency data, or a stub
+   *     with valid defaults if no data is available
    */
   public LocationalData getLocationalData() {
     Optional locationalData =
@@ -158,8 +154,8 @@ public LocationalData getLocationalData() {
   }
 
   /**
-   * Cleans up resources by canceling the data collection thread and waiting for termination.
-   * Logs any interruption errors to the DriverStation.
+   * Cleans up resources by canceling the data collection thread and waiting for termination. Logs
+   * any interruption errors to the DriverStation.
    */
   private void clean() {
     try {
@@ -199,8 +195,8 @@ public static Limelight getLimelight(String limelightAddress) {
   }
 
   /**
-   * Clears all cached Limelight instances and cleans up their resources.
-   * This method should be called when shutting down or resetting the system.
+   * Clears all cached Limelight instances and cleans up their resources. This method should be
+   * called when shutting down or resetting the system.
    */
   static void eraseInstances() {
     for (RestLimelight limelight : limelights.values()) {
@@ -210,19 +206,20 @@ static void eraseInstances() {
   }
 
   /**
-   * Implementation of LocationalData that parses data from JSON responses received from the Limelight.
-   * This class provides access to robot pose estimates, AprilTag information, and timing data.
+   * Implementation of LocationalData that parses data from JSON responses received from the
+   * Limelight. This class provides access to robot pose estimates, AprilTag information, and timing
+   * data.
    */
   private class RestLocationalData implements LocationalData {
     /** The root JSON object containing all Limelight data. */
     private final JSONObject root;
-    
+
     /** The timestamp when the response was received, in seconds. */
     private final double responseTimestamp;
 
     /**
      * Creates a new RestLocationalData instance from a data collection result.
-     * 
+     *
      * @param result the result containing JSON data and timestamp
      */
     RestLocationalData(DataCollection.Result result) {
@@ -230,17 +227,13 @@ private class RestLocationalData implements LocationalData {
       this.responseTimestamp = result.responseTimestamp();
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public boolean isValid() {
       return getBooleanFromInt(root, "v").orElse(false);
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public boolean hasTarget() {
       return getArr(root, "Fiducial").map(not(JSONArray::isEmpty)).orElse(false);
@@ -248,7 +241,7 @@ public boolean hasTarget() {
 
     /**
      * Checks if a pose array is invalid or contains only zero values.
-     * 
+     *
      * @param arr the JSON array to validate
      * @return true if the array is invalid (wrong length, no target, or all zeros), false otherwise
      */
@@ -268,10 +261,9 @@ private boolean invalidArray(JSONArray arr) {
     }
 
     /**
-     * Parses a JSON array into a Pose3d object.
-     * The array is expected to contain 6 elements: [x, y, z, roll, pitch, yaw] where
-     * rotation values are in degrees.
-     * 
+     * Parses a JSON array into a Pose3d object. The array is expected to contain 6 elements: [x, y,
+     * z, roll, pitch, yaw] where rotation values are in degrees.
+     *
      * @param arr the JSON array containing pose data
      * @return an Optional containing the parsed Pose3d, or empty if the array is invalid
      */
@@ -288,57 +280,43 @@ private Optional parseArr(JSONArray arr) {
           new Pose3d(arr.getDouble(0), arr.getDouble(1), arr.getDouble(2), rotation));
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public OptionalDouble getTimestamp() {
       return unboxDouble(getDouble(root, "ts"));
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public OptionalDouble getCaptureLatency() {
       return unboxDouble(getDouble(root, "cl"));
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public OptionalDouble getTargetingLatency() {
       return unboxDouble(getDouble(root, "tl"));
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public Optional getBotpose() {
       return getArr(root, "botpose").flatMap(this::parseArr);
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public Optional getBotPoseEstimate() {
       return getArr(root, "botpose").flatMap(this::parseArr).map(this::toBotPoseEstimate);
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public Optional getBotposeBlue() {
       return getArr(root, "botpose_wpiblue").flatMap(this::parseArr);
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public Optional getBotPoseEstimateBlue() {
       return getArr(root, "botpose_wpiblue").flatMap(this::parseArr).map(this::toBotPoseEstimate);
@@ -347,28 +325,29 @@ public Optional getBotPoseEstimateBlue() {
     /**
      * Gets the position of the robot with the red driverstation as the origin.
      *
-     * @return an Optional containing the robot's Pose3d in red alliance coordinates, or empty if unavailable
+     * @return an Optional containing the robot's Pose3d in red alliance coordinates, or empty if
+     *     unavailable
      */
     @Override
     public Optional getBotposeRed() {
       return getArr(root, "botpose_wpired").flatMap(this::parseArr);
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public Optional getBotPoseEstimateRed() {
       return getArr(root, "botpose_wpired").flatMap(this::parseArr).map(this::toBotPoseEstimate);
     }
 
     /**
-     * Converts a Pose3d into a BotPoseEstimate with timestamp and visible tag information.
-     * The timestamp is calculated by subtracting the total latency from the response timestamp.
-     * 
+     * Converts a Pose3d into a BotPoseEstimate with timestamp and visible tag information. The
+     * timestamp is calculated by subtracting the total latency from the response timestamp.
+     *
      * @param pose the 3D pose to convert
      * @return a BotPoseEstimate with the 2D pose projection, adjusted timestamp, and visible tags
-     * @see Chief Delphi Discussion
+     * @see Chief
+     *     Delphi Discussion
      */
     private BotPoseEstimate toBotPoseEstimate(Pose3d pose) {
       // See
@@ -381,16 +360,14 @@ private BotPoseEstimate toBotPoseEstimate(Pose3d pose) {
 
     /**
      * Gets the ID of the currently targeted AprilTag.
-     * 
+     *
      * @return an OptionalLong containing the tag ID, or empty if no tag is targeted
      */
     OptionalLong getTagID() {
       return unboxLong(getLong(root, "pID"));
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public Set getVisibleTags() {
       return getArr(root, "Fiducial")
@@ -408,9 +385,7 @@ public Set getVisibleTags() {
           .orElseGet(Set::of);
     }
 
-    /**
-     * {@inheritDoc}
-     */
+    /** {@inheritDoc} */
     @Override
     public Map getVisibleAprilTagPoses() {
       return getArr(root, "Fiducial")
@@ -429,4 +404,4 @@ public Map getVisibleAprilTagPoses() {
           .orElse(emptyMap());
     }
   }
-}
\ No newline at end of file
+}
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java b/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java
index 36c709c6..e698234b 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java
@@ -5,22 +5,22 @@
 
 /**
  * Stub implementation of LocationalData where all optional values return empty.
- * 
- * 
    This class provides a null-object pattern implementation for LocationalData,
- * useful as a fallback when real Limelight data is unavailable. It can represent
- * either valid or invalid data states, but always returns empty optionals for all
- * pose and timing information.
- * 
+ *
+ * 
    This class provides a null-object pattern implementation for LocationalData, useful as a
+ * fallback when real Limelight data is unavailable. It can represent either valid or invalid data
+ * states, but always returns empty optionals for all pose and timing information.
+ *
  * 
    Two singleton instances are provided:
+ *
  * 
      
- *   
    • {@link #VALID} - represents valid but empty data
      • 
- *   
    • {@link #INVALID} - represents invalid/disconnected state
      • 
+ *   
    • {@link #VALID} - represents valid but empty data
+ *   
    • {@link #INVALID} - represents invalid/disconnected state
  * 
    
  */
 final class StubLocationalData implements LocationalData {
   /** Singleton instance representing valid but empty locational data. */
   static final StubLocationalData VALID = new StubLocationalData(true);
-  
+
   /** Singleton instance representing invalid locational data. */
   static final StubLocationalData INVALID = new StubLocationalData(false);
 
@@ -29,7 +29,7 @@ final class StubLocationalData implements LocationalData {
 
   /**
    * Creates a new StubLocationalData instance.
-   * 
+   *
    * @param valid whether this stub data should report as valid
    */
   private StubLocationalData(boolean valid) {
@@ -38,7 +38,7 @@ private StubLocationalData(boolean valid) {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return the validity state set during construction
    */
   @Override
@@ -48,7 +48,7 @@ public boolean isValid() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always false, as stub data never has targets
    */
   @Override
@@ -58,7 +58,7 @@ public boolean hasTarget() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always empty
    */
   @Override
@@ -68,7 +68,7 @@ public Optional getBotpose() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always empty
    */
   @Override
@@ -78,7 +78,7 @@ public Optional getBotPoseEstimate() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always empty
    */
   @Override
@@ -88,7 +88,7 @@ public Optional getBotposeBlue() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always empty
    */
   @Override
@@ -98,7 +98,7 @@ public Optional getBotPoseEstimateBlue() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always empty
    */
   @Override
@@ -108,7 +108,7 @@ public Optional getBotposeRed() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always empty
    */
   @Override
@@ -118,7 +118,7 @@ public Optional getBotPoseEstimateRed() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always empty
    */
   @Override
@@ -128,7 +128,7 @@ public OptionalDouble getCaptureLatency() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always empty
    */
   @Override
@@ -138,7 +138,7 @@ public OptionalDouble getTargetingLatency() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always empty
    */
   @Override
@@ -148,7 +148,7 @@ public OptionalDouble getTimestamp() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always empty
    */
   @Override
@@ -158,7 +158,7 @@ public OptionalDouble lastMSDelay() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always an empty, immutable set
    */
   @Override
@@ -168,11 +168,11 @@ public Set getVisibleTags() {
 
   /**
    * {@inheritDoc}
-   * 
+   *
    * @return always an empty, immutable map
    */
   @Override
   public Map getVisibleAprilTagPoses() {
     return Collections.emptyMap();
   }
-}
\ No newline at end of file
+}
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java
index 23b0fe43..185dc9d1 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java
@@ -7,9 +7,9 @@
 /**
  * Represents a fiducial (AprilTag) in 3D space.
  *
- * 
    This class stores the fiducial's unique ID and its pose as a 4x4 transformation matrix. It
- * can convert this transform into a {@link Pose3d} object for use with WPILib's geometry and
- * odometry systems.
+ * 
    This class stores the fiducial's unique ID and its pose as a 4x4 transformation matrix. It can
+ * convert this transform into a {@link Pose3d} object for use with WPILib's geometry and odometry
+ * systems.
  */
 public class Fiducial {
 
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java
index f555d9fc..c917be01 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java
@@ -12,8 +12,8 @@
 /**
  * Retrieves and stores fiducial (AprilTag) information from a JSON input stream.
  *
- * 
    The JSON is expected to represent a {@link FieldMap}, which contains an array of
- * {@link Fiducial} objects. Each fiducial is mapped by its unique ID for easy lookup.
+ * 
    The JSON is expected to represent a {@link FieldMap}, which contains an array of {@link
+ * Fiducial} objects. Each fiducial is mapped by its unique ID for easy lookup.
  */
 public class FiducialRetriever {
 
@@ -34,8 +34,7 @@ public FiducialRetriever(InputStream stream) {
     FieldMap map = gson.fromJson(new InputStreamReader(stream, UTF_8), FieldMap.class);
     // Convert array of fiducials to an unmodifiable map keyed by fiducial ID
     fiducialMap =
-        Arrays.stream(map.fiducials)
-            .collect(Collectors.toUnmodifiableMap(Fiducial::getId, f -> f));
+        Arrays.stream(map.fiducials).collect(Collectors.toUnmodifiableMap(Fiducial::getId, f -> f));
   }
 
   /**
diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FieldMap.java b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FieldMap.java
index a85585bb..3e57e6b9 100644
--- a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FieldMap.java
+++ b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FieldMap.java
@@ -7,6 +7,7 @@
  * fiducials under the key "fiducials". Each fiducial is represented by a {@link Fiducial} object.
  *
  * 
    Example JSON format:
+ *
  * 
      * {
  *   "fiducials": [
diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java b/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java
index 390d8e21..34a12a4a 100644
--- a/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java
+++ b/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java
@@ -15,20 +15,22 @@
 
 /**
  * Mock HTTP server that simulates a Limelight camera for testing purposes.
- * 
- * 
    This class extends JUnit's {@link ExternalResource} to provide automatic setup
- * and teardown of a fake HTTP server that mimics the Limelight REST API. It listens
- * on port 5807 and handles two endpoints:
+ *
+ * 
    This class extends JUnit's {@link ExternalResource} to provide automatic setup and teardown of
+ * a fake HTTP server that mimics the Limelight REST API. It listens on port 5807 and handles two
+ * endpoints:
+ *
  * 
      
- *   
    • {@code /results} - GET endpoint that returns vision data in JSON format
      • 
- *   
    • {@code /upload-fieldmap} - POST endpoint that accepts field map uploads
      • 
+ *   
    • {@code /results} - GET endpoint that returns vision data in JSON format
+ *   
    • {@code /upload-fieldmap} - POST endpoint that accepts field map uploads
  * 
    
- * 
+ *
  * 
    Usage example in a JUnit test:
+ *
  * 
    {@code
  * @Rule
  * public FakeLimelight fakeLimelight = new FakeLimelight();
- * 
+ *
  * @Test
  * public void testLimelightConnection() {
  *     JSONObject response = new JSONObject();
@@ -41,14 +43,14 @@
 public class FakeLimelight extends ExternalResource {
   /** Logger for tracking fake Limelight activity. */
   private static final Logger logger = Logger.getLogger("FakeLimelight");
-  
+
   /** The underlying HTTP server instance. */
   HttpServer server;
 
   /**
-   * Sets up the fake Limelight server before each test.
-   * Creates an HTTP server on port 5807 with endpoints for results and field map uploads.
-   * 
+   * Sets up the fake Limelight server before each test. Creates an HTTP server on port 5807 with
+   * endpoints for results and field map uploads.
+   *
    * @throws Throwable if server creation or startup fails
    */
   @Override
@@ -61,8 +63,8 @@ protected void before() throws Throwable {
   }
 
   /**
-   * Tears down the fake Limelight server after each test.
-   * Stops the server with a 2-second grace period and clears the server reference.
+   * Tears down the fake Limelight server after each test. Stops the server with a 2-second grace
+   * period and clears the server reference.
    */
   @Override
   protected void after() {
@@ -72,10 +74,9 @@ protected void after() {
 
   /**
    * HTTP handler that simulates the Limelight's GET /results endpoint.
-   * 
-   * 
    This handler responds to GET requests with a configurable JSON body
-   * representing vision data from the Limelight. It returns 405 Method Not Allowed
-   * for non-GET requests.
+   *
+   * 
    This handler responds to GET requests with a configurable JSON body representing vision data
+   * from the Limelight. It returns 405 Method Not Allowed for non-GET requests.
    */
   private static class FakeGet implements HttpHandler {
     /** The JSON response body to return. */
@@ -83,7 +84,7 @@ private static class FakeGet implements HttpHandler {
 
     /**
      * Sets the response body that will be returned by GET requests.
-     * 
+     *
      * @param body the JSON string to return
      */
     public void setBody(String body) {
@@ -92,7 +93,7 @@ public void setBody(String body) {
 
     /**
      * Gets the current response body.
-     * 
+     *
      * @return the JSON string that will be returned
      */
     public String getBody() {
@@ -101,7 +102,7 @@ public String getBody() {
 
     /**
      * Handles incoming HTTP requests to the /results endpoint.
-     * 
+     *
      * @param exchange the HTTP exchange containing request and response
      * @throws IOException if an I/O error occurs during response writing
      */
@@ -122,19 +123,18 @@ public void handle(HttpExchange exchange) throws IOException {
 
   /**
    * HTTP handler that simulates the Limelight's POST /upload-fieldmap endpoint.
-   * 
-   * 
    This handler accepts POST requests with field map data and stores the
-   * uploaded content for verification in tests. It returns 405 Method Not Allowed
-   * for non-POST requests.
+   *
+   * 
    This handler accepts POST requests with field map data and stores the uploaded content for
+   * verification in tests. It returns 405 Method Not Allowed for non-POST requests.
    */
   private static class FakeFieldMap implements HttpHandler {
     /** The most recently uploaded field map content. */
     private String post;
 
     /**
-     * Handles incoming HTTP requests to the /upload-fieldmap endpoint.
-     * Reads and stores the POST body content for later retrieval.
-     * 
+     * Handles incoming HTTP requests to the /upload-fieldmap endpoint. Reads and stores the POST
+     * body content for later retrieval.
+     *
      * @param exchange the HTTP exchange containing request and response
      * @throws IOException if an I/O error occurs during request reading
      */
@@ -156,13 +156,13 @@ public void handle(HttpExchange exchange) throws IOException {
 
   /** Handler for the /results GET endpoint. */
   private final FakeGet resultsResponse = new FakeGet();
-  
+
   /** Handler for the /upload-fieldmap POST endpoint. */
   private final FakeFieldMap fieldMapResponse = new FakeFieldMap();
 
   /**
-   * Resets the fake Limelight state by clearing the results response body.
-   * Useful for ensuring a clean state between test cases.
+   * Resets the fake Limelight state by clearing the results response body. Useful for ensuring a
+   * clean state between test cases.
    */
   public void reset() {
     resultsResponse.setBody("");
@@ -170,7 +170,7 @@ public void reset() {
 
   /**
    * Sets the JSON response that will be returned by the /results endpoint.
-   * 
+   *
    * @param response the JSONObject containing vision data to return
    */
   public void setResultsResponse(JSONObject response) {
@@ -179,7 +179,7 @@ public void setResultsResponse(JSONObject response) {
 
   /**
    * Gets the most recently uploaded field map content.
-   * 
+   *
    * @return the field map data as a string, or null if no field map has been uploaded
    */
   public String getFieldMap() {
@@ -188,11 +188,11 @@ public String getFieldMap() {
 
   /**
    * Gets the current results response as a JSONObject.
-   * 
+   *
    * @return a JSONObject parsed from the current response body
    * @throws org.json.JSONException if the response body is not valid JSON
    */
   public JSONObject getResultsResponse() {
     return new JSONObject(resultsResponse.getBody());
   }
-}
\ No newline at end of file
+}
diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/LegacyRestLimelightTest.java b/limelight/src/test/java/com/team2813/lib2813/limelight/LegacyRestLimelightTest.java
index aa12e2dc..410ed1be 100644
--- a/limelight/src/test/java/com/team2813/lib2813/limelight/LegacyRestLimelightTest.java
+++ b/limelight/src/test/java/com/team2813/lib2813/limelight/LegacyRestLimelightTest.java
@@ -4,31 +4,29 @@
 
 /**
  * Test suite for verifying backward compatibility with legacy Limelight JSON format.
- * 
- * 
    This class extends {@link RestLimelightTest} to ensure that the RestLimelight
- * implementation continues to work with older JSON responses that include a "Results"
- * root object. This legacy format was used in earlier versions of the Limelight API,
- * and maintaining compatibility ensures that existing systems continue to function
- * correctly.
- * 
- * 
    The key difference between this test and the parent class is that all JSON
- * test data files in the resources directory contain the legacy "Results" root
- * object structure, which this test specifically validates.
- * 
+ *
+ * 
    This class extends {@link RestLimelightTest} to ensure that the RestLimelight implementation
+ * continues to work with older JSON responses that include a "Results" root object. This legacy
+ * format was used in earlier versions of the Limelight API, and maintaining compatibility ensures
+ * that existing systems continue to function correctly.
+ *
+ * 
    The key difference between this test and the parent class is that all JSON test data files in
+ * the resources directory contain the legacy "Results" root object structure, which this test
+ * specifically validates.
+ *
  * @see RestLimelightTest
  */
 public class LegacyRestLimelightTest extends RestLimelightTest {
-  
+
   /**
    * Sets the JSON response for the fake Limelight using the legacy format.
-   * 
-   * 
    This override ensures that JSON objects with the legacy "Results" root
-   * structure are properly handled. All JSON files in the test resources directory
-   * use this legacy format, making this test class essential for verifying
-   * backward compatibility.
-   * 
-   * @param json the JSONObject containing legacy-formatted Limelight data with
-   *             a "Results" root object
+   *
+   * 
    This override ensures that JSON objects with the legacy "Results" root structure are
+   * properly handled. All JSON files in the test resources directory use this legacy format, making
+   * this test class essential for verifying backward compatibility.
+   *
+   * @param json the JSONObject containing legacy-formatted Limelight data with a "Results" root
+   *     object
    */
   @Override
   protected void setJson(JSONObject json) {
@@ -36,4 +34,4 @@ protected void setJson(JSONObject json) {
     // Therefore, this json object is legacy and this test will make sure legacy still works
     fakeLimelight.setResultsResponse(json);
   }
-}
\ No newline at end of file
+}
diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java b/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java
index 8a32bb37..0a1ae930 100644
--- a/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java
+++ b/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java
@@ -28,33 +28,35 @@
 
 /**
  * Abstract base class for Limelight implementation testing.
- * 
+ *
  * 
    This class provides a comprehensive test suite for validating Limelight implementations
  * against various JSON response scenarios. It tests all aspects of the Limelight interface
  * including pose estimation, target detection, AprilTag visibility, and latency measurements.
- * 
- * 
    Subclasses must implement two abstract methods to adapt the tests to their specific
- * Limelight implementation:
+ *
+ * 
    Subclasses must implement two abstract methods to adapt the tests to their specific Limelight
+ * implementation:
+ *
  * 
      
- *   
    • {@link #createLimelight()} - creates the Limelight instance to test
      • 
- *   
    • {@link #setJson(JSONObject)} - configures the test environment with JSON data
      • 
+ *   
    • {@link #createLimelight()} - creates the Limelight instance to test
+ *   
    • {@link #setJson(JSONObject)} - configures the test environment with JSON data
  * 
    
- * 
+ *
  * 
    Test coverage includes:
+ *
  * 
      
- *   
    • Empty/missing data scenarios
      • 
- *   
    • Invalid data handling
      • 
- *   
    • Robot pose estimation in multiple coordinate systems (field, blue alliance, red alliance)
      • 
- *   
    • AprilTag detection and position reporting
      • 
- *   
    • Latency measurements (capture and targeting)
      • 
- *   
    • Field map integration
      • 
+ *   
    • Empty/missing data scenarios
+ *   
    • Invalid data handling
+ *   
    • Robot pose estimation in multiple coordinate systems (field, blue alliance, red alliance)
+ *   
    • AprilTag detection and position reporting
+ *   
    • Latency measurements (capture and targeting)
+ *   
    • Field map integration
  * 
    
  */
 abstract class LimelightTestCase {
 
   /**
-   * Tests that an uninitialized Limelight returns empty values.
-   * Verifies that capture latency is not present when no data has been received.
+   * Tests that an uninitialized Limelight returns empty values. Verifies that capture latency is
+   * not present when no data has been received.
    */
   @Test
   public final void emptyValues() {
@@ -63,9 +65,9 @@ public final void emptyValues() {
   }
 
   /**
-   * Tests handling of invalid/malformed JSON data.
-   * Verifies that the Limelight correctly identifies invalid data and returns empty optionals.
-   * 
+   * Tests handling of invalid/malformed JSON data. Verifies that the Limelight correctly identifies
+   * invalid data and returns empty optionals.
+   *
    * @throws Exception if test resources cannot be loaded
    */
   @Test
@@ -85,9 +87,9 @@ public final void invalidDataTest() throws Exception {
   }
 
   /**
-   * Tests scenario where no target is detected but latency data is present.
-   * Validates that valid latency measurements can be obtained even without a target.
-   * 
+   * Tests scenario where no target is detected but latency data is present. Validates that valid
+   * latency measurements can be obtained even without a target.
+   *
    * @throws Exception if test resources cannot be loaded
    */
   @Test
@@ -111,9 +113,9 @@ public final void absentTest1() throws Exception {
   }
 
   /**
-   * Tests another scenario where no target is detected but latency data differs.
-   * Verifies consistent behavior with different latency values.
-   * 
+   * Tests another scenario where no target is detected but latency data differs. Verifies
+   * consistent behavior with different latency values.
+   *
    * @throws Exception if test resources cannot be loaded
    */
   @Test
@@ -137,10 +139,10 @@ public final void absentTest2() throws Exception {
   }
 
   /**
-   * Tests scenario with a detected target and full pose estimation data.
-   * Validates robot pose in field coordinates, blue alliance coordinates, and red alliance coordinates.
-   * Also verifies that pose estimates include valid timestamps.
-   * 
+   * Tests scenario with a detected target and full pose estimation data. Validates robot pose in
+   * field coordinates, blue alliance coordinates, and red alliance coordinates. Also verifies that
+   * pose estimates include valid timestamps.
+   *
    * @throws Exception if test resources cannot be loaded
    */
   @Test
@@ -187,9 +189,9 @@ public final void presentTest1() throws Exception {
   }
 
   /**
-   * Tests another scenario with a detected target and different pose values.
-   * Ensures consistent pose estimation behavior across different robot positions.
-   * 
+   * Tests another scenario with a detected target and different pose values. Ensures consistent
+   * pose estimation behavior across different robot positions.
+   *
    * @throws Exception if test resources cannot be loaded
    */
   @Test
@@ -231,9 +233,9 @@ public final void presentTest2() throws Exception {
   }
 
   /**
-   * Tests retrieval of robot pose in blue alliance coordinates.
-   * Validates the coordinate transformation from field coordinates to blue alliance origin.
-   * 
+   * Tests retrieval of robot pose in blue alliance coordinates. Validates the coordinate
+   * transformation from field coordinates to blue alliance origin.
+   *
    * @throws Exception if test resources cannot be loaded
    */
   @Test
@@ -253,9 +255,9 @@ public final void getBotposeBlue() throws Exception {
   }
 
   /**
-   * Tests retrieval of robot pose in red alliance coordinates.
-   * Validates the coordinate transformation from field coordinates to red alliance origin.
-   * 
+   * Tests retrieval of robot pose in red alliance coordinates. Validates the coordinate
+   * transformation from field coordinates to red alliance origin.
+   *
    * @throws Exception if test resources cannot be loaded
    */
   @Test
@@ -276,9 +278,9 @@ public final void getBotposeRed() throws Exception {
   }
 
   /**
-   * Tests detection and reporting of visible AprilTag IDs.
-   * Verifies that the Limelight correctly identifies which tags are in view.
-   * 
+   * Tests detection and reporting of visible AprilTag IDs. Verifies that the Limelight correctly
+   * identifies which tags are in view.
+   *
    * @throws Exception if test resources cannot be loaded
    */
   @Test
@@ -291,10 +293,9 @@ public final void getVisibleTags() throws Exception {
   }
 
   /**
-   * Tests retrieval of visible AprilTag positions on the field.
-   * Validates that tag positions are correctly loaded from the field map and
-   * that pose estimates include the set of visible tags.
-   * 
+   * Tests retrieval of visible AprilTag positions on the field. Validates that tag positions are
+   * correctly loaded from the field map and that pose estimates include the set of visible tags.
+   *
    * @throws Exception if test resources cannot be loaded
    */
   @Test
@@ -324,9 +325,9 @@ public final void getVisibleAprilTagPoses() throws Exception {
   }
 
   /**
-   * Tests retrieval of AprilTag locations via the getLocatedAprilTags method.
-   * Validates that tag positions can be queried by their IDs after field map upload.
-   * 
+   * Tests retrieval of AprilTag locations via the getLocatedAprilTags method. Validates that tag
+   * positions can be queried by their IDs after field map upload.
+   *
    * @throws Exception if test resources cannot be loaded
    */
   @Test
@@ -344,25 +345,25 @@ public final void visibleTagLocation() throws Exception {
   }
 
   /**
-   * Creates a Limelight instance for testing.
-   * Subclasses must implement this to provide their specific Limelight implementation.
-   * 
+   * Creates a Limelight instance for testing. Subclasses must implement this to provide their
+   * specific Limelight implementation.
+   *
    * @return a Limelight instance to test
    */
   protected abstract Limelight createLimelight();
 
   /**
-   * Configures the test environment with JSON data.
-   * Subclasses must implement this to inject test data into their Limelight implementation.
-   * 
+   * Configures the test environment with JSON data. Subclasses must implement this to inject test
+   * data into their Limelight implementation.
+   *
    * @param json the JSONObject containing Limelight response data
    */
   protected abstract void setJson(JSONObject json);
 
   /**
-   * Uploads the FRC 2025 Round 2 field map to the Limelight.
-   * This configures AprilTag positions for testing.
-   * 
+   * Uploads the FRC 2025 Round 2 field map to the Limelight. This configures AprilTag positions for
+   * testing.
+   *
    * @param limelight the Limelight instance to configure
    * @throws IOException if the field map resource cannot be loaded
    */
@@ -375,7 +376,7 @@ private void uploadFieldMap(Limelight limelight) throws IOException {
 
   /**
    * Reads a JSON test file from the classpath resources.
-   * 
+   *
    * @param fileName the name of the JSON file to read
    * @return a JSONObject parsed from the file contents
    * @throws IOException if the file cannot be found or read
@@ -393,9 +394,9 @@ private JSONObject readJSON(String fileName) throws IOException {
   }
 
   /**
-   * Asserts that the Limelight has a valid target detected.
-   * Checks both the top-level hasTarget() method and the LocationalData hasTarget() method.
-   * 
+   * Asserts that the Limelight has a valid target detected. Checks both the top-level hasTarget()
+   * method and the LocationalData hasTarget() method.
+   *
    * @param limelight the Limelight to check
    */
   private void assertHasTarget(Limelight limelight) {
@@ -406,9 +407,9 @@ private void assertHasTarget(Limelight limelight) {
   }
 
   /**
-   * Asserts that an OptionalDouble value is approximately equal to an expected value.
-   * If the optional is empty, the assertion fails.
-   * 
+   * Asserts that an OptionalDouble value is approximately equal to an expected value. If the
+   * optional is empty, the assertion fails.
+   *
    * @param expected the expected double value
    * @param actual the OptionalDouble to check
    * @param delta the maximum acceptable difference between expected and actual
@@ -418,4 +419,4 @@ protected static void assertAlmostEqual(double expected, OptionalDouble actual,
         d -> assertEquals(expected, d, delta),
         () -> assertEquals(OptionalDouble.of(expected), actual));
   }
-}
\ No newline at end of file
+}
diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java b/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java
index 6b0e5942..c97c89a9 100644
--- a/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java
+++ b/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java
@@ -9,10 +9,9 @@
 
 /**
  * Unit test class for {@link NetworkTablesLimelight}.
- * 
    
- * This test framework simulates Limelight JSON data being written into NetworkTables and
+ *
+ * 
    This test framework simulates Limelight JSON data being written into NetworkTables and
  * validates how the {@link Limelight} implementation handles pose estimation entries.
- * 
    
  */
 public class NetworkTablesLimelightTest extends LimelightTestCase {
 
@@ -42,10 +41,9 @@ public class NetworkTablesLimelightTest extends LimelightTestCase {
 
   /**
    * Resets Limelight NetworkTable entries after each test.
-   * 
    
-   * Clears the JSON entry and all bot pose estimate entries, advancing the fake timestamp
-   * by two half-millisecond steps.
-   * 
    
+   *
+   * 
    Clears the JSON entry and all bot pose estimate entries, advancing the fake timestamp by two
+   * half-millisecond steps.
    */
   @After
   public void resetNetworkTables() {
@@ -70,10 +68,9 @@ protected Limelight createLimelight() {
 
   /**
    * Sets the Limelight JSON data in NetworkTables.
-   * 
    
-   * Copies JSON pose estimation arrays into their corresponding NetworkTable entries
-   * when latency values are available.
-   * 
    
+   *
+   * 
    Copies JSON pose estimation arrays into their corresponding NetworkTable entries when
+   * latency values are available.
    *
    * @param json the simulated Limelight JSON response
    */
@@ -103,8 +100,8 @@ private static NetworkTableEntry getJsonNTEntry() {
   /**
    * Writes a bot pose estimate array into the specified NetworkTables entry.
    *
-   * @param resultsJson   the Limelight "Results" JSON object
-   * @param entryName     the entry name to populate
+   * @param resultsJson the Limelight "Results" JSON object
+   * @param entryName the entry name to populate
    * @param latencyMillis the latency value in milliseconds
    */
   private static void setBotPoseEstimate(
@@ -118,7 +115,7 @@ private static void setBotPoseEstimate(
   /**
    * Clears a bot pose estimate entry by setting it to an empty array.
    *
-   * @param entryName      the NetworkTables entry to clear
+   * @param entryName the NetworkTables entry to clear
    * @param timestampMicros the timestamp in microseconds
    */
   private static void clearBotPoseEstimate(String entryName, long timestampMicros) {
@@ -129,19 +126,20 @@ private static void clearBotPoseEstimate(String entryName, long timestampMicros)
 
   /**
    * Builds a bot pose estimate array from Limelight JSON data.
-   * 
    
-   * The array is structured as:
+   *
+   * 
    The array is structured as:
+   *
    * 
      
-   *   
    • Indices 0–5: pose data from Limelight
      • 
-   *   
    • Index 6: latency in milliseconds
      • 
-   *   
    • Index 7: tag count
      • 
-   *   
    • Index 8: tag ID
      • 
+   *   
    • Indices 0–5: pose data from Limelight
+   *   
    • Index 6: latency in milliseconds
+   *   
    • Index 7: tag count
+   *   
    • Index 8: tag ID
    * 
    
+   *
    * Additional indices are reserved for multiple tags.
-   * 
    
    *
-   * @param resultsJson   the Limelight "Results" JSON object
-   * @param entryName     the entry to extract
+   * @param resultsJson the Limelight "Results" JSON object
+   * @param entryName the entry to extract
    * @param latencyMillis the latency in milliseconds
    * @return a double array representing the bot pose estimate, or an empty array if not present
    */
diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java b/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java
index b2479a2d..2536f888 100644
--- a/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java
+++ b/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java
@@ -18,33 +18,29 @@
 
 /**
  * Unit tests for {@link RestLimelight}.
- * 
    
- * This test suite uses a {@link FakeLimelight} HTTP server to simulate REST responses from a
+ *
+ * 
    This test suite uses a {@link FakeLimelight} HTTP server to simulate REST responses from a
  * physical Limelight device. It validates Limelight instance management, HTTP endpoint behavior,
  * and field map uploads.
- * 
    
  */
 public class RestLimelightTest extends LimelightTestCase {
 
   /**
    * Fake Limelight instance running as an embedded HTTP server.
-   * 
    
-   * Used to simulate responses from a real Limelight device without requiring hardware.
-   * 
    
+   *
+   * 
    Used to simulate responses from a real Limelight device without requiring hardware.
    */
   @ClassRule public static final FakeLimelight fakeLimelight = new FakeLimelight();
 
-  /**
-   * Resets any cached {@link RestLimelight} instances after each test.
-   */
+  /** Resets any cached {@link RestLimelight} instances after each test. */
   @After
   public void resetLimelights() {
     RestLimelight.eraseInstances();
   }
 
   /**
-   * Resets the {@link FakeLimelight} after each test, ensuring no test state leaks
-   * between test methods.
+   * Resets the {@link FakeLimelight} after each test, ensuring no test state leaks between test
+   * methods.
    */
   @After
   public void resetFakeLimelight() {
@@ -52,8 +48,8 @@ public void resetFakeLimelight() {
   }
 
   /**
-   * Verifies that calls to obtain the default {@link RestLimelight} return the same instance,
-   * and that the default instance is equal to one retrieved by name.
+   * Verifies that calls to obtain the default {@link RestLimelight} return the same instance, and
+   * that the default instance is equal to one retrieved by name.
    */
   @Test
   public void equality() {
@@ -82,8 +78,8 @@ public void fakeWorks() throws Exception {
   }
 
   /**
-   * Verifies that uploading a field map to a {@link RestLimelight} updates
-   * the {@link FakeLimelight} server as expected.
+   * Verifies that uploading a field map to a {@link RestLimelight} updates the {@link
+   * FakeLimelight} server as expected.
    *
    * @throws Exception if reading the field map resource or uploading fails
    */
@@ -121,11 +117,10 @@ protected Limelight createLimelight() {
 
   /**
    * Sets the Limelight JSON results response in the {@link FakeLimelight}.
-   * 
    
-   * Unlike older schemas, the new Limelight JSON schema does not include a {@code "Results"} object
-   * at the root. Instead, all fields are inlined. Since resource test files still contain
+   *
+   * 
    Unlike older schemas, the new Limelight JSON schema does not include a {@code "Results"}
+   * object at the root. Instead, all fields are inlined. Since resource test files still contain
    * {@code "Results"}, this method extracts and forwards that sub-object.
-   * 
    
    *
    * @param json the full Limelight JSON, expected to contain a {@code "Results"} object
    */
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/CommandTester.java b/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/CommandTester.java
index 5b6554e7..0edfee09 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/CommandTester.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/CommandTester.java
@@ -5,14 +5,14 @@
 /**
  * Utility interface for running WPILib {@link Command} objects inside tests.
  *
- * 
    Provides a way to schedule and execute commands to completion within a test context.
- * An implementation is provided via {@link WPILibExtension}.
+ * 
    Provides a way to schedule and execute commands to completion within a test context. An
+ * implementation is provided via {@link WPILibExtension}.
  */
 public interface CommandTester {
 
   /**
-   * Schedules the provided {@link Command} and repeatedly runs it
-   * until the command reports it has finished.
+   * Schedules the provided {@link Command} and repeatedly runs it until the command reports it has
+   * finished.
    *
    * @param command the command to schedule and execute
    */
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java b/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java
index 31ea18f7..343cc46c 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtension.java
@@ -16,11 +16,12 @@
  * JUnit Jupiter extension for testing robot code that depends on WPILib.
  *
  * 
    This extension:
+ *
  * 
      
- *   
    • Initializes the WPILib {@link HAL} and driver station simulation before tests run.
      • 
- *   
    • Configures the {@link CommandScheduler} to ensure a clean environment.
      • 
- *   
    • Provides a {@link CommandTester} parameter for tests, which allows running
- *       WPILib commands until completion.
      • 
+ *   
    • Initializes the WPILib {@link HAL} and driver station simulation before tests run.
+ *   
    • Configures the {@link CommandScheduler} to ensure a clean environment.
+ *   
    • Provides a {@link CommandTester} parameter for tests, which allows running WPILib commands
+ *       until completion.
  * 
    
  *
  * 
    Example usage:
@@ -58,8 +59,8 @@ public final class WPILibExtension
   /**
    * Initializes WPILib components before all tests.
    *
-   * 
    Sets up the HAL, enables the driver station simulation,
-   * and resets the {@link CommandScheduler}.
+   * 
    Sets up the HAL, enables the driver station simulation, and resets the {@link
+   * CommandScheduler}.
    *
    * @param context the JUnit extension context
    * @throws IllegalStateException if the HAL cannot be initialized
@@ -78,8 +79,7 @@ public void beforeAll(ExtensionContext context) {
   }
 
   /**
-   * Cleans up after each test by canceling all commands
-   * and unregistering all subsystems.
+   * Cleans up after each test by canceling all commands and unregistering all subsystems.
    *
    * @param context the JUnit extension context
    */
@@ -90,9 +90,8 @@ public void afterEach(ExtensionContext context) {
   }
 
   /**
-   * Cleans up after all tests by canceling all commands,
-   * unregistering all subsystems, disabling the scheduler,
-   * and resetting the driver station simulation.
+   * Cleans up after all tests by canceling all commands, unregistering all subsystems, disabling
+   * the scheduler, and resetting the driver station simulation.
    *
    * @param context the JUnit extension context
    */
@@ -123,8 +122,8 @@ public boolean supportsParameter(
   /**
    * Provides a {@link CommandTester} instance for parameter injection.
    *
-   * 
    The tester schedules the given command and repeatedly runs
-   * the {@link CommandScheduler} until the command completes.
+   * 
    The tester schedules the given command and repeatedly runs the {@link CommandScheduler}
+   * until the command completes.
    *
    * @param parameterContext the parameter context
    * @param extensionContext the extension context
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java
index 78125886..9f25e32d 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose2dSubject.java
@@ -11,11 +11,11 @@
 /**
  * A Truth {@link Subject} for making assertions about {@link Pose2d} values.
  *
- * 
    This subject provides fluent assertions for comparing poses, including
- * tolerance-based comparisons of translations and rotations.
+ * 
    This subject provides fluent assertions for comparing poses, including tolerance-based
+ * comparisons of translations and rotations.
  *
- * 
    See Truth: Writing your own custom subject
- * for more on extending Truth.
+ * 
    See Truth: Writing your own custom subject for more
+ * on extending Truth.
  */
 public final class Pose2dSubject extends Subject {
 
@@ -23,6 +23,7 @@ public final class Pose2dSubject extends Subject {
    * Entry point for {@link Pose2d} assertions.
    *
    * 
    Usage:
+   *
    * 
    {@code
    * Pose2d actualPose = ...;
    * Pose2d expectedPose = ...;
@@ -40,7 +41,7 @@ public static Pose2dSubject assertThat(@Nullable Pose2d pose) {
   }
 
   /**
-   * Factory for {@link Pose2dSubject}, used with {@link assertAbout}.
+   * Factory for {@link Pose2dSubject}, for use with assertAbout().
    *
    * @return a factory for creating {@link Pose2dSubject} instances
    */
@@ -57,9 +58,8 @@ private Pose2dSubject(FailureMetadata failureMetadata, @Nullable Pose2d subject)
 
   /**
    * Returns a tolerant comparison assertion for the current pose.
-   * 
    
-   * The tolerance applies to both translation (x, y) and rotation (θ).
-   * 
    
+   *
+   * 
    The tolerance applies to both translation (x, y) and rotation (θ).
    *
    * @param tolerance the maximum allowed difference in meters (translation) or radians (rotation)
    * @return a {@link TolerantComparison} for comparing poses with a tolerance
@@ -75,8 +75,8 @@ public void of(Pose2d expected) {
   }
 
   /**
-   * Returns a {@link Translation2dSubject} for making assertions about the translation
-   * component of this pose.
+   * Returns a {@link Translation2dSubject} for making assertions about the translation component of
+   * this pose.
    *
    * @return a subject for the pose's translation
    */
@@ -87,8 +87,8 @@ public Translation2dSubject translation() {
   }
 
   /**
-   * Returns a {@link Rotation2dSubject} for making assertions about the rotation
-   * component of this pose.
+   * Returns a {@link Rotation2dSubject} for making assertions about the rotation component of this
+   * pose.
    *
    * @return a subject for the pose's rotation
    */
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java
index 8cb8e468..a21ae5a8 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Pose3dSubject.java
@@ -11,11 +11,11 @@
 /**
  * A Truth {@link Subject} for making assertions about {@link Pose3d} values.
  *
- * 
    This subject provides fluent assertions for comparing 3D poses, including
- * tolerance-based comparisons of translations and rotations.
+ * 
    This subject provides fluent assertions for comparing 3D poses, including tolerance-based
+ * comparisons of translations and rotations.
  *
- * 
    See Truth: Writing your own custom subject
- * for more on extending Truth.
+ * 
    See Truth: Writing your own custom subject for more
+ * on extending Truth.
  */
 public final class Pose3dSubject extends Subject {
 
@@ -23,6 +23,7 @@ public final class Pose3dSubject extends Subject {
    * Entry point for {@link Pose3d} assertions.
    *
    * 
    Usage:
+   *
    * 
    {@code
    * Pose3d actualPose = ...;
    * Pose3d expectedPose = ...;
@@ -40,7 +41,7 @@ public static Pose3dSubject assertThat(@Nullable Pose3d pose) {
   }
 
   /**
-   * Factory for {@link Pose3dSubject}, used with {@link assertAbout}.
+   * Factory for {@link Pose2dSubject}, for use with assertAbout().
    *
    * @return a factory for creating {@link Pose3dSubject} instances
    */
@@ -74,8 +75,8 @@ public void of(Pose3d expected) {
   }
 
   /**
-   * Returns a {@link Translation3dSubject} for making assertions about the translation
-   * component of this pose.
+   * Returns a {@link Translation3dSubject} for making assertions about the translation component of
+   * this pose.
    *
    * @return a subject for the pose's 3D translation
    */
@@ -86,8 +87,8 @@ public Translation3dSubject translation() {
   }
 
   /**
-   * Returns a {@link Rotation3dSubject} for making assertions about the rotation
-   * component of this pose.
+   * Returns a {@link Rotation3dSubject} for making assertions about the rotation component of this
+   * pose.
    *
    * @return a subject for the pose's 3D rotation
    */
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java
index e14bbce3..0bd531c0 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation2dSubject.java
@@ -12,8 +12,8 @@
 /**
  * A Truth {@link Subject} for making assertions about {@link Rotation2d} values.
  *
- * 
    This subject provides fluent assertions for comparing 2D rotations,
- * including tolerance-based comparisons and checks for zero rotation.
+ * 
    This subject provides fluent assertions for comparing 2D rotations, including tolerance-based
+ * comparisons and checks for zero rotation.
  *
  * 
    Rotations are compared in radians.
  */
@@ -23,6 +23,7 @@ public final class Rotation2dSubject extends Subject {
    * Entry point for {@link Rotation2d} assertions.
    *
    * 
    Usage:
+   *
    * 
    {@code
    * Rotation2d actual = new Rotation2d(Math.PI / 2);
    * Rotation2d expected = new Rotation2d(Math.PI / 2 + 1e-3);
@@ -40,7 +41,7 @@ public static Rotation2dSubject assertThat(@Nullable Rotation2d rotation) {
   }
 
   /**
-   * Factory for {@link Rotation2dSubject}, used with {@link assertAbout}.
+   * Factory for {@link Pose2dSubject}, for use with assertAbout().
    *
    * @return a factory for creating {@link Rotation2dSubject} instances
    */
@@ -84,8 +85,7 @@ public void isZero() {
   }
 
   /**
-   * Returns a {@link DoubleSubject} for making assertions about the rotation’s
-   * raw radians value.
+   * Returns a {@link DoubleSubject} for making assertions about the rotation’s raw radians value.
    *
    * @return a {@link DoubleSubject} for the rotation’s radians
    */
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java
index 952f345b..2a449e84 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Rotation3dSubject.java
@@ -12,15 +12,15 @@
 /**
  * A Truth {@link Subject} for making assertions about {@link Rotation3d} values.
  *
- * 
    This subject provides fluent assertions for comparing 3D rotations,
- * including tolerance-based comparisons of roll, pitch, and yaw angles,
- * as well as checks for zero rotation.
+ * 
    This subject provides fluent assertions for comparing 3D rotations, including tolerance-based
+ * comparisons of roll, pitch, and yaw angles, as well as checks for zero rotation.
  *
  * 
    Rotations are expressed in radians:
+ *
  * 
      
- *   
    • {@link #x()} → roll (counterclockwise about X axis)
      • 
- *   
    • {@link #y()} → pitch (counterclockwise about Y axis)
      • 
- *   
    • {@link #z()} → yaw (counterclockwise about Z axis)
      • 
+ *   
    • {@link #x()} → roll (counterclockwise about X axis)
+ *   
    • {@link #y()} → pitch (counterclockwise about Y axis)
+ *   
    • {@link #z()} → yaw (counterclockwise about Z axis)
  * 
    
  */
 public final class Rotation3dSubject extends Subject {
@@ -29,6 +29,7 @@ public final class Rotation3dSubject extends Subject {
    * Entry point for {@link Rotation3d} assertions.
    *
    * 
    Usage:
+   *
    * 
    {@code
    * Rotation3d actual = new Rotation3d(Math.PI / 2, 0, 0);
    * Rotation3d expected = new Rotation3d(Math.PI / 2 + 1e-3, 0, 0);
@@ -46,7 +47,7 @@ public static Rotation3dSubject assertThat(@Nullable Rotation3d rotation) {
   }
 
   /**
-   * Factory for {@link Rotation3dSubject}, used with {@link assertAbout}.
+   * Factory for {@link Pose2dSubject}, for use with assertAbout().
    *
    * @return a factory for creating {@link Rotation3dSubject} instances
    */
@@ -64,8 +65,8 @@ private Rotation3dSubject(FailureMetadata failureMetadata, @Nullable Rotation3d
   /**
    * Returns a tolerant comparison assertion for this rotation.
    *
-   * 
    The tolerance is expressed in radians and applies to each of the
-   * roll (X), pitch (Y), and yaw (Z) components independently.
+   * 
    The tolerance is expressed in radians and applies to each of the roll (X), pitch (Y), and
+   * yaw (Z) components independently.
    *
    * @param tolerance the maximum allowed difference in radians
    * @return a {@link TolerantComparison} for comparing rotations with a tolerance
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/TolerantComparison.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/TolerantComparison.java
index e601265d..a85e4810 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/TolerantComparison.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/TolerantComparison.java
@@ -9,6 +9,7 @@
  * provided via {@link #of(Object)}.
  *
  * 
    For example, in a fluent assertion style:
+ *
  * 
    {@code
  * assertThat(actualValue).isWithin(tolerance).of(expectedValue);
  * }
    
@@ -20,14 +21,12 @@
  */
 public abstract class TolerantComparison {
 
-  /**
-   * Package-private constructor to prevent subclassing outside of this package.
-   */
+  /** Package-private constructor to prevent subclassing outside of this package. */
   TolerantComparison() {}
 
   /**
-   * Fails the assertion if the subject is not within the tolerance of the given expected value.
-   * The subject and tolerance must have been specified earlier in the fluent call chain.
+   * Fails the assertion if the subject is not within the tolerance of the given expected value. The
+   * subject and tolerance must have been specified earlier in the fluent call chain.
    *
    * @param expected the value the subject is expected to be approximately equal to
    */
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java
index 5b32fada..f332350b 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation2dSubject.java
@@ -16,6 +16,7 @@
  * approximate comparisons within a tolerance, and check special properties like being zero.
  *
  * 
    Example usage:
+ *
  * 
    {@code
  * Translation2d translation = new Translation2d(1.0, 2.0);
  * Translation2dSubject.assertThat(translation)
@@ -37,8 +38,8 @@ public static Translation2dSubject assertThat(@Nullable Translation2d translatio
   }
 
   /**
-   * Returns a Truth {@link Factory} for {@link Translation2dSubject}, used with
-   * {@link com.google.common.truth.Truth#assertAbout(Subject.Factory)}.
+   * Returns a Truth {@link Factory} for {@link Translation2dSubject}, used with {@link
+   * com.google.common.truth.Truth#assertAbout(Subject.Factory)}.
    *
    * @return a factory for creating {@link Translation2dSubject} instances
    */
@@ -60,8 +61,8 @@ private Translation2dSubject(FailureMetadata failureMetadata, @Nullable Translat
   }
 
   /**
-   * Returns a {@link TolerantComparison} to assert that this translation is within the given
-   * {@code tolerance} of an expected {@link Translation2d}.
+   * Returns a {@link TolerantComparison} to assert that this translation is within the given {@code
+   * tolerance} of an expected {@link Translation2d}.
    *
    * @param tolerance the allowed absolute difference in both x and y components
    * @return a {@link TolerantComparison} for fluent approximate comparisons
@@ -76,9 +77,7 @@ public void of(Translation2d expected) {
     };
   }
 
-  /**
-   * Fails if the {@link Translation2d} is not the zero vector.
-   */
+  /** Fails if the {@link Translation2d} is not the zero vector. */
   public void isZero() {
     if (!Translation2d.kZero.equals(actual)) {
       failWithActual(simpleFact("expected to be zero"));
diff --git a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java
index cb278d0e..985bd5c9 100644
--- a/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java
+++ b/testing/src/main/java/com/team2813/lib2813/testing/truth/Translation3dSubject.java
@@ -16,6 +16,7 @@
  * perform approximate comparisons within a tolerance, and check special properties like being zero.
  *
  * 
    Example usage:
+ *
  * 
    {@code
  * Translation3d translation = new Translation3d(1.0, 2.0, 3.0);
  * Translation3dSubject.assertThat(translation)
@@ -38,8 +39,8 @@ public static Translation3dSubject assertThat(@Nullable Translation3d translatio
   }
 
   /**
-   * Returns a Truth {@link Factory} for {@link Translation3dSubject}, used with
-   * {@link com.google.common.truth.Truth#assertAbout(Subject.Factory)}.
+   * Returns a Truth {@link Factory} for {@link Translation3dSubject}, used with {@link
+   * com.google.common.truth.Truth#assertAbout(Subject.Factory)}.
    *
    * @return a factory for creating {@link Translation3dSubject} instances
    */
@@ -61,8 +62,8 @@ private Translation3dSubject(FailureMetadata failureMetadata, @Nullable Translat
   }
 
   /**
-   * Returns a {@link TolerantComparison} to assert that this translation is within the given
-   * {@code tolerance} of an expected {@link Translation3d}.
+   * Returns a {@link TolerantComparison} to assert that this translation is within the given {@code
+   * tolerance} of an expected {@link Translation3d}.
    *
    * @param tolerance the allowed absolute difference in x, y, and z components
    * @return a {@link TolerantComparison} for fluent approximate comparisons
@@ -78,9 +79,7 @@ public void of(Translation3d expected) {
     };
   }
 
-  /**
-   * Fails if the {@link Translation3d} is not the zero vector.
-   */
+  /** Fails if the {@link Translation3d} is not the zero vector. */
   public void isZero() {
     if (!Translation3d.kZero.equals(actual)) {
       failWithActual(simpleFact("expected to be zero"));
diff --git a/testing/src/test/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtensionTest.java b/testing/src/test/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtensionTest.java
index e09494de..84dae72f 100644
--- a/testing/src/test/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtensionTest.java
+++ b/testing/src/test/java/com/team2813/lib2813/testing/junit/jupiter/WPILibExtensionTest.java
@@ -114,9 +114,7 @@ public static void verifyFakeCommandNotScheduledAfterAllTests() {
     }
   } // end SampleTest
 
-  /**
-   * Verifies that {@link WPILibExtension} executes SampleTest correctly and without failures.
-   */
+  /** Verifies that {@link WPILibExtension} executes SampleTest correctly and without failures. */
   @Test
   void verifyExtension() {
     withDriverStationTemporarilyEnabled(