diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 05ef9f3..94ec8c9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/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); diff --git a/src/main/java/frc/robot/subsystems/intake/enums/IntakeGamepieces.java b/src/main/java/frc/robot/subsystems/intake/enums/IntakeGamepiece.java similarity index 66% rename from src/main/java/frc/robot/subsystems/intake/enums/IntakeGamepieces.java rename to src/main/java/frc/robot/subsystems/intake/enums/IntakeGamepiece.java index 3a87c37..4fccfd5 100644 --- a/src/main/java/frc/robot/subsystems/intake/enums/IntakeGamepieces.java +++ b/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 } diff --git a/src/main/java/frc/robot/subsystems/intake/enums/IntakeScoreType.java b/src/main/java/frc/robot/subsystems/intake/enums/IntakeScoreType.java deleted file mode 100644 index f1fe54e..0000000 --- a/src/main/java/frc/robot/subsystems/intake/enums/IntakeScoreType.java +++ /dev/null @@ -1,14 +0,0 @@ -package frc.robot.subsystems.intake.enums; - -public enum IntakeScoreType { - HIGH_CONE, - HIGH_CONE_AUTO, - HIGH_CUBE, - HIGH_CUBE_AUTO, - MID_CONE, - MID_CONE_TIPPED, - MID_CUBE, - MID_CUBE_AUTO, - LOW, - LOW_AUTO -} diff --git a/src/main/java/frc/robot/subsystems/intake/states/ScoringState.java b/src/main/java/frc/robot/subsystems/intake/states/ScoringState.java new file mode 100644 index 0000000..3260774 --- /dev/null +++ b/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; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighCone.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighCone.java new file mode 100644 index 0000000..2bbe772 --- /dev/null +++ b/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"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighConeAuto.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/HighConeAuto.java new file mode 100644 index 0000000..f36850d --- /dev/null +++ b/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"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidCone.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidCone.java new file mode 100644 index 0000000..2e582e2 --- /dev/null +++ b/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"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidConeTipped.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cone/MidConeTipped.java new file mode 100644 index 0000000..f545e37 --- /dev/null +++ b/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"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCube.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCube.java new file mode 100644 index 0000000..3f1ce58 --- /dev/null +++ b/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"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCubeAuto.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/HighCubeAuto.java new file mode 100644 index 0000000..0592eed --- /dev/null +++ b/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"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCube.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCube.java new file mode 100644 index 0000000..40758a9 --- /dev/null +++ b/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"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCubeAuto.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/cube/MidCubeAuto.java new file mode 100644 index 0000000..022b8e6 --- /dev/null +++ b/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"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/level/Low.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/level/Low.java new file mode 100644 index 0000000..daad78d --- /dev/null +++ b/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"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/states/scoring/level/LowAuto.java b/src/main/java/frc/robot/subsystems/intake/states/scoring/level/LowAuto.java new file mode 100644 index 0000000..5135b0b --- /dev/null +++ b/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"); + } +}