Adding a few subsystems, not sure I like the use of periodic for the shooter and intake pivot subsystems

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

@@ -94,28 +94,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/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 leftMotorConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig();
+
+    static {
+        leftMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kInvertMotors);
+        leftMotorConfig.absoluteEncoder
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
+        leftMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .positionWrappingEnabled(true)
+            .positionWrappingInputRange(0, 2 * Math.PI)
+            .feedForward
+                .sva(kS, kV, kA);
+
+        rightMotorConfig
+            .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/SpindexerConstants.java

@@ -0,0 +1,32 @@
+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;
+
+public class SpindexerConstants {
+    // TODO Real values
+    public static final int kMotorCANID = 0;
+
+    public static final int kStatorCurrentLimit = 80;
+    public static final int kSupplyCurrentLimit = 30;
+
+    public static final InvertedValue kInversionState = InvertedValue.Clockwise_Positive;
+    public static final NeutralModeValue kIdleMode = NeutralModeValue.Brake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final CurrentLimitsConfigs kCurrentLimitConfig = new CurrentLimitsConfigs();
+    public static final MotorOutputConfigs kMotorConfig = new MotorOutputConfigs();
+    
+    static {
+        kCurrentLimitConfig.StatorCurrentLimitEnable = true;
+        kCurrentLimitConfig.SupplyCurrentLimitEnable = true;
+        kCurrentLimitConfig.StatorCurrentLimit = kStatorCurrentLimit;
+        kCurrentLimitConfig.SupplyCurrentLimit = kSupplyCurrentLimit;
+
+        kMotorConfig.Inverted = kInversionState;
+        kMotorConfig.NeutralMode = kIdleMode;
+    }
+}

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

@@ -0,0 +1,82 @@
+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.leftMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        rightMotor.configure(
+            IntakePivotConstants.rightMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        controller = leftMotor.getClosedLoopController();
+
+        encoder = leftMotor.getAbsoluteEncoder();
+    }
+
+    @Override
+    public void periodic() {
+        if(currentTargetPosition == null) {
+            leftMotor.disable();
+            rightMotor.disable();
+        } else {
+            controller.setSetpoint(currentTargetPosition.getPositionRadians(), ControlType.kPosition);
+        }
+
+        Logger.recordOutput(
+            "IntakePivot/TargetPosition", 
+            currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians());
+        Logger.recordOutput("IntakePivot/CurrentPosition", encoder.getPosition());
+        Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint());
+    }
+
+    public Command setTargetPosition(IntakePivotPosition position) {
+        return runOnce(() -> {
+            currentTargetPosition = position;
+        });
+    }
+
+    public Command stop() {
+        return runOnce(() -> {
+            currentTargetPosition = null;
+        });
+    }
+
+    public Optional<IntakePivotPosition> getCurrentTargetPosition() {
+        return Optional.of(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/Spindexer.java

@@ -0,0 +1,42 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+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 motor;
+
+    private DutyCycleOut motorOutput;
+
+    public Spindexer() {
+        motor = new TalonFX(SpindexerConstants.kMotorCANID);
+
+        motor.getConfigurator().apply(SpindexerConstants.kCurrentLimitConfig);
+        motor.getConfigurator().apply(SpindexerConstants.kMotorConfig);
+
+        motorOutput = new DutyCycleOut(0);
+    }
+
+    public Command spinToShooter() {
+        return run(() -> {
+            motor.setControl(motorOutput.withOutput(1));
+        });
+    }
+
+    public Command spinToIntake() {
+        return run(() -> {
+            motor.setControl(motorOutput.withOutput(-1));
+        });
+    }
+
+    public Command stop() {
+        return run(() -> {
+            motor.setControl(motorOutput.withOutput(0));
+        });
+    }
+    
+}

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;
     }