More work bringing 2026 into 2027, ignoring errors for pos/vel conversion factor for now

src/main/java/first/robot/Robot.java

@@ -6,8 +6,11 @@ package first.robot;
 
 import org.wpilib.command2.Command;
 import org.wpilib.command2.CommandScheduler;
+import org.wpilib.epilogue.Epilogue;
+import org.wpilib.epilogue.Logged;
 import org.wpilib.framework.TimedRobot;
 
+@Logged
 public class Robot extends TimedRobot {
   private Command autonomousCommand;
 
@@ -20,6 +23,7 @@ public class Robot extends TimedRobot {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
+    Epilogue.update(this);
   }
 
   @Override

src/main/java/first/robot/RobotContainer.java

@@ -6,7 +6,9 @@ package first.robot;
 
 import org.wpilib.command2.Command;
 import org.wpilib.command2.Commands;
+import org.wpilib.epilogue.Logged;
 
+@Logged
 public class RobotContainer {
   public RobotContainer() {
     configureBindings();

src/main/java/first/robot/constants/ClimberConstants.java

@@ -1,6 +1,6 @@
 package first.robot.constants;
 
-import java.util.function.Function;
+import org.wpilib.hardware.bus.CANPort;
 
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -24,6 +24,8 @@ public class ClimberConstants {
         }
     }
 
+    public static final CANPort kCANPort = CANPort.CAN_S0;
+
     public static final int kMotorCANID = 0;
 
     public static final double kConversionFactor = 0;
@@ -45,19 +47,17 @@ public class ClimberConstants {
 
     public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig();
 
-    public static final Function<Double, Double> kPositionConversionFunction = (d) -> {
-        return d * kConversionFactor;
-    };
-
-    public static final Function<Double, Double> kVelocityConversionFunction = (d) -> {
-        return kPositionConversionFunction.apply(d) / 60;
-    };
-
     static {
         kMotorConfig
             .inverted(kMotorInverted)
             .smartCurrentLimit(kCurrentLimit)
             .idleMode(kIdleMode);
+        kMotorConfig.encoder
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
         kMotorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
             .pid(kP, kI, kD)

src/main/java/first/robot/constants/CompetitionConstants.java

@@ -0,0 +1,36 @@
+package first.robot.constants;
+
+import org.wpilib.fields.Fields;
+import org.wpilib.math.geometry.Pose2d;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.util.Units;
+
+public class CompetitionConstants {
+    public static final Fields kTagLayout = Fields.FRC_2026_REBUILT_ANDY_MARK;
+
+    public static final double kHubGoalHeightMeters = Units.inchesToMeters(72);
+
+    // TODO Real Values
+    public static final Transform3d kRobotToShooter = new Transform3d();
+
+    public static final Pose2d kBlueHubLocation = new Pose2d(
+        Units.inchesToMeters(182.11), 
+        Units.inchesToMeters(158.84), 
+        Rotation2d.fromDegrees(0)
+    );
+
+    // TODO The origination value produced by the April Tag Field Layout object may
+    // influence what this value should actually be. See AprilTagFieldLayout.getOrigin
+    // For now, the X axis position (forward/backward) is calculated as though the blue
+    // alliance wall right hand side is the originiation point, so, the distance from
+    // the blue alliance wall, to the blue alliance hub center point, plus
+    // the distance between the center of the blue alliance hub and the center of
+    // the red alliance hub
+    public static final Pose2d kRedHubLocation = new Pose2d(
+        Units.inchesToMeters(182.11 + 143.5 * 2),
+        Units.inchesToMeters(158.84),
+        Rotation2d.fromDegrees(0)
+    );
+
+}

src/main/java/first/robot/constants/DrivetrainConstants.java

@@ -0,0 +1,58 @@
+package first.robot.constants;
+
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.kinematics.SwerveDriveKinematics;
+import org.wpilib.math.linalg.Matrix;
+import org.wpilib.math.linalg.VecBuilder;
+import org.wpilib.math.numbers.N1;
+import org.wpilib.math.numbers.N3;
+import org.wpilib.math.util.Units;
+
+public class DrivetrainConstants {
+    public static final double kMaxSpeedMetersPerSecond = 4.663;
+    public static final double kMaxAngularSpeed = 2 * Math.PI;
+
+    public static final double kTrackWidth = Units.inchesToMeters(23.75);
+    public static final double kWheelBase = Units.inchesToMeters(18.75);
+ 
+    public static final double kFrontLeftMagEncoderOffset = 2.965;
+    public static final double kFrontRightMagEncoderOffset = 1.120;
+    public static final double kRearLeftMagEncoderOffset = 3.761;
+    public static final double kRearRightMagEncoderOffset = 2.573;
+
+    public static final int kFrontLeftDrivingCANID = 4;
+    public static final int kFrontRightDrivingCANID = 3;
+    public static final int kRearLeftDrivingCANID = 1;
+    public static final int kRearRightDrivingCANID = 2;
+
+    public static final int kFrontLeftTurningCANID = 7; // 8
+    public static final int kFrontRightTurningCANID = 21; //9
+    public static final int kRearLeftTurningCANID = 6; //7
+    public static final int kRearRightTurningCANID = 8; //6
+
+    public static final int kFrontLeftAnalogInPort = 3;
+    public static final int kFrontRightAnalogInPort = 2;
+    public static final int kRearLeftAnalogInPort = 0;
+    public static final int kRearRightAnalogInPort = 1;
+
+    public static final boolean kGyroReversed = true;
+
+    // TODO Hold over from 2025, adjust?
+    public static final double kHeadingP = .65;
+    public static final double kXTranslationP = .5;
+    public static final double kYTranslationP = .5;
+    public static final double kYawPIDTolerance = Units.degreesToRadians(1);
+
+    // TODO How much do we trust gyro and encoders vs vision estimates.
+    // NOTE: Bigger values indicate LESS trust. Generally all three values for a given matrix should be the same
+    public static final Matrix<N3, N1> kSensorFusionOdometryStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);
+    public static final Matrix<N3, N1> kVisionOdometryStdDevs = VecBuilder.fill(0.3, 0.3, 0.3);
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
+    public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
+        new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)
+    );
+}

src/main/java/first/robot/constants/HoodConstants.java

@@ -0,0 +1,90 @@
+package first.robot.constants;
+
+import java.util.Map;
+
+import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap;
+import org.wpilib.math.util.Units;
+
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+import first.robot.constants.ShooterConstants.ShooterSpeeds;
+
+public class HoodConstants {
+    // TODO Real Values
+    public static final int kMotorCANID = 12;
+
+    public static final double kConversionFactor = (1.0/3.0)*(8.0/147.0)*2*Math.PI;
+
+    public static final double kP = 1.75;
+    public static final double kI = 0;
+    public static final double kD = 0;
+    public static final double kS = 0.435;
+    public static final double kV = 0;
+    public static final double kA = 0;
+    public static final double kStartupAngle = 0.0;
+    public static final double kMaxManualSpeedMultiplier = 0.1;
+    public static final double kTolerance = Math.toRadians(0.5);
+
+    public static final double kAmpsToTriggerPositionReset = 10;
+
+    // TODO This is just barely longer than the default frame time for output current information
+    // Should this be longer?
+    public static final double kTimeAboveThresholdToReset = .25;
+
+    public static final int kCurrentLimit = 15;
+
+    public static final boolean kInverted = true;
+    public static final boolean kUseInterpolatorForAngle = false;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    public static final Map<ShooterSpeeds, InterpolatingDoubleTreeMap> kHoodInterpolators = Map.of(
+        ShooterSpeeds.kHubSpeed, new InterpolatingDoubleTreeMap()
+    );
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kConfig = new SparkMaxConfig();
+
+    static {
+        kConfig
+            .idleMode(kIdleMode)
+            .inverted(kInverted)
+            .smartCurrentLimit(kCurrentLimit);
+        kConfig.encoder
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
+        kConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .allowedClosedLoopError(kTolerance, ClosedLoopSlot.kSlot0)
+            .feedForward
+                .sva(kS, kV, kA);
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 40)), 
+            Double.valueOf(Units.degreesToRadians(9.5)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 60)), 
+            Double.valueOf(Units.degreesToRadians(12.5)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 80)),
+            Double.valueOf(Units.degreesToRadians(16.25)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 100)), 
+            Double.valueOf(Units.degreesToRadians(20.5)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 120)), 
+            Double.valueOf(Units.degreesToRadians(23.5)));
+    }
+}

