diff --git a/.github/instructions/default.instructions.md b/.github/instructions/default.instructions.md
index 4943d793..5b44e087 100644
--- a/.github/instructions/default.instructions.md
+++ b/.github/instructions/default.instructions.md
@@ -1,6 +1,6 @@
 # FRC Team W8 Library - AI Assistant Instructions
 
-You are a senior Java engineer with 10+ years of experience. You are helping high school students on an FRC (FIRST Robotics Competition) team. Write clear, educational code that students can learn from and maintain.
+You are a sr Java engineer with 10+ years of experience. You are helping high school students on an FRC (FIRST Robotics Competition) team. Write clear, educational code that students can learn from and maintain.
 
 ## Technology Stack
 
diff --git a/src/main/java/frc/lib/devices/ObjectDetection.java b/src/main/java/frc/lib/devices/ObjectDetection.java
index 7316d199..0a1b9733 100644
--- a/src/main/java/frc/lib/devices/ObjectDetection.java
+++ b/src/main/java/frc/lib/devices/ObjectDetection.java
@@ -6,9 +6,6 @@
 
 import org.littletonrobotics.junction.Logger;
 import java.util.ArrayList;
-import frc.lib.io.objectdetection.ObjectDetectionIO;
-import frc.lib.io.objectdetection.ObjectDetectionIOInputsAutoLogged;
-import frc.lib.io.objectdetection.ObjectDetectionIO.TargetObservation;
 import java.util.Arrays;
 import java.lang.Math;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -16,6 +13,9 @@
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import frc.lib.io.objectdetection.ObjectDetectionIOInputsAutoLogged;
+import frc.lib.io.objectdetection.ObjectDetectionIO;
+import frc.lib.io.objectdetection.ObjectDetectionIO.TargetObservation;
 
 /**
  * Device level implementation of an object detection camera. While the IO layer is responsible for
diff --git a/src/main/java/frc/lib/io/motor/MotorIO.java b/src/main/java/frc/lib/io/motor/MotorIO.java
index 5606904c..0c14f017 100644
--- a/src/main/java/frc/lib/io/motor/MotorIO.java
+++ b/src/main/java/frc/lib/io/motor/MotorIO.java
@@ -21,6 +21,7 @@
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.Volts;
 
+import org.apache.commons.lang3.NotImplementedException;
 import org.littletonrobotics.junction.AutoLog;
 
 import edu.wpi.first.units.AngularAccelerationUnit;
@@ -97,7 +98,7 @@ abstract class MotorInputs {
      */
     public default String getName()
     {
-        return "";
+        throw new NotImplementedException("This method has not been implemented");
     }
 
     /**
@@ -107,19 +108,25 @@ public default String getName()
      * @param inputs The structure to populate with updated sensor values.
      */
     public default void updateInputs(MotorInputs inputs)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Sets the motor to coast mode.
      */
     public default void runCoast()
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Sets the motor to brake mode.
      */
     public default void runBrake()
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Runs the motor using direct voltage control.
@@ -127,7 +134,9 @@ public default void runBrake()
      * @param voltage Desired voltage output.
      */
     public default void runVoltage(Voltage voltage)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Runs the motor with a specified current output.
@@ -135,7 +144,9 @@ public default void runVoltage(Voltage voltage)
      * @param current Desired torque-producing current.
      */
     public default void runCurrent(Current current)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Runs the motor with a specified current output and duty cycle.
@@ -144,7 +155,9 @@ public default void runCurrent(Current current)
      * @param dutyCycle Desired dutycycle of current output, limiting top speed
      */
     public default void runCurrent(Current current, double dutyCycle)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Runs the motor using duty cycle (percentage of available voltage).
@@ -152,7 +165,9 @@ public default void runCurrent(Current current, double dutyCycle)
      * @param dutyCycle Fractional output between 0 and 1.
      */
     public default void runDutyCycle(double dutyCycle)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Runs the motor to a specific position.
@@ -166,7 +181,20 @@ public default void runDutyCycle(double dutyCycle)
     public default void runPosition(Angle position, AngularVelocity cruiseVelocity,
         AngularAcceleration acceleration,
         Velocity<AngularAccelerationUnit> maxJerk, PIDSlot slot)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
