Rename IntakeGamepieces enum to IntakeGamepiece

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

@@ -6,8 +6,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 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.enums.IntakeGamepiece;
+import frc.robot.subsystems.intake.states.ScoringState;
 
 /**
  * This an example implementation of our intake subsystem from 2023.
@@ -52,91 +52,18 @@ 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, IntakeGamepiece 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());
-              }
-            }
-          }
+    return run(() -> {
+          Commands.runOnce(
+                  () -> outtakeCommand(scoringState.getOuttakeSpeed(expectedGamepiece)), this)
+              .andThen(Commands.waitSeconds(0.5))
+              .andThen(stopIntakeCommand());
         })
         .andThen(stopIntakeCommand());
   }
@@ -147,7 +74,7 @@ public Command intakeScoreCommand(IntakeScoreType type, IntakeGamepieces expecte
    * @param gamepiece the type of gamepiece to expect
    * @return a command that forces the intake to hold the specified gamepiece
    */
-  public Command intakeHoldCommand(IntakeGamepieces gamepiece) {
+  public Command intakeHoldCommand(IntakeGamepiece gamepiece) {
     return run(() -> {
           this.intakeMotor.set(IntakeConstants.INTAKING_SPEED);
 
@@ -158,12 +85,12 @@ public Command intakeHoldCommand(IntakeGamepieces gamepiece) {
                   this.pdp.getCurrent(IntakeConstants.INTAKE_MOTOR_CHANNEL)
                       < IntakeConstants.INTAKE_AMP_THRESHOLD);
 
-          if (gamepiece.equals(IntakeGamepieces.CUBE)) {
+          if (gamepiece.equals(IntakeGamepiece.CUBE)) {
             // wait a short amount of time so the gamepiece gets pulled in
             Commands.waitSeconds(IntakeConstants.INTAKE_CUBE_DELAY);
             this.intakeMotor.set(IntakeConstants.HOLD_CUBE_SPEED);
           }
-          if (gamepiece.equals(IntakeGamepieces.CONE)) {
+          if (gamepiece.equals(IntakeGamepiece.CONE)) {
             // we have a cone, so run the motor at a higher speed
             Commands.waitSeconds(IntakeConstants.INTAKE_CONE_DELAY);
             this.intakeMotor.set(IntakeConstants.HOLD_CONE_SPEED);

src/main/java/frc/robot/subsystems/intake/enums/IntakeGamepieces.java → src/main/java/frc/robot/subsystems/intake/enums/IntakeGamepiece.java

@@ -1,6 +1,6 @@
 package frc.robot.subsystems.intake.enums;
 
-public enum IntakeGamepieces {
+public enum IntakeGamepiece {
   CONE,
   CUBE
 }

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

@@ -0,0 +1,66 @@
+package frc.robot.subsystems.intake.states;
+
+import frc.robot.subsystems.intake.enums.IntakeGamepiece;
+
+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(IntakeGamepiece 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,14 @@
+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");
+  }
+}