src/main/java/first/robot/constants/IntakePivotConstants.java

@@ -0,0 +1,89 @@
+package first.robot.constants;
+
+import org.wpilib.hardware.bus.CANPort;
+
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class IntakePivotConstants {
+    public enum IntakePivotPosition {
+        kUp(Math.toRadians(116.0)),
+        kDown(Math.toRadians(0.0));
+
+        private double positionRadians;
+        private double positionRotations;
+
+        private IntakePivotPosition(double positionRadians) {
+            this.positionRadians = positionRadians;
+            this.positionRotations = positionRadians / kConversionFactor;
+        }
+
+        public double getPositionRadians() {
+            return positionRadians;
+        }
+
+        public double getPositionRotations() {
+            return positionRotations;
+        }
+    }
+
+    public static final CANPort kCANPort = CANPort.CAN_S0;
+
+    public static final int kLeftMotorCANID = 16;
+    public static final int kRightMotorCANID = 9;
+
+    public static final double kConversionFactor = 60.0/11.0*60.0/18.0*38.0/16.0;
+
+    // Ultra conservative multiplier to prevent 1/8" lexan destruction, modify at your own peril
+    public static final double kMaxManualSpeedMultiplier = .3;
+
+    public static final double kP = 0;
+    public static final double kI = 0;
+    public static final double kD = 0;
+    public static final double kS = 0;
+    public static final double kV = 5.26;
+    public static final double kA = 0.05;
+    public static final double kG = 0.25;
+
+    public static final boolean kInvertMotors = false;
+
+    public static final int kCurrentLimit = 30;
+
+    public static final IdleMode kIdleMode = IdleMode.kCoast;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+    
+    public static final SparkMaxConfig KLeftMotorConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig();
+
+    static {
+        KLeftMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(false);
+        KLeftMotorConfig.encoder
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
+        KLeftMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .positionWrappingEnabled(true)
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionWrappingInputRange(0, 2 * Math.PI)
+            .feedForward
+                .svag(kS, kV, kA, kG);
+
+        kRightMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(true)
+            ;//.follow(kLeftMotorCANID);
+    }
+}

src/main/java/first/robot/constants/IntakeRollerConstants.java

@@ -0,0 +1,40 @@
+package first.robot.constants;
+
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import org.wpilib.hardware.bus.CANPort;
+
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class IntakeRollerConstants {
+    public static final CANPort kCANPort = CANPort.CAN_S0;
+    
+    public static final int kRightMotorCANID = 20;
+    public static final int kLeftMotorCANID = 1;
+
+    public static final int kCurrentLimit = 65;
+
+    public static final boolean kInvertLeftMotor = false;
+    public static final boolean kInvertRightMotor = true;
+
+    public static final double kSpeed = 1;
+
+    public static final IdleMode kIdleMode = IdleMode.kCoast;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig();
+
+    static {
+        leftMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kInvertLeftMotor);
+        rightMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kInvertRightMotor)
+        ;
+    }
+}

src/main/java/first/robot/constants/KrakenMotorConstants.java

@@ -0,0 +1,5 @@
+package first.robot.constants;
+
+public class KrakenMotorConstants {
+    public static final double kFreeSpeedRPM = 6000;
+}

src/main/java/first/robot/constants/ModuleConstants.java

