From 4acf4b52937032680b817cbebf8ffd9175a8c4ab Mon Sep 17 00:00:00 2001 From: Hayden Heroux Date: Sun, 28 Apr 2024 13:25:08 -0400 Subject: [PATCH] refactor(swerve): Use subsystem command. --- .../{robot/swerve => lib}/DriveRequest.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/swerve/DriveCommand.java | 98 ------------------- src/main/java/frc/robot/swerve/Swerve.java | 33 ++++++- 4 files changed, 33 insertions(+), 102 deletions(-) rename src/main/java/frc/{robot/swerve => lib}/DriveRequest.java (99%) delete mode 100644 src/main/java/frc/robot/swerve/DriveCommand.java diff --git a/src/main/java/frc/robot/swerve/DriveRequest.java b/src/main/java/frc/lib/DriveRequest.java similarity index 99% rename from src/main/java/frc/robot/swerve/DriveRequest.java rename to src/main/java/frc/lib/DriveRequest.java index 8968836..a38f36c 100644 --- a/src/main/java/frc/robot/swerve/DriveRequest.java +++ b/src/main/java/frc/lib/DriveRequest.java @@ -1,4 +1,4 @@ -package frc.robot.swerve; +package frc.lib; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ec7035..e8ccdb4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -97,7 +97,7 @@ private void initializeTelemetry() { /** Configures subsystem default commands. */ private void configureDefaultCommands() { - swerve.setDefaultCommand(swerve.driveWithController(driverController)); + swerve.setDefaultCommand(swerve.teleopDrive(driverController)); } /** Configures controller bindings. */ diff --git a/src/main/java/frc/robot/swerve/DriveCommand.java b/src/main/java/frc/robot/swerve/DriveCommand.java deleted file mode 100644 index d42afda..0000000 --- a/src/main/java/frc/robot/swerve/DriveCommand.java +++ /dev/null @@ -1,98 +0,0 @@ -package frc.robot.swerve; - -import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.odometry.Odometry; -import java.util.function.Function; - -/** Drives the swerve using driver input. */ -public class DriveCommand extends Command { - /** Swerve subsystem. */ - private final Swerve swerve; - - /** Odometry subsystem. */ - private final Odometry odometry; - - /** Controller used to get driver input. */ - private final CommandXboxController driverController; - - /** Translation acceleration limiter. */ - private final SlewRateLimiter xAccelerationLimiter; - - /** Translation acceleration limiter. */ - private final SlewRateLimiter yAccelerationLimiter; - - /** Rotation velocity clamper. */ - private final Function rotationVelocityClamper; - - /** 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); - - xAccelerationLimiter = swerve.translationMotionProfileConfig().createAccelerationLimiter(); - yAccelerationLimiter = swerve.translationMotionProfileConfig().createAccelerationLimiter(); - - rotationVelocityClamper = swerve.rotationMotionProfileConfig().createVelocityClamper(); - - request = DriveRequest.fromController(driverController); - } - - @Override - public void initialize() {} - - @Override - public void execute() { - request = DriveRequest.fromController(driverController); - - final ChassisSpeeds chassisSpeeds = - clampChassisSpeeds( - ChassisSpeeds.fromFieldRelativeSpeeds( - request.translationAxis().getX() * swerve.maximumTranslationVelocity(), - request.translationAxis().getY() * swerve.maximumTranslationVelocity(), - swerve.maximumRotationVelocity().times(request.rotationVelocityAxis()).getRadians(), - odometry.getDriverRelativeHeading())); - - swerve.setChassisSpeeds(chassisSpeeds); - } - - @Override - public void end(boolean interrupted) {} - - @Override - public boolean isFinished() { - return false; - } - - /** - * Clamps desired chassis speeds to be within velocity and acceleration constraints. - * - * @param desiredChassisSpeeds the desired chassis speeds. - * @return the clamped chassis speeds. - */ - private ChassisSpeeds clampChassisSpeeds(ChassisSpeeds desiredChassisSpeeds) { - double vxMetersPerSecond = - xAccelerationLimiter.calculate(desiredChassisSpeeds.vxMetersPerSecond); - double vyMetersPerSecond = - yAccelerationLimiter.calculate(desiredChassisSpeeds.vyMetersPerSecond); - double omegaRadiansPerSecond = - Units.rotationsToRadians(rotationVelocityClamper.apply( - Units.radiansToRotations(desiredChassisSpeeds.omegaRadiansPerSecond))); - - return new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); - } -} diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 0f44a23..78191bc 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -1,19 +1,25 @@ package frc.robot.swerve; +import java.util.function.Function; + +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.lib.DriveRequest; import frc.lib.Subsystem; import frc.lib.Telemetry; import frc.lib.config.MotionProfileConfig; import frc.lib.controller.SwerveModuleIO; import frc.robot.RobotConstants; +import frc.robot.odometry.Odometry; /** Swerve subsystem. */ public class Swerve extends Subsystem { @@ -242,8 +248,31 @@ public Rotation2d maximumRotationVelocity() { * @param controller the Xbox controller to use. * @return a command that drives the swerve using an Xbox controller. */ - public Command driveWithController(CommandXboxController controller) { - return new DriveCommand(controller); + public Command teleopDrive(CommandXboxController controller) { + final SlewRateLimiter xAccelerationLimiter = translationMotionProfileConfig.createAccelerationLimiter(); + final SlewRateLimiter yAccelerationLimiter = translationMotionProfileConfig.createAccelerationLimiter(); + + final Function rotationVelocityLimiter = rotationMotionProfileConfig.createVelocityClamper(); + + final Function chassisSpeedsLimiter = chassisSpeeds -> { + return new ChassisSpeeds( + xAccelerationLimiter.calculate(chassisSpeeds.vxMetersPerSecond), + yAccelerationLimiter.calculate(chassisSpeeds.vyMetersPerSecond), + Units.rotationsToRadians(rotationVelocityLimiter.apply(Units.radiansToRotations(chassisSpeeds.omegaRadiansPerSecond))) + ); + }; + + final Function chassisSpeedsGetter = request -> { + return ChassisSpeeds.fromFieldRelativeSpeeds( + request.translationAxis().getX() * translationMotionProfileConfig.maximumVelocity(), + request.translationAxis().getY() * translationMotionProfileConfig.maximumVelocity(), + request.rotationVelocityAxis() * Units.rotationsToRadians(rotationMotionProfileConfig.maximumVelocity()), + Odometry.getInstance().getDriverRelativeHeading()); + }; + + return run(() -> { + setChassisSpeeds(chassisSpeedsLimiter.apply(chassisSpeedsGetter.apply(DriveRequest.fromController(controller)))); + }); } /**