From 45608555e935e54f60a4a3b1f4a9704b5db51272 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 21 Oct 2025 23:08:44 -0400
Subject: [PATCH 01/29] first pass at rev IO

---
 .../java/frc/lib/io/motor/MotorIORev.java     | 238 ++++++++++++++++++
 .../java/frc/lib/io/motor/MotorIORevSim.java  | 151 +++++++++++
 src/main/java/frc/robot/RobotContainer.java   |   3 +-
 .../rotary/RotarySubsystemConstants.java      |  29 ++-
 vendordeps/REVLib.json                        |  71 ++++++
 vendordeps/libgrapplefrc2025.json             | 146 +++++------
 vendordeps/photonlib.json                     | 140 +++++------
 7 files changed, 632 insertions(+), 146 deletions(-)
 create mode 100644 src/main/java/frc/lib/io/motor/MotorIORev.java
 create mode 100644 src/main/java/frc/lib/io/motor/MotorIORevSim.java
 create mode 100644 vendordeps/REVLib.json

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..5795e49a
--- /dev/null
+++ b/src/main/java/frc/lib/io/motor/MotorIORev.java
@@ -0,0 +1,238 @@
+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.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+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.AngularAccelerationUnit;
+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.Current;
+import edu.wpi.first.units.measure.Velocity;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+public class MotorIORev implements MotorIO {
+
+    public SparkFlex 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 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 gearRatio The gear ratio applied to the motor's output.
+     * @param wheelRadius The radius of the wheel attached to the motor, used for motion
+     *        calculations.
+     * @param isBrake Boolean indicating whether the motor should use brake mode (true) or coast
+     *        mode (false).
+     * @param inverted Boolean indicating whether the motor should be inverted relative to the
+     *        control signal.
+     * @param motorType The type of motor (e.g., brushless, brushed) to configure.
+     * @param motionProfile The motion profile settings for the motor, including PID and velocity
+     *        parameters.
+     *
+     * @see MotorIO
+     */
+    public MotorIORev(
+        String name,
+        int id,
+        boolean isFlex,
+        SparkBaseConfig config)
+    {
+        this.name = name;
+
+        if (isFlex) {
+            motor = new SparkFlex(id, MotorType.kBrushless);
+        } else {
+            // motor = new SparkMax(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)
+    {
+        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 = null;
+        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, AngularVelocity cruiseVelocity,
+        AngularAcceleration acceleration,
+        Velocity<AngularAccelerationUnit> maxJerk, PIDSlot slot)
+    {
+        controller.setReference(position.in(Rotation), getRevControlType(ControlType.POSITION),
+            getClosedLoopSlot(slot));
+    }
+
+
+    @Override
+    public void runVelocity(AngularVelocity velocity, AngularAcceleration acceleration,
+        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();
+    }
+
+    protected void close()
+    {
+        this.motor.close();
+    }
+
+    protected SparkFlex 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..ae797ed0
--- /dev/null
+++ b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
@@ -0,0 +1,151 @@
+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.sim.SparkFlexSim;
+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.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import edu.wpi.first.math.system.plant.DCMotor;
+import com.revrobotics.spark.SparkSim;
+
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import edu.wpi.first.units.AngularAccelerationUnit;
+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.Velocity;
+
+
+/**
+ * Simulates a REV motor controller (SparkFlex or SparkMax) for FRC robot code.
+ * <p>
+ * Extends {@link MotorIORev} and implements {@link MotorIOSim} to enable simulation of leader and
+ * follower motors, closed-loop control, and state updates for testing without hardware.
+ * </p>
+ *
+ * <p>
+ * Features:
+ * <ul>
+ * <li>Supports SparkFlex and SparkMax controllers</li>
+ * <li>Configures followers with inversion</li>
+ * <li>Simulates position and velocity control</li>
+ * <li>Updates simulated inputs</li>
+ * </ul>
+ * </p>
+ *
+ * @see MotorIORev
+ * @see MotorIOSim
+ */
+public class MotorIORevSim extends MotorIORev implements MotorIOSim {
+
+    public record RevFollowerFollower(int id, boolean opposesLeader) {
+    }
+
+    public SparkFlex 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,
+        int id,
+        boolean isFlex,
+        DCMotor gearBox,
+        SparkBaseConfig config,
+        RevFollowerFollower... followerData)
+    {
+        super(name, id, isFlex, config);
+
+        motor = this.getMotor();
+
+        if (isFlex) {
+            simState = new SparkFlexSim(motor, gearBox);
+        } else {
+            // motor = new SparkMax(id, MotorType.kBrushless);
+        }
+
+
+        motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+        simState.enable();
+    }
+
+    @Override
+    public void runPosition(Angle position, AngularVelocity cruiseVelocity,
+        AngularAcceleration acceleration,
+        Velocity<AngularAccelerationUnit> maxJerk, PIDSlot slot)
+    {
+        simState.setPosition(position.in(Rotations));
+
+    }
+
+
+    @Override
+    public void runVelocity(AngularVelocity velocity, AngularAcceleration acceleration,
+        PIDSlot slot)
+    {
+        simState.setVelocity(velocity.in(RotationsPerSecond) * 60);
+
+    }
+
+    @Override
+    public double getRotorToSensorRatio()
+    {
+        return 1;
+    }
+
+    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.motor.close();
+    }
+
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index fa3474ab..8470d4cf 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -183,7 +183,8 @@ public RobotContainer()
                         VisionConstants.aprilTagLayout,
                         PoseStrategy.CONSTRAINED_SOLVEPNP,
                         visionSim.get()));
-                rotary = new RotarySubsystem(RotarySubsystemConstants.getSim());
+                // rotary = new RotarySubsystem(RotarySubsystemConstants.getSim());
+                rotary = new RotarySubsystem(RotarySubsystemConstants.getRevSim());
             }
 
             default -> {
diff --git a/src/main/java/frc/robot/subsystems/rotary/RotarySubsystemConstants.java b/src/main/java/frc/robot/subsystems/rotary/RotarySubsystemConstants.java
index f6b1a628..9fd7cec8 100644
--- a/src/main/java/frc/robot/subsystems/rotary/RotarySubsystemConstants.java
+++ b/src/main/java/frc/robot/subsystems/rotary/RotarySubsystemConstants.java
@@ -28,12 +28,16 @@
 import edu.wpi.first.units.measure.Velocity;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import frc.lib.io.absoluteencoder.AbsoluteEncoderIOCANCoderSim;
+import frc.lib.io.motor.MotorIORevSim;
 import frc.lib.io.motor.MotorIOTalonFX;
 import frc.lib.io.motor.MotorIOTalonFX.TalonFXFollower;
 import frc.lib.io.motor.MotorIOTalonFXSim;
 import frc.lib.mechanisms.rotary.*;
 import frc.lib.mechanisms.rotary.RotaryMechanism.RotaryMechCharacteristics;
 import frc.robot.Ports;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
 
 /** Add your docs here. */
 public class RotarySubsystemConstants {
@@ -61,7 +65,8 @@ public class RotarySubsystemConstants {
         new RotaryMechCharacteristics(OFFSET, ARM_LENGTH, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE);
 
     public static final Mass ARM_MASS = Kilograms.of(.01);
-    public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1);
+    // public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1);
+    public static final DCMotor DCMOTOR = DCMotor.getNeoVortex(1);
     public static final MomentOfInertia MOI = KilogramSquareMeters
         .of(SingleJointedArmSim.estimateMOI(ARM_LENGTH.in(Meters), ARM_MASS.in(Kilograms)));
 
@@ -129,10 +134,30 @@ public static RotaryMechanismReal getReal()
                 NAME + "Encoder", getCANcoderConfig(false))));
     }
 