@@ -0,0 +1,139 @@
+package first.robot.constants;
+
+import org.wpilib.math.util.Units;
+
+import com.ctre.phoenix6.configs.AudioConfigs;
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.FeedbackConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class ModuleConstants {
+    public enum ModuleName {
+        kFrontLeft("FrontLeft"),
+        kFrontRight("FrontRight"),
+        kRearLeft("RearLeft"),
+        kRearRight("RearRight");
+
+        private String loggableName;
+
+        private ModuleName(String loggableName) {
+            this.loggableName = loggableName;
+        }
+
+        public String getLoggableName() {
+            return loggableName;
+        }
+        
+    }
+
+    // DRIVING MOTOR CONFIG (Kraken)
+    public static final double kDrivingMotorReduction = (50 * 16 * 45)/(14.0 * 28.0 * 15.0);
+
+    public static final double kDrivingMotorFeedSpeedRPS = KrakenMotorConstants.kFreeSpeedRPM / 60;
+    public static final double kWheelDiameterMeters = Units.inchesToMeters(4);
+    public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
+    public static final double kDriveWheelFreeSpeedRPS = (kDrivingMotorFeedSpeedRPS * kWheelCircumferenceMeters) /
+        kDrivingMotorReduction;
+    public static final double kDrivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
+    public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRPS;
+    
+    // TODO Hold over from 2025, adjust?
+    public static final double kDriveP = .06;
+    public static final double kDriveI = 0;
+    public static final double kDriveD = 0;
+    public static final double kDriveS = 0;
+    public static final double kDriveV = kDrivingVelocityFeedForward;
+    public static final double kDriveA = 0;
+    public static final double kClosedLoopRampRate = .01;
+
+    // TODO Hold over from 2025, adjust?
+    public static final int kDriveMotorStatorCurrentLimit = 90;
+    public static final int kDriveMotorSupplyCurrentLimit = 40;
+
+    // TODO Hold over from 2025, adjust?
+    public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
+    public static final NeutralModeValue kDriveIdleMode = NeutralModeValue.Brake;
+
+    // TURNING MOTOR CONFIG (NEO)
+    public static final double kTurningMotorReduction = 150.0/7.0;
+    public static final double kTurningFactor = 2 * Math.PI / kTurningMotorReduction;
+    // TODO Adjust? Let over from 2025
+    public static final double kTurnP = 12;
+    public static final double kTurnI = 0;
+    public static final double kTurnD = 0;
+
+    public static final double kTurnTolerance = Math.toRadians(0.25);
+
+    public static final boolean kIsEncoderInverted = false;
+
+    // TODO How sensitive is too sensitive?
+    public static final double kAutoResetPositionDeadband = Units.degreesToRadians(.25);
+    
+    public static final int kTurnMotorCurrentLimit = 20;
+
+    public static final IdleMode kTurnIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
+
+    public static final FeedbackConfigs kDriveFeedConfig = new FeedbackConfigs();
+    public static final CurrentLimitsConfigs kDriveCurrentLimitConfig = new CurrentLimitsConfigs();
+    public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
+    public static final AudioConfigs kAudioConfig = new AudioConfigs();
+    public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
+    public static final ClosedLoopRampsConfigs kDriveClosedLoopRampConfig = new ClosedLoopRampsConfigs();
+
+    static {
+        kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
+
+        kDriveCurrentLimitConfig.StatorCurrentLimitEnable = true;
+        kDriveCurrentLimitConfig.SupplyCurrentLimitEnable = true;
+        kDriveCurrentLimitConfig.StatorCurrentLimit = kDriveMotorStatorCurrentLimit;
+        kDriveCurrentLimitConfig.SupplyCurrentLimit = kDriveMotorSupplyCurrentLimit;
+
+        kDriveMotorConfig.Inverted = kDriveInversionState;
+        kDriveMotorConfig.NeutralMode = kDriveIdleMode;
+
+        kAudioConfig.AllowMusicDurDisable = true;
+
+        kDriveSlot0Config.kP = kDriveP;
+        kDriveSlot0Config.kI = kDriveI;
+        kDriveSlot0Config.kD = kDriveD;
+        kDriveSlot0Config.kS = kDriveS;
+        kDriveSlot0Config.kV = kDriveV;
+        kDriveSlot0Config.kA = kDriveA;
+
+        kDriveClosedLoopRampConfig.withVoltageClosedLoopRampPeriod(kClosedLoopRampRate);
+
+        turningConfig
+            .idleMode(kTurnIdleMode)
+            .smartCurrentLimit(kTurnMotorCurrentLimit)
+            .inverted(true);
+        turningConfig.encoder
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionConversionFactor(kTurningFactor)
+            .velocityConversionFactor(kTurningFactor / 60.0);
+        turningConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kTurnP, kTurnI, kTurnD)
+            .outputRange(-1, 1)
+            .allowedClosedLoopError(kTurnTolerance, ClosedLoopSlot.kSlot0)
+            .positionWrappingEnabled(true)
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionWrappingInputRange(0, 2 * Math.PI);
+            
+    }
+}

src/main/java/first/robot/constants/OIConstants.java

@@ -0,0 +1,29 @@
+package first.robot.constants;
+
+import org.wpilib.util.Color;
+
+public class OIConstants {
+    public static final int kDriverControllerPort = 0;
+    public static final int kOperatorControllerPort = 1;
+    
+    public static final double kDriveDeadband = .01;
+
+    public static final double kJoystickExponential = 3;
+
+    public static final String kTeleopTab = "Teleoperated";
+
+    public static final String kCurrentActiveHub = "Alliance Hub Currently Active";
+
+    public static final String[] kRedBlueDisplay = {
+        Color.RED.toHexString(),
+        Color.BLUE.toHexString()
+    };
+
+    public static final String[] kRedDisplay = {
+        Color.RED.toHexString()
+    };
+
+    public static final String[] kBlueDisplay = {
+        Color.BLUE.toHexString()
+    };
+}

src/main/java/first/robot/constants/PhotonConstants.java

@@ -0,0 +1,48 @@
+package first.robot.constants;
+
+import java.util.List;
+
+import org.wpilib.math.geometry.Rotation3d;
+import org.wpilib.math.geometry.Transform3d;
+import org.wpilib.math.util.Units;
+
+import first.robot.utilities.PhotonVisionConfig;
+
+public class PhotonConstants {
+    public static final String kCamera1Name = "CameraPV1";
+    public static final String kCamera2Name = "CameraPV2";
+
+    // TODO Need actual values for all of this
+    public static final Transform3d kCamera1RobotToCam = new Transform3d(
+        Units.inchesToMeters(1.5),
+        Units.inchesToMeters(-8.5),
+        Units.inchesToMeters(28.5),
+        new Rotation3d(
+            Units.degreesToRadians(0), 
+            Units.degreesToRadians(-24.0), 
+            Units.degreesToRadians(30.0)
+        )
+    );
+    public static final Transform3d kCamera2RobotToCam = new Transform3d(
+        Units.inchesToMeters(1.5),
+        Units.inchesToMeters(-10.5),
+        Units.inchesToMeters(28.5),
+        new Rotation3d(
+            Units.degreesToRadians(0.0), 
+            Units.degreesToRadians(-24.0), 
+            Units.degreesToRadians(-10.0)
+        )
+    );
+
+    public static final double kCamera1HeightMeters = 0;
+    public static final double kCamera1PitchRadians = 0;
+    public static final double kCamera2HeightMeters = 0;
+    public static final double kCamera2PitchRadians = 0;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final List<PhotonVisionConfig> configs = List.of(
+        new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
+        new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians)
+    );
+}

src/main/java/first/robot/constants/RollerIndexerConstants.java

