diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 62c5884..485ccc2 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -22,7 +22,7 @@ public class Arm extends Subsystem { /** Arm singleton. */ private static Arm instance = null; - /** Shoulder config. */ + /** Shoulder controller config. */ private final MechanismConfig shoulderConfig = new MechanismConfig() .withAbsoluteEncoderConfig( @@ -81,9 +81,8 @@ private Arm() { previousTimeSeconds = Timer.getFPGATimestamp(); - setPosition(ArmState.STOW_POSITION); - setpoint = ArmState.STOW_POSITION; - goal = ArmState.STOW_POSITION; + setpoint = ArmState.STOWED; + goal = ArmState.STOWED; } /** diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java index d464b01..c734930 100644 --- a/src/main/java/frc/robot/arm/ArmState.java +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -10,19 +10,19 @@ public record ArmState(State shoulderRotations) { /** Stow position. */ - public static final ArmState STOW_POSITION = new ArmState(Rotation2d.fromDegrees(-26)); + public static final ArmState STOWED = new ArmState(Rotation2d.fromDegrees(-26)); /** Subwoofer shot position. */ - public static final ArmState SUBWOOFER_POSITION = new ArmState(Rotation2d.fromDegrees(-26)); + public static final ArmState SUBWOOFER = new ArmState(Rotation2d.fromDegrees(-26)); /** Eject position. */ - public static final ArmState EJECT_POSITION = new ArmState(Rotation2d.fromDegrees(30)); + public static final ArmState EJECT = new ArmState(Rotation2d.fromDegrees(30)); /** Skim shot position. */ - public static final ArmState SKIM_POSITION = new ArmState(Rotation2d.fromDegrees(30)); + public static final ArmState SKIM = new ArmState(Rotation2d.fromDegrees(30)); /** Amp position. */ - public static final ArmState AMP_POSITION = new ArmState(Rotation2d.fromDegrees(60)); + public static final ArmState AMP = new ArmState(Rotation2d.fromDegrees(60)); /** * Arm state. diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index 591c843..c41cdd2 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -57,8 +57,6 @@ private Auto() { NamedCommands.registerCommand("stow", superstructure.stow()); NamedCommands.registerCommand( "shoot", superstructure.subwoofer().withTimeout(1.5)); // 1 second could work - NamedCommands.registerCommand( - "shootNoPull", superstructure.subwooferNoPull().withTimeout(1.5)); // 1 second could work NamedCommands.registerCommand("intake", superstructure.intakeInstant()); autoChooser = AutoBuilder.buildAutoChooser(); diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 865f63b..7b5f0e6 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -17,7 +17,7 @@ public class Intake extends Subsystem { /** Intake singleton. */ private static Intake instance = null; - /** Front roller config. */ + /** Front roller controller config. */ private final MechanismConfig frontRollerConfig = new MechanismConfig() .withMotorConfig( @@ -38,7 +38,7 @@ public class Intake extends Subsystem { .withProportionalGain(0.1) // volts per rotation per second ); - /** Back roller config. */ + /** Back roller controller config. */ private final MechanismConfig backRollerConfig = new MechanismConfig() .withMotorConfig( @@ -83,8 +83,8 @@ private Intake() { backRollerValues = new VelocityControllerIOValues(); - setpoint = IntakeState.IDLING; - goal = IntakeState.IDLING; + setpoint = IntakeState.IDLE; + goal = IntakeState.IDLE; } /** diff --git a/src/main/java/frc/robot/intake/IntakeState.java b/src/main/java/frc/robot/intake/IntakeState.java index bf9c54c..4011a86 100644 --- a/src/main/java/frc/robot/intake/IntakeState.java +++ b/src/main/java/frc/robot/intake/IntakeState.java @@ -8,10 +8,10 @@ public record IntakeState( double frontRollerVelocityRotationsPerSecond, double backRollerVelocityRotationsPerSecond) { /** Idling state. */ - public static final IntakeState IDLING = new IntakeState(0, 0); + public static final IntakeState IDLE = new IntakeState(0, 0); /** Intaking state. */ - public static final IntakeState INTAKING = new IntakeState(34.0, 34.0); + public static final IntakeState INTAKE = new IntakeState(34.0, 34.0); /** Ejecting state. */ public static final IntakeState EJECTING = new IntakeState(-34.0, -34.0); diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index 3d96121..c23bdc2 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -18,6 +18,7 @@ public class Shooter extends Subsystem { /** Shooter singleton. */ private static Shooter instance = null; + /** Flywheel controller config. */ private final MechanismConfig flywheelConfig = new MechanismConfig() .withMotorConfig( @@ -49,7 +50,7 @@ public class Shooter extends Subsystem { /** Flywheel controller acceleration limiter. */ private final SlewRateLimiter flywheelAccelerationLimiter; - /** Serializer config. */ + /** Serializer controller config. */ private final MechanismConfig serializerConfig = new MechanismConfig() .withMotorConfig( diff --git a/src/main/java/frc/robot/shooter/ShooterFactory.java b/src/main/java/frc/robot/shooter/ShooterFactory.java index 7ceb30a..4b01269 100644 --- a/src/main/java/frc/robot/shooter/ShooterFactory.java +++ b/src/main/java/frc/robot/shooter/ShooterFactory.java @@ -13,28 +13,28 @@ public class ShooterFactory { /** - * Creates the serializer controller. + * Creates the flywheel controller. * - * @param config the serializer controller config. - * @return the serializer controller. + * @param config the flywheel controller config. + * @return the flywheel controller. */ - public static VelocityControllerIO createSerializer(MechanismConfig config) { + public static VelocityControllerIO createFlywheel(MechanismConfig config) { if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) { - return new VelocityControllerIOTalonFXPIDF(new CAN(42), config); + return new VelocityControllerIOTalonFXPIDF(new CAN(44), config); } return new VelocityControllerIOSim(); } /** - * Creates the flywheel controller. + * Creates the serializer controller. * - * @param config the flywheel controller config. - * @return the flywheel controller. + * @param config the serializer controller config. + * @return the serializer controller. */ - public static VelocityControllerIO createFlywheel(MechanismConfig config) { + public static VelocityControllerIO createSerializer(MechanismConfig config) { if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) { - return new VelocityControllerIOTalonFXPIDF(new CAN(44), config); + return new VelocityControllerIOTalonFXPIDF(new CAN(42), config); } return new VelocityControllerIOSim(); diff --git a/src/main/java/frc/robot/shooter/ShooterState.java b/src/main/java/frc/robot/shooter/ShooterState.java index 0014f97..5f67235 100644 --- a/src/main/java/frc/robot/shooter/ShooterState.java +++ b/src/main/java/frc/robot/shooter/ShooterState.java @@ -11,22 +11,22 @@ public record ShooterState( public static final ShooterState IDLING = new ShooterState(0, 0); /** Intaking state. */ - public static final ShooterState INTAKING = new ShooterState(0, 34); + public static final ShooterState INTAKE = new ShooterState(0, 34); /** Pulling state. */ - public static final ShooterState PULLING = new ShooterState(-20, -10); + public static final ShooterState PULL = new ShooterState(-20, -10); /** Ejecting state. */ - public static final ShooterState EJECTING = new ShooterState(0, -44); + public static final ShooterState EJECT = new ShooterState(0, -44); /** Subwoofer shooting state. */ - public static final ShooterState SUBWOOFER_SHOOTING = new ShooterState(60, 44); + public static final ShooterState SUBWOOFER = new ShooterState(60, 44); /** Skim shooting state. */ - public static final ShooterState SKIM_SHOOTING = new ShooterState(60, 44); + public static final ShooterState SKIM = new ShooterState(60, 44); /** Amp shooting state. */ - public static final ShooterState AMPING = new ShooterState(10, 20); + public static final ShooterState AMP = new ShooterState(10, 20); /** * Shooter state. diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index ca6ed8b..d58014e 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -14,38 +14,42 @@ import frc.robot.shooter.ShooterState; import java.util.function.Supplier; -/** Subsystem class for the superstructure subsystem. */ +/** Superstructure subsystem. */ public class Superstructure extends Subsystem { - /** Instance variable for the superstructure subsystem singleton. */ + /** Superstructure singleton. */ private static Superstructure instance = null; - /** Reference to the arm subsystem. */ + /** Arm subsystem reference. */ private final Arm arm; - /** Reference to the intake subsystem. */ + /** Intake subsystem reference. */ private final Intake intake; - /** Reference to the shooter subsystem. */ + /** Shooter subsystem reference. */ private final Shooter shooter; - private SuperstructureState measurement, goal; + /** Superstructure measurement. Updated periodically. */ + private SuperstructureState measurement; - /** Creates a new instance of the superstructure subsystem. */ + /** Superstructure goal. */ + private SuperstructureState goal; + + /** Initializes the superstructure subsystem. */ private Superstructure() { arm = Arm.getInstance(); intake = Intake.getInstance(); shooter = Shooter.getInstance(); - setPosition(SuperstructureState.STOW); + arm.setPosition(SuperstructureState.STOWED.armState()); - goal = SuperstructureState.STOW; + goal = SuperstructureState.STOWED; } /** - * Gets the instance of the superstructure subsystem. + * Returns the superstructure subsystem instance. * - * @return the instance of the superstructre subsystem. + * @return the superstructure subsystem instance. */ public static Superstructure getInstance() { if (instance == null) { @@ -75,6 +79,13 @@ public void addToShuffleboard(ShuffleboardTab tab) { Telemetry.addColumn(tab, "At Goal?").addBoolean("At Goal?", this::atGoal); } + /** + * Adds a superstructure state to Shuffleboard. + * + * @param tab the Shuffleboard tab. + * @param name the name of the superstructure state. + * @param state the state to display on Shuffleboard. + */ private void addStateToShuffleboard( ShuffleboardTab tab, String name, Supplier state) { ShuffleboardLayout layout = Telemetry.addColumn(tab, name); @@ -103,10 +114,11 @@ private void addStateToShuffleboard( () -> state.get().shooterState().serializerVelocityRotationsPerSecond()); } - public void setPosition(SuperstructureState state) { - arm.setPosition(state.armState()); - } - + /** + * Sets the superstructure goal. + * + * @param goal the superstructure goal. + */ public void setGoal(SuperstructureState goal) { this.goal = goal; @@ -115,96 +127,170 @@ public void setGoal(SuperstructureState goal) { shooter.setGoal(goal.shooterState()); } + /** + * Returns true if the superstructure is at the goal state. + * + * @return true if the superstructure is at the goal state. + */ public boolean atGoal() { return arm.atGoal() && intake.atGoal() && shooter.atGoal(); } + /** + * Sets the superstructure goal and ends immediately. + * + * @param goal the superstructure goal. + * @return a command that sets the superstructure goal and ends immediately. + */ private Command instant(SuperstructureState goal) { return runOnce(() -> setGoal(goal)); } - private Command hold(SuperstructureState goal) { - return run(() -> setGoal(goal)); - } - + /** + * Sets the superstructure goal and ends when the goal is reached. + * + * @param goal the superstructure goal. + * @return a command that sets the superstructure goal and ends when the goal is reached. + */ private Command to(SuperstructureState goal) { return run(() -> setGoal(goal)).until(this::atGoal); } + /** + * Sets the superstructure goal and ends when interrupted. + * + * @param goal the superstructure goal. + * @return a command that sets the superstructure goal and ends when interrupted. + */ + private Command hold(SuperstructureState goal) { + return run(() -> setGoal(goal)); + } + + /** + * Stows the superstructure. Ends when interrupted. + * + * @return a command that stows the superstructure. + */ public Command stow() { - return hold(SuperstructureState.STOW).withName("STOW"); + return hold(SuperstructureState.STOWED).withName("STOWED"); } + /** + * Stows then intakes. Ends when interrupted. + * + * @return a command that stows then intakes. + */ public Command intake() { - return to(SuperstructureState.STOW) + return to(SuperstructureState.STOWED) .andThen(hold(SuperstructureState.INTAKE)) .withName("INTAKE"); } + /** + * Intakes. Ends immediately. + * + * @return a command that intakes. + */ public Command intakeInstant() { return instant(SuperstructureState.INTAKE).withName("INTAKE_IMMEDIATE"); } + /** + * Pulls a note while moving to the shot position. Ends after a duration is elapsed. + * + * @param shot the shot position to move to. + * @return a command that pulls a note while moving to the shot position. + */ private Command pull(SuperstructureState shot) { final SuperstructureState pull = - new SuperstructureState(shot.armState(), IntakeState.IDLING, ShooterState.PULLING); + new SuperstructureState(shot.armState(), IntakeState.IDLE, ShooterState.PULL); - return hold(pull).withTimeout(SuperstructureConstants.PULL_DURATION); + return hold(pull).withTimeout(0.2); } + /** + * Spins up and moves to a shot but does not fire it. Ends after the shot is spun up and in + * position or a duration is elapsed. + * + * @param shot the shot to spin up for. + * @return a commands that spins up and moves to a shot but does not fire it. + */ private Command ready(SuperstructureState shot) { final ShooterState spin = new ShooterState(shot.shooterState().flywheelVelocityRotationsPerSecond(), 0); final SuperstructureState ready = - new SuperstructureState(shot.armState(), IntakeState.IDLING, spin); + new SuperstructureState(shot.armState(), IntakeState.IDLE, spin); - return to(ready).withTimeout(SuperstructureConstants.READY_DURATION); + return to(ready).withTimeout(1.0); } - private Command autoShoot(SuperstructureState shot) { - return pull(shot) - .andThen(ready(shot)) - .andThen(Commands.waitSeconds(SuperstructureConstants.READY_PAUSE_DURATION)) - .andThen(hold(shot)); + /** + * Prepares a shot. Ends after the shot is spun up and in position or a duration is elapsed. + * + * @param shot the shot. + * @return a command that prepares a shot. + */ + public Command prepare(SuperstructureState shot) { + return pull(shot).andThen(ready(shot)); } - private Command autoShootNoPull(SuperstructureState shot) { - return ready(shot) - .andThen(Commands.waitSeconds(SuperstructureConstants.READY_PAUSE_DURATION)) - .andThen(hold(shot)); + /** + * Readies and shoots a shot. Ends when interrupted. + * + * @param shot the shot. + * @return a command that readies and shoots a shot. + */ + public Command shoot(SuperstructureState shot) { + return ready(shot).andThen(Commands.waitSeconds(0.25)).andThen(hold(shot)); } - public Command subwooferNoPull() { - return autoShootNoPull(SuperstructureState.SUBWOOFER).withName("SUBWOOFER_NO_PULL"); + /** + * Pulls, readies, and shoots a shot. Ends when interrupted. + * + * @param shot the shot. + * @return a command that pulls, readies, and shoots a shot. + */ + private Command autoShoot(SuperstructureState shot) { + return pull(shot).andThen(shoot(shot)); } + /** + * Automatically shoots a note into the speaker from the subwoofer. Ends when interrupted. + * + * @return a command that automatically shoots a note into the speaker from the subwoofer. + */ public Command subwoofer() { return autoShoot(SuperstructureState.SUBWOOFER).withName("SUBWOOFER"); } + /** + * Automatically skims a note across the floor. Ends when interrupted. + * + * @return a command that automatically skims a note across the floor. + */ public Command skim() { return autoShoot(SuperstructureState.SKIM).withName("SKIM"); } + /** + * Automatically drops a note into the amp. Ends when interrupted. + * + * @return a command that drops a note into the amp. + */ public Command amp() { return autoShoot(SuperstructureState.AMP).withName("AMP"); } + /** + * Ejects a note. Ends when interrupted. + * + * @return a command that ejects a note. + */ public Command eject() { return to(SuperstructureState.EJECT_POSITION) - .andThen(Commands.waitSeconds(SuperstructureConstants.EJECT_PAUSE_DURATION)) + .andThen(Commands.waitSeconds(0.25)) .andThen(hold(SuperstructureState.EJECT)) - .withName("EJECT"); - } - - public Command prepare(SuperstructureState shot) { - return pull(shot).andThen(ready(shot)); - } - - public Command shoot(SuperstructureState shot) { - return ready(shot) - .andThen(Commands.waitSeconds(SuperstructureConstants.READY_PAUSE_DURATION)) - .andThen(hold(shot)); + .withName("EJECTING"); } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/superstructure/SuperstructureConstants.java deleted file mode 100644 index 3ae60ef..0000000 --- a/src/main/java/frc/robot/superstructure/SuperstructureConstants.java +++ /dev/null @@ -1,12 +0,0 @@ -package frc.robot.superstructure; - -public class SuperstructureConstants { - - public static final double PULL_DURATION = 0.2; - - public static final double READY_DURATION = 1.0; - - public static final double READY_PAUSE_DURATION = 0.25; - - public static final double EJECT_PAUSE_DURATION = 0.25; -} diff --git a/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java b/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java index 2ee1f20..97a241b 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureMechanism.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.util.Color8Bit; import frc.robot.RobotConstants; -/** Helper class for rendering superstructure mechanisms. */ +/** Superstructure mechanism renderer. */ public class SuperstructureMechanism { private static SuperstructureMechanism instance = null; diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index d35ac70..e631769 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -5,32 +5,37 @@ import frc.robot.shooter.ShooterState; import java.util.Objects; -/** Represents the state of the superstructure. */ +/** Superstructure state. */ public record SuperstructureState( ArmState armState, IntakeState intakeState, ShooterState shooterState) { - public static final SuperstructureState STOW = - new SuperstructureState(ArmState.STOW_POSITION, IntakeState.IDLING, ShooterState.IDLING); + /** Stowed state. */ + public static final SuperstructureState STOWED = + new SuperstructureState(ArmState.STOWED, IntakeState.IDLE, ShooterState.IDLING); + /** Intaking state. */ public static final SuperstructureState INTAKE = - new SuperstructureState(ArmState.STOW_POSITION, IntakeState.INTAKING, ShooterState.INTAKING); + new SuperstructureState(ArmState.STOWED, IntakeState.INTAKE, ShooterState.INTAKE); + /** Ready ejecting state. */ public static final SuperstructureState EJECT_POSITION = - new SuperstructureState(ArmState.EJECT_POSITION, IntakeState.EJECTING, ShooterState.IDLING); + new SuperstructureState(ArmState.EJECT, IntakeState.EJECTING, ShooterState.IDLING); + /** Ejecting state. */ public static final SuperstructureState EJECT = - new SuperstructureState(ArmState.EJECT_POSITION, IntakeState.EJECTING, ShooterState.EJECTING); + new SuperstructureState(ArmState.EJECT, IntakeState.EJECTING, ShooterState.EJECT); + /** Subwoofer shooting state. */ public static final SuperstructureState SUBWOOFER = - new SuperstructureState( - ArmState.SUBWOOFER_POSITION, IntakeState.IDLING, ShooterState.SUBWOOFER_SHOOTING); + new SuperstructureState(ArmState.SUBWOOFER, IntakeState.IDLE, ShooterState.SUBWOOFER); + /** Skim shooting state. */ public static final SuperstructureState SKIM = - new SuperstructureState( - ArmState.SKIM_POSITION, IntakeState.IDLING, ShooterState.SKIM_SHOOTING); + new SuperstructureState(ArmState.SKIM, IntakeState.IDLE, ShooterState.SKIM); + /** Amp shooting state. */ public static final SuperstructureState AMP = - new SuperstructureState(ArmState.AMP_POSITION, IntakeState.IDLING, ShooterState.AMPING); + new SuperstructureState(ArmState.AMP, IntakeState.IDLE, ShooterState.AMP); /** * Creates a new superstructure state.