From 372b84719f41db8bb0b496e3dd072c19004a3b78 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Fri, 26 Apr 2024 17:05:34 -0400 Subject: [PATCH] refactor: Continue WIP. --- src/main/java/frc/robot/RobotContainer.java | 4 +- src/main/java/frc/robot/arm/Arm.java | 28 ++-- src/main/java/frc/robot/arm/ArmFactory.java | 6 +- src/main/java/frc/robot/arm/ArmState.java | 34 ++++- src/main/java/frc/robot/auto/Auto.java | 33 ++--- src/main/java/frc/robot/intake/Intake.java | 127 ++++++++++++++---- .../frc/robot/intake/IntakeConstants.java | 68 ---------- .../java/frc/robot/intake/IntakeFactory.java | 14 +- .../java/frc/robot/intake/IntakeState.java | 30 +++-- .../superstructure/SuperstructureState.java | 15 ++- 10 files changed, 196 insertions(+), 163 deletions(-) delete mode 100644 src/main/java/frc/robot/intake/IntakeConstants.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6efa492..5a0024c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -107,11 +107,11 @@ public Command rumble(RumbleType side) { } /** - * Gets the command to run during the autonomous period. + * Returns the command to run during the autonomous period. * * @return the command to run during the autonomous period. */ public Command getAutonomousCommand() { - return auto.getAutonomousCommand(); + return auto.getSelectedCommand(); } } diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 596d9f0..62c5884 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -19,10 +19,10 @@ /** Arm subsystem. */ public class Arm extends Subsystem { - /** Arm subsystem singleton. */ + /** Arm singleton. */ private static Arm instance = null; - /** Shoulder's config. */ + /** Shoulder config. */ private final MechanismConfig shoulderConfig = new MechanismConfig() .withAbsoluteEncoderConfig( @@ -58,10 +58,10 @@ public class Arm extends Subsystem { /** Motion profile of the shoulder. */ private final TrapezoidProfile shoulderMotionProfile; - /** Arm's goal. Set by superstructure. */ + /** Arm goal. Set by superstructure. */ private ArmState goal; - /** Arm's setpoint. Updated periodically to reach goal within constraints. */ + /** Arm setpoint. Updated periodically to reach goal within constraints. */ private ArmState setpoint; /** @@ -70,7 +70,7 @@ public class Arm extends Subsystem { */ private double previousTimeSeconds; - /** Creates the arm subsystem and configures arm hardware. */ + /** Initializes the arm subsystem and configures arm hardware. */ private Arm() { shoulder = ArmFactory.createShoulder(shoulderConfig); shoulder.configure(); @@ -81,15 +81,15 @@ private Arm() { previousTimeSeconds = Timer.getFPGATimestamp(); - setPosition(ArmState.STOW); - setpoint = ArmState.STOW; - goal = ArmState.STOW; + setPosition(ArmState.STOW_POSITION); + setpoint = ArmState.STOW_POSITION; + goal = ArmState.STOW_POSITION; } /** - * Gets the instance of the arm subsystem. + * Returns the arm subsystem instance. * - * @return the instance of the arm subsystem. + * @return the arm subsystem instance. */ public static Arm getInstance() { if (instance == null) { @@ -130,9 +130,9 @@ public void addToShuffleboard(ShuffleboardTab tab) { } /** - * Returns the arm's state. + * Returns the arm state. * - * @return the arm's state. + * @return the arm state. */ public ArmState getState() { return new ArmState( @@ -141,9 +141,9 @@ public ArmState getState() { } /** - * Sets the arm's goal state. + * Sets the arm goal state. * - * @param goal the arm's goal state. + * @param goal the arm goal state. */ public void setGoal(ArmState goal) { this.goal = goal; diff --git a/src/main/java/frc/robot/arm/ArmFactory.java b/src/main/java/frc/robot/arm/ArmFactory.java index 2ccce8b..191eca5 100644 --- a/src/main/java/frc/robot/arm/ArmFactory.java +++ b/src/main/java/frc/robot/arm/ArmFactory.java @@ -9,13 +9,13 @@ import frc.robot.RobotConstants; import frc.robot.RobotConstants.Subsystem; -/** Helper class for creating hardware for the arm subsystem. */ +/** Factory for creating arm subsystem hardware. */ public class ArmFactory { /** - * Creates a shoulder motor. + * Creates the shoulder controller. * - * @return a shoulder motor. + * @return the shoulder controller. */ public static PositionControllerIO createShoulder(MechanismConfig config) { if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.ARM)) { diff --git a/src/main/java/frc/robot/arm/ArmState.java b/src/main/java/frc/robot/arm/ArmState.java index a3cae85..d464b01 100644 --- a/src/main/java/frc/robot/arm/ArmState.java +++ b/src/main/java/frc/robot/arm/ArmState.java @@ -6,28 +6,48 @@ import edu.wpi.first.math.util.Units; import java.util.Objects; +/** Arm state. */ public record ArmState(State shoulderRotations) { - public static final ArmState STOW = new ArmState(Rotation2d.fromDegrees(-26)); + /** Stow position. */ + public static final ArmState STOW_POSITION = new ArmState(Rotation2d.fromDegrees(-26)); - public static final ArmState SUBWOOFER = new ArmState(Rotation2d.fromDegrees(-26)); + /** Subwoofer shot position. */ + public static final ArmState SUBWOOFER_POSITION = new ArmState(Rotation2d.fromDegrees(-26)); - public static final ArmState EJECT = new ArmState(Rotation2d.fromDegrees(30)); + /** Eject position. */ + public static final ArmState EJECT_POSITION = new ArmState(Rotation2d.fromDegrees(30)); - public static final ArmState SKIM = new ArmState(Rotation2d.fromDegrees(30)); + /** Skim shot position. */ + public static final ArmState SKIM_POSITION = new ArmState(Rotation2d.fromDegrees(30)); - public static final ArmState LOB = new ArmState(Rotation2d.fromDegrees(-26)); - - public static final ArmState AMP = new ArmState(Rotation2d.fromDegrees(60)); + /** Amp position. */ + public static final ArmState AMP_POSITION = new ArmState(Rotation2d.fromDegrees(60)); + /** + * Arm state. + * + * @param shoulderRotations the shoulder state in rotations and rotations per second. + */ public ArmState { Objects.requireNonNull(shoulderRotations); } + /** + * Arm state. + * + * @param shoulder the shoulder position. + */ public ArmState(Rotation2d shoulder) { this(new State(shoulder.getRotations(), 0)); } + /** + * Returns true if this arm state is at another arm state. + * + * @param other another arm state. + * @return true if this arm state is at another arm state. + */ public boolean at(ArmState other) { return MathUtil.isNear( shoulderRotations.position, diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index 28dfbf7..591c843 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -16,25 +16,25 @@ import frc.robot.swerve.Swerve; import frc.robot.swerve.SwerveConstants; -/** Subsystem class for the auto subsystem. */ +/** Auto subsystem. */ public class Auto extends Subsystem { - /** Instance variable for the auto subsystem singleton. */ + /** Auto singleton. */ private static Auto instance = null; - /** Reference to the odometry subsystem. */ + /** Odometry subsystem reference. */ private final Odometry odometry; - /** Reference to the superstructure subsystem. */ + /** Superstructure subsystem reference. */ private final Superstructure superstructure; - /** Reference to the swerve subsystem. */ + /** Swerve subsystem reference. */ private final Swerve swerve; - /** Sendable chooser for the subsystem. */ + /** Auto command chooser. */ private final SendableChooser autoChooser; - /** Creates a new instance of the auto subsystem. */ + /** Initializes the auto subsystem. */ private Auto() { odometry = Odometry.getInstance(); superstructure = Superstructure.getInstance(); @@ -65,9 +65,9 @@ private Auto() { } /** - * Gets the instance of the auto subsystem. + * Returns the auto subsystem instance. * - * @return the instance of the auto subsystem. + * @return the auto subsystem instance. */ public static Auto getInstance() { if (instance == null) { @@ -86,20 +86,11 @@ public void addToShuffleboard(ShuffleboardTab tab) { } /** - * Gets the command to run during the autonomous period. + * Returns the selected auto command. * - * @return the command to run during the autonomous period. + * @return the selected auto command. */ - public Command getAutonomousCommand() { + public Command getSelectedCommand() { return autoChooser.getSelected(); } - - /** - * Gets the chooser for the command to run during the autonomous period. - * - * @return the chooser for the command to run during the autonomous period. - */ - public SendableChooser getAutonomousChooser() { - return autoChooser; - } } diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 5d56005..7579279 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -3,43 +3,94 @@ 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.intake.IntakeConstants.BackRollerConstants; -import frc.robot.intake.IntakeConstants.FrontRollerConstants; /** Subsystem class for the intake subsystem. */ public class Intake extends Subsystem { - /** Instance variable for the intake subsystem singleton. */ + /** Intake singleton. */ private static Intake instance = null; + /** Front roller config. */ + private final MechanismConfig frontRollerConfig = + new MechanismConfig() + .withMotorConfig( + new MotorConfig() + .withCCWPositive(false) + .withNeutralBrake(false) + .withMotorToMechanismRatio(24.0 / 16.0)) + .withMotionProfileConfig( + new MotionProfileConfig().withMaximumVelocity(66) // rotations per second + ) + .withFeedforwardConfig( + new FeedforwardControllerConfig() + .withStaticFeedforward(0.13) // volts + .withVelocityFeedforward(0.1683) // volts per rotation per second + ) + .withFeedbackConfig( + new FeedbackControllerConfig() + .withProportionalGain(0.1) // volts per rotation per second + ); + + /** Back roller config. */ + private final MechanismConfig backRollerConfig = + new MechanismConfig() + .withMotorConfig( + new MotorConfig() + .withCCWPositive(false) + .withNeutralBrake(false) + .withMotorToMechanismRatio(24.0 / 16.0)) + .withMotionProfileConfig( + new MotionProfileConfig().withMaximumVelocity(66) // rotations per second + ) + .withFeedforwardConfig( + new FeedforwardControllerConfig() + .withStaticFeedforward(0.13) // volts + .withVelocityFeedforward(0.1759) // volts per rotation per second + ) + .withFeedbackConfig( + new FeedbackControllerConfig() + .withProportionalGain(0.1) // volts per rotation per second + ); + /** Rollers. */ private final VelocityControllerIO frontRoller, backRoller; /** Roller values. */ private final VelocityControllerIOValues frontRollerValues, backRollerValues; - private IntakeState setpoint, goal; + /** Intake goal. Set by superstructure. */ + private IntakeState goal; + + /** Intake setpoint. Updated periodically to reach goal within constraints. */ + private IntakeState setpoint; - /** Creates a new instance of the intake subsystem. */ + /** Initializes the intake subsystem and configures intake hardware. */ private Intake() { - frontRoller = IntakeFactory.createFrontRoller(); - frontRollerValues = new VelocityControllerIOValues(); + frontRoller = IntakeFactory.createFrontRoller(frontRollerConfig); frontRoller.configure(); - backRoller = IntakeFactory.createBackRoller(); - backRollerValues = new VelocityControllerIOValues(); + frontRollerValues = new VelocityControllerIOValues(); + + backRoller = IntakeFactory.createBackRoller(backRollerConfig); backRoller.configure(); + backRollerValues = new VelocityControllerIOValues(); + setpoint = IntakeState.IDLE; goal = IntakeState.IDLE; } /** - * Gets the instance of the intake subsystem. + * Returns the intake subsystem instance. * - * @return the instance of the intake subsystem. + * @return the intake subsystem instance. */ public static Intake getInstance() { if (instance == null) { @@ -66,28 +117,58 @@ public void addToShuffleboard(ShuffleboardTab tab) { VelocityControllerIO.addToShuffleboard(tab, "Back Roller", backRollerValues); } - public Trigger noteStuck() { - return new Trigger(() -> frontRollerStuck() || backRollerStuck()); - } - - private boolean frontRollerStuck() { - return frontRollerValues.motorAmps > FrontRollerConstants.NOTE_AMPS; - } - - private boolean backRollerStuck() { - return backRollerValues.motorAmps > BackRollerConstants.NOTE_AMPS; - } - + /** + * Returns the intake state. + * + * @return the intake state. + */ public IntakeState getState() { return new IntakeState( frontRollerValues.velocityRotationsPerSecond, backRollerValues.velocityRotationsPerSecond); } + /** + * Sets the intake goal state. + * + * @param goal the intake goal state. + */ public void setGoal(IntakeState goal) { this.goal = goal; } + /** + * Returns true if the intake is at the goal state. + * + * @return true if the intake is at the goal state. + */ public boolean atGoal() { return getState().at(goal); } + + /** + * Returns a trigger for if a note is stuck. + * + * @return a trigger for if a note is stuck. + */ + public Trigger noteStuck() { + return new Trigger(() -> frontRollerStuck() || backRollerStuck()); + } + + /** + * Returns true if the front roller is stuck. + * + * @return true if the front roller is stuck. + */ + private boolean frontRollerStuck() { + return frontRollerValues.motorAmps > 40.0; + } + + /** + * Returns true if the back roller is stuck. + * + * @return true if the back roller is stuck. + */ + private boolean backRollerStuck() { + return backRollerValues.motorAmps > 40.0; + } } diff --git a/src/main/java/frc/robot/intake/IntakeConstants.java b/src/main/java/frc/robot/intake/IntakeConstants.java deleted file mode 100644 index 98dedcf..0000000 --- a/src/main/java/frc/robot/intake/IntakeConstants.java +++ /dev/null @@ -1,68 +0,0 @@ -package frc.robot.intake; - -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 intake subsystem. */ -public class IntakeConstants { - /** Constants for the front roller. */ - public static class FrontRollerConstants { - /** Front roller's CAN. */ - public static final CAN CAN = new CAN(50); - - /** Front roller's config. */ - public static final MechanismConfig CONFIG = - new MechanismConfig() - .withMotorConfig( - new MotorConfig() - .withCCWPositive(false) - .withNeutralBrake(false) - .withMotorToMechanismRatio(24.0 / 16.0)) - .withMotionProfileConfig( - new MotionProfileConfig().withMaximumVelocity(66) // rotations per second - ) - .withFeedforwardConfig( - new FeedforwardControllerConfig() - .withStaticFeedforward(0.13) // volts - .withVelocityFeedforward(0.1683) // volts per rotation per second - ) - .withFeedbackConfig( - new FeedbackControllerConfig() - .withProportionalGain(0.1) // volts per rotation per second - ); - - public static final double NOTE_AMPS = 40; - } - - /** Constants for the back roller. */ - public static class BackRollerConstants { - /** Back roller's CAN. */ - public static final CAN CAN = new CAN(40); - - public static final MechanismConfig CONFIG = - new MechanismConfig() - .withMotorConfig( - new MotorConfig() - .withCCWPositive(false) - .withNeutralBrake(false) - .withMotorToMechanismRatio(24.0 / 16.0)) - .withMotionProfileConfig( - new MotionProfileConfig().withMaximumVelocity(66) // rotations per second - ) - .withFeedforwardConfig( - new FeedforwardControllerConfig() - .withStaticFeedforward(0.13) // volts - .withVelocityFeedforward(0.1759) // volts per rotation per second - ) - .withFeedbackConfig( - new FeedbackControllerConfig() - .withProportionalGain(0.1) // volts per rotation per second - ); - - public static final double NOTE_AMPS = 40; - } -} diff --git a/src/main/java/frc/robot/intake/IntakeFactory.java b/src/main/java/frc/robot/intake/IntakeFactory.java index 9cbdfcd..d6d42b0 100644 --- a/src/main/java/frc/robot/intake/IntakeFactory.java +++ b/src/main/java/frc/robot/intake/IntakeFactory.java @@ -1,30 +1,28 @@ package frc.robot.intake; +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.intake.IntakeConstants.BackRollerConstants; -import frc.robot.intake.IntakeConstants.FrontRollerConstants; /** Helper class for creating hardware for the intake subsystem. */ public class IntakeFactory { - public static VelocityControllerIO createFrontRoller() { + public static VelocityControllerIO createFrontRoller(MechanismConfig config) { if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE)) { - return new VelocityControllerIOTalonFXPIDF( - FrontRollerConstants.CAN, FrontRollerConstants.CONFIG); + return new VelocityControllerIOTalonFXPIDF(new CAN(50), config); } return new VelocityControllerIOSim(); } - public static VelocityControllerIO createBackRoller() { + public static VelocityControllerIO createBackRoller(MechanismConfig config) { if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE)) { - return new VelocityControllerIOTalonFXPIDF( - BackRollerConstants.CAN, BackRollerConstants.CONFIG); + return new VelocityControllerIOTalonFXPIDF(new CAN(40), config); } return new VelocityControllerIOSim(); diff --git a/src/main/java/frc/robot/intake/IntakeState.java b/src/main/java/frc/robot/intake/IntakeState.java index 393c6e7..75c0b8f 100644 --- a/src/main/java/frc/robot/intake/IntakeState.java +++ b/src/main/java/frc/robot/intake/IntakeState.java @@ -3,30 +3,40 @@ import edu.wpi.first.math.MathUtil; import java.util.Objects; +/** Intake state */ public record IntakeState( double frontRollerVelocityRotationsPerSecond, double backRollerVelocityRotationsPerSecond) { + /** Idle state. */ public static final IntakeState IDLE = new IntakeState(0, 0); - public static final IntakeState INTAKE = new IntakeState((double) 34, (double) 34); + /** Intaking state. */ + public static final IntakeState INTAKING = new IntakeState(34.0, 34.0); - public static final IntakeState EJECT = new IntakeState((double) -34, (double) -34); + /** Ejecting state. */ + public static final IntakeState EJECTING = new IntakeState(-34.0, -34.0); + /** + * Intake state. + * + * @param frontRollerVelocityRotationsPerSecond front roller velocity in rotations per second. + * @param backRollerVelocityRotationsPerSecond back roller velocity in rotations per second. + */ public IntakeState { Objects.requireNonNull(frontRollerVelocityRotationsPerSecond); Objects.requireNonNull(backRollerVelocityRotationsPerSecond); } + /** + * Returns true if this intake state is at another intake state. + * + * @param other another intake state. + * @return true if this intake state is at another intake state. + */ public boolean at(IntakeState other) { - final double kToleranceRotationsPerSecond = 1; - return MathUtil.isNear( - frontRollerVelocityRotationsPerSecond, - other.frontRollerVelocityRotationsPerSecond, - kToleranceRotationsPerSecond) + frontRollerVelocityRotationsPerSecond, other.frontRollerVelocityRotationsPerSecond, 1.0) && MathUtil.isNear( - backRollerVelocityRotationsPerSecond, - other.backRollerVelocityRotationsPerSecond, - kToleranceRotationsPerSecond); + backRollerVelocityRotationsPerSecond, other.backRollerVelocityRotationsPerSecond, 1.0); } } diff --git a/src/main/java/frc/robot/superstructure/SuperstructureState.java b/src/main/java/frc/robot/superstructure/SuperstructureState.java index 99acbd9..a9acc18 100644 --- a/src/main/java/frc/robot/superstructure/SuperstructureState.java +++ b/src/main/java/frc/robot/superstructure/SuperstructureState.java @@ -10,25 +10,26 @@ public record SuperstructureState( ArmState armState, IntakeState intakeState, ShooterState shooterState) { public static final SuperstructureState STOW = - new SuperstructureState(ArmState.STOW, IntakeState.IDLE, ShooterState.IDLE); + new SuperstructureState(ArmState.STOW_POSITION, IntakeState.IDLE, ShooterState.IDLE); public static final SuperstructureState INTAKE = - new SuperstructureState(ArmState.STOW, IntakeState.INTAKE, ShooterState.INTAKE); + new SuperstructureState(ArmState.STOW_POSITION, IntakeState.INTAKING, ShooterState.INTAKE); public static final SuperstructureState EJECT_POSITION = - new SuperstructureState(ArmState.EJECT, IntakeState.EJECT, ShooterState.IDLE); + new SuperstructureState(ArmState.EJECT_POSITION, IntakeState.EJECTING, ShooterState.IDLE); public static final SuperstructureState EJECT = - new SuperstructureState(ArmState.EJECT, IntakeState.EJECT, ShooterState.EJECT); + new SuperstructureState(ArmState.EJECT_POSITION, IntakeState.EJECTING, ShooterState.EJECT); public static final SuperstructureState SUBWOOFER = - new SuperstructureState(ArmState.SUBWOOFER, IntakeState.IDLE, ShooterState.SUBWOOFER); + new SuperstructureState( + ArmState.SUBWOOFER_POSITION, IntakeState.IDLE, ShooterState.SUBWOOFER); public static final SuperstructureState SKIM = - new SuperstructureState(ArmState.SKIM, IntakeState.IDLE, ShooterState.SKIM); + new SuperstructureState(ArmState.SKIM_POSITION, IntakeState.IDLE, ShooterState.SKIM); public static final SuperstructureState AMP = - new SuperstructureState(ArmState.AMP, IntakeState.IDLE, ShooterState.AMP); + new SuperstructureState(ArmState.AMP_POSITION, IntakeState.IDLE, ShooterState.AMP); /** * Creates a new superstructure state.