@@ -0,0 +1,28 @@
+package first.robot.constants;
+
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class RollerIndexerConstants {
+    public static final int kMotor1CANID = 50;
+    public static final int kMotor2CANID = 4;
+
+    public static final int kCurrentLimit = 40;
+
+    public static final double kMotorSpeed = 1;
+
+    public static final boolean kMotorInverted = true;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig();
+    
+    static {
+        kMotorConfig
+            .inverted(kMotorInverted)
+            .smartCurrentLimit(kCurrentLimit)
+            .idleMode(kIdleMode);
+    }
+}

src/main/java/first/robot/constants/ShooterConstants.java

@@ -0,0 +1,112 @@
+package first.robot.constants;
+
+import org.wpilib.math.util.Units;
+
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class ShooterConstants {
+    public enum ShooterSpeeds {
+        kHubSpeed(3000.0),
+        kFeedSpeed(5000.0),
+        kIdleSpeed(750.0);
+
+        private double speedMPS;
+        private double speedRPM;
+
+        private ShooterSpeeds(double speedRPM) {
+            this.speedMPS = speedRPM * kWheelDiameter*Math.PI;
+            this.speedRPM = speedRPM;
+        }
+
+        public double getSpeedMPS() {
+            return speedMPS * kWheelDiameter*Math.PI;
+        }
+
+        public double getSpeedRPM(){
+            return speedRPM;
+        }
+    }
+
+    // TODO Conversion factor?
+
+    public static final double kWheelDiameter = Units.inchesToMeters(4);
+
+    // TODO Real values
+    public static final int kLeftShooterMotorCANID = 2;
+    public static final int kRightShooterMotorCANID = 5;
+
+    public static final boolean kLeftShooterMotorInverted = true;
+    public static final boolean kRightShooterMotorInverted = false;
+
+    public static final double kLeftP = 0.75;//0.01;//0.001;
+    public static final double kLeftI = 0;
+    public static final double kLeftD = 0;//0.1;//1.8;
+    public static final double kLeftS = 0;
+    public static final double kLeftV = 0.00129;
+    public static final double kLeftA = 0;
+
+    public static final double kRightP = 0.75;//0.001;//0.001;
+    public static final double kRightI = 0;
+    public static final double kRightD = 0;//0.1;
+    public static final double kRightS = 0;
+    public static final double kRightV = 0.00125;
+    public static final double kRightA = 0;
+
+    public static final double kMaxManualSpeedMultiplier = 1;
+
+    public static final double kShooterHeightMeters = 0;
+
+    // TODO Is this value sane?
+    public static final int kCurrentLimit = 60;
+
+    public static final IdleMode kShooterIdleMode = IdleMode.kCoast;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kLeftMotorConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig();
+
+    static {
+        kLeftMotorConfig
+            .idleMode(kShooterIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kLeftShooterMotorInverted);
+        kLeftMotorConfig.absoluteEncoder
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionConversionFactor(1)
+            .velocityConversionFactor(60)
+            .averageDepth(8); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
+        kLeftMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .pid(kLeftP, kLeftI, kLeftD, ClosedLoopSlot.kSlot0)
+            .outputRange(-1, 1)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
+            .feedForward
+                .sva(kLeftS, kLeftV, kLeftA, ClosedLoopSlot.kSlot0);
+
+        kRightMotorConfig
+            .idleMode(kShooterIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kRightShooterMotorInverted);
+        kRightMotorConfig.absoluteEncoder
+            // Ignoring the red here for now. REVLib 2027alpha8 is entirely missing
+            // a solution for this. Issue is open and pending
+            // https://github.com/wpilibsuite/SystemcoreTesting/issues/401
+            .positionConversionFactor(1)
+            .velocityConversionFactor(60)
+            .averageDepth(8)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
+            .inverted(true);
+        kRightMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .pid(kRightP, kRightI, kRightD)
+            .outputRange(-1, 1)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
+            .feedForward
+                .sva(kRightS, kRightV, kRightA, ClosedLoopSlot.kSlot0);
+    }
+}

src/main/java/first/robot/constants/SpindexerConstants.java

@@ -0,0 +1,50 @@
+package first.robot.constants;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class SpindexerConstants {
+    // TODO Real values
+    public static final int kSpindexerMotorCANID = 0;
+    public static final int kFeederMotorCANID = 4;
+
+    public static final int kSpindexerStatorCurrentLimit = 95;
+    public static final int kSpindexerSupplyCurrentLimit = 50;
+    public static final int kFeederCurrentLimit = 30;
+
+    public static final double kSpindexerSpeed = 1;
+    public static final double kFeederSpeed = 1;
+
+    public static final boolean kFeederMotorInverted = false;
+
+    public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive;
+    public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Coast;
+
+    public static final IdleMode kFeederIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kFeederConfig = new SparkMaxConfig();
+
+    public static final CurrentLimitsConfigs kSpindexerCurrentLimitConfig = new CurrentLimitsConfigs();
+    public static final MotorOutputConfigs kSpindexerMotorConfig = new MotorOutputConfigs();
+    
+    static {
+        kSpindexerCurrentLimitConfig.StatorCurrentLimitEnable = true;
+        kSpindexerCurrentLimitConfig.SupplyCurrentLimitEnable = true;
+        kSpindexerCurrentLimitConfig.StatorCurrentLimit = kSpindexerStatorCurrentLimit;
+        kSpindexerCurrentLimitConfig.SupplyCurrentLimit = kSpindexerSupplyCurrentLimit;
+
+        kSpindexerMotorConfig.Inverted = kSpindexerInversionState;
+        kSpindexerMotorConfig.NeutralMode = kSpindexerIdleMode;
+
+        kFeederConfig
+            .inverted(kFeederMotorInverted)
+            .smartCurrentLimit(kFeederCurrentLimit)
+            .idleMode(kFeederIdleMode);
+    }
+}

src/main/java/first/robot/interfaces/IIndexer.java

@@ -0,0 +1,13 @@
+package first.robot.interfaces;
+
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+
+public interface IIndexer {
+    public Command spinToShooter();
+    public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM);
+    public Command spinToIntake();
+    public Command stop();
+    public Command instantaneousStop();
+}

src/main/java/first/robot/subsystems/Climber.java

