Final commit before merging to main to continue progress

src/main/java/frc/robot/RobotContainer.java

@@ -17,10 +17,12 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.PhotonVision;
 import frc.robot.utilities.Elastic;
 import frc.robot.utilities.Utilities;
 
 public class RobotContainer {
+  private PhotonVision vision;
   private Drivetrain drivetrain;
 
   private CommandXboxController driver;
@@ -30,8 +32,11 @@ public class RobotContainer {
   private Timer shiftTimer;
 
   public RobotContainer() {
+    vision = new PhotonVision();
     drivetrain = new Drivetrain();
 
+    vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
+
     driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 
     shiftTimer = new Timer();
@@ -94,28 +99,28 @@ public class RobotContainer {
     new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> {
       SmartDashboard.putStringArray(
         OIConstants.kCurrentActiveHub, 
-        Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
       );
     }));
 
     new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> {
       SmartDashboard.putStringArray(
         OIConstants.kCurrentActiveHub, 
-        Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
+        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
       );
     }));
 
     new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> {
       SmartDashboard.putStringArray(
         OIConstants.kCurrentActiveHub, 
-        Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
       );
     }));
 
     new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> {
       SmartDashboard.putStringArray(
         OIConstants.kCurrentActiveHub, 
-        Utilities.ShiftFirst() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
+        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
       );
     }));
 

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

@@ -1,6 +1,5 @@
 package frc.robot.constants;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.util.Units;
@@ -13,10 +12,10 @@ public class DrivetrainConstants {
     public static final double kTrackWidth = Units.inchesToMeters(23.75);
     public static final double kWheelBase = Units.inchesToMeters(18.75);
  
-    public static final double kFrontLeftMagEncoderOffset = 2.98;
-    public static final double kFrontRightMagEncoderOffset = 1.18;
-    public static final double kRearLeftMagEncoderOffset = 3.69;
-    public static final double kRearRightMagEncoderOffset = 2.54;
+    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 = 0;
     public static final int kFrontRightDrivingCANID = 3;

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

@@ -0,0 +1,43 @@
+package frc.robot.constants;
+
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+public class HoodConstants {
+    // TODO Real Values
+    public static final int kMotorCANID = 0;
+
+    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 = 0;
+    public static final double kA = 0;
+    public static final double kStartupAngle = 0;
+
+    public static final int kCurrentLimit = 15;
+
+    public static final boolean kInverted = false;
+
+    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 kConfig = new SparkMaxConfig();
+
+    static {
+        kConfig
+            .idleMode(kIdleMode)
+            .inverted(kInverted)
+            .smartCurrentLimit(kCurrentLimit);
+        kConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .positionWrappingEnabled(true)
+            .positionWrappingInputRange(0, Math.PI * 2)
+            .feedForward
+                .sva(kS, kV, kA);
+    }
+}

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

@@ -0,0 +1,69 @@
+package frc.robot.constants;
+
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+public class IntakePivotConstants {
+    // TODO Real values
+    public enum IntakePivotPosition {
+        kUp(0),
+        kDown(0);
+        private double positionRadians;
+
+        private IntakePivotPosition(double positionRadians) {
+            this.positionRadians = positionRadians;
+        }
+
+        public double getPositionRadians() {
+            return positionRadians;
+        }
+    }
+
+    public static final int kLeftMotorCANID = 0;
+    public static final int kRightMotorCANID = 1;
+
+    public static final double kConversionFactor = 0;
+
+    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 = 0;
+    public static final double kA = 0;
+
+    public static final boolean kInvertMotors = false;
+
+    public static final int kCurrentLimit = 30;
+
+    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 KLeftMotorConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig();
+
+    static {
+        KLeftMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kInvertMotors);
+        KLeftMotorConfig.absoluteEncoder
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
+        KLeftMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .positionWrappingEnabled(true)
+            .positionWrappingInputRange(0, 2 * Math.PI)
+            .feedForward
+                .sva(kS, kV, kA);
+
+        kRightMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kInvertMotors)
+            .follow(kLeftMotorCANID);
+    }
+}

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