+
+    /**
+     * Runs the motor to a specific position.
+     *
+     * @param position Target position.
+     * @param slot PID slot index.
+     */
+    public default void runPosition(Angle position, PIDSlot slot)
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Runs the motor at a target velocity.
@@ -177,7 +205,20 @@ public default void runPosition(Angle position, AngularVelocity cruiseVelocity,
      */
     public default void runVelocity(AngularVelocity velocity, AngularAcceleration acceleration,
         PIDSlot slot)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
+
+    /**
+     * Runs the motor at a target velocity.
+     *
+     * @param velocity Desired velocity.
+     * @param slot PID slot index.
+     */
+    public default void runVelocity(AngularVelocity velocity, PIDSlot slot)
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Sets the position of the motor's internal encoder
@@ -185,5 +226,16 @@ public default void runVelocity(AngularVelocity velocity, AngularAcceleration ac
      * @param position Desired position to set encoder to
      */
     public default void setEncoderPosition(Angle position)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
+
+    /**
+     * Closes and cleans up motor controller resources. Called when the motor is no longer needed to
+     * properly release hardware.
+     */
+    public default void close()
+    {
+        // Default implementation does nothing for interfaces that don't need cleanup
+    }
 }
diff --git a/src/main/java/frc/lib/io/motor/MotorIORev.java b/src/main/java/frc/lib/io/motor/MotorIORev.java
new file mode 100644
index 00000000..35c84baf
--- /dev/null
+++ b/src/main/java/frc/lib/io/motor/MotorIORev.java
@@ -0,0 +1,236 @@
+package frc.lib.io.motor;
+
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Celsius;
+import static edu.wpi.first.units.Units.Rotation;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
+import java.util.EnumMap;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+import frc.lib.util.Device;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+public class MotorIORev implements MotorIO {
+
+    public SparkBase motor;
+    public SparkClosedLoopController controller;
+    private RelativeEncoder encoder;
+
+    private final String name;
+
+    /**
+     * REV WORKAROUNDS
+     * 
+     * The motorIO interface was made for CTRE hardware. Some code does not map 1:1 to REV. The
+     * below are workarounds to make the mapping easier.
+     */
+
+    /**
+     * Maps MotorIO.PIDSlot to REV ClosedLoopSlot.
+     */
+    private static final EnumMap<MotorIO.PIDSlot, ClosedLoopSlot> SLOT_MAP =
+        new EnumMap<>(MotorIO.PIDSlot.class);
+    static {
+        SLOT_MAP.put(MotorIO.PIDSlot.SLOT_0, ClosedLoopSlot.kSlot0);
+        SLOT_MAP.put(MotorIO.PIDSlot.SLOT_1, ClosedLoopSlot.kSlot1);
+        SLOT_MAP.put(MotorIO.PIDSlot.SLOT_2, ClosedLoopSlot.kSlot2);
+    }
+
+    /**
+     * Gets the corresponding REV ClosedLoopSlot for a given MotorIO.PIDSlot.
+     * 
+     * @param slot The MotorIO.PIDSlot to convert.
+     */
+    protected ClosedLoopSlot getClosedLoopSlot(MotorIO.PIDSlot slot)
+    {
+        return SLOT_MAP.getOrDefault(slot, ClosedLoopSlot.kSlot0);
+    }
+
+    /**
+     * Maps MotorIO.ControlType to REV ControlType.
+     */
+    private static final EnumMap<ControlType, com.revrobotics.spark.SparkBase.ControlType> CONTROL_TYPE_MAP =
+        new EnumMap<>(ControlType.class);
+    static {
+        CONTROL_TYPE_MAP.put(ControlType.VOLTAGE,
+            com.revrobotics.spark.SparkBase.ControlType.kVoltage);
+        CONTROL_TYPE_MAP.put(ControlType.CURRENT,
+            com.revrobotics.spark.SparkBase.ControlType.kCurrent);
+        CONTROL_TYPE_MAP.put(ControlType.DUTYCYCLE,
+            com.revrobotics.spark.SparkBase.ControlType.kDutyCycle);
+        CONTROL_TYPE_MAP.put(ControlType.POSITION,
+            com.revrobotics.spark.SparkBase.ControlType.kPosition);
+        CONTROL_TYPE_MAP.put(ControlType.VELOCITY,
+            com.revrobotics.spark.SparkBase.ControlType.kVelocity);
+    }
+
+    /**
+     * Gets the corresponding REV ControlType for a given MotorIO.ControlType.
+     * 
+     * @param type The MotorIO.ControlType to convert.
+     */
+    protected com.revrobotics.spark.SparkBase.ControlType getRevControlType(ControlType type)
+    {
+        return CONTROL_TYPE_MAP.getOrDefault(type,
+            com.revrobotics.spark.SparkBase.ControlType.kDutyCycle);
+    }
+
+
+
+    /**
+     * Constructor for a standalone leader motor. This motor is configured as the primary motor in a
+     * system and can be manually controlled by the user. It also configures various settings for
+     * motion profiling and closed-loop control.
+     *
+     * @param name The name of the motor for identification purposes.
+     * @param id The unique identifier for the motor (CAN ID).
+     * @param isFlex Boolean indicating whether the motor is a SparkFlex (true) or a SparkMax
+     *        (false).
+     * @param config The configuration settings for the motor.
+     * @see MotorIO
+     */
+    public MotorIORev(
+        String name,
+        Device.CAN id,
+        boolean isFlex,
+        SparkBaseConfig config)
+    {
+        this.name = name;
+
+        if (isFlex) {
+            motor = new SparkFlex(id.id(), MotorType.kBrushless);
+        } else {
+            motor = new SparkMax(id.id(), MotorType.kBrushless);
+        }
+
+        controller = motor.getClosedLoopController();
+        encoder = motor.getEncoder();
+
+        motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
+    @Override
+    public String getName()
+    {
+        return name;
+    }
+
+    @Override
+    public void updateInputs(MotorInputs inputs)
+    {
+        // TODO: implement isConnected check for REV motors
+        inputs.connected = true;
+        inputs.position = Rotation.of(encoder.getPosition());
+        inputs.velocity = RotationsPerSecond.of(encoder.getVelocity() / 60.0);
+        inputs.appliedVoltage = Volts.of(motor.getAppliedOutput() * motor.getBusVoltage());
+        inputs.supplyCurrent = Amps.of(motor.getOutputCurrent());
+        inputs.temperature = Celsius.of(motor.getMotorTemperature());
+        inputs.positionError = Angle.ofBaseUnits(0, Rotation);
+        inputs.velocityError = AngularVelocity.ofBaseUnits(0, RotationsPerSecond);
+        inputs.goalPosition = Angle.ofBaseUnits(0, Rotation);
+        inputs.controlType = null;
+    }
+
+    @Override
+    public void runBrake()
+    {
+        SparkBaseConfig newConfig = newEmptyConfig();
+        newConfig.idleMode(IdleMode.kBrake);
+        motor.configure(newConfig, ResetMode.kNoResetSafeParameters,
+            PersistMode.kNoPersistParameters);
+    }
+
+    @Override
+    public void runCoast()
+    {
+        SparkBaseConfig newConfig = newEmptyConfig();
+        newConfig.idleMode(IdleMode.kCoast);
+        motor.configure(newConfig, ResetMode.kNoResetSafeParameters,
+            PersistMode.kNoPersistParameters);
+    }
+
+    @Override
+    public void runVoltage(Voltage volts)
+    {
+        controller.setReference(volts.in(Volts), getRevControlType(ControlType.VOLTAGE));
+    }
+
+    @Override
+    public void runCurrent(Current current)
+    {
+        controller.setReference(current.in(Amps), getRevControlType(ControlType.CURRENT));
+    }
+
+    @Override
+    public void runDutyCycle(double percent)
+    {
+        motor.set(percent);
+    }
+
+    @Override
+    public void runPosition(Angle position, PIDSlot slot)
+    {
+        controller.setReference(position.in(Rotation),
+            getRevControlType(ControlType.POSITION),
+            getClosedLoopSlot(slot));
+    }
+
+
+    @Override
+    public void runVelocity(AngularVelocity velocity,
+        PIDSlot slot)
+    {
+        controller.setReference(velocity.in(RotationsPerSecond) * 60,
+            getRevControlType(ControlType.VELOCITY),
+            getClosedLoopSlot(slot));
+    }
+
+    @Override
+    public void setEncoderPosition(Angle position)
+    {
+        encoder.setPosition(position.in(Rotation));
+
+    }
+
+    private SparkBaseConfig newEmptyConfig()
+    {
+        return (motor instanceof SparkFlex) ? new SparkFlexConfig() : new SparkMaxConfig();
+    }
+
+    @Override
+    public void close()
+    {
+        if (this.motor != null) {
+            // Clear references to help with cleanup
+            this.controller = null;
+            this.encoder = null;
+
+            // Close the motor (this should release the CAN ID)
+            this.motor.close();
+            this.motor = null;
+        }
+    }
+
+    protected SparkBase getMotor()
+    {
+        return this.motor;
+    }
+
+}
diff --git a/src/main/java/frc/lib/io/motor/MotorIORevSim.java b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
new file mode 100644
index 00000000..97a0934f
--- /dev/null
+++ b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
@@ -0,0 +1,140 @@
+package frc.lib.io.motor;
+
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Celsius;
+import static edu.wpi.first.units.Units.Rotation;
+import static edu.wpi.first.units.Units.Volts;
+
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import edu.wpi.first.math.system.plant.DCMotor;
+import com.revrobotics.spark.SparkSim;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import frc.lib.util.Device;
+
+
+/**
+ * Simulated implementation of {@link MotorIORev} for REV Robotics motors using WPILib simulation.
+ * Implements {@link MotorIOSim} to provide simulation-specific behavior.
+ *
+ * <p>
+ * Constructor arguments:
+ * <ul>
+ * <li><b>name</b> - Name of the motor</li>
+ * <li><b>id</b> - CAN ID of the motor</li>
+ * <li><b>isFlex</b> - True if using SparkFlex, false for SparkMax</li>
+ * <li><b>gearBox</b> - DCMotor gearbox model</li>
+ * <li><b>config</b> - Motor configuration</li>
+ * <li><b>followerData</b> - Varargs of follower motor data (ID and inversion)</li>
+ * </ul>
+ *
+ * <p>
+ * This class wraps a simulated SparkFlex or SparkMax motor, allowing position and velocity control
+ * in a simulation environment. It provides methods to run the motor in position or velocity mode,
+ * update simulation inputs, and manage simulation state.
+ *
+ * @see MotorIORev
+ * @see MotorIOSim
+ */
+public class MotorIORevSim extends MotorIORev implements MotorIOSim {
+
+    public record RevFollower(int id, boolean opposesLeader) {
+    }
+
+    public SparkBase motor;
+    public SparkClosedLoopController controller;
+    private SparkSim simState;
+
+    /**
+     * Constructs a MotorIORevSim instance.
+     * 
+     * @param name Name of the motor
+     * @param id CAN ID of the motor
+     * @param isFlex True if using SparkFlex, false for SparkMax
+     * @param gearBox DCMotor gearbox model
+     * @param config Motor configuration
+     * @param followerData Varargs of follower motor data (ID and inversion)
+     * 
+     * @see MotorIO
+     */
+    public MotorIORevSim(
+        String name,
+        Device.CAN id,
+        boolean isFlex,
+        DCMotor gearBox,
+        SparkBaseConfig config,
+        RevFollower... followerData)
+    {
+        super(name, id, isFlex, config);
+
+        motor = this.getMotor();
+
+        simState = new SparkSim(motor, gearBox);
+
+        motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+        simState.enable();
+    }
+
+    @Override
+    public void runPosition(Angle position, PIDSlot slot)
+    {
+        simState.setPosition(position.in(Rotations));
+
+    }
+
+
+    @Override
+    public void runVelocity(AngularVelocity velocity,
+        PIDSlot slot)
+    {
+        simState.setVelocity(velocity.in(RotationsPerSecond) * 60);
+
+    }
+
+    @Override
+    public double getRotorToSensorRatio()
+    {
+        return 1;
+    }
+
+    @Override
+    public double getSensorToMechanismRatio()
+    {
+        return 1;
+    }
+
+
+    @Override
+    public void updateInputs(MotorInputs inputs)
+    {
+
+        simState.setBusVoltage(12.0);
+
+        simState.iterate(
+            simState.getVelocity(),
+            simState.getBusVoltage(),
+            0.02);
+
+        inputs.position = Rotation.of(motor.getEncoder().getPosition());
+        inputs.velocity = RotationsPerSecond.of(motor.getEncoder().getVelocity() / 60);
+        inputs.appliedVoltage = Volts.of(simState.getAppliedOutput() * simState.getBusVoltage());
+        inputs.supplyCurrent = Amps.of(simState.getMotorCurrent());
+        inputs.temperature = Celsius.of(0);
+
+    }
+
+    @Override
+    public void close()
+    {
+        super.close();
+    }
+
+}
diff --git a/src/main/java/frc/lib/io/motor/MotorIOSim.java b/src/main/java/frc/lib/io/motor/MotorIOSim.java
index 53d82ca6..7e2c3d4d 100644
--- a/src/main/java/frc/lib/io/motor/MotorIOSim.java
+++ b/src/main/java/frc/lib/io/motor/MotorIOSim.java
@@ -19,38 +19,62 @@
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
 
+/**
+ * Extension of MotorIO interface for simulated motors.
+ * 
+ * <p>
+ * This interface adds methods specific to simulation, allowing WPILib physics simulations to feed
+ * data back into the motor IO layer. This creates a realistic simulation where the simulated
+ * physics affects the motor's sensor readings.
+ */
 public interface MotorIOSim extends MotorIO {
     /**
-     * Setter for the simulated mechanism position, typically taken from a WPILib mechanism
-     * simulation
+     * Sets the simulated mechanism position.
+     * 
+     * <p>
+     * This is called by WPILib mechanism simulations (like SingleJointedArmSim) to update the
+     * motor's position sensor based on the simulated physics. This creates a feedback loop where
+     * motor output affects the simulation, and the simulation updates the sensors.
      * 
-     * @param position The new mechanism position
+     * @param position The new mechanism position from the physics simulation
      */
     public default void setPosition(Angle position)
     {}
 
     /**
-     * Setter for the simulated mechanism velocity, typically taken from a WPILib mechanism
-     * simulation
+     * Sets the simulated motor rotor velocity.
+     * 
+     * <p>
+     * The rotor is the spinning part inside the motor. This method updates the velocity sensor
+     * reading based on what the physics simulation calculated. Rotor velocity is before any gear
+     * reduction.
      * 
-     * @param velocity The new mechanism velocity
+     * @param velocity The new motor rotor velocity from the physics simulation
      */
     public default void setRotorVelocity(AngularVelocity velocity)
     {}
 
     /**
-     * Setter for the simulated mechanism acceleration, typically taken from a WPILib mechanism
-     * simulation
+     * Sets the simulated motor rotor acceleration.
      * 
-     * @param acceleration The new mechanism acceleration
+     * <p>
+     * This updates the acceleration value based on the physics simulation. Acceleration is useful
+     * for advanced control algorithms and can help detect mechanism problems.
+     * 
+     * @param acceleration The new motor rotor acceleration from the physics simulation
      */
     public default void setRotorAcceleration(AngularAcceleration acceleration)
     {}
 
     /**
-     * Getter for the gear ratio to the sensor
+     * Gets the gear ratio from the motor rotor to the sensor.
+     * 
+     * <p>
+     * This is the first stage of gearing, from the motor's internal rotor to wherever the encoder
+     * is mounted. For example, if there's a 2:1 reduction between the motor and encoder, this
+     * returns 2.0.
      * 
-     * @return The gear ratio to the sensor
+     * @return The rotor-to-sensor gear ratio
      */
     public default double getRotorToSensorRatio()
     {
@@ -58,15 +82,27 @@ public default double getRotorToSensorRatio()
     }
 
     /**
-     * Getter for the gear ratio from the sensor to the mechanism
+     * Gets the gear ratio from the sensor to the final mechanism.
      * 
-     * @return The gear ratio from the sensor to the mechanism
+     * <p>
+     * This is the second stage of gearing, from the encoder to the actual mechanism you're
+     * controlling. For example, if there's a 2:1 reduction between the encoder and your arm pivot,
+     * this returns 2.0. The total reduction is rotor-to-sensor × sensor-to-mechanism.
+     * 
+     * @return The sensor-to-mechanism gear ratio
      */
     public default double getSensorToMechanismRatio()
     {
         return 0.0;
     }
 
+    /**
+     * Closes and cleans up simulation resources.
+     * 
+     * <p>
+     * Called when the simulation is shutting down to properly release any resources.
+     */
+    @Override
     public default void close()
     {}
 }
diff --git a/src/main/java/frc/lib/io/motor/MotorIOTalonFX.java b/src/main/java/frc/lib/io/motor/MotorIOTalonFX.java
index cf251c74..fb8f507d 100644
--- a/src/main/java/frc/lib/io/motor/MotorIOTalonFX.java
+++ b/src/main/java/frc/lib/io/motor/MotorIOTalonFX.java
@@ -45,6 +45,16 @@
  */
 public class MotorIOTalonFX implements MotorIO {
 
+    /**
+     * Configuration data for a TalonFX motor that follows another TalonFX motor.
+     * 
+     * <p>
+     * Follower motors mirror the output of a main motor, useful for mechanisms that require
+     * multiple motors working together (like a dual-motor elevator).
+     * 
+     * @param id The CAN device ID of the follower motor
+     * @param opposesMain Whether this follower should spin opposite to the main motor
+     */
     public record TalonFXFollower(Device.CAN id, boolean opposesMain) {
     }
 
@@ -84,11 +94,17 @@ public record TalonFXFollower(Device.CAN id, boolean opposesMain) {
 
     /**
      * Constructs and initializes a TalonFX motor.
-     *
-     * @param name The name of the motor(s)
-     * @param config Configuration to apply to the motor(s)
+     * 
+     * <p>
+     * This constructor applies the provided configuration to the main motor and all followers. It
+     * sets up the follower relationship, initializes status signals for telemetry, and configures
+     * signal update rates. All followers must be on the same CAN bus as the main motor.
+     * 
+     * @param name The name of the motor(s) for logging and identification
+     * @param config Configuration to apply to the motor(s) including PID, limits, and gear ratios
      * @param main CAN ID of the main motor
-     * @param followerData Configuration data for the follower(s)
+     * @param followerData Configuration data for the follower motor(s), can be empty if no
+     *        followers
      */
     public MotorIOTalonFX(String name, TalonFXConfiguration config, Device.CAN main,
         TalonFXFollower... followerData)
@@ -149,8 +165,12 @@ public MotorIOTalonFX(String name, TalonFXConfiguration config, Device.CAN main,
 
     /**
      * Checks if the motor is currently running a position control mode.
-     *
-     * @return True if the motor is using a position control mode.
+     * 
+     * <p>
+     * Position control modes command the motor to move to and hold a specific position. This
+     * includes Motion Magic modes that use motion profiling.
+     * 
+     * @return true if the motor is using a position control mode
      */
     protected boolean isRunningPositionControl()
     {
@@ -164,8 +184,11 @@ protected boolean isRunningPositionControl()
 
     /**
      * Checks if the motor is currently running a velocity control mode.
-     *
-     * @return True if the motor is using a velocity control mode.
+     * 
+     * <p>
+     * Velocity control modes command the motor to spin at a specific speed.
+     * 
+     * @return true if the motor is using a velocity control mode
      */
     protected boolean isRunningVelocityControl()
     {
@@ -178,8 +201,12 @@ protected boolean isRunningVelocityControl()
 
     /**
      * Checks if the motor is running any Motion Magic mode.
-     *
-     * @return True if the motor is using a Motion Magic mode.
+     * 
+     * <p>
+     * Motion Magic is CTRE's advanced motion profiling control mode that automatically generates
+     * smooth motion with controlled velocity, acceleration, and jerk (rate of acceleration).
+     * 
+     * @return true if the motor is using a Motion Magic mode
      */
     protected boolean isRunningMotionMagic()
     {
@@ -192,9 +219,13 @@ protected boolean isRunningMotionMagic()
     }
 
     /**
-     * Returns the current control type.
-     *
-     * @return The current control type.
+     * Determines the current control type being used by the motor.
+     * 
+     * <p>
+     * This identifies which control mode the motor is currently in (coast, brake, voltage, current,
+     * position, velocity, etc.).
+     * 
+     * @return The current ControlType
      */
     protected ControlType getCurrentControlType()
     {
@@ -219,8 +250,13 @@ protected ControlType getCurrentControlType()
 
     /**
      * Updates the passed-in MotorInputs structure with the latest sensor readings.
-     *
-     * @param inputs Motor input structure to populate.
+     * 
+     * <p>
+     * This method refreshes all status signals from the motor and populates the inputs object with
+     * current telemetry data. The data varies based on the current control mode - for example,
+     * position error and trajectory data are only populated during position control.
+     * 
+     * @param inputs Motor input structure to populate with current sensor data
      */
     @Override
     public void updateInputs(MotorInputs inputs)
@@ -282,7 +318,12 @@ public void updateInputs(MotorInputs inputs)
     }
 
     /**
-     * Sets the motor to coast mode.
+     * Sets the motor to coast mode (no active braking).
+     * 
+     * <p>
+     * In coast mode, the motor spins freely when not powered. The mechanism will slow down
+     * gradually due to friction but won't actively try to stop. Use this when you want the
+     * mechanism to move freely.
      */
     @Override
     public void runCoast()
@@ -291,7 +332,12 @@ public void runCoast()
     }
 
     /**
-     * Sets the motor to brake mode.
+     * Sets the motor to brake mode (active braking).
+     * 
+     * <p>
+     * In brake mode, the motor actively resists rotation when not powered. This causes the
+     * mechanism to stop quickly and hold position. Use this when you need the mechanism to stay in
+     * place.
      */
     @Override
     public void runBrake()
@@ -301,8 +347,11 @@ public void runBrake()
 
     /**
      * Runs the motor using direct voltage control.
-     *
-     * @param voltage Desired voltage output.
+     * 
+     * <p>
+     * Voltage control directly applies a voltage to the motor.
+     * 
+     * @param voltage Desired voltage output (positive = forward, negative = reverse)
      */
     @Override
     public void runVoltage(Voltage voltage)
@@ -311,9 +360,14 @@ public void runVoltage(Voltage voltage)
     }
 
     /**
-     * Runs the motor with a specified current output.
-     *
-     * @param current Desired torque-producing current.
+     * Runs the motor with a specified torque-producing current output.
+     * 
+     * <p>
+     * Current control directly controls the current flowing through the motor, which is
+     * proportional to torque output. This provides more consistent torque regardless of motor
+     * speed, compared to voltage control.
+     * 
+     * @param current Desired torque-producing current (not total supply current)
      */
     @Override
     public void runCurrent(Current current)
@@ -322,10 +376,14 @@ public void runCurrent(Current current)
     }
 
     /**
-     * Runs the motor with a specified current output and duty cycle.
-     *
-     * @param current Desired torque-producing current.
-     * @param dutyCycle Desired dutycycle of current output, limiting top speed
+     * Runs the motor with a specified current output limited by duty cycle.
+     * 
+     * <p>
+     * This version allows you to limit the maximum speed of the motor while using current control.
+     * The duty cycle caps the percentage of available voltage.
+     * 
+     * @param current Desired torque-producing current
+     * @param dutyCycle Maximum duty cycle (0.0 to 1.0) limiting top speed
      */
     @Override
     public void runCurrent(Current current, double dutyCycle)
@@ -335,9 +393,13 @@ public void runCurrent(Current current, double dutyCycle)
     }
 
     /**
-     * Runs the motor using duty cycle (percentage of available voltage).
-     *
-     * @param dutyCycle Fractional output between 0 and 1.
+     * Runs the motor using duty cycle control (percentage of available voltage) with FOC.
+     * 
+     * <p>
+     * Duty cycle control outputs a percentage of the current battery voltage. Unlike direct voltage
+     * control, this automatically adjusts for battery voltage changes.
+     * 
+     * @param dutyCycle Fractional output between 0.0 and 1.0 (will be clamped to this range)
      */
     @Override
     public void runDutyCycle(double dutyCycle)
@@ -347,13 +409,19 @@ public void runDutyCycle(double dutyCycle)
     }
 
     /**
-     * Runs the motor to a specific position.
-     *
-     * @param position Target position.
-     * @param cruiseVelocity Cruise velocity.
-     * @param acceleration Max acceleration.
-     * @param maxJerk Max jerk (rate of acceleration).
-     * @param slot PID slot index.
+     * Runs the motor to a specific position using Motion Magic with dynamic profiling.
+     * 
+     * <p>
+     * Motion Magic generates a smooth motion profile with controlled cruise velocity, acceleration,
+     * and jerk. Dynamic Motion Magic allows these parameters to be changed on-the-fly for each new
+     * position command. This provides smooth, predictable motion that's easy on mechanisms and
+     * looks professional.
+     * 
+     * @param position Target position to move to
+     * @param cruiseVelocity Maximum velocity during the move
+     * @param acceleration Maximum acceleration during speed-up and slow-down
+     * @param maxJerk Maximum jerk (rate of acceleration change) for smoothness
+     * @param slot PID slot index to use for position control gains
      */
     @Override
     public void runPosition(Angle position, AngularVelocity cruiseVelocity,
@@ -366,11 +434,15 @@ public void runPosition(Angle position, AngularVelocity cruiseVelocity,
     }
 
     /**
-     * Runs the motor at a target velocity.
-     *
-     * @param velocity Desired velocity.
-     * @param acceleration Max acceleration.
-     * @param slot PID slot index.
+     * Runs the motor at a target velocity using FOC current control.
+     * 
+     * <p>
+     * Velocity control maintains a constant speed using a PID controller. The motor will
+     * automatically adjust output to maintain the target velocity as load changes.
+     * 
+     * @param velocity Desired angular velocity (positive = forward, negative = reverse)
+     * @param acceleration Maximum acceleration when changing speeds
+     * @param slot PID slot index to use for velocity control gains
      */
     @Override
     public void runVelocity(AngularVelocity velocity, AngularAcceleration acceleration,
@@ -381,6 +453,16 @@ public void runVelocity(AngularVelocity velocity, AngularAcceleration accelerati
                 .withSlot(slot.getNum()));
     }
 
+    /**
+     * Manually sets the encoder position to a specific value.
+     * 
+     * <p>
+     * This is useful for zeroing the encoder when the mechanism is at a known position, or for
+     * resetting position after hitting a hard stop. Use carefully as incorrect position can cause
+     * the mechanism to move unexpectedly.
+     * 
+     * @param position The new position value to set the encoder to
+     */
     @Override
     public void setEncoderPosition(Angle position)
     {
diff --git a/src/main/java/frc/lib/mechanisms/Mechanism.java b/src/main/java/frc/lib/mechanisms/Mechanism.java
index e0ab8ea7..2c294d3b 100644
--- a/src/main/java/frc/lib/mechanisms/Mechanism.java
+++ b/src/main/java/frc/lib/mechanisms/Mechanism.java
@@ -15,10 +15,8 @@
 
 package frc.lib.mechanisms;
 
-import static edu.wpi.first.units.Units.Amps;
-import static edu.wpi.first.units.Units.Radians;
-import static edu.wpi.first.units.Units.RadiansPerSecond;
 import java.util.function.Supplier;
+import org.apache.commons.lang3.NotImplementedException;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.units.AngularAccelerationUnit;
 import edu.wpi.first.units.measure.Angle;
@@ -33,19 +31,25 @@ public interface Mechanism {
 
     /** Call this method periodically */
     public default void periodic()
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Sets the mechanism to coast mode.
      */
     public default void runCoast()
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Sets the mechanism to brake mode.
      */
     public default void runBrake()
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Runs the mechanism using direct voltage control.
@@ -53,7 +57,9 @@ public default void runBrake()
      * @param voltage Desired voltage output.
      */
     public default void runVoltage(Voltage voltage)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Runs the mechanism with a specified current output.
@@ -61,7 +67,9 @@ public default void runVoltage(Voltage voltage)
      * @param current Desired torque-producing current.
      */
     public default void runCurrent(Current current)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Runs the mechanism using duty cycle (percentage of available voltage).
@@ -69,7 +77,9 @@ public default void runCurrent(Current current)
      * @param dutyCycle Fractional output between 0 and 1.
      */
     public default void runDutyCycle(double dutyCycle)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Runs the mechanism to a specific position.
@@ -83,7 +93,20 @@ public default void runDutyCycle(double dutyCycle)
     public default void runPosition(Angle position, AngularVelocity cruiseVelocity,
         AngularAcceleration acceleration,
         Velocity<AngularAccelerationUnit> maxJerk, PIDSlot slot)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
+
+    /**
+     * Runs the mechanism to a specific position.
+     *
+     * @param position Target position.
+     * @param slot PID slot index.
+     */
+    public default void runPosition(Angle position, PIDSlot slot)
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Runs the mechanism at a target velocity.
@@ -94,7 +117,20 @@ public default void runPosition(Angle position, AngularVelocity cruiseVelocity,
      */
     public default void runVelocity(AngularVelocity velocity, AngularAcceleration acceleration,
         PIDSlot slot)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
+
+    /**
+     * Runs the mechanism at a target velocity.
+     *
+     * @param velocity Desired velocity.
+     * @param slot PID slot index.
+     */
+    public default void runVelocity(AngularVelocity velocity, PIDSlot slot)
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Sets the position of the motor's internal encoder
@@ -102,11 +138,13 @@ public default void runVelocity(AngularVelocity velocity, AngularAcceleration ac
      * @param position Desired position to set encoder to
      */
     public default void setEncoderPosition(Angle position)
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     public default Current getSupplyCurrent()
     {
-        return Amps.of(0.0);
+        throw new NotImplementedException("This method has not been implemented");
     }
 
     /**
@@ -116,21 +154,23 @@ public default Current getSupplyCurrent()
      */
     public default Angle getPosition()
     {
-        return Radians.of(0.0);
+        throw new NotImplementedException("This method has not been implemented");
     }
 
     public default Current getTorqueCurrent()
     {
-        return Amps.of(0);
+        throw new NotImplementedException("This method has not been implemented");
     }
 
     public default AngularVelocity getVelocity()
     {
-        return RadiansPerSecond.of(0.0);
+        throw new NotImplementedException("This method has not been implemented");
     }
 
     public default void close()
-    {}
+    {
+        throw new NotImplementedException("This method has not been implemented");
+    }
 
     /**
      * Supplier for the Pose3d of the mechanism
@@ -139,6 +179,8 @@ public default void close()
      */
     public default Supplier<Pose3d> getPoseSupplier()
     {
-        return () -> Pose3d.kZero;
+        throw new NotImplementedException("This method has not been implemented");
     }
+
+
 }
diff --git a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
index e0d8f1cd..a7255a03 100644
--- a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
+++ b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
@@ -24,6 +24,8 @@ public static enum RotaryAxis {
         ROLL
     }
 
+    private final String name;
+
     public static record RotaryMechCharacteristics(
         Translation3d offset,
         Distance armLength,
@@ -39,6 +41,7 @@ public static record RotaryMechCharacteristics(
 
     public RotaryMechanism(String name, RotaryMechCharacteristics characteristics)
     {
+        this.name = name;
         visualizer = new RotaryVisualizer(name, characteristics);
     }
 
@@ -75,6 +78,7 @@ public void periodic()
         visualizer.setCurrentAngle(inputs.position);
         visualizer.setTrajectoryAngle(getTrajectoryAngle());
         visualizer.setGoalAngle(getGoalAngle());
+
     }
 
     @Override
diff --git a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismReal.java b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismReal.java
index be56d0e9..afd44225 100644
--- a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismReal.java
+++ b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismReal.java
@@ -89,6 +89,18 @@ public void runPosition(Angle position, AngularVelocity cruiseVelocity,
         io.runPosition(position, cruiseVelocity, acceleration, maxJerk, slot);
     }
 
+    @Override
+    public void runPosition(Angle position, PIDSlot slot)
+    {
+        io.runPosition(position, slot);
+    }
+
+    @Override
+    public void runVelocity(AngularVelocity velocity, PIDSlot slot)
+    {
+        io.runVelocity(velocity, slot);
+    }
+
     @Override
     public void runVelocity(AngularVelocity velocity, AngularAcceleration acceleration,
         PIDSlot slot)
diff --git a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismSim.java b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismSim.java
index 0934f1e7..0011efb4 100644
--- a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismSim.java
+++ b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismSim.java
@@ -145,6 +145,18 @@ public void runPosition(Angle position, AngularVelocity cruiseVelocity,
         io.runPosition(position, cruiseVelocity, acceleration, maxJerk, slot);
     }
 
+    @Override
+    public void runPosition(Angle position, PIDSlot slot)
+    {
+        io.runPosition(position, slot);
+    }
+
+    @Override
+    public void runVelocity(AngularVelocity velocity, PIDSlot slot)
+    {
+        io.runVelocity(velocity, slot);
+    }
+
     @Override
     public void runVelocity(AngularVelocity velocity, AngularAcceleration acceleration,
         PIDSlot slot)
@@ -163,4 +175,12 @@ public AngularVelocity getVelocity()
     {
         return inputs.velocity;
     }
+
+    @Override
+    public void close()
+    {
+        if (io != null) {
+            io.close();
+        }
+    }
 }
diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java
index c4795462..872dfdb8 100644
--- a/src/main/java/frc/robot/Ports.java
+++ b/src/main/java/frc/robot/Ports.java
@@ -35,6 +35,8 @@ public class Ports {
 
     public static final Device.CAN pdh = new CAN(50, "rio");
 
+    public static final Device.CAN revRotarySubsytemMotorMain = new CAN(7, "rio");
+
     public static final Device.CAN RotarySubsystemMotorMain = new CAN(3, "rio");
     public static final Device.CAN RotarySubsystemMotorFollower = new CAN(4, "rio");
     public static final Device.CAN RotarySubsystemEncoder = new CAN(6, "rio");
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 8fc784a4..a224818d 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -35,6 +35,7 @@
 import frc.lib.io.vision.VisionIO;
 import frc.lib.io.vision.VisionIOPhotonVision;
 import frc.lib.io.vision.VisionIOPhotonVisionSim;
+import frc.lib.mechanisms.rotary.RotaryMechanism;
 import frc.lib.util.LoggedDashboardChooser;
 import frc.lib.util.LoggedTunableNumber;
 import frc.lib.util.LoggedTuneableProfiledPID;
@@ -77,6 +78,8 @@
 import frc.robot.subsystems.vision.VisionConstants;
 import frc.robot.subsystems.objectDetector.ObjectDetector;
 import frc.robot.subsystems.objectDetector.ObjectDetectorConstants;
+import frc.robot.subsystems.revRotary.RevRotary;
+import frc.robot.subsystems.revRotary.RevRotaryConstants;
 import frc.robot.util.BallSimulator;
 import frc.robot.subsystems.lasercan1.LaserCAN1;
 import frc.robot.subsystems.lasercan1.LaserCAN1Constants;
@@ -110,6 +113,7 @@ public class RobotContainer {
     private final Linear linear;
     private final Vision vision;
     private final Rotary rotary;
+    private final RevRotary revRotary;
     private final ObjectDetector objectDetector;
 
     // Controller
@@ -134,6 +138,7 @@ public RobotContainer()
         beamBreak1 = BeamBreak1Constants.get();
         linear = LinearConstants.get();
         rotary = RotaryConstants.get();
+        revRotary = RevRotaryConstants.get();
         servo1 = Servo1Constants.get();
         vision = VisionConstants.get(drive); // TODO: Will be refactored in the future to RobotState
         objectDetector = ObjectDetectorConstants.get(drive);
@@ -227,7 +232,9 @@ private void configureButtonBindings()
         SmartDashboard.putData("Rotary: Stow", rotary.setSetpoint(Rotary.Setpoint.STOW));
         SmartDashboard.putData("Rotary: Raised",
             rotary.setSetpoint(Rotary.Setpoint.RAISED));
-
+        SmartDashboard.putData("RevRotary: Stow", revRotary.setSetpoint(RevRotary.Setpoint.STOW));
+        SmartDashboard.putData("RevRotary: Raised",
+            revRotary.setSetpoint(RevRotary.Setpoint.RAISED));
         LoggedTunableNumber ballVel = new LoggedTunableNumber("Ball Sim Velocity (fps)", 15);
         SmartDashboard.putData("Shoot Ball", Commands
             .runOnce(() -> BallSimulator.launch(FeetPerSecond.of(ballVel.getAsDouble()),
@@ -317,4 +324,15 @@ public void checkStartPose()
                 false);
         }
     }
+
+    /**
+     * Helper method for unit testing.
+     * 
+     * If we have any REV subsystems, close their resources here. IF we don't do this then we will
+     * get duplicate CAN ID errors when running multiple tests.
+     */
+    public void closeREVSubsystems()
+    {
+        revRotary.close();
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetectorConstants.java b/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetectorConstants.java
index ddedd791..b6e0f9f2 100644
--- a/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetectorConstants.java
+++ b/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetectorConstants.java
@@ -13,7 +13,9 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj.Timer;
-import frc.lib.io.objectdetection.*;
+import frc.lib.io.objectdetection.ObjectDetectionIO;
+import frc.lib.io.objectdetection.ObjectDetectionIOPhotonVision;
+import frc.lib.io.objectdetection.ObjectDetectionIOSim;
 import frc.robot.Constants;
 import frc.robot.subsystems.drive.Drive;
 
diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
new file mode 100644
index 00000000..8e198a9a
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
@@ -0,0 +1,96 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.revRotary;
+
+import static edu.wpi.first.units.Units.Degrees;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.io.motor.MotorIO.PIDSlot;
+import frc.lib.mechanisms.rotary.RotaryMechanism;
+import frc.lib.util.LoggedTunableNumber;
+import frc.lib.util.LoggerHelper;
+import lombok.Getter;
+import lombok.RequiredArgsConstructor;
+
+public class RevRotary extends SubsystemBase {
+
+    private final RotaryMechanism io;
+
+    private static final LoggedTunableNumber STOW_SETPOINT =
+        new LoggedTunableNumber("REV STOW", 0.0);
+    private static final LoggedTunableNumber RAISED_SETPOINT =
+        new LoggedTunableNumber("REV RAISED", 90);
+
+    @RequiredArgsConstructor
+    @Getter
+    public enum Setpoint {
+        STOW(STOW_SETPOINT),
+        RAISED(RAISED_SETPOINT);
+
+        private final LoggedTunableNumber tunableNumber;
+
+        public Angle getSetpoint()
+        {
+            return Degrees.of(tunableNumber.get());
+        }
+    }
+
+
+    public RevRotary(RotaryMechanism io)
+    {
+        this.io = io;
+
+        setSetpoint(RevRotaryConstants.DEFAULT_SETPOINT).ignoringDisable(true).schedule();
+    }
+
+    @Override
+    public void periodic()
+    {
+        LoggerHelper.recordCurrentCommand(RevRotaryConstants.NAME, this);
+        io.periodic();
+        // robotstate.setRotaryPose(io.getPoseSupplier().get());
+    }
+
+    public Command setSetpoint(Setpoint setpoint)
+    {
+        return this.runOnce(
+            () -> io.runPosition(setpoint.getSetpoint(),
+                PIDSlot.SLOT_0))
+            .withName("Go To " + setpoint.toString() + " Setpoint");
+    };
+
+    public boolean nearGoal(Angle targetPosition)
+    {
+        return io.nearGoal(targetPosition, RevRotaryConstants.TOLERANCE);
+    }
+
+    public Command waitUntilGoalCommand(Angle position)
+    {
+        return Commands.waitUntil(() -> {
+            return nearGoal(position);
+        });
+    }
+
+    public Command setGoalCommandWithWait(Setpoint setpoint)
+    {
+        return waitUntilGoalCommand(setpoint.getSetpoint())
+            .deadlineFor(setSetpoint(setpoint))
+            .withName("Go To " + setpoint.toString() + " Setpoint with wait");
+    }
+
+    public AngularVelocity getVelocity()
+    {
+        return io.getVelocity();
+    }
+
+    public void close()
+    {
+        io.close();
+    }
+
+}
diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
new file mode 100644
index 00000000..d0ba8fb8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
@@ -0,0 +1,196 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.revRotary;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.KilogramSquareMeters;
+import static edu.wpi.first.units.Units.Kilograms;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.Second;
+import java.util.Optional;
+import static edu.wpi.first.units.Units.Meters;
+import frc.robot.Constants;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.AngularAccelerationUnit;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularAcceleration;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Mass;
+import edu.wpi.first.units.measure.MomentOfInertia;
+import edu.wpi.first.units.measure.Velocity;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import frc.lib.io.motor.MotorIO;
+import frc.lib.io.motor.MotorIORev;
+import frc.lib.io.motor.MotorIORevSim;
+import frc.lib.io.motor.MotorIOSim;
+import frc.lib.mechanisms.rotary.*;
+import frc.lib.mechanisms.rotary.RotaryMechanism.RotaryAxis;
+import frc.lib.mechanisms.rotary.RotaryMechanism.RotaryMechCharacteristics;
+import frc.robot.Ports;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.config.ClosedLoopConfig;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+
+/** Add your docs here. */
+public class RevRotaryConstants {
+    public static String NAME = "REVRotary";
+
+    public static final Angle TOLERANCE = Degrees.of(2.0);
+
+    public static final AngularVelocity CRUISE_VELOCITY =
+        Units.RadiansPerSecond.of(1);
+    public static final AngularAcceleration ACCELERATION =
+        CRUISE_VELOCITY.div(0.1).per(Units.Second);
+    public static final Velocity<AngularAccelerationUnit> JERK = ACCELERATION.per(Second);
+
+    private static final double ROTOR_TO_SENSOR = (2.0 / 1.0);
+    private static final double SENSOR_TO_MECHANISM = (2.0 / 1.0);
+    private static final double GEAR_RATIO = ROTOR_TO_SENSOR * SENSOR_TO_MECHANISM;
+
+    public static final Translation3d OFFSET = Translation3d.kZero;
+
+    public static final Angle MIN_ANGLE = Degrees.of(0.0);
+    public static final Angle MAX_ANGLE = Rotations.of(.5);
+    public static final Angle STARTING_ANGLE = Rotations.of(0.0);
+    public static final Distance ARM_LENGTH = Meters.of(1.0);
+
+    public static final RotaryMechCharacteristics CONSTANTS =
+        new RotaryMechCharacteristics(OFFSET, ARM_LENGTH, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE,
+            RotaryAxis.PITCH);
+
+    public static final Mass ARM_MASS = Kilograms.of(.01);
+    public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1);
+    public static final MomentOfInertia MOI = KilogramSquareMeters
+        .of(SingleJointedArmSim.estimateMOI(ARM_LENGTH.in(Meters), ARM_MASS.in(Kilograms)));
+
+    private static final Angle ENCODER_OFFSET = Rotations.of(0.0);
+
+    public static final RevRotary.Setpoint DEFAULT_SETPOINT =
+        RevRotary.Setpoint.STOW;
+
+
+
+    // Positional PID
+    private static ClosedLoopConfig SLOT0CONFIG = new ClosedLoopConfig()
+        .pid(30.0, 0, 5.0, ClosedLoopSlot.kSlot0); // Added D gain to match TalonFX config
+
+    /**
+     * Creates and returns the SparkFlex/SparkMax motor controller configuration for the rotary mechanism.
+     * 
+     * <p>
+     * This configuration includes:
+     * <ul>
+     * <li>Voltage compensation for consistent power output</li>
+     * <li>Idle (brake) mode to hold position when not moving</li>
+     * <li>Motor inversion setting</li>
+     * <li>Encoder position and velocity conversion factors for proper feedback scaling</li>
+     * <li>PID gains applied to the selected closed-loop slot</li>
+     * <!-- Add other REV-specific features as needed -->
+     * </ul>
+     * 
+     * @return A configured {@link SparkBaseConfig} object ready to apply to a REV Robotics SparkFlex or SparkMax motor controller
+     */
+    public static SparkBaseConfig getREVConfig()
+    {
+        SparkFlexConfig config = new SparkFlexConfig();
+
+        config.voltageCompensation(12.0);
+        config.idleMode(IdleMode.kBrake);
+        config.inverted(false);
+
+        // Add gear ratio configuration for position/velocity conversion
+        config.encoder.positionConversionFactor(1.0 / GEAR_RATIO);
+        config.encoder.velocityConversionFactor(1.0 / GEAR_RATIO / 60.0); // RPM to RPS
+
+        // Add soft limits to match TalonFX behavior
+
+        config.apply(SLOT0CONFIG);
+
+        return config;
+    }
+
+    /**
+     * Creates the real robot implementation of the rotary mechanism.
+     * 
+     * <p>
+     * This method instantiates the actual hardware objects (TalonFX motors and CANcoder) that will
+     * be used when running on a real robot.
+     * 
+     * @return A RotaryMechanismReal object configured with real hardware
+     */
+    public static RevRotary getReal()
+    {
+        MotorIO io = new MotorIORev(NAME, Ports.revRotarySubsytemMotorMain, true, getREVConfig());
+
+        return new RevRotary(new RotaryMechanismReal(io, CONSTANTS, Optional.empty()));
+    }
+
+    /**
+     * Creates the simulation implementation of the rotary mechanism.
+     * 
+     * <p>
+     * This method creates a physics-based simulation of the mechanism using WPILib's simulation
+     * classes. It models the motor, moment of inertia, and other physical properties to provide
+     * realistic behavior in simulation.
+     * 
+     * @return A RotaryMechanismSim object configured for physics simulation
+     */
+    public static RevRotary getSim()
+    {
+        MotorIOSim io = new MotorIORevSim(
+            NAME,
+            Ports.revRotarySubsytemMotorMain,
+            true,
+            DCMOTOR,
+            getREVConfig());
+
+        return new RevRotary(new RotaryMechanismSim(
+            io,
+            DCMOTOR,
+            MOI,
+            false,
+            CONSTANTS,
+            Optional.empty()));
+    }
+
+    /**
+     * Creates the log replay implementation of the rotary mechanism.
+     * 
+     * <p>
+     * This is used with AdvantageKit's log replay feature, which allows you to replay logged data
+     * and debug robot code without having the actual robot or running simulation.
+     * 
+     * @return A RotaryMechanism object for log replay
+     */
+    public static RevRotary getReplay()
+    {
+        return new RevRotary(new RotaryMechanism(NAME, CONSTANTS) {});
+    }
+
+    /**
+     * Method to get the appropriate RotaryMechanism based on the current robot mode.
+     * 
+     * @return RotaryMechanism instance for the current mode (real, sim, or replay)
+     */
+    public static RevRotary get()
+    {
+        switch (Constants.currentMode) {
+            case REAL:
+                return getReal();
+            case SIM:
+                return getSim();
+            case REPLAY:
+                return getReplay();
+            default:
+                throw new IllegalStateException("Unrecognized Robot Mode");
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/rotary/Rotary.java b/src/main/java/frc/robot/subsystems/rotary/Rotary.java
index 1600fcc3..b497ffc1 100644
--- a/src/main/java/frc/robot/subsystems/rotary/Rotary.java
+++ b/src/main/java/frc/robot/subsystems/rotary/Rotary.java
@@ -26,19 +26,27 @@
 public class Rotary extends SubsystemBase {
 
     private final RotaryMechanism io;
-    private static final LoggedTunableNumber STOW_SETPOINT = new LoggedTunableNumber("TEST", 0.0);
-    private static final LoggedTunableNumber RAISED_SETPOINT =
-        new LoggedTunableNumber("RAISED", -90);
+    private static LoggedTunableNumber STOW_SETPOINT =
+        new LoggedTunableNumber("Rotary STOW", 0.0);
+    private static LoggedTunableNumber RAISED_SETPOINT =
+        new LoggedTunableNumber("Rotary RAISED", -90);
 
     @RequiredArgsConstructor
-    @SuppressWarnings("Immutable")
     @Getter
     public enum Setpoint {
-        HOME(Degrees.of(0.0)),
-        STOW(Degrees.of(STOW_SETPOINT.get())),
-        RAISED(Degrees.of(RAISED_SETPOINT.get()));
-
-        private final Angle setpoint;
+        HOME(null),
+        STOW(STOW_SETPOINT),
+        RAISED(RAISED_SETPOINT);
+
+        private final LoggedTunableNumber tunableNumber;
+
+        public Angle getSetpoint()
+        {
+            if (tunableNumber == null) {
+                return Degrees.of(0.0);
+            }
+            return Degrees.of(tunableNumber.get());
+        }
     }
 
     private Debouncer homeDebouncer = new Debouncer(0.1, DebounceType.kRising);
diff --git a/src/main/java/frc/robot/subsystems/rotary/RotaryConstants.java b/src/main/java/frc/robot/subsystems/rotary/RotaryConstants.java
index 6c859af2..344c0b3a 100644
--- a/src/main/java/frc/robot/subsystems/rotary/RotaryConstants.java
+++ b/src/main/java/frc/robot/subsystems/rotary/RotaryConstants.java
@@ -37,6 +37,7 @@
 import frc.robot.Constants;
 import frc.robot.Ports;
 
+
 public class RotaryConstants {
     public static String NAME = "Rotary";
 
@@ -82,6 +83,23 @@ public class RotaryConstants {
         .withKI(0.0)
         .withKD(5.0);
 
+    /**
+     * Creates and returns the TalonFX motor controller configuration for the rotary mechanism.
+     * 
+     * <p>
+     * This configuration includes:
+     * <ul>
+     * <li>Current limits to prevent motor damage and brownouts</li>
+     * <li>Voltage limits for power output</li>
+     * <li>Brake mode to hold position when not moving</li>
+     * <li>Software limit switches to prevent mechanism damage</li>
+     * <li>Gear ratios for proper position/velocity feedback</li>
+     * <li>Remote CANcoder feedback for absolute positioning</li>
+     * <li>PID gains for control</li>
+     * </ul>
+     * 
+     * @return A configured TalonFXConfiguration object ready to apply to a motor controller
+     */
     public static TalonFXConfiguration getFXConfig()
     {
         TalonFXConfiguration config = new TalonFXConfiguration();
@@ -117,6 +135,17 @@ public static TalonFXConfiguration getFXConfig()
         return config;
     }
 
+    /**
+     * Creates and returns the CANcoder absolute encoder configuration.
+     * 
+     * <p>
+     * The CANcoder provides absolute position feedback, meaning it knows the true position of the
+     * mechanism even after power cycling. The magnet offset calibrates the encoder's zero position.
+     * 
+     * @param sim Whether this configuration is for simulation (true) or real robot (false). In
+     *        simulation, the offset is set to 0.0 since it's not needed.
+     * @return A configured CANcoderConfiguration object
+     */
     public static CANcoderConfiguration getCANcoderConfig(boolean sim)
     {
         CANcoderConfiguration config = new CANcoderConfiguration();
diff --git a/src/test/java/frc/robot/RobotContainerTest.java b/src/test/java/frc/robot/RobotContainerTest.java
index badacdfc..68bd7c0e 100644
--- a/src/test/java/frc/robot/RobotContainerTest.java
+++ b/src/test/java/frc/robot/RobotContainerTest.java
@@ -1,36 +1,55 @@
-/* Copyright (C) 2025 Windham Windup
+/*
+ * Copyright (C) 2025 Windham Windup
  *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * any later version.
+ * This program is free software: you can redistribute it and/or modify it under the terms of the
+ * GNU General Public License as published by the Free Software Foundation, either version 3 of the
+ * License, or any later version.
  *
- * This program is distributed in
- the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
+ * This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
+ * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ * You should have received a copy of the GNU General Public License along with this program. If
+ * not, see <https://www.gnu.org/licenses/>.
  */
 
- package frc.robot;
-
- import static org.junit.jupiter.api.Assertions.fail;
- 
- import org.junit.jupiter.api.Test;
- 
- public class RobotContainerTest {
- 
-     @Test
-     public void createRobotContainer() {
-         // Instantiate RobotContainer
-         try {
-             new RobotContainer();
-         } catch (Exception e) {
-             e.printStackTrace();
-             fail("Failed to instantiate RobotContainer, see stack trace above.");
-         }
-     }
- }
\ No newline at end of file
+package frc.robot;
+
+import static org.junit.jupiter.api.Assertions.fail;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+
+public class RobotContainerTest {
+    private RobotContainer robotContainer;
+
+    @BeforeEach
+    void setup()
+    {
+        assert HAL.initialize(500, 0);
+
+
+        /* enable the robot */
+        DriverStationSim.setEnabled(true);
+        DriverStationSim.notifyNewData();
+
+        /* delay ~100ms so the devices can start up and enable */
+        Timer.delay(0.100);
+    }
+
+    @Test
+    public void createRobotContainer()
+    {
+        try {
+            robotContainer = new RobotContainer();
+            robotContainer.closeREVSubsystems();
+        } catch (Exception e) {
+            e.printStackTrace();
+            fail("Failed to instantiate RobotContainer, see stack trace above.");
+        }
+    }
+}
diff --git a/src/test/java/frc/robot/subsystems/revRotary/RevRotaryTest.java b/src/test/java/frc/robot/subsystems/revRotary/RevRotaryTest.java
new file mode 100644
index 00000000..1b816366
--- /dev/null
+++ b/src/test/java/frc/robot/subsystems/revRotary/RevRotaryTest.java
@@ -0,0 +1,209 @@
+/*
+ * Copyright (C) 2025 Windham Windup
+ *
+ * This program is free software: you can redistribute it and/or modify it under the terms of the
+ * GNU General Public License as published by the Free Software Foundation, either version 3 of the
+ * License, or any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
+ * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with this program. If
+ * not, see <https://www.gnu.org/licenses/>.
+ */
+
+package frc.robot.subsystems.revRotary;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.fail;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.MethodOrderer;
+import org.junit.jupiter.api.TestMethodOrder;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.TestUtil;
+
+@TestMethodOrder(MethodOrderer.MethodName.class)
+public class RevRotaryTest implements AutoCloseable {
+    RevRotary revRotary;
+
+    @BeforeEach
+    void setup()
+    {
+        assert HAL.initialize(500, 0); // initialize the HAL, crash if failed
+
+        revRotary = RevRotaryConstants.get();
+
+        /* enable the robot */
+        DriverStationSim.setEnabled(true);
+        DriverStationSim.notifyNewData();
+
+        /* delay ~100ms so the devices can start up and enable */
+        Timer.delay(0.100);
+    }
+
+    @SuppressWarnings("PMD.SignatureDeclareThrowsException")
+    @AfterEach
+    void shutdown() throws Exception
+    {
+        // Close the subsystem and its resources
+        close();
+    }
+
+    @Test
+    void testGoToStow()
+    {
+        TestUtil.runTest(revRotary.setSetpoint(RevRotary.Setpoint.STOW), 3, revRotary);
+        try {
+            // Check to see if RevRotary subsystem is within tolerance of STOW setpoint
+            assertTrue(revRotary.nearGoal(RevRotary.Setpoint.STOW.getSetpoint()));
+        } catch (Exception e) {
+            fail("Failed to run RevRotary Subsystem to STOW: " + e.getMessage());
+        }
+    }
+
+    @Test
+    void testGoToRaised()
+    {
+        TestUtil.runTest(revRotary.setSetpoint(RevRotary.Setpoint.RAISED), 3, revRotary);
+        try {
+            // Check to see if RevRotary subsystem is within tolerance of RAISED setpoint
+            assertTrue(revRotary.nearGoal(RevRotary.Setpoint.RAISED.getSetpoint()));
+        } catch (Exception e) {
+            fail("Failed to run RevRotary Subsystem to RAISED: " + e.getMessage());
+        }
+    }
+
+    @Test
+    void testGoToGoalWithWait()
+    {
+        TestUtil.runTest(revRotary.setGoalCommandWithWait(RevRotary.Setpoint.STOW), 3, revRotary);
+        try {
+            // Check position to verify the subsystem is in tolerance of STOW setpoint
+            assertTrue(revRotary.nearGoal(RevRotary.Setpoint.STOW.getSetpoint()));
+        } catch (Exception e) {
+            fail("Failed to run RevRotary Subsystem to STOW with wait: " + e.getMessage());
+        }
+    }
+
+    @Test
+    void testSetpointEnum()
+    {
+        // Test that all setpoints can be retrieved and have valid values
+        try {
+            RevRotary.Setpoint stow = RevRotary.Setpoint.STOW;
+            RevRotary.Setpoint raised = RevRotary.Setpoint.RAISED;
+
+            assertNotNull(stow.getSetpoint());
+            assertNotNull(raised.getSetpoint());
+            assertNotNull(stow.getTunableNumber());
+            assertNotNull(raised.getTunableNumber());
+        } catch (Exception e) {
+            fail("Failed to retrieve setpoint values: " + e.getMessage());
+        }
+    }
+
+    @Test
+    void testNearGoalTolerance()
+    {
+        // Move to STOW position
+        TestUtil.runTest(revRotary.setSetpoint(RevRotary.Setpoint.STOW), 3, revRotary);
+
+        try {
+            // Test that we're near the STOW goal
+            assertTrue(revRotary.nearGoal(RevRotary.Setpoint.STOW.getSetpoint()));
+
+            // Test that we're not near the RAISED goal when at STOW
+            assertFalse(revRotary.nearGoal(RevRotary.Setpoint.RAISED.getSetpoint()));
+        } catch (Exception e) {
+            fail("Failed near goal tolerance test: " + e.getMessage());
+        }
+    }
+
+    @Test
+    void testGetVelocity()
+    {
+        // Start a movement
+        TestUtil.runTest(revRotary.setSetpoint(RevRotary.Setpoint.RAISED), 0.5, revRotary);
+
+        try {
+            // Velocity should be accessible during movement
+            assertNotNull(revRotary.getVelocity());
+        } catch (Exception e) {
+            fail("Failed get velocity test: " + e.getMessage());
+        }
+    }
+
+    @Test
+    void testMultipleSetpoints()
+    {
+        // Test moving between multiple setpoints
+        TestUtil.runTest(revRotary.setSetpoint(RevRotary.Setpoint.STOW), 3, revRotary);
+        assertTrue(revRotary.nearGoal(RevRotary.Setpoint.STOW.getSetpoint()));
+
+        TestUtil.runTest(revRotary.setSetpoint(RevRotary.Setpoint.RAISED), 3, revRotary);
+        assertTrue(revRotary.nearGoal(RevRotary.Setpoint.RAISED.getSetpoint()));
+
+        TestUtil.runTest(revRotary.setSetpoint(RevRotary.Setpoint.STOW), 3, revRotary);
+        assertTrue(revRotary.nearGoal(RevRotary.Setpoint.STOW.getSetpoint()));
+    }
+
+    @Test
+    void testCommandNaming()
+    {
+        // Test that commands have descriptive names for debugging
+        Command stowCmd = revRotary.setSetpoint(RevRotary.Setpoint.STOW);
+        assertTrue(stowCmd.getName().contains("STOW"));
+
+        Command raisedCmd = revRotary.setSetpoint(RevRotary.Setpoint.RAISED);
+        assertTrue(raisedCmd.getName().contains("RAISED"));
+
+        Command waitCmd = revRotary.setGoalCommandWithWait(RevRotary.Setpoint.STOW);
+        assertTrue(waitCmd.getName().contains("wait"));
+    }
+
+    @Test
+    void testPeriodicExecution()
+    {
+        // Test that periodic() can be called without errors
+        try {
+            revRotary.periodic();
+            revRotary.periodic();
+            revRotary.periodic();
+        } catch (Exception e) {
+            fail("Failed periodic execution test: " + e.getMessage());
+        }
+    }
+
+    @Test
+    void testRapidSetpointChanges()
+    {
+        // Test that rapid setpoint changes are handled correctly
+        TestUtil.runTest(revRotary.setSetpoint(RevRotary.Setpoint.RAISED), 1, revRotary);
+        TestUtil.runTest(revRotary.setSetpoint(RevRotary.Setpoint.STOW), 1, revRotary);
+        TestUtil.runTest(revRotary.setSetpoint(RevRotary.Setpoint.RAISED), 3, revRotary);
+
+        try {
+            // Should end up at the final setpoint
+            assertTrue(revRotary.nearGoal(RevRotary.Setpoint.RAISED.getSetpoint()));
+        } catch (Exception e) {
+            fail("Failed rapid setpoint changes test: " + e.getMessage());
+        }
+    }
+
+    @Override
+    public void close()
+    {
+        if (revRotary != null) {
+            revRotary.close();
+            revRotary = null;
+        }
+    }
+}
diff --git a/src/test/java/frc/robot/subsystems/rotary/RotaryTest.java b/src/test/java/frc/robot/subsystems/rotary/RotaryTest.java
index 42a7681b..49081fab 100644
--- a/src/test/java/frc/robot/subsystems/rotary/RotaryTest.java
+++ b/src/test/java/frc/robot/subsystems/rotary/RotaryTest.java
@@ -16,6 +16,8 @@
 package frc.robot.subsystems.rotary;
 
 import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
 import static org.junit.jupiter.api.Assertions.fail;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
@@ -24,6 +26,7 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import frc.robot.TestUtil;
+import edu.wpi.first.wpilibj2.command.Command;
 
 public class RotaryTest implements AutoCloseable {
     Rotary rotary;
@@ -62,7 +65,7 @@ void goToGoal()
         }
     }
 
-    @Test // marks this method as a test
+    @Test
     void goToGoalWithWait()
     {
         TestUtil.runTest(rotary.setGoalCommandWithWait(Rotary.Setpoint.STOW), 3, rotary);
@@ -74,9 +77,98 @@ void goToGoalWithWait()
         }
     }
 
+    @Test
+    void testSetpointEnum()
+    {
+        // Test that all setpoints can be retrieved and have valid values
+        try {
+            Rotary.Setpoint stow = Rotary.Setpoint.STOW;
+            Rotary.Setpoint raised = Rotary.Setpoint.RAISED;
+            Rotary.Setpoint home = Rotary.Setpoint.HOME;
+
+            assertNotNull(stow.getSetpoint());
+            assertNotNull(raised.getSetpoint());
+            assertNotNull(home.getSetpoint());
+        } catch (Exception e) {
+            fail("Failed to retrieve setpoint values: " + e.getMessage());
+        }
+    }
+
+    @Test
+    void testNearGoalTolerance()
+    {
+        // Move to STOW position
+        TestUtil.runTest(rotary.setSetpoint(Rotary.Setpoint.STOW), 3, rotary);
+
+        try {
+            // Test that we're near the goal
+            assertTrue(rotary.nearGoal(Rotary.Setpoint.STOW.getSetpoint()));
+
+            // Test that we're not near a different goal
+            assertFalse(rotary.nearGoal(Rotary.Setpoint.RAISED.getSetpoint()));
+        } catch (Exception e) {
+            fail("Failed near goal tolerance test: " + e.getMessage());
+        }
+    }
+
+
+
+    @Test
+    void testSetStateCommand()
+    {
+        // Test that setState command executes without errors
+        TestUtil.runTest(rotary.setStateCommand(Rotary.Setpoint.RAISED), 1, rotary);
+        // State setting is internal, so we just verify no exceptions thrown
+    }
+
+    @Test
+    void testGetVelocity()
+    {
+        // Start a movement
+        TestUtil.runTest(rotary.setSetpoint(Rotary.Setpoint.RAISED), 0.5, rotary);
+
+        try {
+            // Velocity should be non-zero during movement
+            assertNotNull(rotary.getVelocity());
+        } catch (Exception e) {
+            fail("Failed get velocity test: " + e.getMessage());
+        }
+    }
+
+    @Test
+    void testMultipleSetpoints()
+    {
+        // Test moving between multiple setpoints
+        TestUtil.runTest(rotary.setSetpoint(Rotary.Setpoint.STOW), 3, rotary);
+        assertTrue(rotary.nearGoal(Rotary.Setpoint.STOW.getSetpoint()));
+
+        TestUtil.runTest(rotary.setSetpoint(Rotary.Setpoint.RAISED), 3, rotary);
+        assertTrue(rotary.nearGoal(Rotary.Setpoint.RAISED.getSetpoint()));
+
+        TestUtil.runTest(rotary.setSetpoint(Rotary.Setpoint.STOW), 3, rotary);
+        assertTrue(rotary.nearGoal(Rotary.Setpoint.STOW.getSetpoint()));
+    }
+
+    @Test
+    void testCommandNaming()
+    {
+        // Test that commands have descriptive names
+        Command stowCmd = rotary.setSetpoint(Rotary.Setpoint.STOW);
+        assertTrue(stowCmd.getName().contains("STOW"));
+
+        Command raisedCmd = rotary.setSetpoint(Rotary.Setpoint.RAISED);
+        assertTrue(raisedCmd.getName().contains("RAISED"));
+
+        Command waitCmd = rotary.setGoalCommandWithWait(Rotary.Setpoint.STOW);
+        assertTrue(waitCmd.getName().contains("wait"));
+    }
+
     @Override
     public void close()
     {
-        rotary.close();
+        if (rotary != null) {
+            rotary.close();
+            rotary = null;
+        }
     }
 }
diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
new file mode 100644
index 00000000..ac62be88
--- /dev/null
+++ b/vendordeps/REVLib.json
@@ -0,0 +1,71 @@
+{
+    "fileName": "REVLib.json",
+    "name": "REVLib",
+    "version": "2025.0.3",
+    "frcYear": "2025",
+    "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+    "mavenUrls": [
+        "https://maven.revrobotics.com/"
+    ],
+    "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-java",
+            "version": "2025.0.3"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2025.0.3",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-cpp",
+            "version": "2025.0.3",
+            "libName": "REVLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "com.revrobotics.frc",
+            "artifactId": "REVLib-driver",
+            "version": "2025.0.3",
+            "libName": "REVLibDriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ]
+}
\ No newline at end of file
diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2025.json
index a7112800..a49af315 100644
--- a/vendordeps/libgrapplefrc2025.json
+++ b/vendordeps/libgrapplefrc2025.json
@@ -1,74 +1,74 @@
 {
-  "fileName": "libgrapplefrc2025.json",
-  "name": "libgrapplefrc",
-  "version": "2025.0.8",
-  "frcYear": "2025",
-  "uuid": "8ef3423d-9532-4665-8339-206dae1d7168",
-  "mavenUrls": [
-    "https://storage.googleapis.com/grapple-frc-maven"
-  ],
-  "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json",
-  "javaDependencies": [
-    {
-      "groupId": "au.grapplerobotics",
-      "artifactId": "libgrapplefrcjava",
-      "version": "2025.0.8"
-    }
-  ],
-  "jniDependencies": [
-    {
-      "groupId": "au.grapplerobotics",
-      "artifactId": "libgrapplefrcdriver",
-      "version": "2025.0.8",
-      "skipInvalidPlatforms": true,
-      "isJar": false,
-      "validPlatforms": [
-        "windowsx86-64",
-        "windowsx86",
-        "linuxarm64",
-        "linuxx86-64",
-        "linuxathena",
-        "linuxarm32",
-        "osxuniversal"
-      ]
-    }
-  ],
-  "cppDependencies": [
-    {
-      "groupId": "au.grapplerobotics",
-      "artifactId": "libgrapplefrccpp",
-      "version": "2025.0.8",
-      "libName": "grapplefrc",
-      "headerClassifier": "headers",
-      "sharedLibrary": true,
-      "skipInvalidPlatforms": true,
-      "binaryPlatforms": [
-        "windowsx86-64",
-        "windowsx86",
-        "linuxarm64",
-        "linuxx86-64",
-        "linuxathena",
-        "linuxarm32",
-        "osxuniversal"
-      ]
-    },
-    {
-      "groupId": "au.grapplerobotics",
-      "artifactId": "libgrapplefrcdriver",
-      "version": "2025.0.8",
-      "libName": "grapplefrcdriver",
-      "headerClassifier": "headers",
-      "sharedLibrary": true,
-      "skipInvalidPlatforms": true,
-      "binaryPlatforms": [
-        "windowsx86-64",
-        "windowsx86",
-        "linuxarm64",
-        "linuxx86-64",
-        "linuxathena",
-        "linuxarm32",
-        "osxuniversal"
-      ]
-    }
-  ]
-}
+    "fileName": "libgrapplefrc2025.json",
+    "name": "libgrapplefrc",
+    "version": "2025.1.3",
+    "frcYear": "2025",
+    "uuid": "8ef3423d-9532-4665-8339-206dae1d7168",
+    "mavenUrls": [
+        "https://storage.googleapis.com/grapple-frc-maven"
+    ],
+    "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json",
+    "javaDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcjava",
+            "version": "2025.1.3"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcdriver",
+            "version": "2025.1.3",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrccpp",
+            "version": "2025.1.3",
+            "libName": "grapplefrc",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcdriver",
+            "version": "2025.1.3",
+            "libName": "grapplefrcdriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ]
+}
\ No newline at end of file
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index f7a03fa9..4e7068c7 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,71 +1,71 @@
 {
-  "fileName": "photonlib.json",
-  "name": "photonlib",
-  "version": "v2025.3.1",
-  "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
-  "frcYear": "2025",
-  "mavenUrls": [
-    "https://maven.photonvision.org/repository/internal",
-    "https://maven.photonvision.org/repository/snapshots"
-  ],
-  "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
-  "jniDependencies": [
-    {
-      "groupId": "org.photonvision",
-      "artifactId": "photontargeting-cpp",
-      "version": "v2025.3.1",
-      "skipInvalidPlatforms": true,
-      "isJar": false,
-      "validPlatforms": [
-        "windowsx86-64",
-        "linuxathena",
-        "linuxx86-64",
-        "osxuniversal"
-      ]
-    }
-  ],
-  "cppDependencies": [
-    {
-      "groupId": "org.photonvision",
-      "artifactId": "photonlib-cpp",
-      "version": "v2025.3.1",
-      "libName": "photonlib",
-      "headerClassifier": "headers",
-      "sharedLibrary": true,
-      "skipInvalidPlatforms": true,
-      "binaryPlatforms": [
-        "windowsx86-64",
-        "linuxathena",
-        "linuxx86-64",
-        "osxuniversal"
-      ]
-    },
-    {
-      "groupId": "org.photonvision",
-      "artifactId": "photontargeting-cpp",
-      "version": "v2025.3.1",
-      "libName": "photontargeting",
-      "headerClassifier": "headers",
-      "sharedLibrary": true,
-      "skipInvalidPlatforms": true,
-      "binaryPlatforms": [
-        "windowsx86-64",
-        "linuxathena",
-        "linuxx86-64",
-        "osxuniversal"
-      ]
-    }
-  ],
-  "javaDependencies": [
-    {
-      "groupId": "org.photonvision",
-      "artifactId": "photonlib-java",
-      "version": "v2025.3.1"
-    },
-    {
-      "groupId": "org.photonvision",
-      "artifactId": "photontargeting-java",
-      "version": "v2025.3.1"
-    }
-  ]
-}
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2025.3.2",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2025",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+    "jniDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2025.3.2",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-cpp",
+            "version": "v2025.3.2",
+            "libName": "photonlib",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2025.3.2",
+            "libName": "photontargeting",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-java",
+            "version": "v2025.3.2"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-java",
+            "version": "v2025.3.2"
+        }
+    ]
+}
\ No newline at end of file