@@ -0,0 +1,88 @@
+package first.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.epilogue.Logged;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.ClimberConstants;
+import first.robot.constants.ClimberConstants.ClimberPositions;
+
+@Logged
+public class Climber extends SubsystemBase {
+    private SparkMax motor;
+
+    private RelativeEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    private ClimberPositions targetPosition;
+
+    public Climber() {
+        motor = new SparkMax(
+            ClimberConstants.kCANPort,
+            ClimberConstants.kMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        motor.configure(
+            ClimberConstants.kMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        encoder = motor.getEncoder();
+
+        controller = motor.getClosedLoopController();
+
+        targetPosition = null;
+    }
+
+    public double getTargetPositionMeters() {
+        if (targetPosition == null) {
+            return -1;
+        } else {
+            return targetPosition.getPositionMeters();
+        }
+    }
+
+    public double getCurrentPositionMeters() {
+        return encoder.getPosition().get();
+    }
+
+    public boolean isAtSetpoint() {
+        return controller.isAtSetpoint().get();
+    }
+
+    public Command maintainPosition(ClimberPositions position) {
+        return run(() -> {
+            targetPosition = position;
+
+            controller.setSetpoint(
+                position.getPositionMeters(), 
+                ControlType.kPosition
+            );
+        });
+    }
+
+    public Command manualSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            targetPosition = null;
+
+            motor.setThrottle(speed.getAsDouble());
+        });
+    }
+
+    public Command stop() {
+        return manualSpeed(() -> 0);
+    }
+}

src/main/java/first/robot/utilities/Elastic.java

@@ -0,0 +1,390 @@
+package first.robot.utilities;
+
+import org.wpilib.networktables.NetworkTableInstance;
+import org.wpilib.networktables.PubSubOption;
+import org.wpilib.networktables.StringPublisher;
+import org.wpilib.networktables.StringTopic;
+
+import io.avaje.json.JsonException;
+import io.avaje.jsonb.Json;
+import io.avaje.jsonb.JsonType;
+import io.avaje.jsonb.Jsonb;
+
+public final class Elastic {
+  private static final StringTopic notificationTopic =
+      NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications");
+  private static final StringPublisher notificationPublisher =
+      notificationTopic.publish(PubSubOption.SEND_ALL, PubSubOption.KEEP_DUPLICATES);
+  private static final StringTopic selectedTabTopic =
+      NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab");
+  private static final StringPublisher selectedTabPublisher =
+      selectedTabTopic.publish(PubSubOption.KEEP_DUPLICATES);
+  private static final Jsonb jsonb = Jsonb.builder().build();
+  private static final JsonType<Notification> notificationJsonType =
+      jsonb.type(Notification.class);
+
+  /**
+   * Represents the possible levels of notifications for the Elastic dashboard. These levels are
+   * used to indicate the severity or type of notification.
+   */
+  public enum NotificationLevel {
+    /** Informational Message */
+    INFO,
+    /** Warning message */
+    WARNING,
+    /** Error message */
+    ERROR
+  }
+
+  /**
+   * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string
+   * before being published.
+   *
+   * @param notification the {@link Notification} object containing notification details
+   */
+  public static void sendNotification(Notification notification) {
+    try {
+      notificationPublisher.set(notificationJsonType.toJson(notification));
+    } catch (JsonException e) {
+      e.printStackTrace();
+    }
+  }
+
+  /**
+   * Selects the tab of the dashboard with the given name. If no tab matches the name, this will
+   * have no effect on the widgets or tabs in view.
+   *
+   * <p>If the given name is a number, Elastic will select the tab whose index equals the number
+   * provided.
+   *
+   * @param tabName the name of the tab to select
+   */
+  public static void selectTab(String tabName) {
+    selectedTabPublisher.set(tabName);
+  }
+
+  /**
+   * Selects the tab of the dashboard at the given index. If this index is greater than or equal to
+   * the number of tabs, this will have no effect.
+   *
+   * @param tabIndex the index of the tab to select.
+   */
+  public static void selectTab(int tabIndex) {
+    selectTab(Integer.toString(tabIndex));
+  }
+
+  /**
+   * Represents an notification object to be sent to the Elastic dashboard. This object holds
+   * properties such as level, title, description, display time, and dimensions to control how the
+   * notification is displayed on the dashboard.
+   */
+  @Json
+  public static class Notification {
+    @Json.Property("level")
+    private NotificationLevel level;
+
+    @Json.Property("title")
+    private String title;
+
+    @Json.Property("description")
+    private String description;
+
+    @Json.Property("displayTime")
+    private int displayTimeMillis;
+
+    @Json.Property("width")
+    private double width;
+
+    @Json.Property("height")
+    private double height;
+
+    /**
+     * Creates a new Notification with all default parameters. This constructor is intended to be
+     * used with the chainable decorator methods
+     *
+     * <p>Title and description fields are empty.
+     */
+    public Notification() {
+      this(NotificationLevel.INFO, "", "");
+    }
+
+    /**
+     * Creates a new Notification with all properties specified.
+     *
+     * @param level the level of the notification (e.g., INFO, WARNING, ERROR)
+     * @param title the title text of the notification
+     * @param description the descriptive text of the notification
+     * @param displayTimeMillis the time in milliseconds for which the notification is displayed
+     * @param width the width of the notification display area
+     * @param height the height of the notification display area, inferred if below zero
+     */
+    public Notification(
+        NotificationLevel level,
+        String title,
+        String description,
+        int displayTimeMillis,
+        double width,
+        double height) {
+      this.level = level;
+      this.title = title;
+      this.displayTimeMillis = displayTimeMillis;
+      this.description = description;
+      this.height = height;
+      this.width = width;
+    }
+
+    /**
+     * Creates a new Notification with default display time and dimensions.
+     *
+     * @param level the level of the notification
+     * @param title the title text of the notification
+     * @param description the descriptive text of the notification
+     */
+    public Notification(NotificationLevel level, String title, String description) {
+      this(level, title, description, 3000, 350, -1);
+    }
+
+    /**
+     * Creates a new Notification with a specified display time and default dimensions.
+     *
+     * @param level the level of the notification
+     * @param title the title text of the notification
+     * @param description the descriptive text of the notification
+     * @param displayTimeMillis the display time in milliseconds
+     */
+    public Notification(
+        NotificationLevel level, String title, String description, int displayTimeMillis) {
+      this(level, title, description, displayTimeMillis, 350, -1);
+    }
+
+    /**
+     * Creates a new Notification with specified dimensions and default display time. If the height
+     * is below zero, it is automatically inferred based on screen size.
+     *
+     * @param level the level of the notification
+     * @param title the title text of the notification
+     * @param description the descriptive text of the notification
+     * @param width the width of the notification display area
+     * @param height the height of the notification display area, inferred if below zero
+     */
+    public Notification(
+        NotificationLevel level, String title, String description, double width, double height) {
+      this(level, title, description, 3000, width, height);
+    }
+
+    /**
+     * Updates the level of this notification
+     *
+     * @param level the level to set the notification to
+     */
+    public void setLevel(NotificationLevel level) {
+      this.level = level;
+    }
+
+    /**
+     * @return the level of this notification
+     */
+    public NotificationLevel getLevel() {
+      return level;
+    }
+
+    /**
+     * Updates the title of this notification
+     *
+     * @param title the title to set the notification to
+     */
+    public void setTitle(String title) {
+      this.title = title;
+    }
+
+    /**
+     * Gets the title of this notification
+     *
+     * @return the title of this notification
+     */
+    public String getTitle() {
+      return title;
+    }
+
+    /**
+     * Updates the description of this notification
+     *
+     * @param description the description to set the notification to
+     */
+    public void setDescription(String description) {
+      this.description = description;
+    }
+
+    public String getDescription() {
+      return description;
+    }
+
+    /**
+     * Updates the display time of the notification
+     *
+     * @param seconds the number of seconds to display the notification for
+     */
+    public void setDisplayTimeSeconds(double seconds) {
+      setDisplayTimeMillis((int) Math.round(seconds * 1000));
+    }
+
+    /**
+     * Updates the display time of the notification in milliseconds
+     *
+     * @param displayTimeMillis the number of milliseconds to display the notification for
+     */
+    public void setDisplayTimeMillis(int displayTimeMillis) {
+      this.displayTimeMillis = displayTimeMillis;
+    }
+
+    /**
+     * Gets the display time of the notification in milliseconds
+     *
+     * @return the number of milliseconds the notification is displayed for
+     */
+    public int getDisplayTimeMillis() {
+      return displayTimeMillis;
+    }
+
+    /**
+     * Updates the width of the notification
+     *
+     * @param width the width to set the notification to
+     */
+    public void setWidth(double width) {
+      this.width = width;
+    }
+
+    /**
+     * Gets the width of the notification
+     *
+     * @return the width of the notification
+     */
+    public double getWidth() {
+      return width;
+    }
+
+    /**
+     * Updates the height of the notification
+     *
+     * <p>If the height is set to -1, the height will be determined automatically by the dashboard
+     *
+     * @param height the height to set the notification to
+     */
+    public void setHeight(double height) {
+      this.height = height;
+    }
+
+    /**
+     * Gets the height of the notification
+     *
+     * @return the height of the notification
+     */
+    public double getHeight() {
+      return height;
+    }
+
+    /**
+     * Modifies the notification's level and returns itself to allow for method chaining
+     *
+     * @param level the level to set the notification to
+     * @return the current notification
+     */
+    public Notification withLevel(NotificationLevel level) {
+      this.level = level;
+      return this;
+    }
+
+    /**
+     * Modifies the notification's title and returns itself to allow for method chaining
+     *
+     * @param title the title to set the notification to
+     * @return the current notification
+     */
+    public Notification withTitle(String title) {
+      setTitle(title);
+      return this;
+    }
+
+    /**
+     * Modifies the notification's description and returns itself to allow for method chaining
+     *
+     * @param description the description to set the notification to
+     * @return the current notification
+     */
+    public Notification withDescription(String description) {
+      setDescription(description);
+      return this;
+    }
+
+    /**
+     * Modifies the notification's display time and returns itself to allow for method chaining
+     *
+     * @param seconds the number of seconds to display the notification for
+     * @return the current notification
+     */
+    public Notification withDisplaySeconds(double seconds) {
+      return withDisplayMilliseconds((int) Math.round(seconds * 1000));
+    }
+
+    /**
+     * Modifies the notification's display time and returns itself to allow for method chaining
+     *
+     * @param displayTimeMillis the number of milliseconds to display the notification for
+     * @return the current notification
+     */
+    public Notification withDisplayMilliseconds(int displayTimeMillis) {
+      setDisplayTimeMillis(displayTimeMillis);
+      return this;
+    }
+
+    /**
+     * Modifies the notification's width and returns itself to allow for method chaining
+     *
+     * @param width the width to set the notification to
+     * @return the current notification
+     */
+    public Notification withWidth(double width) {
+      setWidth(width);
+      return this;
+    }
+
+    /**
+     * Modifies the notification's height and returns itself to allow for method chaining
+     *
+     * @param height the height to set the notification to
+     * @return the current notification
+     */
+    public Notification withHeight(double height) {
+      setHeight(height);
+      return this;
+    }
+
+    /**
+     * Modifies the notification's height and returns itself to allow for method chaining
+     *
+     * <p>This will set the height to -1 to have it automatically determined by the dashboard
+     *
+     * @return the current notification
+     */
+    public Notification withAutomaticHeight() {
+      setHeight(-1);
+      return this;
+    }
+
+    /**
+     * Modifies the notification to disable the auto dismiss behavior
+     *
+     * <p>This sets the display time to 0 milliseconds
+     *
+     * <p>The auto dismiss behavior can be re-enabled by setting the display time to a number
+     * greater than 0
+     *
+     * @return the current notification
+     */
+    public Notification withNoAutoDismiss() {
+      setDisplayTimeMillis(0);
+      return this;
+    }
+  }
+}