@@ -0,0 +1,34 @@
+package frc.robot.constants;
+
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+public class IntakeRollerConstants {
+    // TODO Real values
+    public static final int kLeftMotorCANID = 0;
+    public static final int kRightMotorCANID = 0;
+
+    public static final int kCurrentLimit = 30;
+
+    public static final boolean kInvertMotors = false;
+
+    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(kInvertMotors);
+
+        rightMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kInvertMotors)
+            .follow(kLeftMotorCANID);
+    }
+}

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

@@ -1,6 +1,9 @@
 package frc.robot.constants;
 
+import java.util.List;
+
 import edu.wpi.first.math.geometry.Transform3d;
+import frc.robot.utilities.PhotonVisionConfig;
 
 public class PhotonConstants {
     public static final String kCamera1Name = "pv1";
@@ -14,4 +17,11 @@ public class PhotonConstants {
     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/frc/robot/constants/ShooterConstants.java

@@ -0,0 +1,114 @@
+package frc.robot.constants;
+
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import edu.wpi.first.math.util.Units;
+
+public class ShooterConstants {
+    public enum ShooterSpeeds {
+        kHubSpeed(0, 0),
+        kFeedSpeed(0, 0);
+
+        private double frontRollerMPS;
+        private double rearRollerMPS;
+
+        private ShooterSpeeds(double frontRollerMPS, double rearRollerMPS) {
+            this.frontRollerMPS = frontRollerMPS;
+            this.rearRollerMPS = rearRollerMPS;
+        }
+
+        public double getFrontRollerMPS() {
+            return frontRollerMPS;
+        }
+
+        public double getRearRollerMPS() {
+            return rearRollerMPS;
+        }
+    }
+
+    // TODO Conversion factor?
+
+    public static final double kWheelDiameter = Units.inchesToMeters(6);
+
+    // TODO Real values
+    public static final int kFrontShooterMotor1CANID = 0;
+    public static final int kFrontShooterMotor2CANID = 0;
+    public static final int kRearShooterMotor1CANID = 0;
+    public static final int kRearShooterMotor2CANID = 0;
+
+    public static final boolean kFrontShooterMotor1Inverted = false;
+    public static final boolean kFrontShooterMotor2Inverted = false;
+    public static final boolean kRearShooterMotor1Inverted = false;
+    public static final boolean kRearShooterMotor2Inverted = false;
+
+    public static final double kFrontP = 0;
+    public static final double kFrontI = 0;
+    public static final double kFrontD = 0;
+    public static final double kFrontS = 0;
+    public static final double kFrontV = 0;
+    public static final double kFrontA = 0;
+
+    public static final double kRearP = 0;
+    public static final double kRearI = 0;
+    public static final double kRearD = 0;
+    public static final double kRearS = 0;
+    public static final double kRearV = 0;
+    public static final double kRearA = 0;
+
+    // TODO Is this value sane?
+    public static final int kCurrentLimit = 30;
+
+    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 kFrontMotor1Config = new SparkMaxConfig();
+    public static final SparkMaxConfig kFrontMotor2Config = new SparkMaxConfig();
+    public static final SparkMaxConfig kRearMotor1Config = new SparkMaxConfig();
+    public static final SparkMaxConfig kRearMotor2Config = new SparkMaxConfig();
+
+
+    static {
+        kFrontMotor1Config
+            .idleMode(kShooterIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kFrontShooterMotor1Inverted);
+        kFrontMotor1Config.absoluteEncoder
+            .positionConversionFactor(kWheelDiameter * Math.PI)
+            .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
+        kFrontMotor1Config.closedLoop
+            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .pid(kFrontP, kFrontI, kFrontD)
+            .outputRange(-1, 1)
+            .feedForward
+                .sva(kFrontS, kFrontV, kFrontA);
+
+        kFrontMotor2Config
+            .idleMode(kShooterIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kFrontShooterMotor2Inverted)
+            .follow(kFrontShooterMotor1CANID);
+
+        kRearMotor1Config
+            .idleMode(kShooterIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kRearShooterMotor1Inverted);
+        kRearMotor1Config.absoluteEncoder
+            .positionConversionFactor(kWheelDiameter * Math.PI)
+            .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
+        kRearMotor1Config.closedLoop
+            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .pid(kRearP, kRearI, kRearD)
+            .outputRange(-1, 1)
+            .feedForward
+                .sva(kRearS, kRearV, kRearA);
+
+        kRearMotor2Config
+            .idleMode(kShooterIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kRearShooterMotor2Inverted)
+            .follow(kRearShooterMotor1CANID);
+    }
+}

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

@@ -0,0 +1,47 @@
+package frc.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.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+public class SpindexerConstants {
+    // TODO Real values
+    public static final int kSpindexerMotorCANID = 0;
+    public static final int kFeederMotorCANID = 0;
+
+    public static final int kSpindexerStatorCurrentLimit = 80;
+    public static final int kSpindexerSupplyCurrentLimit = 30;
+    public static final int kFeederCurrentLimit = 30;
+
+    public static final boolean kFeederMotorInverted = false;
+
+    public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive;
+    public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Brake;
+
+    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/frc/robot/subsystems/Drivetrain.java

@@ -28,7 +28,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
-import frc.robot.utilities.PhotonVision;
+import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
 import frc.robot.utilities.SwerveModule;
 
 public class Drivetrain extends SubsystemBase {
@@ -48,34 +48,34 @@ public class Drivetrain extends SubsystemBase {
         frontLeft = new SwerveModule(
             "FrontLeft", 
             DrivetrainConstants.kFrontLeftDrivingCANID, 
-            DrivetrainConstants.kFrontLeftTurningCANID);/*, 
+            DrivetrainConstants.kFrontLeftTurningCANID, 
             DrivetrainConstants.kFrontLeftAnalogInPort, 
             DrivetrainConstants.kFrontLeftMagEncoderOffset
-        ); */
+        );
 
         frontRight = new SwerveModule(
             "FrontRight", 
             DrivetrainConstants.kFrontRightDrivingCANID, 
-            DrivetrainConstants.kFrontRightTurningCANID);/*, 
+            DrivetrainConstants.kFrontRightTurningCANID, 
             DrivetrainConstants.kFrontRightAnalogInPort, 
             DrivetrainConstants.kFrontRightMagEncoderOffset
-        );*/
+        );
 
         rearLeft = new SwerveModule(
             "RearLeft", 
             DrivetrainConstants.kRearLeftDrivingCANID, 
-            DrivetrainConstants.kRearLeftTurningCANID);/*, 
+            DrivetrainConstants.kRearLeftTurningCANID, 
             DrivetrainConstants.kRearLeftAnalogInPort, 
             DrivetrainConstants.kRearLeftMagEncoderOffset
-        ); */
+        );
 
         rearRight = new SwerveModule(
             "RearRight", 
             DrivetrainConstants.kRearRightDrivingCANID, 
-            DrivetrainConstants.kRearRightTurningCANID);/*, 
+            DrivetrainConstants.kRearRightTurningCANID, 
             DrivetrainConstants.kRearRightAnalogInPort, 
             DrivetrainConstants.kRearRightMagEncoderOffset
-        ); */
+        );
 
         gyro = new AHRS(NavXComType.kMXP_SPI);
 
@@ -173,7 +173,7 @@ public class Drivetrain extends SubsystemBase {
     }
 
     // TODO check both cameras
-    public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
+    /*public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
         if (camera1 == null) {
             return new PrintCommand("Camera 1 not available");
         }
@@ -197,7 +197,7 @@ public class Drivetrain extends SubsystemBase {
                 () -> false
            )
         );
-    }
+    }*/
 
     public Command drivePathPlannerPath(PathPlannerPath path) {
         if(AutoConstants.kAutoConfigOk) {
@@ -229,6 +229,13 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
+    public void consumeVisualPose(VisualPose pose) {
+        estimator.addVisionMeasurement(
+            pose.visualPose(), 
+            pose.timestamp()
+        );
+    }
+
     public void resetEncoders() {
         frontLeft.resetEncoders();
         frontRight.resetEncoders();

src/main/java/frc/robot/subsystems/Hood.java

@@ -0,0 +1,60 @@
+package frc.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.HoodConstants;
+
+public class Hood extends SubsystemBase {
+    private SparkMax motor;
+
+    private AbsoluteEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    private double currentTargetRadians;
+
+    public Hood() {
+        motor = new SparkMax(HoodConstants.kMotorCANID, MotorType.kBrushless);
+
+        encoder = motor.getAbsoluteEncoder();
+
+        controller = motor.getClosedLoopController();
+
+        currentTargetRadians = HoodConstants.kStartupAngle;
+    }
+
+    @Override
+    public void periodic() {
+        Logger.recordOutput("Hood/CurrentTarget", currentTargetRadians);
+        Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition());
+        Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
+    }
+
+    public Command trackToAngle(DoubleSupplier radianAngleSupplier) {
+        return run(() -> {
+            currentTargetRadians = radianAngleSupplier.getAsDouble();
+
+            controller.setSetpoint(currentTargetRadians, ControlType.kPosition);
+        });
+    }
+
+    public Command stop() {
+        return run(() -> {
+            motor.disable();
+        });
+    }
+
+    public double getTargetRadians() {
+        return currentTargetRadians;
+    }
+}

src/main/java/frc/robot/subsystems/IntakePivot.java

@@ -0,0 +1,79 @@
+package frc.robot.subsystems;
+
+import java.util.Optional;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IntakePivotConstants;
+import frc.robot.constants.IntakePivotConstants.IntakePivotPosition;
+
+public class IntakePivot extends SubsystemBase {
+    private SparkMax leftMotor;
+    private SparkMax rightMotor;
+
+    private AbsoluteEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    private IntakePivotPosition currentTargetPosition;
+
+    public IntakePivot() {
+        leftMotor = new SparkMax(IntakePivotConstants.kLeftMotorCANID, MotorType.kBrushless);
+        rightMotor = new SparkMax(IntakePivotConstants.kRightMotorCANID, MotorType.kBrushless);
+
+        leftMotor.configure(
+            IntakePivotConstants.KLeftMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        rightMotor.configure(
+            IntakePivotConstants.kRightMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        controller = leftMotor.getClosedLoopController();
+
+        encoder = leftMotor.getAbsoluteEncoder();
+    }
+
+    @Override
+    public void periodic() {
+        Logger.recordOutput(
+            "IntakePivot/TargetPosition", 
+            currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians());
+        Logger.recordOutput("IntakePivot/CurrentPosition", encoder.getPosition());
+        Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint());
+    }
+
+    public Command maintainPosition(IntakePivotPosition position) {
+        currentTargetPosition = position;
+
+        return run(() -> {
+            if(currentTargetPosition == null) {
+                leftMotor.disable();
+            } else {
+                controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition);
+            }
+        });
+    }
+
+    public Command stop() {
+        return maintainPosition(null);
+    }
+
+    public Optional<IntakePivotPosition> getCurrentTargetPosition() {
+        return Optional.ofNullable(currentTargetPosition);
+    }
+}

src/main/java/frc/robot/subsystems/IntakeRoller.java

@@ -0,0 +1,51 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.IntakeRollerConstants;
+
+public class IntakeRoller extends SubsystemBase {
+    private SparkMax leftMotor;
+    private SparkMax rightMotor;
+
+    public IntakeRoller() {
+        leftMotor = new SparkMax(IntakeRollerConstants.kLeftMotorCANID, MotorType.kBrushless);
+        rightMotor = new SparkMax(IntakeRollerConstants.kRightMotorCANID, MotorType.kBrushless);
+
+        leftMotor.configure(
+            IntakeRollerConstants.leftMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        rightMotor.configure(
+            IntakeRollerConstants.rightMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+    }
+
+    public Command runIn() {
+        return run(() -> {
+            leftMotor.set(1);
+        });
+    }
+
+    public Command runOut() {
+        return run(() -> {
+            leftMotor.set(-1);
+        });
+    }
+
+    public Command stop() {
+        return run(() -> {
+            leftMotor.set(0);
+        });
+    }
+    
+}

src/main/java/frc/robot/subsystems/PhotonVision.java

@@ -0,0 +1,198 @@
+package frc.robot.subsystems;
+
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+import java.util.Optional;
+import java.util.OptionalDouble;
+import java.util.function.Consumer;
+import java.util.stream.Stream;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonUtils;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.constants.CompetitionConstants;
+import frc.robot.constants.PhotonConstants;
+import frc.robot.interfaces.IAprilTagProvider;
+import frc.robot.interfaces.IVisualPoseProvider;
+import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
+import frc.robot.utilities.PhotonVisionConfig;
+
+public class PhotonVision extends SubsystemBase {
+    private PhotonCamera[] cameras;
+    private PhotonPoseEstimator[] estimators;
+    private List<PhotonPipelineResult> latestResults;
+
+    private ArrayList<Consumer<VisualPose>> poseEstimateConsumers;
+
+    public PhotonVision() {
+        cameras = new PhotonCamera[PhotonConstants.configs.size()];
+        estimators = new PhotonPoseEstimator[PhotonConstants.configs.size()];
+        latestResults = new ArrayList<PhotonPipelineResult>();
+
+        for(int i = 0; i < PhotonConstants.configs.size(); i++) {
+            cameras[i] = new PhotonCamera(PhotonConstants.configs.get(i).cameraName());
+            estimators[i] = new PhotonPoseEstimator(
+                CompetitionConstants.kTagLayout, 
+                PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, 
+                PhotonConstants.configs.get(i).robotToCamera()
+            );
+            latestResults.add(null);
+        }
+
+        poseEstimateConsumers = new ArrayList<Consumer<VisualPose>>();
+    }
+
+    @Override
+    public void periodic() {
+        for(int i = 0; i < cameras.length; i++) {
+            List<PhotonPipelineResult> results = cameras[i].getAllUnreadResults();
+
+            if(!results.isEmpty()) {
+                latestResults.set(i, results.get(results.size() - 1));
+            }
+
+            Optional<EstimatedRobotPose> pose = estimators[i].update(latestResults.get(i));
+
+            if(!pose.isEmpty()) {
+                VisualPose visualPose = new VisualPose(
+                    pose.get().estimatedPose.toPose2d(), 
+                    pose.get().timestampSeconds
+                );
+
+                for(Consumer<VisualPose> consumer: poseEstimateConsumers) {
+                    consumer.accept(visualPose);
+                }
+            }
+        }
+    }
+
+    public void testMethod(int targetID) {
+        Optional<PhotonTrackedTarget> target = latestResults.stream()
+            .filter((p) -> p != null)
+            .map(PhotonPipelineResult::getTargets)
+            .map(List::stream)
+            .reduce(Stream::concat)
+            .get()
+            .filter((p) -> p.getFiducialId() == targetID)
+            .max(
+                Comparator.comparingDouble((ptt) -> {
+                    return (double)ptt.getDetectedObjectConfidence();
+                })
+            );
+
+    }
+
+    public void addPoseEstimateConsumer(Consumer<VisualPose> consumer) {
+        poseEstimateConsumers.add(consumer);
+    }
+
+    /*public Trigger tagPrescenseTrigger(int targetTag) {
+        return new Trigger(() -> {
+            return List.of(latestResults).stream()
+                .filter((p) -> p != null)
+                .anyMatch((p) -> {
+                    return p.getTargets().stream().map(PhotonTrackedTarget::getFiducialId).anyMatch((i) -> {
+                        return i == targetTag;
+                    });
+                });
+            });
+    }*/
+/* 
+    @Override
+    public OptionalDouble getTagDistanceFromCameraByID(int id) {
+        if (latestResult == null) {
+            return OptionalDouble.empty();
+        }
+
+        if (!latestResult.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
+        
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(
+            PhotonUtils.calculateDistanceToTargetMeters(
+                cameraHeightMeters, 
+                CompetitionConstants.kTagLayout.getTagPose(id).get().getZ(),
+                cameraPitchRadians, 
+                Units.degreesToRadians(desiredTarget.get().getPitch()))
+        );
+    }
+
+    @Override
+    public OptionalDouble getTagPitchByID(int id) {
+        if(latestResult == null) {
+            OptionalDouble.empty();
+        }
+        
+        if (!latestResult.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
+
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(
+            desiredTarget.get().getPitch()
+        );
+    }
+
+    @Override
+    public OptionalDouble getTagYawByID(int id) {
+        if(latestResult == null) {
+            OptionalDouble.empty();
+        }
+        
+        if (!latestResult.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
+
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(
+            desiredTarget.get().getYaw()
+        );
+    }
+
+    private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> targets, int id) {
+        for (PhotonTrackedTarget target : targets) {
+            if (target.getFiducialId() == id) {
+                return Optional.of(target);
+            }
+        }
+
+        return Optional.empty();
+    }
+
+    @Override
+    public int[] getVisibleTagIDs() {
+        if(latestResult == null) {
+            return new int[] {};
+        }
+
+        return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
+    }
+    */
+}

src/main/java/frc/robot/subsystems/Shooter.java

@@ -0,0 +1,122 @@
+package frc.robot.subsystems;
+
+import java.util.Optional;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ShooterConstants;
+import frc.robot.constants.ShooterConstants.ShooterSpeeds;
+
+public class Shooter extends SubsystemBase {
+    private SparkMax frontMotor1;
+    private SparkMax frontMotor2;
+    private SparkMax rearMotor1;
+    private SparkMax rearMotor2;
+
+    private AbsoluteEncoder frontEncoder;
+    private AbsoluteEncoder rearEncoder;
+
+    private SparkClosedLoopController frontClosedLoopController;
+    private SparkClosedLoopController rearClosedLoopController;
+
+    private ShooterSpeeds targetSpeeds;
+
+    public Shooter() {
+        frontMotor1 = new SparkMax(ShooterConstants.kFrontShooterMotor1CANID, MotorType.kBrushless);
+        frontMotor2 = new SparkMax(ShooterConstants.kFrontShooterMotor2CANID, MotorType.kBrushless);
+        rearMotor1 = new SparkMax(ShooterConstants.kRearShooterMotor1CANID, MotorType.kBrushless);
+        rearMotor2 = new SparkMax(ShooterConstants.kRearShooterMotor2CANID, MotorType.kBrushless);
+
+        frontMotor1.configure(
+            ShooterConstants.kFrontMotor1Config, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        rearMotor1.configure(
+            ShooterConstants.kRearMotor1Config, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        frontMotor2.configure(
+            ShooterConstants.kFrontMotor2Config, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        rearMotor2.configure(
+            ShooterConstants.kRearMotor2Config, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        frontEncoder = frontMotor1.getAbsoluteEncoder();
+        rearEncoder = rearMotor1.getAbsoluteEncoder();
+
+        frontClosedLoopController = frontMotor1.getClosedLoopController();
+        rearClosedLoopController = rearMotor1.getClosedLoopController();
+
+        // TODO Set this to the initial startup speed
+        targetSpeeds = null;
+    }
+
+    @Override
+    public void periodic() {
+        Logger.recordOutput(
+            "Shooter/FrontRollers/TargetMPS", 
+            targetSpeeds == null ? 0 : targetSpeeds.getFrontRollerMPS()
+        );
+
+        Logger.recordOutput(
+            "Shooter/RearRollers/TargetMPS", 
+            targetSpeeds == null ? 0 : targetSpeeds.getRearRollerMPS()
+        );
+
+        Logger.recordOutput("Shooter/FrontRollers/CurrentMPS", frontEncoder.getVelocity());
+        Logger.recordOutput("Shooter/RearRollers/CurrentMPS", rearEncoder.getVelocity());
+
+        // TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance?
+        Logger.recordOutput("Shooter/FrontRollers/AtSetpoint", frontClosedLoopController.isAtSetpoint());
+        Logger.recordOutput("Shooter/RearRollers/AtSetpoint", rearClosedLoopController.isAtSetpoint());
+    }
+
+    public Command maintainSpeed(ShooterSpeeds speeds) {
+        targetSpeeds = speeds;
+
+        return run(() -> {
+            if(targetSpeeds == null) {
+                frontMotor1.disable();
+                rearMotor1.disable();
+            } else {
+                frontClosedLoopController.setSetpoint(
+                    targetSpeeds.getFrontRollerMPS(), 
+                    ControlType.kVelocity
+                );
+
+                rearClosedLoopController.setSetpoint(
+                    targetSpeeds.getRearRollerMPS(), 
+                    ControlType.kVelocity
+                );
+            }
+        });
+    }
+
+    public Command stop() {
+        return maintainSpeed(null);
+    }
+
+    public Optional<ShooterSpeeds> getTargetSpeeds() {
+        return Optional.ofNullable(targetSpeeds);
+    }
+}

src/main/java/frc/robot/subsystems/Spindexer.java

@@ -0,0 +1,59 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.SpindexerConstants;
+
+public class Spindexer extends SubsystemBase {
+    private TalonFX spindexerMotor;
+
+    private SparkMax feederMotor;
+
+    private DutyCycleOut spindexerMotorOutput;
+
+    public Spindexer() {
+        spindexerMotor = new TalonFX(SpindexerConstants.kSpindexerMotorCANID);
+
+        feederMotor = new SparkMax(SpindexerConstants.kFeederMotorCANID, MotorType.kBrushless);
+
+        spindexerMotor.getConfigurator().apply(SpindexerConstants.kSpindexerCurrentLimitConfig);
+        spindexerMotor.getConfigurator().apply(SpindexerConstants.kSpindexerMotorConfig);
+
+        feederMotor.configure(
+            SpindexerConstants.kFeederConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        spindexerMotorOutput = new DutyCycleOut(0);
+    }
+
+    public Command spinToShooter() {
+        return run(() -> {
+            spindexerMotor.setControl(spindexerMotorOutput.withOutput(1));
+            feederMotor.set(1);
+        });
+    }
+
+    public Command spinToIntake() {
+        return run(() -> {
+            spindexerMotor.setControl(spindexerMotorOutput.withOutput(-1));
+            feederMotor.set(-1);
+        });
+    }
+
+    public Command stop() {
+        return run(() -> {
+            spindexerMotor.setControl(spindexerMotorOutput.withOutput(0));
+            feederMotor.set(0);
+        });
+    }
+    
+}

src/main/java/frc/robot/utilities/PhotonVision.java

@@ -1,165 +0,0 @@
-package frc.robot.utilities;
-
-import java.io.IOException;
-import java.util.List;
-import java.util.Optional;
-import java.util.OptionalDouble;
-
-import org.photonvision.EstimatedRobotPose;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.PhotonUtils;
-import org.photonvision.targeting.PhotonPipelineResult;
-import org.photonvision.targeting.PhotonTrackedTarget;
-
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.util.Units;
-import frc.robot.constants.CompetitionConstants;
-import frc.robot.interfaces.IAprilTagProvider;
-import frc.robot.interfaces.IVisualPoseProvider;
-
-public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
-
-    private final PhotonCamera camera;
-
-    private final PhotonPoseEstimator photonPoseEstimator;
-
-    private final double cameraHeightMeters;
-    private final double cameraPitchRadians;
-
-    private PhotonPipelineResult latestResult;
-
-    public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException {
-        camera = new PhotonCamera(cameraName);
-
-        photonPoseEstimator = new PhotonPoseEstimator(
-            CompetitionConstants.kTagLayout, 
-            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, 
-            robotToCam
-        );
-
-        this.cameraHeightMeters = cameraHeightMeters;
-        this.cameraPitchRadians = cameraPitchRadians;
-
-        this.latestResult = null;
-    }
-
-    // TODO This periodic method has to be called from somewhere, even though the cameras
-    // could be used in multiple other subsystems
-    public void periodic() {
-        // TODO Do we care about missed results? Probably not, if we're taking long enough to miss results something else is wrong
-        List<PhotonPipelineResult> results = camera.getAllUnreadResults();
-
-        if(!results.isEmpty()) {
-            latestResult = results.get(results.size() - 1);
-        }
-    }
-
-    @Override
-    public Optional<VisualPose> getVisualPose() {
-        if(latestResult == null) {
-            return Optional.empty();
-        }
-
-        Optional<EstimatedRobotPose> pose = photonPoseEstimator.update(latestResult);
-
-        if (pose.isEmpty()) {
-            return Optional.empty();
-        }
-
-        return Optional.of(
-            new VisualPose(
-                pose.get().estimatedPose.toPose2d(), 
-                pose.get().timestampSeconds
-            )
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagDistanceFromCameraByID(int id) {
-        if (latestResult == null) {
-            return OptionalDouble.empty();
-        }
-
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-        
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            PhotonUtils.calculateDistanceToTargetMeters(
-                cameraHeightMeters, 
-                CompetitionConstants.kTagLayout.getTagPose(id).get().getZ(),
-                cameraPitchRadians, 
-                Units.degreesToRadians(desiredTarget.get().getPitch()))
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagPitchByID(int id) {
-        if(latestResult == null) {
-            OptionalDouble.empty();
-        }
-        
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            desiredTarget.get().getPitch()
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagYawByID(int id) {
-        if(latestResult == null) {
-            OptionalDouble.empty();
-        }
-        
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            desiredTarget.get().getYaw()
-        );
-    }
-
-    private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> targets, int id) {
-        for (PhotonTrackedTarget target : targets) {
-            if (target.getFiducialId() == id) {
-                return Optional.of(target);
-            }
-        }
-
-        return Optional.empty();
-    }
-
-    @Override
-    public int[] getVisibleTagIDs() {
-        if(latestResult == null) {
-            return new int[] {};
-        }
-
-        return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
-    }
-    
-}

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

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

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

@@ -0,0 +1,23 @@
+package frc.robot.utilities;
+
+import java.util.function.DoubleSupplier;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class SparkMAXTester extends SubsystemBase {
+    private SparkMax spark;
+
+    public SparkMAXTester(int deviceID) {
+        spark = new SparkMax(deviceID, MotorType.kBrushless);
+    }
+
+    public Command setSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            spark.set(speed.getAsDouble());
+        });
+    }
+}

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

@@ -12,7 +12,6 @@ import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -135,6 +134,7 @@ public class SwerveModule {
     }
 
     public void periodic() {
+        
         if(!isAbsoluteEncoderDisabled) {
             Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
         }

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

@@ -4,28 +4,22 @@ import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
 public class Utilities {
-    static String gameData;
-
-    static Boolean Blue = false;
-
-    static Boolean Red = false;
-
     public static final double kG = -9.81;
     
-    public static Alliance ShiftFirst() {
-        gameData = DriverStation.getGameSpecificMessage();
-
+    public static Alliance whoHasFirstShift() {
+        String gameData = DriverStation.getGameSpecificMessage();
 
         if(gameData.length() > 0) {
             switch (gameData.charAt(0)) {
-            case 'B' :
-                return Alliance.Red;
-            case 'R' :
-                return Alliance.Blue;
-            default :
-            return null;
+                case 'B':
+                    return Alliance.Red;
+                case 'R':
+                    return Alliance.Blue;
+                default:
+                    return null;
             }
         }
+        
         return null;
     }