diff --git a/src/main/java/frc/lib/config/MotionProfileConfig.java b/src/main/java/frc/lib/config/MotionProfileConfig.java index 3263522..ffdf644 100644 --- a/src/main/java/frc/lib/config/MotionProfileConfig.java +++ b/src/main/java/frc/lib/config/MotionProfileConfig.java @@ -1,11 +1,10 @@ package frc.lib.config; -import java.util.function.Function; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import java.util.function.Function; /** Motion profile config. */ public class MotionProfileConfig { @@ -70,7 +69,7 @@ public double maximumAcceleration() { /** * Creates a new velocity clamper using this motion profile config. - * + * * @return a new velocity clamper using this motion profile config. */ public Function createVelocityClamper() { diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index d9f1e05..f95eb91 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -98,7 +98,8 @@ private Shooter() { serializerValues = new VelocityControllerIOValues(); - serializerAccelerationLimiter = serializerConfig.motionProfileConfig().createAccelerationLimiter(); + serializerAccelerationLimiter = + serializerConfig.motionProfileConfig().createAccelerationLimiter(); setpoint = ShooterState.IDLING; goal = ShooterState.IDLING; diff --git a/src/main/java/frc/robot/swerve/DriveCommand.java b/src/main/java/frc/robot/swerve/DriveCommand.java index 011792d..a887bba 100644 --- a/src/main/java/frc/robot/swerve/DriveCommand.java +++ b/src/main/java/frc/robot/swerve/DriveCommand.java @@ -1,13 +1,12 @@ package frc.robot.swerve; -import java.util.function.Function; - 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 { @@ -22,7 +21,7 @@ public class DriveCommand extends Command { /** Translation acceleration limiter. */ private final SlewRateLimiter xAccelerationLimiter; - + /** Translation acceleration limiter. */ private final SlewRateLimiter yAccelerationLimiter; @@ -86,9 +85,13 @@ public boolean isFinished() { * @return the clamped chassis speeds. */ private ChassisSpeeds clampChassisSpeeds(ChassisSpeeds desiredChassisSpeeds) { - double vxMetersPerSecond = xAccelerationLimiter.calculate(desiredChassisSpeeds.vxMetersPerSecond); - double vyMetersPerSecond = yAccelerationLimiter.calculate(desiredChassisSpeeds.vyMetersPerSecond); - double omegaRadiansPerSecond = rotationVelocityClamper.apply(Units.radiansToRotations(desiredChassisSpeeds.omegaRadiansPerSecond)); + double vxMetersPerSecond = + xAccelerationLimiter.calculate(desiredChassisSpeeds.vxMetersPerSecond); + double vyMetersPerSecond = + yAccelerationLimiter.calculate(desiredChassisSpeeds.vyMetersPerSecond); + double omegaRadiansPerSecond = + rotationVelocityClamper.apply( + Units.radiansToRotations(desiredChassisSpeeds.omegaRadiansPerSecond)); return new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond); } diff --git a/src/main/java/frc/robot/swerve/DriveRequest.java b/src/main/java/frc/robot/swerve/DriveRequest.java index d9ee296..8968836 100644 --- a/src/main/java/frc/robot/swerve/DriveRequest.java +++ b/src/main/java/frc/robot/swerve/DriveRequest.java @@ -75,8 +75,7 @@ public static DriveRequest fromController(CommandXboxController controller) { TranslationMode translationMode = TranslationMode.FIELD_CENTRIC; - Translation2d headingAxis = - new Translation2d(-controller.getRightY(), -controller.getRightX()); + Translation2d headingAxis = new Translation2d(-controller.getRightY(), -controller.getRightX()); RotationMode rotationMode; @@ -94,6 +93,7 @@ public static DriveRequest fromController(CommandXboxController controller) { rotationVelocityAxis = headingAxis.getY(); } - return new DriveRequest(translationMode, rotationMode, translationAxis, headingAxis, rotationVelocityAxis); + return new DriveRequest( + translationMode, rotationMode, translationAxis, headingAxis, rotationVelocityAxis); } } diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index e20946e..8ceb7ef 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -28,10 +28,14 @@ public class Swerve extends Subsystem { private final SwerveDriveKinematics swerveKinematics; /** Translation motion profile config. */ - private final MotionProfileConfig translationMotionProfileConfig = new MotionProfileConfig().withMaximumVelocity(4.5).withMaximumAcceleration(18); + private final MotionProfileConfig translationMotionProfileConfig = + new MotionProfileConfig() + .withMaximumVelocity(4.5) // meters per second + .withMaximumAcceleration(18); // meters per second per second /** Rotation motion profile config. */ - private final MotionProfileConfig rotationMotionProfileConfig = new MotionProfileConfig().withMaximumVelocity(0.25); + private final MotionProfileConfig rotationMotionProfileConfig = + new MotionProfileConfig().withMaximumVelocity(0.25); // rotations per second /** Initializes the swerve subsystem and configures swerve hardware. */ private Swerve() { @@ -188,7 +192,7 @@ public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) { /** * Returns the motion profile config. - * + * * @return the motion profile config. */ public MotionProfileConfig translationMotionProfileConfig() { @@ -197,7 +201,7 @@ public MotionProfileConfig translationMotionProfileConfig() { /** * Returns the maximum translation velocity. - * + * * @return the maximum translation velocity. */ public double maximumTranslationVelocity() { @@ -206,7 +210,7 @@ public double maximumTranslationVelocity() { /** * Returns the maximum translation acceleration. - * + * * @return the maximum translation acceleration. */ public double maximumTranslationAcceleration() { @@ -215,7 +219,7 @@ public double maximumTranslationAcceleration() { /** * Returns the rotation motion profile config. - * + * * @return the rotation motion profile config. */ public MotionProfileConfig rotationMotionProfileConfig() { @@ -224,7 +228,7 @@ public MotionProfileConfig rotationMotionProfileConfig() { /** * Returns the maximum rotation velocity. - * + * * @return the maximum rotation velocity. */ public Rotation2d maximumRotationVelocity() {