src/main/java/first/robot/utilities/PhotonVisionConfig.java

@@ -0,0 +1,10 @@
+package first.robot.utilities;
+
+import org.wpilib.math.geometry.Transform3d;
+
+public record PhotonVisionConfig (
+    String cameraName,
+    Transform3d robotToCamera,
+    double cameraHeightMeters,
+    double cameraPitchRadians
+) {}

src/main/java/first/robot/utilities/SparkMAXTester.java

@@ -0,0 +1,38 @@
+package first.robot.utilities;
+
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.hardware.bus.CANPort;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+/**
+ * A simple subsystem that can be used to test a single SparkMax and associated NEO motor
+ */
+public class SparkMAXTester extends SubsystemBase {
+    private SparkMax spark;
+
+    /**
+     * Constructor
+     * 
+     * @param deviceID The CAN ID of the SparkMAX that needs testing
+     */
+    public SparkMAXTester(CANPort canPort, int deviceID) {
+        spark = new SparkMax(canPort, deviceID, MotorType.kBrushless);
+    }
+
+    /**
+     * Sets the speed of the motor
+     * 
+     * @param speed A method or lambda which returns a double between -1 and 1
+     * @return A Command object that runs indefinitely to control motor speed
+     */
+    public Command setSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            spark.setThrottle(speed.getAsDouble());
+        });
+    }
+}

