Moved scoring states for intake to separate classes

src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java

@@ -7,7 +7,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.intake.enums.IntakeGamepieces;
-import frc.robot.subsystems.intake.enums.IntakeScoreType;
+import frc.robot.subsystems.intake.states.ScoringState;
 
 /**
  * This an example implementation of our intake subsystem from 2023.
@@ -52,93 +52,19 @@ public Command stopIntakeCommand() {
   /**
    * Runs a command to score a gamepiece.
    *
-   * @param type the type of the score you want to make
+   * @param scoringState the state to run the intake in
    * @param expectedPiece the type of gamepiece to expect when scoring
    * @return a command that scores a gamepiece
    */
-  public Command intakeScoreCommand(IntakeScoreType type, IntakeGamepieces expectedPiece) {
-    return run(() -> {
-          switch (type) {
-            case MID_CONE -> {
-              Commands.runOnce(() -> outtakeCommand(IntakeConstants.OuttakeSpeeds.MID_CONE), this)
-                  .andThen(Commands.waitSeconds(0.5))
-                  .andThen(stopIntakeCommand());
-            }
-
-            case MID_CONE_TIPPED -> {
-              Commands.runOnce(
-                      () -> outtakeCommand(IntakeConstants.OuttakeSpeeds.MID_CONE_TIPPED), this)
-                  .andThen(Commands.waitSeconds(0.5))
-                  .andThen(stopIntakeCommand());
-            }
-
-            case MID_CUBE -> {
-              Commands.runOnce(() -> outtakeCommand(IntakeConstants.OuttakeSpeeds.MID_CUBE), this)
-                  .andThen(Commands.waitSeconds(0.5))
-                  .andThen(stopIntakeCommand());
-            }
-
-            case MID_CUBE_AUTO -> {
-              Commands.runOnce(
-                      () -> outtakeCommand(IntakeConstants.OuttakeSpeeds.MID_CUBE_AUTO), this)
-                  .andThen(Commands.waitSeconds(0.5))
-                  .andThen(stopIntakeCommand());
-            }
-
-            case HIGH_CONE -> {
-              Commands.runOnce(() -> outtakeCommand(IntakeConstants.OuttakeSpeeds.HIGH_CONE), this)
-                  .andThen(Commands.waitSeconds(0.5))
-                  .andThen(stopIntakeCommand());
-            }
-
-            case HIGH_CONE_AUTO -> {
-              Commands.runOnce(
-                      () -> outtakeCommand(IntakeConstants.OuttakeSpeeds.HIGH_CONE_AUTO), this)
-                  .andThen(Commands.waitSeconds(0.5))
-                  .andThen(stopIntakeCommand());
-            }
+  public Command intakeScoreCommand(ScoringState scoringState, IntakeGamepieces expectedGamepiece) {
+    SmartDashboard.putString("INTAKE STATE", scoringState.getStateName());
 
-            case HIGH_CUBE -> {
-              Commands.runOnce(() -> outtakeCommand(IntakeConstants.OuttakeSpeeds.HIGH_CUBE), this)
-                  .andThen(Commands.waitSeconds(0.5))
-                  .andThen(stopIntakeCommand());
-            }
-
-            case HIGH_CUBE_AUTO -> {
-              Commands.runOnce(
-                      () -> outtakeCommand(IntakeConstants.OuttakeSpeeds.HIGH_CUBE_AUTO), this)
-                  .andThen(Commands.waitSeconds(0.5))
-                  .andThen(stopIntakeCommand());
-            }
-
-            case LOW -> {
-              if (expectedPiece.equals(IntakeGamepieces.CUBE)) {
-                Commands.runOnce(() -> outtakeCommand(IntakeConstants.OuttakeSpeeds.LOW_CUBE), this)
-                    .andThen(Commands.waitSeconds(1))
-                    .andThen(stopIntakeCommand());
-              } else {
-                Commands.runOnce(() -> outtakeCommand(IntakeConstants.OuttakeSpeeds.LOW_CONE), this)
-                    .andThen(Commands.waitSeconds(1))
-                    .andThen(stopIntakeCommand());
-              }
-            }
-
-            case LOW_AUTO -> {
-              if (expectedPiece.equals(IntakeGamepieces.CUBE)) {
-                Commands.runOnce(
-                        () -> outtakeCommand(IntakeConstants.OuttakeSpeeds.LOW_CUBE_AUTO), this)
-                    .andThen(Commands.waitSeconds(1))
-                    .andThen(stopIntakeCommand());
-              } else {
-                Commands.runOnce(
-                        () -> outtakeCommand(IntakeConstants.OuttakeSpeeds.LOW_CONE_AUTO), this)
-                    .andThen(Commands.waitSeconds(1))
-                    .andThen(stopIntakeCommand());
-              }
-            }
-          }
-        })
-        .andThen(stopIntakeCommand());
+    return run(() -> {
+      Commands.runOnce(
+        () -> outtakeCommand(scoringState.getOuttakeSpeed(expectedGamepiece)), this)
+      .andThen(Commands.waitSeconds(0.5))
+      .andThen(stopIntakeCommand());
+    }).andThen(stopIntakeCommand());
   }
 
   /**

src/main/java/frc/robot/subsystems/intake/states/ScoringState.java

@@ -0,0 +1,61 @@
+package frc.robot.subsystems.intake.states;
+
+import frc.robot.subsystems.intake.enums.IntakeGamepieces;
+
+public abstract class ScoringState {
+    double outtakeSpeed;
+    double cubeOuttakeSpeed;
+    double coneOuttakeSpeed;
+
+    String stateName;
+
+    /**
+     * Creates a scoring state with the same outtake speed for cubes and cones
+     * @param outtakeSpeed The speed that should be used to outtake the gamepiece
+     * @param stateName The name of the scoring state that should be used for logging
+     */
+    protected ScoringState(double outtakeSpeed, String stateName) {
+        this.outtakeSpeed = outtakeSpeed;
+        this.stateName = stateName;
+    }
+
+    /**
+     * Creates a scoring state with different outtake speeds for cubes and cones
+     * @param cubeOuttakeSpeed The speed that should be used to outtake the cube gamepiece
+     * @param coneOuttakeSpeed The speed that should be used to outtake the cone gamepiece
+     * @param stateName The name of the scoring state that should be used for logging
+     */
+    protected ScoringState(double cubeOuttakeSpeed, double coneOuttakeSpeed, String stateName) {
+        this.cubeOuttakeSpeed = cubeOuttakeSpeed;
+        this.coneOuttakeSpeed = coneOuttakeSpeed;
+        this.stateName = stateName;
+    }
+
+    /**
+     * Gets the name of the scoring state
+     * @return The name of the scoring state
+     */
+    public String getStateName() {
+        return this.stateName;
+    }
+
+    /**
+     * Gets the outtake speed for the scoring state depending on the gamepiece that is expected to be scored
+     * @return The outtake speed for the scoring state
+     */
+    public double getOuttakeSpeed(IntakeGamepieces expectedGamepiece) {
+        if (expectedGamepiece == null) {
+            return this.outtakeSpeed;
+        }
+
+        // Based on the gamepiece that is expected to be scored, return the appropriate outtake speed
+        switch(expectedGamepiece) {
+            case CUBE:
+                return this.cubeOuttakeSpeed;
+            case CONE:
+                return this.coneOuttakeSpeed;
+            default:
+                return 0.0;
+        }
+    }
+}

