From 0908617d9b0f62114569de4a39bd07dbab09a245 Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sat, 27 Apr 2024 11:50:33 -0400 Subject: [PATCH] refactor: Continue WIP. --- src/main/java/frc/robot/RobotContainer.java | 4 +- src/main/java/frc/robot/arm/Arm.java | 2 +- src/main/java/frc/robot/intake/Intake.java | 2 +- .../java/frc/robot/odometry/Odometry.java | 2 +- src/main/java/frc/robot/shooter/Shooter.java | 2 +- .../robot/superstructure/Superstructure.java | 2 +- .../java/frc/robot/swerve/DriveCommand.java | 39 ++++----- .../java/frc/robot/swerve/DriveRequest.java | 83 ++++++++++--------- src/main/java/frc/robot/swerve/Swerve.java | 67 +++++++++------ 9 files changed, 110 insertions(+), 93 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f2c86ea..5ec7035 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,7 @@ public class RobotContainer { /** Robot container singleton. */ - public static RobotContainer instance = null; + private static RobotContainer instance = null; /** Arm subsystem reference. */ private final Arm arm; @@ -129,7 +129,7 @@ private void configureBindings() { * @param side the side of the controller to rumble. * @return a command that rumbles the controller. */ - public Command rumble(RumbleType side) { + private Command rumble(RumbleType side) { return Commands.startEnd( () -> rumbleController.setRumble(side, 1), () -> rumbleController.setRumble(side, 0)); } diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 485ccc2..d0d0f86 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -19,7 +19,7 @@ /** Arm subsystem. */ public class Arm extends Subsystem { - /** Arm singleton. */ + /** Arm subsystem singleton. */ private static Arm instance = null; /** Shoulder controller config. */ diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 7b5f0e6..3f1b608 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -14,7 +14,7 @@ /** Intake subsystem. */ public class Intake extends Subsystem { - /** Intake singleton. */ + /** Intake subsystem singleton. */ private static Intake instance = null; /** Front roller controller config. */ diff --git a/src/main/java/frc/robot/odometry/Odometry.java b/src/main/java/frc/robot/odometry/Odometry.java index b61cb9b..af44103 100644 --- a/src/main/java/frc/robot/odometry/Odometry.java +++ b/src/main/java/frc/robot/odometry/Odometry.java @@ -23,7 +23,7 @@ /** Odometry subsystem. */ public class Odometry extends Subsystem { - /** Odometry singleton. */ + /** Odometry subsystem singleton. */ private static Odometry instance = null; /** Gyroscope. */ diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index c23bdc2..777b512 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -15,7 +15,7 @@ /** Shooter subsystem. */ public class Shooter extends Subsystem { - /** Shooter singleton. */ + /** Shooter subsystem singleton. */ private static Shooter instance = null; /** Flywheel controller config. */ diff --git a/src/main/java/frc/robot/superstructure/Superstructure.java b/src/main/java/frc/robot/superstructure/Superstructure.java index 76f6b3e..9c8fa0a 100644 --- a/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/superstructure/Superstructure.java @@ -17,7 +17,7 @@ /** Superstructure subsystem. */ public class Superstructure extends Subsystem { - /** Superstructure singleton. */ + /** Superstructure subsystem singleton. */ private static Superstructure instance = null; /** Arm subsystem reference. */ diff --git a/src/main/java/frc/robot/swerve/DriveCommand.java b/src/main/java/frc/robot/swerve/DriveCommand.java index 82bcb75..62d0d02 100644 --- a/src/main/java/frc/robot/swerve/DriveCommand.java +++ b/src/main/java/frc/robot/swerve/DriveCommand.java @@ -1,8 +1,6 @@ package frc.robot.swerve; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -11,29 +9,37 @@ /** Drives the swerve using driver input. */ public class DriveCommand extends Command { - /* Swerve subsystem. */ + /** Swerve subsystem. */ private final Swerve swerve; - /* Odometry subsystem. */ + + /** Odometry subsystem. */ private final Odometry odometry; - /* Xbox controller used to get driver input. */ + /** Controller used to get driver input. */ private final CommandXboxController driverController; /** Previous requested chassis speed. */ private ChassisSpeeds previousChassisSpeeds; - /* Current and previous requests from the driver controller. */ + /** Request from the driver controller. */ private DriveRequest request; + /** + * Initializes the drive command. + * + * @param driverController controller to use as input. + */ public DriveCommand(CommandXboxController driverController) { swerve = Swerve.getInstance(); odometry = Odometry.getInstance(); + this.driverController = driverController; + addRequirements(swerve); previousChassisSpeeds = new ChassisSpeeds(); - this.driverController = driverController; + request = DriveRequest.fromController(driverController); } @Override @@ -43,22 +49,13 @@ public void initialize() {} public void execute() { request = DriveRequest.fromController(driverController); - Translation2d translationVelocityMetersPerSecond = - request.translation().times(SwerveConstants.MAXIMUM_SPEED); - - Rotation2d driverRelativeHeading = odometry.getDriverRelativeHeading(); - - Rotation2d omega = new Rotation2d(); - - omega = request.omega(); - - ChassisSpeeds chassisSpeeds = + final ChassisSpeeds chassisSpeeds = clampChassisSpeeds( ChassisSpeeds.fromFieldRelativeSpeeds( - translationVelocityMetersPerSecond.getX(), - translationVelocityMetersPerSecond.getY(), - omega.getRadians(), - driverRelativeHeading)); + request.velocity().getX(), + request.velocity().getY(), + request.omega().getRadians(), + odometry.getDriverRelativeHeading())); swerve.setChassisSpeeds(chassisSpeeds); diff --git a/src/main/java/frc/robot/swerve/DriveRequest.java b/src/main/java/frc/robot/swerve/DriveRequest.java index f8e5c6f..ef94827 100644 --- a/src/main/java/frc/robot/swerve/DriveRequest.java +++ b/src/main/java/frc/robot/swerve/DriveRequest.java @@ -5,35 +5,52 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +/** Drive request. */ public record DriveRequest( DriveRequest.TranslationMode translationMode, DriveRequest.RotationMode rotationMode, - Translation2d translation, - Translation2d rotation) { + Translation2d translationAxis, + Translation2d rotationAxis) { - public enum TranslationMode { + /** Translation mode. */ + private enum TranslationMode { + /** Field-centric driving. */ FIELD_CENTRIC, + /** Robot-centric driving. */ ROBOT_CENTRIC } - public enum RotationMode { + /** Rotation mode. */ + private enum RotationMode { + /** Drifting (no rotation requested). */ DRIFTING, + /** Spinning (velocity requested). */ SPINNING, - SNAPPING + /** Aligning (heading requested). */ + ALIGNING } + /** + * Returns true if no rotation is requested. + * + * @param heading the heading axis. + * @param aligning true if aligning is requested. + * @return + */ private static boolean isDrifting(Translation2d heading, boolean aligning) { if (aligning) { - final double kMinHeadingDisplacement = 0.7; - - return heading.getNorm() < kMinHeadingDisplacement; + return heading.getNorm() < 0.7; } - final double kOmegaDeadband = 0.1; - - return Math.abs(heading.getY()) < kOmegaDeadband; + return Math.abs(heading.getY()) < 0.1; } + /** + * Creates a new driver request from controller inputs. + * + * @param controller the input controller. + * @return a new driver request from controller inputs. + */ public static DriveRequest fromController(CommandXboxController controller) { boolean snipingRequested = Math.abs(controller.getLeftTriggerAxis()) > 0.5; boolean aligningRequested = Math.abs(controller.getRightTriggerAxis()) > 0.5; @@ -55,8 +72,7 @@ public static DriveRequest fromController(CommandXboxController controller) { Translation2d translationVector = new Translation2d(translationMagnitude, translationDirection); - TranslationMode translationMode = - snipingRequested ? TranslationMode.ROBOT_CENTRIC : TranslationMode.FIELD_CENTRIC; + TranslationMode translationMode = TranslationMode.FIELD_CENTRIC; Translation2d rotationVector = new Translation2d(-controller.getRightY(), -controller.getRightX()); @@ -66,7 +82,7 @@ public static DriveRequest fromController(CommandXboxController controller) { if (isDrifting(rotationVector, aligningRequested)) { rotationMode = RotationMode.DRIFTING; } else if (aligningRequested) { - rotationMode = RotationMode.SNAPPING; + rotationMode = RotationMode.ALIGNING; } else { rotationMode = RotationMode.SPINNING; } @@ -74,34 +90,23 @@ public static DriveRequest fromController(CommandXboxController controller) { return new DriveRequest(translationMode, rotationMode, translationVector, rotationVector); } - public static boolean startedDrifting(DriveRequest past, DriveRequest present) { - return past.rotationMode == RotationMode.SPINNING - && present.rotationMode == RotationMode.DRIFTING; - } - - public boolean isRobotCentric() { - return translationMode == TranslationMode.ROBOT_CENTRIC; - } - - public boolean isSpinning() { - return rotationMode == RotationMode.SPINNING; - } - - public boolean isSnapping() { - return rotationMode == RotationMode.SNAPPING; - } - - public boolean isDrifting() { - return rotationMode == RotationMode.DRIFTING; + /** + * Returns the requested translation velocity in meters per second. + * + * @return the requested translation velocity in meters per second. + */ + public Translation2d velocity() { + return translationAxis.times(SwerveConstants.MAXIMUM_SPEED); } + /** + * Returns the requested rotation velocity. + * + * @return the requested rotation velocity. + */ public Rotation2d omega() { - if (Math.abs(this.rotation().getY()) < 0.1) return new Rotation2d(); - - return SwerveConstants.MAXIMUM_ROTATION_SPEED.times(this.rotation().getY()); - } + if (Math.abs(rotationAxis.getY()) < 0.1) return new Rotation2d(); - public Rotation2d driverHeading() { - return rotation().getAngle(); + return SwerveConstants.MAXIMUM_ROTATION_SPEED.times(rotationAxis.getY()); } } diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index e68ecf3..2feb045 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -13,19 +13,19 @@ import frc.lib.Telemetry; import frc.robot.RobotConstants; -/** Subsystem class for the swerve subsystem. */ +/** Swerve subsystem. */ public class Swerve extends Subsystem { - /** Instance variable for the swerve subsystem singleton. */ + /** Swerve subsystem singleton. */ private static Swerve instance = null; - /** Swerve modules controlled by the swerve subsystem. */ + /** Swerve modules. */ private final SwerveModuleIO[] swerveModules = new SwerveModuleIO[4]; /** Swerve kinematics. */ private final SwerveDriveKinematics swerveKinematics; - /** Creates a new instance of the swerve subsystem. */ + /** Initializes the swerve subsystem and configures swerve hardware. */ private Swerve() { swerveModules[0] = SwerveFactory.createModule(SwerveConstants.NORTH_WEST_MODULE_CONFIG); swerveModules[1] = SwerveFactory.createModule(SwerveConstants.NORTH_EAST_MODULE_CONFIG); @@ -41,9 +41,9 @@ private Swerve() { } /** - * Gets the instance of the swerve subsystem. + * Returns the swerve subsystem instance. * - * @return the instance of the swerve subsystem. + * @return the swerve subsystem instance. */ public static Swerve getInstance() { if (instance == null) { @@ -89,18 +89,18 @@ public void addToShuffleboard(ShuffleboardTab tab) { } /** - * Gets the swerve's kinematics. + * Returns the swerve kinematics. * - * @return the swerve's kinematics. + * @return the swerve kinematics. */ public SwerveDriveKinematics getKinematics() { return swerveKinematics; } /** - * Gets the state of each of the swerve's modules. + * Returns the module states. * - * @return the state of each of the swerve's modules. + * @return the module states. */ public SwerveModuleState[] getModuleStates() { SwerveModuleState[] moduleStates = new SwerveModuleState[4]; @@ -113,9 +113,9 @@ public SwerveModuleState[] getModuleStates() { } /** - * Gets the setpoint of each of the swerve's modules. + * Returns the module setpoints. * - * @return the setpoint of each of the swerve's modules. + * @return the module setpoints. */ public SwerveModuleState[] getModuleSetpoints() { SwerveModuleState[] moduleSetpoints = new SwerveModuleState[4]; @@ -128,9 +128,9 @@ public SwerveModuleState[] getModuleSetpoints() { } /** - * Gets the position of each of the swerve's modules. + * Returns the module positions. * - * @return the position of each of the swerve's modules. + * @return the module positions. */ public SwerveModulePosition[] getModulePositions() { SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; @@ -143,18 +143,18 @@ public SwerveModulePosition[] getModulePositions() { } /** - * Gets the swerve's speeds. + * Returns the chassis speeds. * - * @return the swerve's speeds. + * @return the chassis speeds. */ public ChassisSpeeds getChassisSpeeds() { return swerveKinematics.toChassisSpeeds(getModuleStates()); } /** - * Sets the swerve's speeds. + * Sets the swerve speeds. * - * @param speeds the swerve's speeds. + * @param speeds the swerve speeds. */ public void setChassisSpeeds(ChassisSpeeds speeds) { speeds = ChassisSpeeds.discretize(speeds, RobotConstants.PERIODIC_DURATION); @@ -165,10 +165,10 @@ public void setChassisSpeeds(ChassisSpeeds speeds) { } /** - * Sets each of the swerve modules' setpoints. + * Sets the swerve module setpoints. * - * @param setpoints the setpoints for each of the swerve's modules. - * @param lazy if true, optimize the module setpoint. + * @param setpoints the setpoints. + * @param lazy if true, optimize the module setpoints. */ public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) { SwerveDriveKinematics.desaturateWheelSpeeds(setpoints, SwerveConstants.MAXIMUM_SPEED); @@ -179,7 +179,7 @@ public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) { } /** - * Returns a command that drives the swerve using an Xbox controller. + * Drives the swerve using an Xbox controller. * * @param controller the Xbox controller to use. * @return a command that drives the swerve using an Xbox controller. @@ -189,12 +189,12 @@ public Command driveWithController(CommandXboxController controller) { } /** - * Set the steer motor setpoints for each of the swerve modules. + * Orients the swerve modules. * - * @param orientations orientations for each swerve modules. - * @return a command that orients all swerve modules. + * @param orientations swerve module orientations. + * @return a command that orients the swerve modules. */ - public Command orientModules(Rotation2d[] orientations) { + private Command orientModules(Rotation2d[] orientations) { return run( () -> { setSetpoints( @@ -208,6 +208,11 @@ public Command orientModules(Rotation2d[] orientations) { }); } + /** + * Orients the swerve modules forwards (+X). + * + * @return a command that orients the swerve modules forwards (+X). + */ public Command forwards() { return orientModules( new Rotation2d[] { @@ -218,6 +223,11 @@ public Command forwards() { }); } + /** + * Orients the swerve modules sideways (+Y). + * + * @return a command that orients the swerve modules sideways (+Y). + */ public Command sideways() { return orientModules( new Rotation2d[] { @@ -228,6 +238,11 @@ public Command sideways() { }); } + /** + * Orients the swerve modules in a cross. + * + * @return a command that orients the swerve modules in a cross. + */ public Command cross() { return orientModules( new Rotation2d[] {