src/main/java/first/robot/utilities/SwerveModule.java

@@ -0,0 +1,226 @@
+package first.robot.utilities;
+
+import org.wpilib.hardware.bus.CANPort;
+import org.wpilib.hardware.rotation.AnalogEncoder;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModulePosition;
+import org.wpilib.math.kinematics.SwerveModuleVelocity;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.ModuleConstants;
+import first.robot.constants.ModuleConstants.ModuleName;
+
+/*
+ * This thread
+ * 
+ * https://www.chiefdelphi.com/t/best-easiest-way-to-connect-wire-and-program-thrifty-absolute-magnetic-encoder-to-rev-spark-max-motor-controller/439040/30
+ * 
+ * implies that the best use of the thrifty absolute encoder is to use it as a reference for the Spark relative encoder and then
+ * used the closed loop control on the controller for turning
+ * 
+ * IDK if that's really necessary, the read rate of the analog ports is 100HZ, I suppose the only benefit is the higher rate of
+ * the controller closed loop controller. 
+ */
+public class SwerveModule {
+    private ModuleName moduleName;
+
+    private TalonFX drive;
+    private SparkMax turning;
+
+    private RelativeEncoder turningRelativeEncoder;
+
+    private AnalogEncoder turningAbsoluteEncoder;
+
+    private SparkClosedLoopController turningClosedLoopController;
+
+    private VelocityVoltage driveVelocityRequest;
+
+    private SwerveModuleVelocity lastTargetState;
+    private SwerveModuleVelocity lastTargetStateOptimized;
+
+    private boolean isAbsoluteEncoderDisabled;
+    private boolean turningEncoderAutoRezeroEnabled;
+
+    /**
+     * Builds the swerve module but with the Absolute Encoder disabled.
+     * 
+     * This constructor assumes you zeroed the swerve modules (faced all the bevel gears to the left)
+     * before booting up the robot.
+     * 
+     * @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on
+     * @param moduleName The module name, Front Left, Front Right, etc.
+     * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
+     * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
+     */
+    public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID) {
+        this(canPort, moduleName, drivingCANID, turningCANID, -1, -1);
+    }
+
+    /**
+     * Builds the swerve module with the normal features, disables automatic rezeroing of the turning encoder
+     * from the absolute encoder. 
+     * 
+     * @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on
+     * @param moduleName The module name, Front Left, Front Right, etc.
+     * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
+     * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
+     * @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder
+     * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module 
+     */
+    public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
+        this(canPort, moduleName, drivingCANID, turningCANID, analogEncoderID, analogEncoderOffset, false);
+    }
+
+    /**
+     * Builds the swerve module with the normal features, and gives the option to enable automatic turning encoder rezeroing
+     * when the turning motor is not moving
+     * 
+     * @param canPort The CANPort enumeration that represents the CAN bus that the motors for this module live on
+     * @param moduleName The module name, Front Left, Front Right, etc.
+     * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
+     * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
+     * @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder
+     * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module 
+     * @param turningEncoderAutoRezeroEnabled Should the turning encoder in the NEO automatically rezero from the absolute encoder
+     */
+    public SwerveModule(CANPort canPort, ModuleName moduleName, int drivingCANID, int turningCANID, 
+        int analogEncoderID, double analogEncoderOffset, boolean turningEncoderAutoRezeroEnabled) {
+        isAbsoluteEncoderDisabled = (analogEncoderID == -1) || (analogEncoderOffset < 0);
+
+        drive = new TalonFX(drivingCANID, new CANBus(canPort));
+        turning = new SparkMax(canPort, turningCANID, MotorType.kBrushless);
+
+        turningRelativeEncoder = turning.getEncoder();
+
+        if(!isAbsoluteEncoderDisabled) {
+            turningAbsoluteEncoder = new AnalogEncoder(analogEncoderID, 2 * Math.PI, analogEncoderOffset);
+        }
+        
+        turningClosedLoopController = turning.getClosedLoopController();
+
+        drive.getConfigurator().apply(ModuleConstants.kDriveCurrentLimitConfig);
+        drive.getConfigurator().apply(ModuleConstants.kDriveFeedConfig);
+        drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
+        drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
+        drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+        drive.getConfigurator().apply(ModuleConstants.kDriveClosedLoopRampConfig);
+
+        turning.configure(
+            ModuleConstants.turningConfig,
+            ResetMode.kResetSafeParameters,
+            PersistMode.kPersistParameters
+        );
+
+        driveVelocityRequest = new VelocityVoltage(0);
+
+        if(isAbsoluteEncoderDisabled){
+            turningRelativeEncoder.setPosition(0);
+        } else {
+            turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
+        }
+        
+        drive.setPosition(0);
+
+        this.lastTargetState = getState();
+        this.lastTargetStateOptimized = getState();
+
+        this.turningEncoderAutoRezeroEnabled = turningEncoderAutoRezeroEnabled;
+
+        this.moduleName = moduleName;
+    }
+
+    public void periodic() {
+        
+        if(!isAbsoluteEncoderDisabled) {
+            Logger.recordOutput(moduleName.getLoggableName() + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
+        }
+        
+        Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetState", lastTargetState);
+        Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
+        Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModuleState", getState());
+        Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModulePosition", getPosition());
+        Logger.recordOutput(moduleName.getLoggableName() + "/RelativeEncoderPosition", getTurningEncoderPosition());
+
+        // TODO Re-enable this? Was turned off when there was drivetrain issues
+        // Now that there aren't, do we try this again?
+        /*
+        if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) {
+            if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) {
+                resetEncoders();
+            }
+        }*/
+    }
+
+    public ModuleName getModuleName() {
+        return moduleName;
+    }
+
+    public SwerveModuleVelocity getState() {
+        return new SwerveModuleVelocity(
+            drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
+            new Rotation2d(getTurningEncoderPosition())
+        );
+    }
+
+    public SwerveModulePosition getPosition() {
+        return new SwerveModulePosition(
+            drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
+            new Rotation2d(getTurningEncoderPosition())
+        );
+    }
+
+    public void disableOutput() {
+        drive.disable();
+        turning.disable();
+    }
+
+    public void setDesiredState(SwerveModuleVelocity desiredState) {
+        lastTargetState = new SwerveModuleVelocity(desiredState.velocity, desiredState.angle);
+        
+        desiredState.optimize(new Rotation2d(getTurningEncoderPosition()));
+
+        lastTargetStateOptimized = desiredState;
+
+        drive.setControl(
+            driveVelocityRequest.withVelocity(
+                desiredState.velocity / ModuleConstants.kWheelCircumferenceMeters
+            ).withFeedForward(
+                desiredState.velocity / ModuleConstants.kWheelCircumferenceMeters
+            )
+        );
+
+        turningClosedLoopController.setSetpoint(
+            desiredState.angle.getRadians(), 
+            ControlType.kPosition
+        );
+    }
+
+    public double getTurningEncoderPosition() {
+        return turningRelativeEncoder.getPosition().get() * (ModuleConstants.kIsEncoderInverted ? -1 : 1);
+    }
+
+    public void resetEncoders() {
+        drive.setPosition(0);
+
+        zeroTurningEncoder();
+    }
+
+    public void zeroTurningEncoder() {
+        if(isAbsoluteEncoderDisabled) {
+            turningRelativeEncoder.setPosition(0);
+        } else {
+            turningRelativeEncoder.setPosition(turningAbsoluteEncoder.get());
+        }
+    }
+}
+

