diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 7579279..865f63b 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -11,7 +11,7 @@ import frc.lib.controller.VelocityControllerIO; import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues; -/** Subsystem class for the intake subsystem. */ +/** Intake subsystem. */ public class Intake extends Subsystem { /** Intake singleton. */ @@ -83,8 +83,8 @@ private Intake() { backRollerValues = new VelocityControllerIOValues(); - setpoint = IntakeState.IDLE; - goal = IntakeState.IDLE; + setpoint = IntakeState.IDLING; + goal = IntakeState.IDLING; } /** diff --git a/src/main/java/frc/robot/intake/IntakeState.java b/src/main/java/frc/robot/intake/IntakeState.java index 75c0b8f..bf9c54c 100644 --- a/src/main/java/frc/robot/intake/IntakeState.java +++ b/src/main/java/frc/robot/intake/IntakeState.java @@ -7,8 +7,8 @@ public record IntakeState( double frontRollerVelocityRotationsPerSecond, double backRollerVelocityRotationsPerSecond) { - /** Idle state. */ - public static final IntakeState IDLE = new IntakeState(0, 0); + /** Idling state. */ + public static final IntakeState IDLING = new IntakeState(0, 0); /** Intaking state. */ public static final IntakeState INTAKING = 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 2bfdbf2..3d96121 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -1,51 +1,112 @@ package frc.robot.shooter; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.Subsystem; +import frc.lib.config.FeedbackControllerConfig; +import frc.lib.config.FeedforwardControllerConfig; +import frc.lib.config.MechanismConfig; +import frc.lib.config.MotionProfileConfig; +import frc.lib.config.MotorConfig; import frc.lib.controller.VelocityControllerIO; import frc.lib.controller.VelocityControllerIO.VelocityControllerIOValues; -import frc.robot.shooter.ShooterConstants.FlywheelConstants; -import frc.robot.shooter.ShooterConstants.SerializerConstants; -/** Subsystem class for the shooter subsystem. */ +/** Shooter subsystem. */ public class Shooter extends Subsystem { - /** Instance variable for the shooter subsystem singleton. */ + /** Shooter singleton. */ private static Shooter instance = null; - /** Serializer. */ + private final MechanismConfig flywheelConfig = + new MechanismConfig() + .withMotorConfig( + new MotorConfig() + .withCCWPositive(false) + .withNeutralBrake(true) + .withMotorToMechanismRatio(28.0 / 16.0)) + .withMotionProfileConfig( + new MotionProfileConfig() + .withMaximumVelocity(60) // rotations per second + .withMaximumAcceleration(200) // rotations per second per second + ) + .withFeedforwardConfig( + new FeedforwardControllerConfig() + .withStaticFeedforward(0.14) // volts + .withVelocityFeedforward(0.2) // volts per rotation per second + ) + .withFeedbackConfig( + new FeedbackControllerConfig() + .withProportionalGain(0.14) // volts per rotation per second + ); + + /** Flywheel controller. */ + private final VelocityControllerIO flywheel; + + /** Flywheel controller values. */ + private final VelocityControllerIOValues flywheelValues; + + /** Flywheel controller acceleration limiter. */ + private final SlewRateLimiter flywheelAccelerationLimiter; + + /** Serializer config. */ + private final MechanismConfig serializerConfig = + new MechanismConfig() + .withMotorConfig( + new MotorConfig() + .withCCWPositive(true) + .withNeutralBrake(false) + .withMotorToMechanismRatio(36.0 / 16.0)) + .withMotionProfileConfig( + new MotionProfileConfig() + .withMaximumVelocity(45) // rotations per second + .withMaximumAcceleration(450) // rotations per second per second + ) + .withFeedforwardConfig( + new FeedforwardControllerConfig() + .withStaticFeedforward(0.14) // volts + .withVelocityFeedforward(0.2617) // volts per rotation per second + ); + + /** Serializer controller. */ private final VelocityControllerIO serializer; - /** Serializer values. */ + /** Serializer controller values. */ private final VelocityControllerIOValues serializerValues; - /** Flywheel. */ - private final VelocityControllerIO flywheel; + /** Serializer controller acceleration limiter. */ + private final SlewRateLimiter serializerAccelerationLimiter; - /** Flywheel values. */ - private final VelocityControllerIOValues flywheelValues; + /** Shooter goal. Set by superstructure. */ + private ShooterState goal; - private ShooterState setpoint, goal; + /** Shooter setpoint. Updated periodically to reach goal within constraints. */ + private ShooterState setpoint; - /** Creates a new instance of the shooter subsystem. */ + /** Initializes the shooter subsystem and configures shooter hardware. */ private Shooter() { - serializer = ShooterFactory.createSerializer(); - serializerValues = new VelocityControllerIOValues(); - serializer.configure(); + flywheel = ShooterFactory.createFlywheel(flywheelConfig); + flywheel.configure(); - flywheel = ShooterFactory.createFlywheel(); flywheelValues = new VelocityControllerIOValues(); - flywheel.configure(); - setpoint = ShooterState.IDLE; - goal = ShooterState.IDLE; + flywheelAccelerationLimiter = flywheelConfig.motionProfileConfig().createRateLimiter(); + + serializer = ShooterFactory.createSerializer(serializerConfig); + serializer.configure(); + + serializerValues = new VelocityControllerIOValues(); + + serializerAccelerationLimiter = serializerConfig.motionProfileConfig().createRateLimiter(); + + setpoint = ShooterState.IDLING; + goal = ShooterState.IDLING; } /** - * Gets the instance of the shooter subsystem. + * Returns the shooter subsystem instance. * - * @return the instance of the shooter subsystem. + * @return the shooter subsystem instance. */ public static Shooter getInstance() { if (instance == null) { @@ -57,17 +118,15 @@ public static Shooter getInstance() { @Override public void periodic() { - serializer.update(serializerValues); flywheel.update(flywheelValues); + serializer.update(serializerValues); setpoint = goal; double flywheelSetpoint = - FlywheelConstants.ACCELERATION_LIMITER.calculate( - setpoint.flywheelVelocityRotationsPerSecond()); + flywheelAccelerationLimiter.calculate(setpoint.flywheelVelocityRotationsPerSecond()); double serializerSetpoint = - SerializerConstants.ACCELERATION_LIMITER.calculate( - setpoint.serializerVelocityRotationsPerSecond()); + serializerAccelerationLimiter.calculate(setpoint.serializerVelocityRotationsPerSecond()); flywheel.setSetpoint(flywheelSetpoint); serializer.setSetpoint(serializerSetpoint); @@ -75,30 +134,55 @@ public void periodic() { @Override public void addToShuffleboard(ShuffleboardTab tab) { - VelocityControllerIO.addToShuffleboard(tab, "Serializer", serializerValues); VelocityControllerIO.addToShuffleboard(tab, "Flywheel", flywheelValues); + VelocityControllerIO.addToShuffleboard(tab, "Serializer", serializerValues); tab.addBoolean("Serializer Current Spike?", this::serializerCurrentSpike); } - private boolean serializerCurrentSpike() { - return serializerValues.motorAmps > SerializerConstants.NOTE_AMPS; - } - - public Trigger serializedNote() { - return new Trigger(this::serializerCurrentSpike); - } - + /** + * Returns the shooter state. + * + * @return the shooter state. + */ public ShooterState getState() { return new ShooterState( flywheelValues.velocityRotationsPerSecond, serializerValues.velocityRotationsPerSecond); } + /** + * Sets the shooter goal state. + * + * @param goal the shooter goal state. + */ public void setGoal(ShooterState goal) { this.goal = goal; } + /** + * Returns true if the shooter is at the goal state. + * + * @return true if the shooter is at the goal state. + */ public boolean atGoal() { return getState().at(goal); } + + /** + * Returns a trigger for if a note is serialized. + * + * @return a trigger for if a note is serialized. + */ + public Trigger serializedNote() { + return new Trigger(this::serializerCurrentSpike); + } + + /** + * Returns true if the serializer has a current spike. + * + * @return true if the serializer has a current spike. + */ + private boolean serializerCurrentSpike() { + return serializerValues.motorAmps > 20.0; + } } diff --git a/src/main/java/frc/robot/shooter/ShooterConstants.java b/src/main/java/frc/robot/shooter/ShooterConstants.java deleted file mode 100644 index 983d8a4..0000000 --- a/src/main/java/frc/robot/shooter/ShooterConstants.java +++ /dev/null @@ -1,103 +0,0 @@ -package frc.robot.shooter; - -import edu.wpi.first.math.filter.SlewRateLimiter; -import frc.lib.CAN; -import frc.lib.config.FeedbackControllerConfig; -import frc.lib.config.FeedforwardControllerConfig; -import frc.lib.config.MechanismConfig; -import frc.lib.config.MotionProfileConfig; -import frc.lib.config.MotorConfig; - -/** Constants for the shooter subsystem. */ -public class ShooterConstants { - - /** Constants for the serializer motor used in the shooter subsystem. */ - public static class SerializerConstants { - /** Serializer's CAN. */ - public static final CAN CAN = new CAN(42); - - /** Serializer's config. */ - public static final MechanismConfig CONFIG = - new MechanismConfig() - .withMotorConfig( - new MotorConfig() - .withCCWPositive(true) - .withNeutralBrake(false) - .withMotorToMechanismRatio(36.0 / 16.0)) - .withMotionProfileConfig( - new MotionProfileConfig() - .withMaximumVelocity(45) // rotations per second - .withMaximumAcceleration(450) // rotations per second per second - ) - .withFeedforwardConfig( - new FeedforwardControllerConfig() - .withStaticFeedforward(0.14) // volts - .withVelocityFeedforward(0.2617) // volts per rotation per second - ); - - public static final double INTAKE_SPEED = 34; - - public static final double PULL_SPEED = -10; - - public static final double EJECT_SPEED = -44; - - public static final double SLOW_FEED_SPEED = 20; - - public static final double FAST_FEED_SPEED = 44; - - public static final SlewRateLimiter ACCELERATION_LIMITER = - CONFIG.motionProfileConfig().createRateLimiter(); - - public static final double NOTE_AMPS = 20; - - public static final double TOLERANCE = 5; - } - - /** Constants for the flywheel motor used in the shooter subsystem. */ - public static class FlywheelConstants { - /** Flywheel's CAN. */ - public static final CAN CAN = new CAN(44); - - /** Flywheel's config. */ - public static final MechanismConfig CONFIG = - new MechanismConfig() - .withMotorConfig( - new MotorConfig() - .withCCWPositive(false) - .withNeutralBrake(true) - .withMotorToMechanismRatio(28.0 / 16.0)) - .withMotionProfileConfig( - new MotionProfileConfig() - .withMaximumVelocity(60) // rotations per second - .withMaximumAcceleration(200) // rotations per second per second - ) - .withFeedforwardConfig( - new FeedforwardControllerConfig() - .withStaticFeedforward(0.14) // volts - .withVelocityFeedforward(0.2) // volts per rotation per second - ) - .withFeedbackConfig( - new FeedbackControllerConfig() - .withProportionalGain(0.14) // volts per rotation per second - ); - - public static final double PULL_SPEED = -20; - - public static final double SPEAKER_SPEED = 60; - - public static final double PODIUM_SPEED = 60; - - public static final double LOB_SPEED = 60; - - public static final double SKIM_SPEED = 60; - - public static final double AMP_SPEED = 10; - - public static final double BLOOP_SPEED = 30; - - public static final SlewRateLimiter ACCELERATION_LIMITER = - CONFIG.motionProfileConfig().createRateLimiter(); - - public static final double TOLERANCE = 5; - } -} diff --git a/src/main/java/frc/robot/shooter/ShooterFactory.java b/src/main/java/frc/robot/shooter/ShooterFactory.java index 3155e1e..7ceb30a 100644 --- a/src/main/java/frc/robot/shooter/ShooterFactory.java +++ b/src/main/java/frc/robot/shooter/ShooterFactory.java @@ -1,39 +1,40 @@ package frc.robot.shooter; +import frc.lib.CAN; +import frc.lib.config.MechanismConfig; import frc.lib.controller.VelocityControllerIO; import frc.lib.controller.VelocityControllerIOSim; import frc.lib.controller.VelocityControllerIOTalonFXPIDF; import frc.robot.Robot; import frc.robot.RobotConstants; import frc.robot.RobotConstants.Subsystem; -import frc.robot.shooter.ShooterConstants.FlywheelConstants; -import frc.robot.shooter.ShooterConstants.SerializerConstants; -/** Helper class for creating hardware for the shooter subsystem. */ +/** Factory for creating shooter subsystem hardware. */ public class ShooterFactory { /** - * Creates a serializer. + * Creates the serializer controller. * - * @return a serializer. + * @param config the serializer controller config. + * @return the serializer controller. */ - public static VelocityControllerIO createSerializer() { + public static VelocityControllerIO createSerializer(MechanismConfig config) { if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) { - return new VelocityControllerIOTalonFXPIDF( - SerializerConstants.CAN, SerializerConstants.CONFIG); + return new VelocityControllerIOTalonFXPIDF(new CAN(42), config); } return new VelocityControllerIOSim(); } /** - * Creates a flywheel. + * Creates the flywheel controller. * - * @return a flywheel. + * @param config the flywheel controller config. + * @return the flywheel controller. */ - public static VelocityControllerIO createFlywheel() { + public static VelocityControllerIO createFlywheel(MechanismConfig config) { if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.SHOOTER)) { - return new VelocityControllerIOTalonFXPIDF(FlywheelConstants.CAN, FlywheelConstants.CONFIG); + return new VelocityControllerIOTalonFXPIDF(new CAN(44), 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 440a4ea..0014f97 100644 --- a/src/main/java/frc/robot/shooter/ShooterState.java +++ b/src/main/java/frc/robot/shooter/ShooterState.java @@ -1,44 +1,54 @@ package frc.robot.shooter; import edu.wpi.first.math.MathUtil; -import frc.robot.shooter.ShooterConstants.FlywheelConstants; -import frc.robot.shooter.ShooterConstants.SerializerConstants; import java.util.Objects; +/** Shooter state. */ public record ShooterState( double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) { - public static final ShooterState IDLE = new ShooterState(0, 0); + /** Idling state. */ + public static final ShooterState IDLING = new ShooterState(0, 0); - public static final ShooterState INTAKE = new ShooterState(0, SerializerConstants.INTAKE_SPEED); + /** Intaking state. */ + public static final ShooterState INTAKING = new ShooterState(0, 34); - public static final ShooterState PULL = - new ShooterState(FlywheelConstants.PULL_SPEED, SerializerConstants.PULL_SPEED); + /** Pulling state. */ + public static final ShooterState PULLING = new ShooterState(-20, -10); - public static final ShooterState EJECT = new ShooterState(0, SerializerConstants.EJECT_SPEED); + /** Ejecting state. */ + public static final ShooterState EJECTING = new ShooterState(0, -44); - public static final ShooterState SUBWOOFER = - new ShooterState(FlywheelConstants.SPEAKER_SPEED, SerializerConstants.FAST_FEED_SPEED); + /** Subwoofer shooting state. */ + public static final ShooterState SUBWOOFER_SHOOTING = new ShooterState(60, 44); - public static final ShooterState SKIM = - new ShooterState(FlywheelConstants.SKIM_SPEED, SerializerConstants.FAST_FEED_SPEED); + /** Skim shooting state. */ + public static final ShooterState SKIM_SHOOTING = new ShooterState(60, 44); - public static final ShooterState AMP = - new ShooterState(FlywheelConstants.AMP_SPEED, SerializerConstants.SLOW_FEED_SPEED); + /** Amp shooting state. */ + public static final ShooterState AMPING = new ShooterState(10, 20); + /** + * Shooter state. + * + * @param flywheelVelocityRotationsPerSecond flywheel velocity in rotations per second. + * @param serializerVelocityRotationsPerSecond serializer velocity in rotations per second. + */ public ShooterState { Objects.requireNonNull(flywheelVelocityRotationsPerSecond); Objects.requireNonNull(serializerVelocityRotationsPerSecond); } + /** + * Returns true if this shooter state is at another shooter state. + * + * @param other another shooter state. + * @return true if this shooter state is at another shooter state. + */ public boolean at(ShooterState other) { return MathUtil.isNear( - flywheelVelocityRotationsPerSecond, - other.flywheelVelocityRotationsPerSecond, - FlywheelConstants.TOLERANCE) + flywheelVelocityRotationsPerSecond, other.flywheelVelocityRotationsPerSecond, 5.0) && MathUtil.isNear( - serializerVelocityRotationsPerSecond, - other.serializerVelocityRotationsPerSecond, - SerializerConstants.TOLERANCE); + serializerVelocityRotationsPerSecond, other.serializerVelocityRotationsPerSecond, 5.0); } } diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index a9b761b..ca6ed8b 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -147,7 +147,7 @@ public Command intakeInstant() { private Command pull(SuperstructureState shot) { final SuperstructureState pull = - new SuperstructureState(shot.armState(), IntakeState.IDLE, ShooterState.PULL); + new SuperstructureState(shot.armState(), IntakeState.IDLING, ShooterState.PULLING); return hold(pull).withTimeout(SuperstructureConstants.PULL_DURATION); } @@ -157,7 +157,7 @@ private Command ready(SuperstructureState shot) { new ShooterState(shot.shooterState().flywheelVelocityRotationsPerSecond(), 0); final SuperstructureState ready = - new SuperstructureState(shot.armState(), IntakeState.IDLE, spin); + new SuperstructureState(shot.armState(), IntakeState.IDLING, spin); return to(ready).withTimeout(SuperstructureConstants.READY_DURATION); } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index a9acc18..d35ac70 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -10,26 +10,27 @@ public record SuperstructureState( ArmState armState, IntakeState intakeState, ShooterState shooterState) { public static final SuperstructureState STOW = - new SuperstructureState(ArmState.STOW_POSITION, IntakeState.IDLE, ShooterState.IDLE); + new SuperstructureState(ArmState.STOW_POSITION, IntakeState.IDLING, ShooterState.IDLING); public static final SuperstructureState INTAKE = - new SuperstructureState(ArmState.STOW_POSITION, IntakeState.INTAKING, ShooterState.INTAKE); + new SuperstructureState(ArmState.STOW_POSITION, IntakeState.INTAKING, ShooterState.INTAKING); public static final SuperstructureState EJECT_POSITION = - new SuperstructureState(ArmState.EJECT_POSITION, IntakeState.EJECTING, ShooterState.IDLE); + new SuperstructureState(ArmState.EJECT_POSITION, IntakeState.EJECTING, ShooterState.IDLING); public static final SuperstructureState EJECT = - new SuperstructureState(ArmState.EJECT_POSITION, IntakeState.EJECTING, ShooterState.EJECT); + new SuperstructureState(ArmState.EJECT_POSITION, IntakeState.EJECTING, ShooterState.EJECTING); public static final SuperstructureState SUBWOOFER = new SuperstructureState( - ArmState.SUBWOOFER_POSITION, IntakeState.IDLE, ShooterState.SUBWOOFER); + ArmState.SUBWOOFER_POSITION, IntakeState.IDLING, ShooterState.SUBWOOFER_SHOOTING); public static final SuperstructureState SKIM = - new SuperstructureState(ArmState.SKIM_POSITION, IntakeState.IDLE, ShooterState.SKIM); + new SuperstructureState( + ArmState.SKIM_POSITION, IntakeState.IDLING, ShooterState.SKIM_SHOOTING); public static final SuperstructureState AMP = - new SuperstructureState(ArmState.AMP_POSITION, IntakeState.IDLE, ShooterState.AMP); + new SuperstructureState(ArmState.AMP_POSITION, IntakeState.IDLING, ShooterState.AMPING); /** * Creates a new superstructure state.