+    public static RotaryMechanismSim getRevSim()
+    {
+        return new RotaryMechanismSim(
+            new MotorIORevSim(
+                NAME,
+                Ports.RotarySubsystemMotorMain.id(),
+                true,
+                DCMOTOR, // Missing gearBox parameter
+                new SparkFlexConfig()
+                    .voltageCompensation(12.0)
+                    .idleMode(IdleMode.kBrake)
+                    .inverted(false)),
+            DCMOTOR, MOI, false, CONSTANTS,
+            Optional.of(new AbsoluteEncoderIOCANCoderSim(Ports.RotarySubsystemEncoder,
+                NAME + "Encoder", getCANcoderConfig(true))));
+    }
+
     public static RotaryMechanismSim getSim()
     {
         return new RotaryMechanismSim(
-            new MotorIOTalonFXSim(NAME, getFXConfig(), Ports.RotarySubsystemMotorMain,
+            new MotorIOTalonFXSim(
+                NAME,
+                getFXConfig(),
+                Ports.RotarySubsystemMotorMain,
                 new TalonFXFollower(Ports.RotarySubsystemMotorFollower, false)),
             DCMOTOR, MOI, false, CONSTANTS,
             Optional.of(new AbsoluteEncoderIOCANCoderSim(Ports.RotarySubsystemEncoder,
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

From 35fc34f63e1d1f0f96c96bde09a3d51c3e4fe79f Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Thu, 23 Oct 2025 22:03:31 -0400
Subject: [PATCH 02/29] add seperate rev subsytem and update docs

---
 .../java/frc/lib/io/motor/MotorIORev.java     |   5 +-
 .../java/frc/lib/io/motor/MotorIORevSim.java  |  28 +--
 .../java/frc/lib/io/motor/MotorIOSim.java     |  61 +++++--
 .../java/frc/lib/io/motor/MotorIOTalonFX.java | 164 ++++++++++++-----
 src/main/java/frc/robot/RobotContainer.java   |  13 +-
 .../revRotary/RevRotarySubsystem.java         |  91 ++++++++++
 .../RevRotarySubsystemConstants.java          | 171 ++++++++++++++++++
 .../subsystems/rotary/RotarySubsystem.java    |  82 ++++++++-
 .../rotary/RotarySubsystemConstants.java      |  76 ++++++--
 9 files changed, 601 insertions(+), 90 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystem.java
 create mode 100644 src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystemConstants.java

diff --git a/src/main/java/frc/lib/io/motor/MotorIORev.java b/src/main/java/frc/lib/io/motor/MotorIORev.java
index 5795e49a..83ba3caf 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORev.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORev.java
@@ -26,6 +26,7 @@
 import edu.wpi.first.units.measure.Velocity;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import frc.lib.util.Device;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 public class MotorIORev implements MotorIO {
@@ -118,14 +119,14 @@ protected com.revrobotics.spark.SparkBase.ControlType getRevControlType(ControlT
      */
     public MotorIORev(
         String name,
-        int id,
+        Device.CAN id,
         boolean isFlex,
         SparkBaseConfig config)
     {
         this.name = name;
 
         if (isFlex) {
-            motor = new SparkFlex(id, MotorType.kBrushless);
+            motor = new SparkFlex(id.id(), MotorType.kBrushless);
         } else {
             // motor = new SparkMax(id, MotorType.kBrushless);
         }
diff --git a/src/main/java/frc/lib/io/motor/MotorIORevSim.java b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
index ae797ed0..16761689 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORevSim.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
@@ -26,24 +26,28 @@
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Velocity;
+import frc.lib.util.Device;
 
 
 /**
- * Simulates a REV motor controller (SparkFlex or SparkMax) for FRC robot code.
- * <p>
- * Extends {@link MotorIORev} and implements {@link MotorIOSim} to enable simulation of leader and
- * follower motors, closed-loop control, and state updates for testing without hardware.
- * </p>
+ * Simulated implementation of {@link MotorIORev} for REV Robotics motors using WPILib simulation.
+ * Implements {@link MotorIOSim} to provide simulation-specific behavior.
  *
  * <p>
- * Features:
+ * Constructor arguments:
  * <ul>
- * <li>Supports SparkFlex and SparkMax controllers</li>
- * <li>Configures followers with inversion</li>
- * <li>Simulates position and velocity control</li>
- * <li>Updates simulated inputs</li>
+ * <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>
+ *
+ * <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
@@ -71,7 +75,7 @@ public record RevFollowerFollower(int id, boolean opposesLeader) {
      */
     public MotorIORevSim(
         String name,
-        int id,
+        Device.CAN id,
         boolean isFlex,
         DCMotor gearBox,
         SparkBaseConfig config,
diff --git a/src/main/java/frc/lib/io/motor/MotorIOSim.java b/src/main/java/frc/lib/io/motor/MotorIOSim.java
index 53d82ca6..d56053a2 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,26 @@ 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.
+     */
     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/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 8470d4cf..916a4603 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -67,6 +67,8 @@
 import frc.robot.subsystems.leds.LEDsConstants;
 import frc.robot.subsystems.linear.Linear;
 import frc.robot.subsystems.linear.LinearConstants;
+import frc.robot.subsystems.revRotary.RevRotarySubsystem;
+import frc.robot.subsystems.revRotary.RevRotarySubsystemConstants;
 import frc.robot.subsystems.rotary.RotarySubsystem;
 import frc.robot.subsystems.rotary.RotarySubsystemConstants;
 import frc.robot.subsystems.rotary.RotarySubsystem.Setpoint;
@@ -107,6 +109,7 @@ public class RobotContainer {
     private final Linear linear;
     private final Vision vision;
     private final RotarySubsystem rotary;
+    private final RevRotarySubsystem revRotary;
 
     // Controller
     private final CommandXboxControllerExtended controller = new CommandXboxControllerExtended(0);
@@ -151,6 +154,7 @@ public RobotContainer()
                         VisionConstants.aprilTagLayout,
                         PoseStrategy.CONSTRAINED_SOLVEPNP));
                 rotary = new RotarySubsystem(RotarySubsystemConstants.getReal());
+                revRotary = new RevRotarySubsystem(RevRotarySubsystemConstants.getReal());
             }
 
             case SIM -> {
@@ -183,8 +187,8 @@ public RobotContainer()
                         VisionConstants.aprilTagLayout,
                         PoseStrategy.CONSTRAINED_SOLVEPNP,
                         visionSim.get()));
-                // rotary = new RotarySubsystem(RotarySubsystemConstants.getSim());
-                rotary = new RotarySubsystem(RotarySubsystemConstants.getRevSim());
+                rotary = new RotarySubsystem(RotarySubsystemConstants.getSim());
+                revRotary = new RevRotarySubsystem(RevRotarySubsystemConstants.getSim());
             }
 
             default -> {
@@ -206,6 +210,7 @@ public RobotContainer()
 
                 linear = new Linear(LinearConstants.getReplay());
                 rotary = new RotarySubsystem(RotarySubsystemConstants.getReplay());
+                revRotary = new RevRotarySubsystem(RevRotarySubsystemConstants.getReplay());
 
                 visionSim = Optional.empty();
                 vision = new Vision(
@@ -304,6 +309,10 @@ private void configureButtonBindings()
         SmartDashboard.putData("Rotary: Stow", rotary.setSetpoint(RotarySubsystem.Setpoint.STOW));
         SmartDashboard.putData("Rotary: Raised",
             rotary.setSetpoint(RotarySubsystem.Setpoint.RAISED));
+        SmartDashboard.putData("REV Rotary: Stow",
+            revRotary.setSetpoint(RevRotarySubsystem.Setpoint.STOW));
+        SmartDashboard.putData("REV Rotary: Raised",
+            revRotary.setSetpoint(RevRotarySubsystem.Setpoint.RAISED));
 
         LoggedTunableNumber ballVel = new LoggedTunableNumber("Ball Sim Velocity (fps)", 15);
         SmartDashboard.putData("Shoot Ball", Commands
diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystem.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystem.java
new file mode 100644
index 00000000..ce5d6bdb
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystem.java
@@ -0,0 +1,91 @@
+// 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 RevRotarySubsystem 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);
+
+    @RequiredArgsConstructor
+    @SuppressWarnings("Immutable")
+    @Getter
+    public enum Setpoint {
+        STOW(Degrees.of(STOW_SETPOINT.get())),
+        RAISED(Degrees.of(RAISED_SETPOINT.get()));
+
+        private final Angle setpoint;
+    }
+
+
+    public RevRotarySubsystem(RotaryMechanism io)
+    {
+        this.io = io;
+
+        setSetpoint(RevRotarySubsystemConstants.DEFAULT_SETPOINT).ignoringDisable(true).schedule();
+    }
+
+    @Override
+    public void periodic()
+    {
+        LoggerHelper.recordCurrentCommand(RevRotarySubsystemConstants.NAME, this);
+        io.periodic();
+    }
+
+    public Command setSetpoint(Setpoint setpoint)
+    {
+        return this.runOnce(
+            () -> io.runPosition(setpoint.getSetpoint(),
+                RevRotarySubsystemConstants.CRUISE_VELOCITY,
+                RevRotarySubsystemConstants.ACCELERATION, RevRotarySubsystemConstants.JERK,
+                PIDSlot.SLOT_0))
+            .withName("Go To " + setpoint.toString() + " Setpoint");
+    };
+
+    public boolean nearGoal(Angle targetPosition)
+    {
+        return io.nearGoal(targetPosition, RevRotarySubsystemConstants.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/RevRotarySubsystemConstants.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystemConstants.java
new file mode 100644
index 00000000..f290992a
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystemConstants.java
@@ -0,0 +1,171 @@
+// 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 com.ctre.phoenix6.configs.*;
+import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+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.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 RevRotarySubsystemConstants {
+    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);
+
+    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);
+
+    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 RevRotarySubsystem.Setpoint DEFAULT_SETPOINT =
+        RevRotarySubsystem.Setpoint.STOW;
+
+
+
+    // Positional PID
+    private static ClosedLoopConfig SLOT0CONFIG = new ClosedLoopConfig()
+        .pid(30.0, 0, 0, ClosedLoopSlot.kSlot0);
+
+    /**
+     * 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 SparkBaseConfig getREVConfig()
+    {
+        SparkFlexConfig config = new SparkFlexConfig();
+
+        config.voltageCompensation(12.0);
+        config.idleMode(IdleMode.kBrake);
+        config.inverted(false);
+        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 RotaryMechanismReal getReal()
+    {
+        MotorIO io = new MotorIORev(NAME, Ports.RotarySubsystemMotorMain, true, getREVConfig());
+
+        return new RotaryMechanismReal(io, CONSTANTS, null);
+    }
+
+    /**
+     * 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 RotaryMechanismSim getSim()
+    {
+        MotorIOSim io = new MotorIORevSim(
+            NAME,
+            Ports.RotarySubsystemMotorMain,
+            true,
+            DCMOTOR,
+            getREVConfig());
+
+        return 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 RotaryMechanism getReplay()
+    {
+        return new RotaryMechanism(NAME, CONSTANTS) {};
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/rotary/RotarySubsystem.java b/src/main/java/frc/robot/subsystems/rotary/RotarySubsystem.java
index fc9648f6..cd0a7716 100644
--- a/src/main/java/frc/robot/subsystems/rotary/RotarySubsystem.java
+++ b/src/main/java/frc/robot/subsystems/rotary/RotarySubsystem.java
@@ -25,17 +25,35 @@ public class RotarySubsystem extends SubsystemBase {
     private static final LoggedTunableNumber RAISED_SETPOINT =
         new LoggedTunableNumber("RAISED", 90);
 
+    /**
+     * Predefined positions for the rotary mechanism.
+     * 
+     * <p>
+     * These setpoints define common positions that the mechanism moves to during operation. The
+     * actual angle values can be tuned live during testing using LoggedTunableNumber.
+     */
     @RequiredArgsConstructor
     @SuppressWarnings("Immutable")
     @Getter
     public enum Setpoint {
+        /**
+         * The stowed (stored) position - where the mechanism rests safely inside the robot frame
+         */
         STOW(Degrees.of(STOW_SETPOINT.get())),
         RAISED(Degrees.of(RAISED_SETPOINT.get()));
 
         private final Angle setpoint;
     }
 
-
+    /**
+     * Constructs a new RotarySubsystem.
+     * 
+     * <p>
+     * This constructor initializes the subsystem and immediately schedules a command to move to the
+     * default setpoint.
+     * 
+     * @param io The RotaryMechanism IO layer (real hardware, simulation, or replay)
+     */
     public RotarySubsystem(RotaryMechanism io)
     {
         this.io = io;
@@ -43,6 +61,13 @@ public RotarySubsystem(RotaryMechanism io)
         setSetpoint(RotarySubsystemConstants.DEFAULT_SETPOINT).ignoringDisable(true).schedule();
     }
 
+    /**
+     * Called every robot loop iteration (every 20ms by default).
+     * 
+     * <p>
+     * This method logs which command is currently using this subsystem and updates all mechanism
+     * telemetry for AdvantageKit logging.
+     */
     @Override
     public void periodic()
     {
@@ -50,6 +75,17 @@ public void periodic()
         io.periodic();
     }
 
+    /**
+     * Creates a command to move the mechanism to a predefined setpoint.
+     * 
+     * <p>
+     * This command uses Motion Magic control, which creates smooth motion profiles with controlled
+     * velocity and acceleration. The command completes instantly - it only starts the motion and
+     * does not wait for the mechanism to reach the target.
+     * 
+     * @param setpoint The predefined position to move to (STOW or RAISED)
+     * @return A Command that starts motion to the setpoint
+     */
     public Command setSetpoint(Setpoint setpoint)
     {
         return this.runOnce(
@@ -59,11 +95,32 @@ public Command setSetpoint(Setpoint setpoint)
             .withName("Go To " + setpoint.toString() + " Setpoint");
     };
 
+    /**
+     * Checks if the mechanism is near the goal position within the defined tolerance.
+     * 
+     * <p>
+     * This is useful for determining when the mechanism has reached its target position. The
+     * tolerance is defined in RotarySubsystemConstants.TOLERANCE.
+     * 
+     * @param targetPosition The position to check against
+     * @return true if the current position is within tolerance of the target
+     */
     public boolean nearGoal(Angle targetPosition)
     {
         return io.nearGoal(targetPosition, RotarySubsystemConstants.TOLERANCE);
     }
 
+    /**
+     * Creates a command that waits until the mechanism reaches the specified position.
+     * 
+     * <p>
+     * This command finishes when nearGoal() returns true. It's useful for creating command
+     * sequences where you need to wait for the mechanism to finish moving before continuing to the
+     * next step.
+     * 
+     * @param position The position to wait for
+     * @return A Command that finishes when the position is reached
+     */
     public Command waitUntilGoalCommand(Angle position)
     {
         return Commands.waitUntil(() -> {
@@ -71,6 +128,17 @@ public Command waitUntilGoalCommand(Angle position)
         });
     }
 
+    /**
+     * Creates a command that moves to a setpoint and waits until it's reached.
+     * 
+     * <p>
+     * This combines setSetpoint() and waitUntilGoalCommand() into a single command. The deadline
+     * composition ensures that if the wait timeout occurs, both the wait and the motion command are
+     * cancelled together.
+     * 
+     * @param setpoint The predefined position to move to and wait for
+     * @return A Command that moves to the setpoint and waits until it's reached
+     */
     public Command setGoalCommandWithWait(Setpoint setpoint)
     {
         return waitUntilGoalCommand(setpoint.getSetpoint())
@@ -78,11 +146,23 @@ public Command setGoalCommandWithWait(Setpoint setpoint)
             .withName("Go To " + setpoint.toString() + " Setpoint with wait");
     }
 
+    /**
+     * Gets the current velocity of the mechanism.
+     * 
+     * @return The current angular velocity
+     */
     public AngularVelocity getVelocity()
     {
         return io.getVelocity();
     }
 
+    /**
+     * Closes and cleans up resources used by this subsystem.
+     * 
+     * <p>
+     * This should be called when the subsystem is no longer needed, typically during robot
+     * shutdown. It ensures proper cleanup of hardware resources.
+     */
     public void close()
     {
         io.close();
diff --git a/src/main/java/frc/robot/subsystems/rotary/RotarySubsystemConstants.java b/src/main/java/frc/robot/subsystems/rotary/RotarySubsystemConstants.java
index 9fd7cec8..aa085a3f 100644
--- a/src/main/java/frc/robot/subsystems/rotary/RotarySubsystemConstants.java
+++ b/src/main/java/frc/robot/subsystems/rotary/RotarySubsystemConstants.java
@@ -65,8 +65,7 @@ public class RotarySubsystemConstants {
         new RotaryMechCharacteristics(OFFSET, ARM_LENGTH, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE);
 
     public static final Mass ARM_MASS = Kilograms.of(.01);
-    // public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1);
-    public static final DCMotor DCMOTOR = DCMotor.getNeoVortex(1);
+    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)));
 
@@ -80,6 +79,23 @@ public class RotarySubsystemConstants {
         .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();
@@ -115,6 +131,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();
@@ -124,6 +151,15 @@ public static CANcoderConfiguration getCANcoderConfig(boolean sim)
         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 RotaryMechanismReal getReal()
     {
         return new RotaryMechanismReal(
@@ -134,23 +170,16 @@ public static RotaryMechanismReal getReal()
                 NAME + "Encoder", getCANcoderConfig(false))));
     }
 
-    public static RotaryMechanismSim getRevSim()
-    {
-        return new RotaryMechanismSim(
-            new MotorIORevSim(
-                NAME,
-                Ports.RotarySubsystemMotorMain.id(),
-                true,
-                DCMOTOR, // Missing gearBox parameter
-                new SparkFlexConfig()
-                    .voltageCompensation(12.0)
-                    .idleMode(IdleMode.kBrake)
-                    .inverted(false)),
-            DCMOTOR, MOI, false, CONSTANTS,
-            Optional.of(new AbsoluteEncoderIOCANCoderSim(Ports.RotarySubsystemEncoder,
-                NAME + "Encoder", getCANcoderConfig(true))));
-    }
-
+    /**
+     * 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 RotaryMechanismSim getSim()
     {
         return new RotaryMechanismSim(
@@ -164,6 +193,15 @@ public static RotaryMechanismSim getSim()
                 NAME + "Encoder", getCANcoderConfig(true))));
     }
 
+    /**
+     * 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 RotaryMechanism getReplay()
     {
         return new RotaryMechanism(NAME, CONSTANTS) {};

From 6b067e88f182ae0584cd1f448e351012592f9379 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Mon, 24 Nov 2025 15:05:20 -0500
Subject: [PATCH 03/29] refactor MotorIORev: clean up imports and improve code
 formatting

---
 src/main/java/frc/lib/io/motor/MotorIORev.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/lib/io/motor/MotorIORev.java b/src/main/java/frc/lib/io/motor/MotorIORev.java
index 83ba3caf..ba8170ab 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORev.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORev.java
@@ -25,7 +25,6 @@
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Velocity;
 import edu.wpi.first.units.measure.Voltage;
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import frc.lib.util.Device;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
@@ -200,7 +199,8 @@ public void runPosition(Angle position, AngularVelocity cruiseVelocity,
         AngularAcceleration acceleration,
         Velocity<AngularAccelerationUnit> maxJerk, PIDSlot slot)
     {
-        controller.setReference(position.in(Rotation), getRevControlType(ControlType.POSITION),
+        controller.setReference(position.in(Rotation),
+            getRevControlType(ControlType.POSITION),
             getClosedLoopSlot(slot));
     }
 

From e152663f53520eca870f87ffbe1257e66d28bf38 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Mon, 24 Nov 2025 15:17:55 -0500
Subject: [PATCH 04/29] merge dev

---
 src/main/java/frc/lib/devices/ObjectDetection.java          | 6 +++---
 .../java/frc/lib/io/objectDetection/ObjectDetectionIO.java  | 2 +-
 .../io/objectDetection/ObjectDetectionIOPhotonVision.java   | 2 +-
 .../frc/lib/io/objectDetection/ObjectDetectionIOSim.java    | 2 +-
 .../frc/robot/subsystems/objectDetector/ObjectDetector.java | 2 +-
 .../subsystems/objectDetector/ObjectDetectorConstants.java  | 4 +++-
 src/main/java/frc/robot/subsystems/rotary/Rotary.java       | 2 +-
 7 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/lib/devices/ObjectDetection.java b/src/main/java/frc/lib/devices/ObjectDetection.java
index 7316d199..b7dd71e8 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.ObjectDetectionIO;
+import frc.lib.io.objectDetection.ObjectDetectionIO.TargetObservation;
+import frc.lib.io.objectDetection.ObjectDetectionIOInputsAutoLogged;
 
 /**
  * Device level implementation of an object detection camera. While the IO layer is responsible for
diff --git a/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIO.java b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIO.java
index 303685b8..a6c4cb6c 100644
--- a/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIO.java
+++ b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIO.java
@@ -13,7 +13,7 @@
  * not, see <https://www.gnu.org/licenses/>.
  */
 
-package frc.lib.io.objectdetection;
+package frc.lib.io.objectDetection;
 
 import org.littletonrobotics.junction.AutoLog;
 
diff --git a/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOPhotonVision.java b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOPhotonVision.java
index 924cf91f..87faa2e6 100644
--- a/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOPhotonVision.java
+++ b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOPhotonVision.java
@@ -13,7 +13,7 @@
  * not, see <https://www.gnu.org/licenses/>.
  */
 
-package frc.lib.io.objectdetection;
+package frc.lib.io.objectDetection;
 
 import org.photonvision.PhotonCamera;
 import org.photonvision.targeting.PhotonPipelineResult;
diff --git a/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOSim.java b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOSim.java
index 743c0129..63454001 100644
--- a/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOSim.java
+++ b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOSim.java
@@ -2,7 +2,7 @@
 // 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.lib.io.objectdetection;
+package frc.lib.io.objectDetection;
 
 import java.util.ArrayList;
 import java.util.List;
diff --git a/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetector.java b/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetector.java
index b05114e2..7421c2a9 100644
--- a/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetector.java
+++ b/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetector.java
@@ -16,7 +16,7 @@
 package frc.robot.subsystems.objectDetector;
 
 import frc.lib.devices.ObjectDetection;
-import frc.lib.io.objectdetection.ObjectDetectionIO;
+import frc.lib.io.objectDetection.ObjectDetectionIO;
 import frc.robot.subsystems.drive.Drive;
 import org.littletonrobotics.junction.Logger;
 import edu.wpi.first.math.geometry.Translation2d;
diff --git a/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetectorConstants.java b/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetectorConstants.java
index ddedd791..a2c1d2ae 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/rotary/Rotary.java b/src/main/java/frc/robot/subsystems/rotary/Rotary.java
index e8d97910..5ff0f2ad 100644
--- a/src/main/java/frc/robot/subsystems/rotary/Rotary.java
+++ b/src/main/java/frc/robot/subsystems/rotary/Rotary.java
@@ -59,7 +59,7 @@ public enum Setpoint {
      * 
      * @param io The RotaryMechanism IO layer (real hardware, simulation, or replay)
      */
-    public RotarySubsystem(RotaryMechanism io)
+    public Rotary(RotaryMechanism io)
     {
         this.io = io;
         this.robotstate = RobotState.getInstance();

From cbb9d840226a783b15958192050f4dbd075cec71 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Mon, 24 Nov 2025 15:19:54 -0500
Subject: [PATCH 05/29] merge rotary

---
 .../frc/robot/subsystems/rotary/Rotary.java   | 104 ++++--------------
 1 file changed, 23 insertions(+), 81 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/rotary/Rotary.java b/src/main/java/frc/robot/subsystems/rotary/Rotary.java
index 5ff0f2ad..1600fcc3 100644
--- a/src/main/java/frc/robot/subsystems/rotary/Rotary.java
+++ b/src/main/java/frc/robot/subsystems/rotary/Rotary.java
@@ -30,35 +30,23 @@ public class Rotary extends SubsystemBase {
     private static final LoggedTunableNumber RAISED_SETPOINT =
         new LoggedTunableNumber("RAISED", -90);
 
-    /**
-     * Predefined positions for the rotary mechanism.
-     * 
-     * <p>
-     * These setpoints define common positions that the mechanism moves to during operation. The
-     * actual angle values can be tuned live during testing using LoggedTunableNumber.
-     */
     @RequiredArgsConstructor
     @SuppressWarnings("Immutable")
     @Getter
     public enum Setpoint {
-        /**
-         * The stowed (stored) position - where the mechanism rests safely inside the robot frame
-         */
+        HOME(Degrees.of(0.0)),
         STOW(Degrees.of(STOW_SETPOINT.get())),
         RAISED(Degrees.of(RAISED_SETPOINT.get()));
 
         private final Angle setpoint;
     }
 
-    /**
-     * Constructs a new RotarySubsystem.
-     * 
-     * <p>
-     * This constructor initializes the subsystem and immediately schedules a command to move to the
-     * default setpoint.
-     * 
-     * @param io The RotaryMechanism IO layer (real hardware, simulation, or replay)
-     */
+    private Debouncer homeDebouncer = new Debouncer(0.1, DebounceType.kRising);
+    private Trigger homedTrigger;
+
+    private final RobotState robotstate;
+    private Setpoint setpoint = Setpoint.STOW;
+
     public Rotary(RotaryMechanism io)
     {
         this.io = io;
@@ -71,13 +59,6 @@ public Rotary(RotaryMechanism io)
 
     }
 
-    /**
-     * Called every robot loop iteration (every 20ms by default).
-     * 
-     * <p>
-     * This method logs which command is currently using this subsystem and updates all mechanism
-     * telemetry for AdvantageKit logging.
-     */
     @Override
     public void periodic()
     {
@@ -87,17 +68,6 @@ public void periodic()
 
     }
 
-    /**
-     * Creates a command to move the mechanism to a predefined setpoint.
-     * 
-     * <p>
-     * This command uses Motion Magic control, which creates smooth motion profiles with controlled
-     * velocity and acceleration. The command completes instantly - it only starts the motion and
-     * does not wait for the mechanism to reach the target.
-     * 
-     * @param setpoint The predefined position to move to (STOW or RAISED)
-     * @return A Command that starts motion to the setpoint
-     */
     public Command setSetpoint(Setpoint setpoint)
     {
         return this.runOnce(
@@ -107,32 +77,11 @@ public Command setSetpoint(Setpoint setpoint)
             .withName("Go To " + setpoint.toString() + " Setpoint");
     };
 
-    /**
-     * Checks if the mechanism is near the goal position within the defined tolerance.
-     * 
-     * <p>
-     * This is useful for determining when the mechanism has reached its target position. The
-     * tolerance is defined in RotarySubsystemConstants.TOLERANCE.
-     * 
-     * @param targetPosition The position to check against
-     * @return true if the current position is within tolerance of the target
-     */
     public boolean nearGoal(Angle targetPosition)
     {
         return io.nearGoal(targetPosition, RotaryConstants.TOLERANCE);
     }
 
-    /**
-     * Creates a command that waits until the mechanism reaches the specified position.
-     * 
-     * <p>
-     * This command finishes when nearGoal() returns true. It's useful for creating command
-     * sequences where you need to wait for the mechanism to finish moving before continuing to the
-     * next step.
-     * 
-     * @param position The position to wait for
-     * @return A Command that finishes when the position is reached
-     */
     public Command waitUntilGoalCommand(Angle position)
     {
         return Commands.waitUntil(() -> {
@@ -140,17 +89,6 @@ public Command waitUntilGoalCommand(Angle position)
         });
     }
 
-    /**
-     * Creates a command that moves to a setpoint and waits until it's reached.
-     * 
-     * <p>
-     * This combines setSetpoint() and waitUntilGoalCommand() into a single command. The deadline
-     * composition ensures that if the wait timeout occurs, both the wait and the motion command are
-     * cancelled together.
-     * 
-     * @param setpoint The predefined position to move to and wait for
-     * @return A Command that moves to the setpoint and waits until it's reached
-     */
     public Command setGoalCommandWithWait(Setpoint setpoint)
     {
         return waitUntilGoalCommand(setpoint.getSetpoint())
@@ -158,23 +96,27 @@ public Command setGoalCommandWithWait(Setpoint setpoint)
             .withName("Go To " + setpoint.toString() + " Setpoint with wait");
     }
 
-    /**
-     * Gets the current velocity of the mechanism.
-     * 
-     * @return The current angular velocity
-     */
+    public Command setStateCommand(Setpoint setpoint)
+    {
+        return this.runOnce(() -> this.setpoint = setpoint)
+            .withName("Elevator Set State: " + setpoint.name());
+    }
+
+    public Command homeCommand()
+    {
+        return Commands.sequence(runOnce(() -> io.runVoltage(Volts.of(-2))),
+            Commands.waitUntil(homedTrigger),
+            runOnce(() -> io.setEncoderPosition(Setpoint.HOME.getSetpoint())),
+            this.setStateCommand(Setpoint.STOW))
+            .withName("Homing");
+
+    }
+
     public AngularVelocity getVelocity()
     {
         return io.getVelocity();
     }
 
-    /**
-     * Closes and cleans up resources used by this subsystem.
-     * 
-     * <p>
-     * This should be called when the subsystem is no longer needed, typically during robot
-     * shutdown. It ensures proper cleanup of hardware resources.
-     */
     public void close()
     {
         io.close();

From 631cc5336eab27d95f255795203ff0e23a254596 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Mon, 24 Nov 2025 15:27:46 -0500
Subject: [PATCH 06/29] merge dev

---
 src/main/java/frc/robot/RobotContainer.java | 121 +++-----------------
 1 file changed, 16 insertions(+), 105 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 99ccb580..8fc784a4 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -68,11 +68,9 @@
 import frc.robot.subsystems.leds.LEDsConstants;
 import frc.robot.subsystems.linear.Linear;
 import frc.robot.subsystems.linear.LinearConstants;
-import frc.robot.subsystems.revRotary.RevRotarySubsystem;
-import frc.robot.subsystems.revRotary.RevRotarySubsystemConstants;
-import frc.robot.subsystems.rotary.RotarySubsystem;
-import frc.robot.subsystems.rotary.RotarySubsystemConstants;
-import frc.robot.subsystems.rotary.RotarySubsystem.Setpoint;
+import frc.robot.subsystems.rotary.Rotary;
+import frc.robot.subsystems.rotary.RotaryConstants;
+import frc.robot.subsystems.rotary.Rotary.Setpoint;
 import frc.robot.subsystems.servo1.Servo1;
 import frc.robot.subsystems.servo1.Servo1Constants;
 import frc.robot.subsystems.vision.Vision;
@@ -111,8 +109,8 @@ public class RobotContainer {
     private final Flywheel flywheel;
     private final Linear linear;
     private final Vision vision;
-    private final RotarySubsystem rotary;
-    private final RevRotarySubsystem revRotary;
+    private final Rotary rotary;
+    private final ObjectDetector objectDetector;
 
     // Controller
     private final CommandXboxControllerExtended controller = new CommandXboxControllerExtended(0);
@@ -129,99 +127,16 @@ public class RobotContainer {
      */
     public RobotContainer()
     {
-        switch (Constants.currentMode) {
-            case REAL -> {
-                // Real robot, instantiate hardware IO implementations
-                drive = new Drive(
-                    new GyroIOPigeon2(),
-                    new ModuleIOTalonFX(DriveConstants.FrontLeft),
-                    new ModuleIOTalonFX(DriveConstants.FrontRight),
-                    new ModuleIOTalonFX(DriveConstants.BackLeft),
-                    new ModuleIOTalonFX(DriveConstants.BackRight));
-
-                leds = new LEDs(LEDsConstants.getLightsIOReal());
-                laserCAN1 = new LaserCAN1(LaserCAN1Constants.getReal(), drive);
-                beamBreak1 = new BeamBreak1(BeamBreak1Constants.getReal());
-                servo1 = new Servo1(Servo1Constants.getReal());
-                flywheel = new Flywheel(FlywheelConstants.getReal());
-
-                linear = new Linear(LinearConstants.getReal());
-
-                visionSim = Optional.empty();
-                vision = new Vision(
-                    drive::addVisionMeasurement,
-                    () -> drive.getTimestampedHeading(),
-                    new VisionIOPhotonVision(
-                        VisionConstants.camera0Name,
-                        VisionConstants.robotToCamera0,
-                        VisionConstants.aprilTagLayout,
-                        PoseStrategy.CONSTRAINED_SOLVEPNP));
-                rotary = new RotarySubsystem(RotarySubsystemConstants.getReal());
-                revRotary = new RevRotarySubsystem(RevRotarySubsystemConstants.getReal());
-            }
-
-            case SIM -> {
-                // Sim robot, instantiate physics sim IO implementations
-                drive = new Drive(
-                    new GyroIO() {},
-                    new ModuleIOSim(DriveConstants.FrontLeft),
-                    new ModuleIOSim(DriveConstants.FrontRight),
-                    new ModuleIOSim(DriveConstants.BackLeft),
-                    new ModuleIOSim(DriveConstants.BackRight));
-
-                leds = new LEDs(LEDsConstants.getLightsIOSim());
-                laserCAN1 =
-                    new LaserCAN1(LaserCAN1Constants.getSim(), drive);
-                beamBreak1 = new BeamBreak1(
-                    BeamBreak1Constants.getSim());
-                servo1 = new Servo1(Servo1Constants.getSim());
-                flywheel = new Flywheel(FlywheelConstants.getSim());
-
-                linear = new Linear(LinearConstants.getSim());
-
-                visionSim = Optional.of(VisionConstants.getSystemSim());
-                vision = new Vision(
-                    drive::addVisionMeasurement,
-                    () -> drive.getTimestampedHeading(),
-                    new VisionIOPhotonVisionSim(
-                        () -> drive.getPose(),
-                        VisionConstants.camera0Name,
-                        VisionConstants.robotToCamera0,
-                        VisionConstants.aprilTagLayout,
-                        PoseStrategy.CONSTRAINED_SOLVEPNP,
-                        visionSim.get()));
-                rotary = new RotarySubsystem(RotarySubsystemConstants.getSim());
-                revRotary = new RevRotarySubsystem(RevRotarySubsystemConstants.getSim());
-            }
-
-            default -> {
-                // Replayed robot, disable IO implementations
-                drive = new Drive(
-                    new GyroIO() {},
-                    new ModuleIO() {},
-                    new ModuleIO() {},
-                    new ModuleIO() {},
-                    new ModuleIO() {});
-
-                leds = new LEDs(LEDsConstants.getLightsIOReplay());
-                laserCAN1 =
-                    new LaserCAN1(LaserCAN1Constants.getReplay(), drive);
-                beamBreak1 =
-                    new BeamBreak1(BeamBreak1Constants.getReplay());
-                servo1 = new Servo1(Servo1Constants.getReplay());
-                flywheel = new Flywheel(FlywheelConstants.getReplay());
-
-                linear = new Linear(LinearConstants.getReplay());
-                rotary = new RotarySubsystem(RotarySubsystemConstants.getReplay());
-                revRotary = new RevRotarySubsystem(RevRotarySubsystemConstants.getReplay());
-
-                visionSim = Optional.empty();
-                vision = new Vision(
-                    drive::addVisionMeasurement,
-                    () -> drive.getTimestampedHeading(),
-                    new VisionIO() {});
-            }
-        }
+        drive = DriveConstants.get();
+        laserCAN1 = LaserCAN1Constants.get();
+        flywheel = FlywheelConstants.get();
+        leds = LEDsConstants.get();
+        beamBreak1 = BeamBreak1Constants.get();
+        linear = LinearConstants.get();
+        rotary = RotaryConstants.get();
+        servo1 = Servo1Constants.get();
+        vision = VisionConstants.get(drive); // TODO: Will be refactored in the future to RobotState
+        objectDetector = ObjectDetectorConstants.get(drive);
 
         conditionalChooser = new LoggedDashboardChooser<>("Conditional Choice");
         conditionalChooser.addOption("True", true);
@@ -311,11 +226,7 @@ private void configureButtonBindings()
         SmartDashboard.putData("Linear: Home", linear.homeCommand());
         SmartDashboard.putData("Rotary: Stow", rotary.setSetpoint(Rotary.Setpoint.STOW));
         SmartDashboard.putData("Rotary: Raised",
-            rotary.setSetpoint(RotarySubsystem.Setpoint.RAISED));
-        SmartDashboard.putData("REV Rotary: Stow",
-            revRotary.setSetpoint(RevRotarySubsystem.Setpoint.STOW));
-        SmartDashboard.putData("REV Rotary: Raised",
-            revRotary.setSetpoint(RevRotarySubsystem.Setpoint.RAISED));
+            rotary.setSetpoint(Rotary.Setpoint.RAISED));
 
         LoggedTunableNumber ballVel = new LoggedTunableNumber("Ball Sim Velocity (fps)", 15);
         SmartDashboard.putData("Shoot Ball", Commands

From 1e90100db1f69ef28b5e7e4fa4dd5241568015c0 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Mon, 24 Nov 2025 15:27:52 -0500
Subject: [PATCH 07/29] merge dev

---
 .../RevRotarySubsystemConstants.java          |  4 +-
 .../subsystems/rotary/RotaryConstants.java    | 74 ++++++-------------
 2 files changed, 24 insertions(+), 54 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystemConstants.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystemConstants.java
index f290992a..feb460a0 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystemConstants.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystemConstants.java
@@ -32,6 +32,7 @@
 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;
@@ -64,7 +65,8 @@ public class RevRotarySubsystemConstants {
     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);
+        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);
diff --git a/src/main/java/frc/robot/subsystems/rotary/RotaryConstants.java b/src/main/java/frc/robot/subsystems/rotary/RotaryConstants.java
index 5432868c..546a6da3 100644
--- a/src/main/java/frc/robot/subsystems/rotary/RotaryConstants.java
+++ b/src/main/java/frc/robot/subsystems/rotary/RotaryConstants.java
@@ -158,59 +158,27 @@ public static CANcoderConfiguration getCANcoderConfig(boolean sim)
         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 RotaryMechanismReal getReal()
-    {
-        return new RotaryMechanismReal(
-            new MotorIOTalonFX(NAME, getFXConfig(), Ports.RotarySubsystemMotorMain,
-                new TalonFXFollower(Ports.RotarySubsystemMotorFollower, false)),
-            CONSTANTS,
-            Optional.of(new AbsoluteEncoderIOCANCoderSim(Ports.RotarySubsystemEncoder,
-                NAME + "Encoder", getCANcoderConfig(false))));
-    }
-
-    /**
-     * 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 RotaryMechanismSim getSim()
-    {
-        return new RotaryMechanismSim(
-            new MotorIOTalonFXSim(
-                NAME,
-                getFXConfig(),
-                Ports.RotarySubsystemMotorMain,
-                new TalonFXFollower(Ports.RotarySubsystemMotorFollower, false)),
-            DCMOTOR, MOI, false, CONSTANTS,
-            Optional.of(new AbsoluteEncoderIOCANCoderSim(Ports.RotarySubsystemEncoder,
-                NAME + "Encoder", getCANcoderConfig(true))));
-    }
-
-    /**
-     * 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 RotaryMechanism getReplay()
+    public static Rotary get()
     {
-        return new RotaryMechanism(NAME, CONSTANTS) {};
+        switch (Constants.currentMode) {
+            case REAL:
+                return new Rotary(new RotaryMechanismReal(
+                    new MotorIOTalonFX(NAME, getFXConfig(), Ports.RotarySubsystemMotorMain,
+                        new TalonFXFollower(Ports.RotarySubsystemMotorFollower, false)),
+                    CONSTANTS,
+                    Optional.of(new AbsoluteEncoderIOCANCoderSim(Ports.RotarySubsystemEncoder,
+                        NAME + "Encoder", getCANcoderConfig(false)))));
+            case SIM:
+                return new Rotary(new RotaryMechanismSim(
+                    new MotorIOTalonFXSim(NAME, getFXConfig(), Ports.RotarySubsystemMotorMain,
+                        new TalonFXFollower(Ports.RotarySubsystemMotorFollower, false)),
+                    DCMOTOR, MOI, false, CONSTANTS,
+                    Optional.of(new AbsoluteEncoderIOCANCoderSim(Ports.RotarySubsystemEncoder,
+                        NAME + "Encoder", getCANcoderConfig(true)))));
+            case REPLAY:
+                return new Rotary(new RotaryMechanism(NAME, CONSTANTS) {});
+            default:
+                throw new IllegalStateException("Unrecognized Robot Mode");
+        }
     }
 }

From b9de937b7628a32a5deaf6de4dd3e98981a9158d Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Mon, 24 Nov 2025 17:40:26 -0500
Subject: [PATCH 08/29] partially working

---
 .../java/frc/lib/io/motor/MotorIORev.java     | 13 +----
 .../mechanisms/rotary/RotaryMechanism.java    |  9 +++
 .../frc/lib/util/LoggedTunableNumber.java     | 19 ++++--
 src/main/java/frc/robot/RobotContainer.java   |  9 ++-
 ...RevRotarySubsystem.java => RevRotary.java} | 37 +++++++-----
 ...Constants.java => RevRotaryConstants.java} | 58 ++++++++++++++-----
 .../frc/robot/subsystems/rotary/Rotary.java   | 26 ++++++---
 .../subsystems/rotary/RotaryConstants.java    |  3 -
 8 files changed, 118 insertions(+), 56 deletions(-)
 rename src/main/java/frc/robot/subsystems/revRotary/{RevRotarySubsystem.java => RevRotary.java} (64%)
 rename src/main/java/frc/robot/subsystems/revRotary/{RevRotarySubsystemConstants.java => RevRotaryConstants.java} (76%)

diff --git a/src/main/java/frc/lib/io/motor/MotorIORev.java b/src/main/java/frc/lib/io/motor/MotorIORev.java
index ba8170ab..22c73157 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORev.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORev.java
@@ -100,20 +100,11 @@ protected com.revrobotics.spark.SparkBase.ControlType getRevControlType(ControlT
      * 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 gearRatio The gear ratio applied to the motor's output.
-     * @param wheelRadius The radius of the wheel attached to the motor, used for motion
-     *        calculations.
-     * @param isBrake Boolean indicating whether the motor should use brake mode (true) or coast
-     *        mode (false).
-     * @param inverted Boolean indicating whether the motor should be inverted relative to the
-     *        control signal.
-     * @param motorType The type of motor (e.g., brushless, brushed) to configure.
-     * @param motionProfile The motion profile settings for the motor, including PID and velocity
-     *        parameters.
-     *
+     * @param config The configuration settings for the motor.
      * @see MotorIO
      */
     public MotorIORev(
diff --git a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
index e0d8f1cd..6b1401b0 100644
--- a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
+++ b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
@@ -7,8 +7,10 @@
 import frc.lib.io.motor.MotorIO.ControlType;
 import frc.lib.io.motor.MotorInputsAutoLogged;
 import frc.lib.mechanisms.Mechanism;
+import static edu.wpi.first.units.Units.Degree;
 import java.util.Optional;
 import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -24,6 +26,8 @@ public static enum RotaryAxis {
         ROLL
     }
 
+    private final String name;
+
     public static record RotaryMechCharacteristics(
         Translation3d offset,
         Distance armLength,
@@ -39,6 +43,7 @@ public static record RotaryMechCharacteristics(
 
     public RotaryMechanism(String name, RotaryMechCharacteristics characteristics)
     {
+        this.name = name;
         visualizer = new RotaryVisualizer(name, characteristics);
     }
 
@@ -75,6 +80,10 @@ public void periodic()
         visualizer.setCurrentAngle(inputs.position);
         visualizer.setTrajectoryAngle(getTrajectoryAngle());
         visualizer.setGoalAngle(getGoalAngle());
+
+        Logger.recordOutput(
+            this.name + "/PositionDegrees",
+            inputs.position.in(Degree));
     }
 
     @Override
diff --git a/src/main/java/frc/lib/util/LoggedTunableNumber.java b/src/main/java/frc/lib/util/LoggedTunableNumber.java
index 50f4526c..0db3a4e9 100644
--- a/src/main/java/frc/lib/util/LoggedTunableNumber.java
+++ b/src/main/java/frc/lib/util/LoggedTunableNumber.java
@@ -69,9 +69,6 @@ public void initDefault(double defaultValue)
         if (!hasDefault) {
             hasDefault = true;
             this.defaultValue = defaultValue;
-            if (Constants.tuningMode) {
-                dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
-            }
         }
     }
 
@@ -84,8 +81,22 @@ public double get()
     {
         if (!hasDefault) {
             return 0.0;
+        } else if (Constants.tuningMode) {
+            if (dashboardNumber == null) {
+                dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
+            }
+            double value = dashboardNumber.get();
+            if (key.contains("STOW")) {
+                System.out.println("TunableNumber " + key + " returning dashboard value: " + value);
+            }
+            return value;
         } else {
-            return Constants.tuningMode ? dashboardNumber.get() : defaultValue;
+            if (key.contains("STOW")) {
+                System.out
+                    .println("TunableNumber " + key + " returning default value: " + defaultValue
+                        + " (tuningMode=" + Constants.tuningMode + ")");
+            }
+            return defaultValue;
         }
     }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 8fc784a4..cb197564 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()),
diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystem.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
similarity index 64%
rename from src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystem.java
rename to src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
index ce5d6bdb..312f47db 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystem.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
@@ -14,55 +14,65 @@
 import frc.lib.mechanisms.rotary.RotaryMechanism;
 import frc.lib.util.LoggedTunableNumber;
 import frc.lib.util.LoggerHelper;
+import frc.robot.RobotState;
 import lombok.Getter;
 import lombok.RequiredArgsConstructor;
 
-public class RevRotarySubsystem extends SubsystemBase {
+public class RevRotary extends SubsystemBase {
 
     private final RotaryMechanism io;
+    private final RobotState robotstate;
+    private Setpoint setpoint = Setpoint.STOW;
 
-    private static final LoggedTunableNumber STOW_SETPOINT = new LoggedTunableNumber("TEST", 0.0);
+    private static final LoggedTunableNumber STOW_SETPOINT =
+        new LoggedTunableNumber("REV TEST", 0.0);
     private static final LoggedTunableNumber RAISED_SETPOINT =
-        new LoggedTunableNumber("RAISED", 90);
+        new LoggedTunableNumber("REV RAISED", 90);
 
     @RequiredArgsConstructor
-    @SuppressWarnings("Immutable")
     @Getter
     public enum Setpoint {
-        STOW(Degrees.of(STOW_SETPOINT.get())),
-        RAISED(Degrees.of(RAISED_SETPOINT.get()));
+        STOW(STOW_SETPOINT),
+        RAISED(RAISED_SETPOINT);
 
-        private final Angle setpoint;
+        private final LoggedTunableNumber tunableNumber;
+
+        public Angle getSetpoint()
+        {
+            return Degrees.of(tunableNumber.get());
+        }
     }
 
 
-    public RevRotarySubsystem(RotaryMechanism io)
+    public RevRotary(RotaryMechanism io)
     {
         this.io = io;
+        this.robotstate = RobotState.getInstance();
 
-        setSetpoint(RevRotarySubsystemConstants.DEFAULT_SETPOINT).ignoringDisable(true).schedule();
+        setSetpoint(RevRotaryConstants.DEFAULT_SETPOINT).ignoringDisable(true).schedule();
     }
 
     @Override
     public void periodic()
     {
-        LoggerHelper.recordCurrentCommand(RevRotarySubsystemConstants.NAME, this);
+        LoggerHelper.recordCurrentCommand(RevRotaryConstants.NAME, this);
         io.periodic();
+        // robotstate.setRotaryPose(io.getPoseSupplier().get());
     }
 
     public Command setSetpoint(Setpoint setpoint)
     {
         return this.runOnce(
             () -> io.runPosition(setpoint.getSetpoint(),
-                RevRotarySubsystemConstants.CRUISE_VELOCITY,
-                RevRotarySubsystemConstants.ACCELERATION, RevRotarySubsystemConstants.JERK,
+                RevRotaryConstants.CRUISE_VELOCITY,
+                RevRotaryConstants.ACCELERATION, RevRotaryConstants.JERK,
                 PIDSlot.SLOT_0))
             .withName("Go To " + setpoint.toString() + " Setpoint");
     };
 
     public boolean nearGoal(Angle targetPosition)
     {
-        return io.nearGoal(targetPosition, RevRotarySubsystemConstants.TOLERANCE);
+        return io.nearGoal(targetPosition, RevRotaryConstants.TOLERANCE);
     }
 
     public Command waitUntilGoalCommand(Angle position)
@@ -88,4 +98,5 @@ public void close()
     {
         io.close();
     }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystemConstants.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
similarity index 76%
rename from src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystemConstants.java
rename to src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
index feb460a0..6fece74f 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotarySubsystemConstants.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
@@ -11,10 +11,7 @@
 import static edu.wpi.first.units.Units.Second;
 import java.util.Optional;
 import static edu.wpi.first.units.Units.Meters;
-import com.ctre.phoenix6.configs.*;
-import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
+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;
@@ -43,7 +40,7 @@
 
 
 /** Add your docs here. */
-public class RevRotarySubsystemConstants {
+public class RevRotaryConstants {
     public static String NAME = "REVRotary";
 
     public static final Angle TOLERANCE = Degrees.of(2.0);
@@ -56,6 +53,7 @@ public class RevRotarySubsystemConstants {
 
     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;
 
@@ -75,14 +73,14 @@ public class RevRotarySubsystemConstants {
 
     private static final Angle ENCODER_OFFSET = Rotations.of(0.0);
 
-    public static final RevRotarySubsystem.Setpoint DEFAULT_SETPOINT =
-        RevRotarySubsystem.Setpoint.STOW;
+    public static final RevRotary.Setpoint DEFAULT_SETPOINT =
+        RevRotary.Setpoint.STOW;
 
 
 
     // Positional PID
     private static ClosedLoopConfig SLOT0CONFIG = new ClosedLoopConfig()
-        .pid(30.0, 0, 0, ClosedLoopSlot.kSlot0);
+        .pid(30.0, 0, 5.0, ClosedLoopSlot.kSlot0); // Added D gain to match TalonFX config
 
     /**
      * Creates and returns the TalonFX motor controller configuration for the rotary mechanism.
@@ -108,6 +106,17 @@ public static SparkBaseConfig getREVConfig()
         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.softLimit.forwardSoftLimit(MAX_ANGLE.in(Rotations));
+        config.softLimit.forwardSoftLimitEnabled(false);
+        config.softLimit.reverseSoftLimit(MIN_ANGLE.in(Rotations));
+        config.softLimit.reverseSoftLimitEnabled(false);
+
         config.apply(SLOT0CONFIG);
 
         return config;
@@ -122,11 +131,11 @@ public static SparkBaseConfig getREVConfig()
      * 
      * @return A RotaryMechanismReal object configured with real hardware
      */
-    public static RotaryMechanismReal getReal()
+    public static RevRotary getReal()
     {
         MotorIO io = new MotorIORev(NAME, Ports.RotarySubsystemMotorMain, true, getREVConfig());
 
-        return new RotaryMechanismReal(io, CONSTANTS, null);
+        return new RevRotary(new RotaryMechanismReal(io, CONSTANTS, Optional.empty()));
     }
 
     /**
@@ -139,7 +148,7 @@ public static RotaryMechanismReal getReal()
      * 
      * @return A RotaryMechanismSim object configured for physics simulation
      */
-    public static RotaryMechanismSim getSim()
+    public static RevRotary getSim()
     {
         MotorIOSim io = new MotorIORevSim(
             NAME,
@@ -148,13 +157,13 @@ public static RotaryMechanismSim getSim()
             DCMOTOR,
             getREVConfig());
 
-        return new RotaryMechanismSim(
+        return new RevRotary(new RotaryMechanismSim(
             io,
             DCMOTOR,
             MOI,
             false,
             CONSTANTS,
-            Optional.empty());
+            Optional.empty()));
     }
 
     /**
@@ -166,8 +175,27 @@ public static RotaryMechanismSim getSim()
      * 
      * @return A RotaryMechanism object for log replay
      */
-    public static RotaryMechanism getReplay()
+    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()
     {
-        return new RotaryMechanism(NAME, CONSTANTS) {};
+        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 546a6da3..344c0b3a 100644
--- a/src/main/java/frc/robot/subsystems/rotary/RotaryConstants.java
+++ b/src/main/java/frc/robot/subsystems/rotary/RotaryConstants.java
@@ -28,7 +28,6 @@
 import edu.wpi.first.units.measure.Velocity;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import frc.lib.io.absoluteencoder.AbsoluteEncoderIOCANCoderSim;
-import frc.lib.io.motor.MotorIORevSim;
 import frc.lib.io.motor.MotorIOTalonFX;
 import frc.lib.io.motor.MotorIOTalonFX.TalonFXFollower;
 import frc.lib.io.motor.MotorIOTalonFXSim;
@@ -37,8 +36,6 @@
 import frc.lib.mechanisms.rotary.RotaryMechanism.RotaryMechCharacteristics;
 import frc.robot.Constants;
 import frc.robot.Ports;
-import com.revrobotics.spark.config.SparkFlexConfig;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 
 public class RotaryConstants {

From 6bdb24fbce21b13c546c09c50d6c95dce1664d6c Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Mon, 24 Nov 2025 17:51:53 -0500
Subject: [PATCH 09/29] rev sim mostly working

---
 src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
index 6b1401b0..ab6b500c 100644
--- a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
+++ b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
@@ -8,6 +8,8 @@
 import frc.lib.io.motor.MotorInputsAutoLogged;
 import frc.lib.mechanisms.Mechanism;
 import static edu.wpi.first.units.Units.Degree;
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Radian;
 import java.util.Optional;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
@@ -83,7 +85,7 @@ public void periodic()
 
         Logger.recordOutput(
             this.name + "/PositionDegrees",
-            inputs.position.in(Degree));
+            inputs.position.in(Degrees));
     }
 
     @Override

From ee5d54974a926eb7b89c5e6931124fe0287d0c31 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Mon, 24 Nov 2025 18:01:43 -0500
Subject: [PATCH 10/29] working rev sim

---
 .../java/frc/robot/subsystems/revRotary/RevRotary.java    | 2 +-
 .../robot/subsystems/revRotary/RevRotaryConstants.java    | 8 ++++----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
index 312f47db..7363dd25 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
@@ -25,7 +25,7 @@ public class RevRotary extends SubsystemBase {
     private Setpoint setpoint = Setpoint.STOW;
 
     private static final LoggedTunableNumber STOW_SETPOINT =
-        new LoggedTunableNumber("REV TEST", 0.0);
+        new LoggedTunableNumber("REV STOW", 0.0);
     private static final LoggedTunableNumber RAISED_SETPOINT =
         new LoggedTunableNumber("REV RAISED", 90);
 
diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
index 6fece74f..d1aebc10 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
@@ -112,10 +112,10 @@ public static SparkBaseConfig getREVConfig()
         config.encoder.velocityConversionFactor(1.0 / GEAR_RATIO / 60.0); // RPM to RPS
 
         // Add soft limits to match TalonFX behavior
-        config.softLimit.forwardSoftLimit(MAX_ANGLE.in(Rotations));
-        config.softLimit.forwardSoftLimitEnabled(false);
-        config.softLimit.reverseSoftLimit(MIN_ANGLE.in(Rotations));
-        config.softLimit.reverseSoftLimitEnabled(false);
+        // config.softLimit.forwardSoftLimit(MAX_ANGLE.in(Rotations));
+        // config.softLimit.forwardSoftLimitEnabled(false);
+        // config.softLimit.reverseSoftLimit(MIN_ANGLE.in(Rotations));
+        // config.softLimit.reverseSoftLimitEnabled(false);
 
         config.apply(SLOT0CONFIG);
 

From 53a3377028890184315f88782ef450bbd59d1cce Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 13:35:41 -0500
Subject: [PATCH 11/29] Refactor rotary subsystem

---
 .github/instructions/default.instructions.md  |  82 +++++++
 src/main/java/frc/lib/io/motor/MotorIO.java   |  65 +++++-
 .../java/frc/lib/io/motor/MotorIORev.java     |   6 +-
 .../java/frc/lib/io/motor/MotorIORevSim.java  |  14 +-
 .../java/frc/lib/mechanisms/Mechanism.java    |  78 +++++--
 .../mechanisms/rotary/RotaryMechanism.java    |   2 -
 .../rotary/RotaryMechanismReal.java           |  12 ++
 .../mechanisms/rotary/RotaryMechanismSim.java |  18 ++
 src/main/java/frc/robot/Ports.java            |   2 +
 .../robot/subsystems/revRotary/RevRotary.java |   2 -
 .../revRotary/RevRotaryConstants.java         |   2 +-
 .../frc/robot/subsystems/rotary/Rotary.java   |   5 -
 .../subsystems/revRotary/RevRotaryTest.java   | 202 ++++++++++++++++++
 .../robot/subsystems/rotary/RotaryTest.java   |  95 +++++++-
 14 files changed, 526 insertions(+), 59 deletions(-)
 create mode 100644 .github/instructions/default.instructions.md
 create mode 100644 src/test/java/frc/robot/subsystems/revRotary/RevRotaryTest.java

diff --git a/.github/instructions/default.instructions.md b/.github/instructions/default.instructions.md
new file mode 100644
index 00000000..1458f4f4
--- /dev/null
+++ b/.github/instructions/default.instructions.md
@@ -0,0 +1,82 @@
+# FRC Team W8 Library - AI Assistant Instructions
+
+You are a sr Java engineer with 10+ years of expereince. 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
+
+- **Language**: Java
+- **Framework**: WPILib - https://frcdocs.wpi.edu/en/latest/docs/software/what-is-wpilib.html
+- **Vendor Libraries**:
+  - CTRE (Phoenix 6) for motors/sensors: https://v6.docs.ctr-electronics.com/en/stable/docs/api-reference/api-usage/api-overview.html
+  - REV (REVLib) for motors/sensors: https://docs.revrobotics.com/revlib
+- **Logging**: AdvantageKit - https://docs.advantagekit.org/
+  - Logs are viewed in AdvantageScope
+
+## Library Architecture (`lib` folder)
+
+This codebase uses a **hardware abstraction layer** to make robot code portable and testable:
+
+### Key Patterns
+
+1. **IO Interfaces**: Define hardware operations (e.g., `ElevatorIO`, `DriveIO`)
+   - Methods for reading sensors and controlling actuators
+   - Contain nested `Inputs` class for sensor data logged by AdvantageKit
+
+2. **IO Implementations**: Hardware-specific code (e.g., `ElevatorIOSparkMax`, `ElevatorIOSim`)
+   - Real hardware implementation uses vendor libraries (CTRE, REV)
+   - Simulation implementations for testing without hardware
+   - Keep vendor-specific code isolated here
+
+3. **Subsystems**: High-level robot logic extends WPILib's `SubsystemBase`
+   - Accept an `IO` interface in the constructor (dependency injection)
+   - Contain robot behavior and state machines
+   - Call `Logger.processInputs()` to log sensor data
+
+### Best Practices
+
+- **Keep subsystems hardware-agnostic**: They should only use the IO interface, never vendor classes directly
+- **Log everything**: Use AdvantageKit's `@AutoLog` for all sensor inputs
+- **One IO implementation per hardware type**: Separate real vs. simulation, different motor controllers, etc.
+- **Throw from default interface methods**: Default methods in IO interfaces should throw `NotImplementedException` to ensure they are explicitly implemented where needed.
+- **Commands for actions**: Use WPILib's command-based framework for robot behaviors
+- **Constants in separate classes**: Keep tunable values organized and easy to find
+
+### Example Usage
+
+```java
+// IO Interface defines contract
+public interface DriveIO {
+  @AutoLog
+  class DriveInputs {
+    double leftVelocityMPS = 0.0;
+    double rightVelocityMPS = 0.0;
+  }
+  
+  void updateInputs(DriveInputs inputs);
+  void setVoltage(double left, double right);
+}
+
+// Subsystem uses IO interface
+public class Drive extends SubsystemBase {
+  private final DriveIO io;
+  private final DriveInputsAutoLogged inputs = new DriveInputsAutoLogged();
+  
+  public Drive(DriveIO io) {
+    this.io = io;
+  }
+  
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("Drive", inputs);
+  }
+}
+```
+
+## Guidance for Students
+
+- Explain your code with comments when introducing new concepts
+- Prefer readability over cleverness
+- Break complex logic into small, named methods
+- Use descriptive variable names (`leftMotorVolts` not `lmv`)
+- Test code in simulation before deploying to robot hardware
diff --git a/src/main/java/frc/lib/io/motor/MotorIO.java b/src/main/java/frc/lib/io/motor/MotorIO.java
index 5606904c..237d5228 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,7 @@ 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");
+    }
 }
diff --git a/src/main/java/frc/lib/io/motor/MotorIORev.java b/src/main/java/frc/lib/io/motor/MotorIORev.java
index 22c73157..646723cc 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORev.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORev.java
@@ -186,9 +186,7 @@ public void runDutyCycle(double percent)
     }
 
     @Override
-    public void runPosition(Angle position, AngularVelocity cruiseVelocity,
-        AngularAcceleration acceleration,
-        Velocity<AngularAccelerationUnit> maxJerk, PIDSlot slot)
+    public void runPosition(Angle position, PIDSlot slot)
     {
         controller.setReference(position.in(Rotation),
             getRevControlType(ControlType.POSITION),
@@ -197,7 +195,7 @@ public void runPosition(Angle position, AngularVelocity cruiseVelocity,
 
 
     @Override
-    public void runVelocity(AngularVelocity velocity, AngularAcceleration acceleration,
+    public void runVelocity(AngularVelocity velocity,
         PIDSlot slot)
     {
         controller.setReference(velocity.in(RotationsPerSecond) * 60,
diff --git a/src/main/java/frc/lib/io/motor/MotorIORevSim.java b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
index 16761689..bc0d838f 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORevSim.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
@@ -8,24 +8,16 @@
 import static edu.wpi.first.units.Units.Volts;
 
 import com.revrobotics.sim.SparkFlexSim;
-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.SparkLowLevel.MotorType;
-import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.config.SparkBaseConfig;
 import edu.wpi.first.math.system.plant.DCMotor;
 import com.revrobotics.spark.SparkSim;
 
-import com.revrobotics.spark.config.SparkFlexConfig;
-import com.revrobotics.spark.config.SparkMaxConfig;
-import edu.wpi.first.units.AngularAccelerationUnit;
 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.Velocity;
 import frc.lib.util.Device;
 
 
@@ -98,9 +90,7 @@ public MotorIORevSim(
     }
 
     @Override
-    public void runPosition(Angle position, AngularVelocity cruiseVelocity,
-        AngularAcceleration acceleration,
-        Velocity<AngularAccelerationUnit> maxJerk, PIDSlot slot)
+    public void runPosition(Angle position, PIDSlot slot)
     {
         simState.setPosition(position.in(Rotations));
 
@@ -108,7 +98,7 @@ public void runPosition(Angle position, AngularVelocity cruiseVelocity,
 
 
     @Override
-    public void runVelocity(AngularVelocity velocity, AngularAcceleration acceleration,
+    public void runVelocity(AngularVelocity velocity,
         PIDSlot slot)
     {
         simState.setVelocity(velocity.in(RotationsPerSecond) * 60);
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 ab6b500c..e763ab91 100644
--- a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
+++ b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
@@ -7,9 +7,7 @@
 import frc.lib.io.motor.MotorIO.ControlType;
 import frc.lib.io.motor.MotorInputsAutoLogged;
 import frc.lib.mechanisms.Mechanism;
-import static edu.wpi.first.units.Units.Degree;
 import static edu.wpi.first.units.Units.Degrees;
-import static edu.wpi.first.units.Units.Radian;
 import java.util.Optional;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
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..ed508765 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,10 @@ public AngularVelocity getVelocity()
     {
         return inputs.velocity;
     }
+
+    @Override
+    public void close()
+    {
+        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/subsystems/revRotary/RevRotary.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
index 7363dd25..b8c9defc 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
@@ -64,8 +64,6 @@ public Command setSetpoint(Setpoint setpoint)
     {
         return this.runOnce(
             () -> io.runPosition(setpoint.getSetpoint(),
-                RevRotaryConstants.CRUISE_VELOCITY,
-                RevRotaryConstants.ACCELERATION, RevRotaryConstants.JERK,
                 PIDSlot.SLOT_0))
             .withName("Go To " + setpoint.toString() + " Setpoint");
     };
diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
index d1aebc10..778b3979 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
@@ -133,7 +133,7 @@ public static SparkBaseConfig getREVConfig()
      */
     public static RevRotary getReal()
     {
-        MotorIO io = new MotorIORev(NAME, Ports.RotarySubsystemMotorMain, true, getREVConfig());
+        MotorIO io = new MotorIORev(NAME, Ports.revRotarySubsytemMotorMain, true, getREVConfig());
 
         return new RevRotary(new RotaryMechanismReal(io, CONSTANTS, Optional.empty()));
     }
diff --git a/src/main/java/frc/robot/subsystems/rotary/Rotary.java b/src/main/java/frc/robot/subsystems/rotary/Rotary.java
index b497ffc1..f424c73e 100644
--- a/src/main/java/frc/robot/subsystems/rotary/Rotary.java
+++ b/src/main/java/frc/robot/subsystems/rotary/Rotary.java
@@ -124,9 +124,4 @@ public AngularVelocity getVelocity()
     {
         return io.getVelocity();
     }
-
-    public void close()
-    {
-        io.close();
-    }
 }
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..98816ed0
--- /dev/null
+++ b/src/test/java/frc/robot/subsystems/revRotary/RevRotaryTest.java
@@ -0,0 +1,202 @@
+/*
+ * 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 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;
+
+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();
+    }
+
+    @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()
+    {
+        revRotary.close();
+    }
+}
diff --git a/src/test/java/frc/robot/subsystems/rotary/RotaryTest.java b/src/test/java/frc/robot/subsystems/rotary/RotaryTest.java
index 42a7681b..6b74db9e 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,93 @@ void goToGoalWithWait()
         }
     }
 
-    @Override
-    public void close()
+    @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()
     {
-        rotary.close();
+        // 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()
+    {}
 }

From cb649e153de054ef526c2dc12b677e3ec127b665 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 14:49:16 -0500
Subject: [PATCH 12/29] test working

---
 src/main/java/frc/lib/io/motor/MotorIO.java   |  9 ++
 .../java/frc/lib/io/motor/MotorIORev.java     | 13 ++-
 .../java/frc/lib/io/motor/MotorIORevSim.java  |  2 +-
 .../mechanisms/rotary/RotaryMechanismSim.java |  4 +-
 src/main/java/frc/robot/RobotContainer.java   | 11 +++
 .../revRotary/RevRotaryConstants.java         |  2 +-
 .../java/frc/robot/RobotContainerTest.java    | 83 ++++++++++++-------
 .../subsystems/revRotary/RevRotaryTest.java   | 12 ++-
 8 files changed, 99 insertions(+), 37 deletions(-)

diff --git a/src/main/java/frc/lib/io/motor/MotorIO.java b/src/main/java/frc/lib/io/motor/MotorIO.java
index 237d5228..0c14f017 100644
--- a/src/main/java/frc/lib/io/motor/MotorIO.java
+++ b/src/main/java/frc/lib/io/motor/MotorIO.java
@@ -229,4 +229,13 @@ 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
index 646723cc..36e6a9d5 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORev.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORev.java
@@ -215,9 +215,18 @@ private SparkBaseConfig newEmptyConfig()
         return (motor instanceof SparkFlex) ? new SparkFlexConfig() : new SparkMaxConfig();
     }
 
-    protected void close()
+    @Override
+    public void close()
     {
-        this.motor.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 SparkFlex getMotor()
diff --git a/src/main/java/frc/lib/io/motor/MotorIORevSim.java b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
index bc0d838f..82270ea5 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORevSim.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
@@ -139,7 +139,7 @@ public void updateInputs(MotorInputs inputs)
     @Override
     public void close()
     {
-        super.motor.close();
+        super.close();
     }
 
 }
diff --git a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismSim.java b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismSim.java
index ed508765..0011efb4 100644
--- a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismSim.java
+++ b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanismSim.java
@@ -179,6 +179,8 @@ public AngularVelocity getVelocity()
     @Override
     public void close()
     {
-        io.close();
+        if (io != null) {
+            io.close();
+        }
     }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cb197564..c8aa1661 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -324,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/revRotary/RevRotaryConstants.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
index 778b3979..d9c08eb1 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
@@ -152,7 +152,7 @@ public static RevRotary getSim()
     {
         MotorIOSim io = new MotorIORevSim(
             NAME,
-            Ports.RotarySubsystemMotorMain,
+            Ports.revRotarySubsytemMotorMain,
             true,
             DCMOTOR,
             getREVConfig());
diff --git a/src/test/java/frc/robot/RobotContainerTest.java b/src/test/java/frc/robot/RobotContainerTest.java
index badacdfc..48159bbf 100644
--- a/src/test/java/frc/robot/RobotContainerTest.java
+++ b/src/test/java/frc/robot/RobotContainerTest.java
@@ -1,36 +1,57 @@
-/* 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.AfterEach;
+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;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+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
index 98816ed0..82406449 100644
--- a/src/test/java/frc/robot/subsystems/revRotary/RevRotaryTest.java
+++ b/src/test/java/frc/robot/subsystems/revRotary/RevRotaryTest.java
@@ -19,15 +19,21 @@
 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.AfterAll;
 import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeAll;
 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 edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.TestUtil;
 
+@TestMethodOrder(MethodOrderer.MethodName.class)
 public class RevRotaryTest implements AutoCloseable {
     RevRotary revRotary;
 
@@ -50,6 +56,7 @@ void setup()
     @AfterEach
     void shutdown() throws Exception
     {
+        // Close the subsystem and its resources
         close();
     }
 
@@ -197,6 +204,9 @@ void testRapidSetpointChanges()
     @Override
     public void close()
     {
-        revRotary.close();
+        if (revRotary != null) {
+            revRotary.close();
+            revRotary = null;
+        }
     }
 }

From ecf6f8f02b86fead452307848cc13373987088e7 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 15:04:06 -0500
Subject: [PATCH 13/29] update sim

---
 src/main/java/frc/lib/io/motor/MotorIORev.java       | 11 +++++------
 src/main/java/frc/lib/io/motor/MotorIORevSim.java    | 12 +++---------
 src/test/java/frc/robot/RobotContainerTest.java      |  2 --
 .../robot/subsystems/revRotary/RevRotaryTest.java    |  3 ---
 4 files changed, 8 insertions(+), 20 deletions(-)

diff --git a/src/main/java/frc/lib/io/motor/MotorIORev.java b/src/main/java/frc/lib/io/motor/MotorIORev.java
index 36e6a9d5..6093ac3c 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORev.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORev.java
@@ -9,28 +9,27 @@
 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.AngularAccelerationUnit;
 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.Current;
-import edu.wpi.first.units.measure.Velocity;
 import edu.wpi.first.units.measure.Voltage;
 import frc.lib.util.Device;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 public class MotorIORev implements MotorIO {
 
-    public SparkFlex motor;
+    public SparkBase motor;
     public SparkClosedLoopController controller;
     private RelativeEncoder encoder;
 
@@ -118,7 +117,7 @@ public MotorIORev(
         if (isFlex) {
             motor = new SparkFlex(id.id(), MotorType.kBrushless);
         } else {
-            // motor = new SparkMax(id, MotorType.kBrushless);
+            motor = new SparkMax(id.id(), MotorType.kBrushless);
         }
 
         controller = motor.getClosedLoopController();
@@ -229,7 +228,7 @@ public void close()
         }
     }
 
-    protected SparkFlex getMotor()
+    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
index 82270ea5..74435b50 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORevSim.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
@@ -7,11 +7,10 @@
 import static edu.wpi.first.units.Units.Rotation;
 import static edu.wpi.first.units.Units.Volts;
 
-import com.revrobotics.sim.SparkFlexSim;
 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.SparkFlex;
 import com.revrobotics.spark.config.SparkBaseConfig;
 import edu.wpi.first.math.system.plant.DCMotor;
 import com.revrobotics.spark.SparkSim;
@@ -49,7 +48,7 @@ public class MotorIORevSim extends MotorIORev implements MotorIOSim {
     public record RevFollowerFollower(int id, boolean opposesLeader) {
     }
 
-    public SparkFlex motor;
+    public SparkBase motor;
     public SparkClosedLoopController controller;
     private SparkSim simState;
 
@@ -77,12 +76,7 @@ public MotorIORevSim(
 
         motor = this.getMotor();
 
-        if (isFlex) {
-            simState = new SparkFlexSim(motor, gearBox);
-        } else {
-            // motor = new SparkMax(id, MotorType.kBrushless);
-        }
-
+        simState = new SparkSim(motor, gearBox);
 
         motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
 
diff --git a/src/test/java/frc/robot/RobotContainerTest.java b/src/test/java/frc/robot/RobotContainerTest.java
index 48159bbf..68bd7c0e 100644
--- a/src/test/java/frc/robot/RobotContainerTest.java
+++ b/src/test/java/frc/robot/RobotContainerTest.java
@@ -17,14 +17,12 @@
 
 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 edu.wpi.first.hal.HAL;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.simulation.DriverStationSim;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 public class RobotContainerTest {
     private RobotContainer robotContainer;
diff --git a/src/test/java/frc/robot/subsystems/revRotary/RevRotaryTest.java b/src/test/java/frc/robot/subsystems/revRotary/RevRotaryTest.java
index 82406449..1b816366 100644
--- a/src/test/java/frc/robot/subsystems/revRotary/RevRotaryTest.java
+++ b/src/test/java/frc/robot/subsystems/revRotary/RevRotaryTest.java
@@ -19,9 +19,7 @@
 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.AfterAll;
 import org.junit.jupiter.api.AfterEach;
-import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.junit.jupiter.api.MethodOrderer;
@@ -30,7 +28,6 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.TestUtil;
 
 @TestMethodOrder(MethodOrderer.MethodName.class)

From e285b3e61902871b0ae917d92d7f36c1413a910e Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 15:21:15 -0500
Subject: [PATCH 14/29] Update
 src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 .../frc/robot/subsystems/revRotary/RevRotaryConstants.java    | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
index d9c08eb1..d2826af0 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
@@ -112,10 +112,6 @@ public static SparkBaseConfig getREVConfig()
         config.encoder.velocityConversionFactor(1.0 / GEAR_RATIO / 60.0); // RPM to RPS
 
         // Add soft limits to match TalonFX behavior
-        // config.softLimit.forwardSoftLimit(MAX_ANGLE.in(Rotations));
-        // config.softLimit.forwardSoftLimitEnabled(false);
-        // config.softLimit.reverseSoftLimit(MIN_ANGLE.in(Rotations));
-        // config.softLimit.reverseSoftLimitEnabled(false);
 
         config.apply(SLOT0CONFIG);
 

From f29b6e6b78a87a1f8ca0dc05fe6ddc9e8386fe11 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 15:21:29 -0500
Subject: [PATCH 15/29] Update src/main/java/frc/robot/RobotContainer.java

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 src/main/java/frc/robot/RobotContainer.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index c8aa1661..a224818d 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -329,7 +329,7 @@ public void checkStartPose()
      * 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.ß
+     * get duplicate CAN ID errors when running multiple tests.
      */
     public void closeREVSubsystems()
     {

From b125589a55e6b62aab34373916b147daee77fd19 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 15:21:39 -0500
Subject: [PATCH 16/29] Update src/main/java/frc/lib/io/motor/MotorIORev.java

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 src/main/java/frc/lib/io/motor/MotorIORev.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/lib/io/motor/MotorIORev.java b/src/main/java/frc/lib/io/motor/MotorIORev.java
index 6093ac3c..ad6303ba 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORev.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORev.java
@@ -141,7 +141,6 @@ public void updateInputs(MotorInputs inputs)
         inputs.appliedVoltage = Volts.of(motor.getAppliedOutput() * motor.getBusVoltage());
         inputs.supplyCurrent = Amps.of(motor.getOutputCurrent());
         inputs.temperature = Celsius.of(motor.getMotorTemperature());
-        inputs.positionError = null;
         inputs.positionError = Angle.ofBaseUnits(0, Rotation);
         inputs.velocityError = AngularVelocity.ofBaseUnits(0, RotationsPerSecond);
         inputs.goalPosition = Angle.ofBaseUnits(0, Rotation);

From 80e922b902d53a911424ac22a1ebdf7ad67b733e Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 15:21:55 -0500
Subject: [PATCH 17/29] Update .github/instructions/default.instructions.md

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 .github/instructions/default.instructions.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/instructions/default.instructions.md b/.github/instructions/default.instructions.md
index 1458f4f4..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 sr Java engineer with 10+ years of expereince. 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
 

From c9cc5b54ababc5b44138cbb29bd12ac258943035 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 15:22:28 -0500
Subject: [PATCH 18/29] Update
 src/main/java/frc/robot/subsystems/revRotary/RevRotary.java

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 src/main/java/frc/robot/subsystems/revRotary/RevRotary.java | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
index b8c9defc..45eae9f4 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
@@ -22,7 +22,6 @@ public class RevRotary extends SubsystemBase {
 
     private final RotaryMechanism io;
     private final RobotState robotstate;
-    private Setpoint setpoint = Setpoint.STOW;
 
     private static final LoggedTunableNumber STOW_SETPOINT =
         new LoggedTunableNumber("REV STOW", 0.0);

From afd56fdef4895a888e6387c84b746999b879b74d Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 15:22:49 -0500
Subject: [PATCH 19/29] Update
 src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 .../revRotary/RevRotaryConstants.java           | 17 ++++++++---------
 1 file changed, 8 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
index d2826af0..d0ba8fb8 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotaryConstants.java
@@ -83,21 +83,20 @@ public class RevRotaryConstants {
         .pid(30.0, 0, 5.0, ClosedLoopSlot.kSlot0); // Added D gain to match TalonFX config
 
     /**
-     * Creates and returns the TalonFX motor controller configuration for the rotary mechanism.
+     * Creates and returns the SparkFlex/SparkMax 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>
+     * <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 TalonFXConfiguration object ready to apply to a motor controller
+     * @return A configured {@link SparkBaseConfig} object ready to apply to a REV Robotics SparkFlex or SparkMax motor controller
      */
     public static SparkBaseConfig getREVConfig()
     {

From 97d8fc8767f14110c028500c1cda9bff3fcd1d1b Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 15:23:31 -0500
Subject: [PATCH 20/29] Update
 src/test/java/frc/robot/subsystems/rotary/RotaryTest.java

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 src/test/java/frc/robot/subsystems/rotary/RotaryTest.java | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/test/java/frc/robot/subsystems/rotary/RotaryTest.java b/src/test/java/frc/robot/subsystems/rotary/RotaryTest.java
index 6b74db9e..49081fab 100644
--- a/src/test/java/frc/robot/subsystems/rotary/RotaryTest.java
+++ b/src/test/java/frc/robot/subsystems/rotary/RotaryTest.java
@@ -165,5 +165,10 @@ void testCommandNaming()
 
     @Override
     public void close()
-    {}
+    {
+        if (rotary != null) {
+            rotary.close();
+            rotary = null;
+        }
+    }
 }

From 21602a8636e65ec756d3795f63bd4ef7ded008d0 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 15:23:54 -0500
Subject: [PATCH 21/29] Update
 src/main/java/frc/lib/io/motor/MotorIORevSim.java

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 src/main/java/frc/lib/io/motor/MotorIORevSim.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/lib/io/motor/MotorIORevSim.java b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
index 74435b50..42552389 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORevSim.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
@@ -105,6 +105,7 @@ public double getRotorToSensorRatio()
         return 1;
     }
 
+    @Override
     public double getSensorToMechanismRatio()
     {
         return 1;

From 81f0df1bde59765eaa6227285d0b0a9b3791dd4d Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 15:24:06 -0500
Subject: [PATCH 22/29] Update src/main/java/frc/lib/io/motor/MotorIOSim.java

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
---
 src/main/java/frc/lib/io/motor/MotorIOSim.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/lib/io/motor/MotorIOSim.java b/src/main/java/frc/lib/io/motor/MotorIOSim.java
index d56053a2..7e2c3d4d 100644
--- a/src/main/java/frc/lib/io/motor/MotorIOSim.java
+++ b/src/main/java/frc/lib/io/motor/MotorIOSim.java
@@ -102,6 +102,7 @@ public default double getSensorToMechanismRatio()
      * <p>
      * Called when the simulation is shutting down to properly release any resources.
      */
+    @Override
     public default void close()
     {}
 }

From 078adda2bce8f144fe6b35decb131b9d943fdadb Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Tue, 25 Nov 2025 20:26:15 +0000
Subject: [PATCH 23/29] Initial plan


From 2448e4161c82833f820db4534eba363f2d8390da Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Tue, 25 Nov 2025 15:28:31 -0500
Subject: [PATCH 24/29] add close method

---
 src/main/java/frc/robot/subsystems/rotary/Rotary.java | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/rotary/Rotary.java b/src/main/java/frc/robot/subsystems/rotary/Rotary.java
index f424c73e..b497ffc1 100644
--- a/src/main/java/frc/robot/subsystems/rotary/Rotary.java
+++ b/src/main/java/frc/robot/subsystems/rotary/Rotary.java
@@ -124,4 +124,9 @@ public AngularVelocity getVelocity()
     {
         return io.getVelocity();
     }
+
+    public void close()
+    {
+        io.close();
+    }
 }

From 01e8bb324d94ce1e43a956d628092455d0a8f524 Mon Sep 17 00:00:00 2001
From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com>
Date: Tue, 25 Nov 2025 20:30:32 +0000
Subject: [PATCH 25/29] Address review comments: fix debug prints,
 initialization logic, naming typo, and unused field

Co-authored-by: garrett928 <22306616+garrett928@users.noreply.github.com>
---
 .../java/frc/lib/io/motor/MotorIORevSim.java  |  4 ++--
 .../frc/lib/util/LoggedTunableNumber.java     | 19 ++++---------------
 .../robot/subsystems/revRotary/RevRotary.java |  3 ---
 3 files changed, 6 insertions(+), 20 deletions(-)

diff --git a/src/main/java/frc/lib/io/motor/MotorIORevSim.java b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
index 42552389..97a0934f 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORevSim.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORevSim.java
@@ -45,7 +45,7 @@
  */
 public class MotorIORevSim extends MotorIORev implements MotorIOSim {
 
-    public record RevFollowerFollower(int id, boolean opposesLeader) {
+    public record RevFollower(int id, boolean opposesLeader) {
     }
 
     public SparkBase motor;
@@ -70,7 +70,7 @@ public MotorIORevSim(
         boolean isFlex,
         DCMotor gearBox,
         SparkBaseConfig config,
-        RevFollowerFollower... followerData)
+        RevFollower... followerData)
     {
         super(name, id, isFlex, config);
 
diff --git a/src/main/java/frc/lib/util/LoggedTunableNumber.java b/src/main/java/frc/lib/util/LoggedTunableNumber.java
index 0db3a4e9..50f4526c 100644
--- a/src/main/java/frc/lib/util/LoggedTunableNumber.java
+++ b/src/main/java/frc/lib/util/LoggedTunableNumber.java
@@ -69,6 +69,9 @@ public void initDefault(double defaultValue)
         if (!hasDefault) {
             hasDefault = true;
             this.defaultValue = defaultValue;
+            if (Constants.tuningMode) {
+                dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
+            }
         }
     }
 
@@ -81,22 +84,8 @@ public double get()
     {
         if (!hasDefault) {
             return 0.0;
-        } else if (Constants.tuningMode) {
-            if (dashboardNumber == null) {
-                dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
-            }
-            double value = dashboardNumber.get();
-            if (key.contains("STOW")) {
-                System.out.println("TunableNumber " + key + " returning dashboard value: " + value);
-            }
-            return value;
         } else {
-            if (key.contains("STOW")) {
-                System.out
-                    .println("TunableNumber " + key + " returning default value: " + defaultValue
-                        + " (tuningMode=" + Constants.tuningMode + ")");
-            }
-            return defaultValue;
+            return Constants.tuningMode ? dashboardNumber.get() : defaultValue;
         }
     }
 
diff --git a/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
index 45eae9f4..8e198a9a 100644
--- a/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
+++ b/src/main/java/frc/robot/subsystems/revRotary/RevRotary.java
@@ -14,14 +14,12 @@
 import frc.lib.mechanisms.rotary.RotaryMechanism;
 import frc.lib.util.LoggedTunableNumber;
 import frc.lib.util.LoggerHelper;
-import frc.robot.RobotState;
 import lombok.Getter;
 import lombok.RequiredArgsConstructor;
 
 public class RevRotary extends SubsystemBase {
 
     private final RotaryMechanism io;
-    private final RobotState robotstate;
 
     private static final LoggedTunableNumber STOW_SETPOINT =
         new LoggedTunableNumber("REV STOW", 0.0);
@@ -46,7 +44,6 @@ public Angle getSetpoint()
     public RevRotary(RotaryMechanism io)
     {
         this.io = io;
-        this.robotstate = RobotState.getInstance();
 
         setSetpoint(RevRotaryConstants.DEFAULT_SETPOINT).ignoringDisable(true).schedule();
     }

From 475d9649367e518673c280bb0abc3aded84a8b48 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Wed, 26 Nov 2025 15:58:54 -0500
Subject: [PATCH 26/29] rename folder

---
 src/main/java/frc/lib/devices/ObjectDetection.java          | 4 ++--
 .../java/frc/lib/io/objectDetection/ObjectDetectionIO.java  | 2 +-
 .../io/objectDetection/ObjectDetectionIOPhotonVision.java   | 2 +-
 .../frc/lib/io/objectDetection/ObjectDetectionIOSim.java    | 2 +-
 .../frc/robot/subsystems/objectDetector/ObjectDetector.java | 2 +-
 .../subsystems/objectDetector/ObjectDetectorConstants.java  | 6 +++---
 6 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/lib/devices/ObjectDetection.java b/src/main/java/frc/lib/devices/ObjectDetection.java
index b7dd71e8..7ee0b785 100644
--- a/src/main/java/frc/lib/devices/ObjectDetection.java
+++ b/src/main/java/frc/lib/devices/ObjectDetection.java
@@ -13,9 +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.ObjectDetectionIO;
-import frc.lib.io.objectDetection.ObjectDetectionIO.TargetObservation;
 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/objectDetection/ObjectDetectionIO.java b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIO.java
index a6c4cb6c..303685b8 100644
--- a/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIO.java
+++ b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIO.java
@@ -13,7 +13,7 @@
  * not, see <https://www.gnu.org/licenses/>.
  */
 
-package frc.lib.io.objectDetection;
+package frc.lib.io.objectdetection;
 
 import org.littletonrobotics.junction.AutoLog;
 
diff --git a/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOPhotonVision.java b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOPhotonVision.java
index 87faa2e6..924cf91f 100644
--- a/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOPhotonVision.java
+++ b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOPhotonVision.java
@@ -13,7 +13,7 @@
  * not, see <https://www.gnu.org/licenses/>.
  */
 
-package frc.lib.io.objectDetection;
+package frc.lib.io.objectdetection;
 
 import org.photonvision.PhotonCamera;
 import org.photonvision.targeting.PhotonPipelineResult;
diff --git a/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOSim.java b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOSim.java
index 63454001..743c0129 100644
--- a/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOSim.java
+++ b/src/main/java/frc/lib/io/objectDetection/ObjectDetectionIOSim.java
@@ -2,7 +2,7 @@
 // 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.lib.io.objectDetection;
+package frc.lib.io.objectdetection;
 
 import java.util.ArrayList;
 import java.util.List;
diff --git a/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetector.java b/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetector.java
index 7421c2a9..b05114e2 100644
--- a/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetector.java
+++ b/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetector.java
@@ -16,7 +16,7 @@
 package frc.robot.subsystems.objectDetector;
 
 import frc.lib.devices.ObjectDetection;
-import frc.lib.io.objectDetection.ObjectDetectionIO;
+import frc.lib.io.objectdetection.ObjectDetectionIO;
 import frc.robot.subsystems.drive.Drive;
 import org.littletonrobotics.junction.Logger;
 import edu.wpi.first.math.geometry.Translation2d;
diff --git a/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetectorConstants.java b/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetectorConstants.java
index a2c1d2ae..b6e0f9f2 100644
--- a/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetectorConstants.java
+++ b/src/main/java/frc/robot/subsystems/objectDetector/ObjectDetectorConstants.java
@@ -13,9 +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.ObjectDetectionIO;
-import frc.lib.io.objectDetection.ObjectDetectionIOPhotonVision;
-import frc.lib.io.objectDetection.ObjectDetectionIOSim;
+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;
 

From 5eed0514fa461fa7e3ca5a5a5ff55a24d1ef1e1f Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Wed, 26 Nov 2025 16:10:33 -0500
Subject: [PATCH 27/29] add comment

---
 src/main/java/frc/lib/io/motor/MotorIORev.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc/lib/io/motor/MotorIORev.java b/src/main/java/frc/lib/io/motor/MotorIORev.java
index ad6303ba..35c84baf 100644
--- a/src/main/java/frc/lib/io/motor/MotorIORev.java
+++ b/src/main/java/frc/lib/io/motor/MotorIORev.java
@@ -135,6 +135,7 @@ public String getName()
     @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);

From c0dd1312eebcf8dc3ace029c952202d554f9e208 Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Wed, 26 Nov 2025 16:16:15 -0500
Subject: [PATCH 28/29] fix import

---
 src/main/java/frc/lib/devices/ObjectDetection.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/lib/devices/ObjectDetection.java b/src/main/java/frc/lib/devices/ObjectDetection.java
index 7ee0b785..0a1b9733 100644
--- a/src/main/java/frc/lib/devices/ObjectDetection.java
+++ b/src/main/java/frc/lib/devices/ObjectDetection.java
@@ -13,7 +13,7 @@
 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.ObjectDetectionIOInputsAutoLogged;
 import frc.lib.io.objectdetection.ObjectDetectionIO;
 import frc.lib.io.objectdetection.ObjectDetectionIO.TargetObservation;
 

From 6743832ec7174abc7426505f5301ed2a35f483bb Mon Sep 17 00:00:00 2001
From: Garrett Hart <garrett7621@gmail.com>
Date: Wed, 26 Nov 2025 17:57:44 -0500
Subject: [PATCH 29/29] remove debug logging

---
 src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
index e763ab91..a7255a03 100644
--- a/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
+++ b/src/main/java/frc/lib/mechanisms/rotary/RotaryMechanism.java
@@ -7,10 +7,8 @@
 import frc.lib.io.motor.MotorIO.ControlType;
 import frc.lib.io.motor.MotorInputsAutoLogged;
 import frc.lib.mechanisms.Mechanism;
-import static edu.wpi.first.units.Units.Degrees;
 import java.util.Optional;
 import java.util.function.Supplier;
-import org.littletonrobotics.junction.Logger;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -81,9 +79,6 @@ public void periodic()
         visualizer.setTrajectoryAngle(getTrajectoryAngle());
         visualizer.setGoalAngle(getGoalAngle());
 
-        Logger.recordOutput(
-            this.name + "/PositionDegrees",
-            inputs.position.in(Degrees));
     }
 
     @Override