src/main/java/first/robot/utilities/Utilities.java

@@ -0,0 +1,119 @@
+package first.robot.utilities;
+
+import java.util.Optional;
+
+import org.wpilib.driverstation.Alliance;
+import org.wpilib.driverstation.MatchState;
+import org.wpilib.math.geometry.Pose2d;
+
+import first.robot.constants.CompetitionConstants;
+
+public class Utilities {
+    public static final double kG = -9.81;
+    
+    /**
+     * Returns the Alliance enumeration that indicates who will have the first
+     * shift. Returns null if the data is not available. 
+     * 
+     * @return The Alliance that will have the first shift, or null if game specific data
+     *  is not present
+     */
+    public static Alliance whoHasFirstShift() {
+        Optional<String> gameData = MatchState.getGameData();
+
+        if(gameData.isPresent()) {
+            String gameDataString = gameData.get();
+
+            if(gameDataString.length() > 0) {
+                switch (gameDataString.charAt(0)) {
+                    case 'B':
+                        return Alliance.RED;
+                    case 'R':
+                        return Alliance.BLUE;
+                    default:
+                        return null;
+                }
+            }
+        }
+        return null;
+    }
+
+    /**
+     * Returns the pose of the hub for the given alliance assigned to our robot.
+     * If no alliance is assigned (which is unlikely) this method returns
+     * the Blue hub pose, which is the closet to the field origin
+     * 
+     * @return The Pose2d object which represents the appropriate pose of the Hub
+     */
+    public static Pose2d getHubPose() {
+        Optional<Alliance> alliance = MatchState.getAlliance();
+
+        if(alliance.isEmpty() || alliance.get() == Alliance.BLUE) {
+            return CompetitionConstants.kBlueHubLocation;
+        } else {
+            return CompetitionConstants.kRedHubLocation;
+        }
+    }
+
+    /**
+     * Returns the AprilTag ID of the tag that is in the center of the hub
+     * for the robot's assigned alliance. If no alliance is assigned (which is unlikely)
+     * the Blue hub's center tag is returned. 
+     * 
+     * @return The tag ID that is in the center of the assigned alliance's hub
+     */
+    public static int getHubCenterAprilTagID() {
+        Optional<Alliance> alliance = MatchState.getAlliance();
+
+        if(alliance.isEmpty() || alliance.get() == Alliance.BLUE) {
+            return 26;
+        } else {
+            return 10;
+        }
+    }
+
+    /**
+     * A ChatGPT possible hallucination related to calcuating whether a shot is possible
+     * for a given speed and change in X and Y position
+     * 
+     * Note that X in this scenario is the physical distance from the shooter exit to 
+     * target. Y is the change in height from the shooter exit to the target height
+     * 
+     * @param targetVMPS The target velocity of the shooter in Meters per Second
+     * @param deltaXM The "as the crow flies" distance between the shooter exit and the target
+     * @param deltaYM The height difference between the shooter exit and the target
+     * @return A true or false value indicating whether the shot is possible
+     */
+    public static boolean shotPossible(double targetVMPS, double deltaXM, double deltaYM) {
+        return Math.pow(targetVMPS, 4) >= 
+            -9.81 * (-9.81 * Math.pow(deltaXM, 2) + 2 * deltaYM * Math.pow(targetVMPS, 2));
+    }
+
+    /**
+     * A ChatGPT possible hallucination that calculates the angle required to make a shot for
+     * a given speed and change in X and Y position
+     * 
+     * Note that X in this scenario is the physical disatance from the shooter exit to
+     * target. Y is the change in height from the shooter exit to the target height
+     * 
+     * Setting softerShot to true changes the angle of attack to a soft, long range shot. False
+     * makes the shot more of a lob
+     * 
+     * @param targetVMPS The target velocity of the shooter in Meters per Second
+     * @param deltaXM The "as the crow flies" distance between the shooter exit and the target
+     * @param deltaYM The height difference between the shooter exit and the target
+     * @param softerShot True for a long range shot, false for a short range lob
+     * @return The angle required of the shooter to make the shot described by the input parameters
+     */
+    public static double shotAngle(double targetVMPS, double deltaXM, double deltaYM, boolean softerShot) {
+        double vPow2 = Math.pow(targetVMPS, 2);
+        double vPow4 = Math.pow(targetVMPS, 4);
+        double sqrtPiece = Math.sqrt(vPow4 - kG * (kG * Math.pow(deltaXM, 2) + 2 * deltaYM * vPow2));
+
+        if(softerShot) {
+            return Math.atan((vPow2 - sqrtPiece) / (kG * deltaXM));
+        } else {
+            return Math.atan((vPow2 + sqrtPiece) / (kG * deltaXM));
+        }
+    }
+}

src/main/java/first/robot/utilities/VisualPose.java

@@ -0,0 +1,9 @@
+package first.robot.utilities;
+
+import org.wpilib.math.geometry.Pose2d;
+
+/**
+ * A record class which represents the source of a visual pose, the pose itself
+ * and the timestamp the pose was generated. 
+ */
+public record VisualPose(String cameraName, Pose2d visualPose, double timestamp) {}

vendordeps/PathplannerLibSystemCoreAlpha.json

@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLibSystemCoreAlpha.json",
     "name": "PathplannerLib",
-    "version": "2027.0.0-alpha-4",
+    "version": "2027.0.0-alpha-4.1",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "wpilibYear": "2027_alpha7",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2027.0.0-alpha-4"
+            "version": "2027.0.0-alpha-4.1"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2027.0.0-alpha-4",
+            "version": "2027.0.0-alpha-4.1",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,