src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighCone.java

@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cone;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class HighCone extends ScoringState {
+
+    HighCone() {
+        super(IntakeConstants.OuttakeSpeeds.HIGH_CONE, "High Cone");
+    }
+}

src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighConeAuto.java

@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cone;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class HighConeAuto extends ScoringState {
+
+    HighConeAuto() {
+        super(IntakeConstants.OuttakeSpeeds.HIGH_CONE_AUTO, "High Cone Auto");
+    }
+}

src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidCone.java

@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cone;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class MidCone extends ScoringState {
+
+    MidCone() {
+        super(IntakeConstants.OuttakeSpeeds.MID_CONE, "Mid Cone");
+    }
+}

src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidConeTipped.java

@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cone;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class MidConeTipped extends ScoringState {
+
+    MidConeTipped() {
+        super(IntakeConstants.OuttakeSpeeds.MID_CONE_TIPPED, "Mid Cone Tipped");
+    }
+}

src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCube.java

@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cube;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class HighCube extends ScoringState {
+
+    HighCube() {
+        super(IntakeConstants.OuttakeSpeeds.HIGH_CUBE, "High Cube");
+    }
+}

src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCubeAuto.java

@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cube;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class HighCubeAuto extends ScoringState {
+
+    HighCubeAuto() {
+        super(IntakeConstants.OuttakeSpeeds.HIGH_CUBE, "High Cube Auto");
+    }
+}

src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCube.java

@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cube;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class MidCube extends ScoringState {
+
+    MidCube() {
+        super(IntakeConstants.OuttakeSpeeds.MID_CUBE, "Mid Cube");
+    }
+}

src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCubeAuto.java

@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.cube;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class MidCubeAuto extends ScoringState {
+
+    MidCubeAuto() {
+        super(IntakeConstants.OuttakeSpeeds.MID_CUBE, "Mid Cube Auto");
+    }
+}

src/main/java/frc/robot/subsystems/intake/states/scoring/level/Low.java

@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.level;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class Low extends ScoringState {
+
+    Low() {
+        super(IntakeConstants.OuttakeSpeeds.LOW_CUBE, IntakeConstants.OuttakeSpeeds.LOW_CONE, "Low");
+    }
+}

src/main/java/frc/robot/subsystems/intake/states/scoring/level/LowAuto.java

@@ -0,0 +1,11 @@
+package frc.robot.subsystems.intake.states.scoring.level;
+
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.states.ScoringState;
+
+public class LowAuto extends ScoringState {
+
+    LowAuto() {
+        super(IntakeConstants.OuttakeSpeeds.LOW_CUBE_AUTO, IntakeConstants.OuttakeSpeeds.LOW_CONE_AUTO, "Low Auto");
+    }
+}