diff --git a/src/main/java/frc/lib/config/MotionProfileConfig.java b/src/main/java/frc/lib/config/MotionProfileConfig.java index fd80c78..3263522 100644 --- a/src/main/java/frc/lib/config/MotionProfileConfig.java +++ b/src/main/java/frc/lib/config/MotionProfileConfig.java @@ -1,5 +1,8 @@ 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; @@ -66,11 +69,20 @@ public double maximumAcceleration() { } /** - * Creates a new rate limiter using this motion profile config. + * Creates a new velocity clamper using this motion profile config. + * + * @return a new velocity clamper using this motion profile config. + */ + public Function createVelocityClamper() { + return velocity -> MathUtil.clamp(velocity, -maximumVelocity, maximumVelocity); + } + + /** + * Creates a new acceleration limiter using this motion profile config. * - * @return a new rate limiter using this motion profile config. + * @return a new acceleration limiter using this motion profile config. */ - public SlewRateLimiter createRateLimiter() { + public SlewRateLimiter createAccelerationLimiter() { return new SlewRateLimiter(maximumAcceleration); } diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index 5e73a2f..b316109 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -14,7 +14,6 @@ import frc.robot.odometry.Odometry; import frc.robot.superstructure.Superstructure; import frc.robot.swerve.Swerve; -import frc.robot.swerve.SwerveConstants; import frc.robot.swerve.SwerveFactory; /** Auto subsystem. */ @@ -49,7 +48,7 @@ private Auto() { new HolonomicPathFollowerConfig( new PIDConstants(1.0), new PIDConstants(1.0), - SwerveConstants.MAXIMUM_SPEED, + swerve.maximumTranslationVelocity(), SwerveFactory.createNorthEastModuleTranslation().getNorm(), // TODO new ReplanningConfig()), AllianceFlipHelper::shouldFlip, diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index 777b512..d9f1e05 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -91,14 +91,14 @@ private Shooter() { flywheelValues = new VelocityControllerIOValues(); - flywheelAccelerationLimiter = flywheelConfig.motionProfileConfig().createRateLimiter(); + flywheelAccelerationLimiter = flywheelConfig.motionProfileConfig().createAccelerationLimiter(); serializer = ShooterFactory.createSerializer(serializerConfig); serializer.configure(); serializerValues = new VelocityControllerIOValues(); - serializerAccelerationLimiter = serializerConfig.motionProfileConfig().createRateLimiter(); + 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 62d0d02..011792d 100644 --- a/src/main/java/frc/robot/swerve/DriveCommand.java +++ b/src/main/java/frc/robot/swerve/DriveCommand.java @@ -1,10 +1,12 @@ package frc.robot.swerve; -import edu.wpi.first.math.MathUtil; +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.RobotConstants; import frc.robot.odometry.Odometry; /** Drives the swerve using driver input. */ @@ -18,8 +20,14 @@ public class DriveCommand extends Command { /** Controller used to get driver input. */ private final CommandXboxController driverController; - /** Previous requested chassis speed. */ - private ChassisSpeeds previousChassisSpeeds; + /** 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; @@ -37,7 +45,10 @@ public DriveCommand(CommandXboxController driverController) { addRequirements(swerve); - previousChassisSpeeds = new ChassisSpeeds(); + xAccelerationLimiter = swerve.translationMotionProfileConfig().createAccelerationLimiter(); + yAccelerationLimiter = swerve.translationMotionProfileConfig().createAccelerationLimiter(); + + rotationVelocityClamper = swerve.rotationMotionProfileConfig().createVelocityClamper(); request = DriveRequest.fromController(driverController); } @@ -52,14 +63,12 @@ public void execute() { final ChassisSpeeds chassisSpeeds = clampChassisSpeeds( ChassisSpeeds.fromFieldRelativeSpeeds( - request.velocity().getX(), - request.velocity().getY(), - request.omega().getRadians(), + request.translationAxis().getX() * swerve.maximumTranslationVelocity(), + request.translationAxis().getY() * swerve.maximumTranslationVelocity(), + swerve.maximumRotationVelocity().times(request.rotationVelocityAxis()).getRadians(), odometry.getDriverRelativeHeading())); swerve.setChassisSpeeds(chassisSpeeds); - - previousChassisSpeeds = chassisSpeeds; } @Override @@ -77,27 +86,9 @@ public boolean isFinished() { * @return the clamped chassis speeds. */ private ChassisSpeeds clampChassisSpeeds(ChassisSpeeds desiredChassisSpeeds) { - double vxMetersPerSecond = - MathUtil.clamp( - desiredChassisSpeeds.vxMetersPerSecond, - previousChassisSpeeds.vxMetersPerSecond - - SwerveConstants.MAXIMUM_ACCELERATION * RobotConstants.PERIODIC_DURATION, - previousChassisSpeeds.vxMetersPerSecond - + SwerveConstants.MAXIMUM_ACCELERATION * RobotConstants.PERIODIC_DURATION); - - double vyMetersPerSecond = - MathUtil.clamp( - desiredChassisSpeeds.vyMetersPerSecond, - previousChassisSpeeds.vyMetersPerSecond - - SwerveConstants.MAXIMUM_ACCELERATION * RobotConstants.PERIODIC_DURATION, - previousChassisSpeeds.vyMetersPerSecond - + SwerveConstants.MAXIMUM_ACCELERATION * RobotConstants.PERIODIC_DURATION); - - double omegaRadiansPerSecond = - MathUtil.clamp( - desiredChassisSpeeds.omegaRadiansPerSecond, - -SwerveConstants.MAXIMUM_ROTATION_SPEED.getRadians(), - SwerveConstants.MAXIMUM_ROTATION_SPEED.getRadians()); + 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 ef94827..d9ee296 100644 --- a/src/main/java/frc/robot/swerve/DriveRequest.java +++ b/src/main/java/frc/robot/swerve/DriveRequest.java @@ -10,7 +10,8 @@ public record DriveRequest( DriveRequest.TranslationMode translationMode, DriveRequest.RotationMode rotationMode, Translation2d translationAxis, - Translation2d rotationAxis) { + Translation2d headingAxis, + double rotationVelocityAxis) { /** Translation mode. */ private enum TranslationMode { @@ -33,16 +34,16 @@ private enum RotationMode { /** * Returns true if no rotation is requested. * - * @param heading the heading axis. + * @param headingAxis the heading axis. * @param aligning true if aligning is requested. - * @return + * @return true if no rotation is requested. */ - private static boolean isDrifting(Translation2d heading, boolean aligning) { + private static boolean isDrifting(Translation2d headingAxis, boolean aligning) { if (aligning) { - return heading.getNorm() < 0.7; + return headingAxis.getNorm() < 0.7; } - return Math.abs(heading.getY()) < 0.1; + return Math.abs(headingAxis.getY()) < 0.1; } /** @@ -70,16 +71,16 @@ public static DriveRequest fromController(CommandXboxController controller) { translationMagnitude *= 0.25; } - Translation2d translationVector = new Translation2d(translationMagnitude, translationDirection); + Translation2d translationAxis = new Translation2d(translationMagnitude, translationDirection); TranslationMode translationMode = TranslationMode.FIELD_CENTRIC; - Translation2d rotationVector = + Translation2d headingAxis = new Translation2d(-controller.getRightY(), -controller.getRightX()); RotationMode rotationMode; - if (isDrifting(rotationVector, aligningRequested)) { + if (isDrifting(headingAxis, aligningRequested)) { rotationMode = RotationMode.DRIFTING; } else if (aligningRequested) { rotationMode = RotationMode.ALIGNING; @@ -87,26 +88,12 @@ public static DriveRequest fromController(CommandXboxController controller) { rotationMode = RotationMode.SPINNING; } - return new DriveRequest(translationMode, rotationMode, translationVector, rotationVector); - } - - /** - * 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); - } + double rotationVelocityAxis = 0.0; - /** - * Returns the requested rotation velocity. - * - * @return the requested rotation velocity. - */ - public Rotation2d omega() { - if (Math.abs(rotationAxis.getY()) < 0.1) return new Rotation2d(); + if (rotationMode == RotationMode.SPINNING) { + rotationVelocityAxis = headingAxis.getY(); + } - return SwerveConstants.MAXIMUM_ROTATION_SPEED.times(rotationAxis.getY()); + 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 143aacf..e20946e 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.lib.Subsystem; import frc.lib.Telemetry; +import frc.lib.config.MotionProfileConfig; import frc.lib.controller.SwerveModuleIO; import frc.robot.RobotConstants; @@ -26,6 +27,12 @@ public class Swerve extends Subsystem { /** Swerve kinematics. */ private final SwerveDriveKinematics swerveKinematics; + /** Translation motion profile config. */ + private final MotionProfileConfig translationMotionProfileConfig = new MotionProfileConfig().withMaximumVelocity(4.5).withMaximumAcceleration(18); + + /** Rotation motion profile config. */ + private final MotionProfileConfig rotationMotionProfileConfig = new MotionProfileConfig().withMaximumVelocity(0.25); + /** Initializes the swerve subsystem and configures swerve hardware. */ private Swerve() { swerveModules[0] = SwerveFactory.createNorthWestModule(); @@ -61,15 +68,15 @@ public void periodic() {} public void addToShuffleboard(ShuffleboardTab tab) { ShuffleboardLayout translationConstants = Telemetry.addColumn(tab, "Translation Constants"); - translationConstants.addDouble("Maximum Velocity (mps)", () -> SwerveConstants.MAXIMUM_SPEED); + translationConstants.addDouble("Maximum Velocity (mps)", this::maximumTranslationVelocity); translationConstants.addDouble( - "Maximum Accleration (mpsps)", () -> SwerveConstants.MAXIMUM_ACCELERATION); + "Maximum Accleration (mpsps)", this::maximumTranslationAcceleration); translationConstants.addDouble( "Wheel Circumference (m)", () -> SwerveConstants.MK4iConstants.WHEEL_CIRCUMFERENCE); ShuffleboardLayout rotationConstants = Telemetry.addColumn(tab, "Rotation Constants"); rotationConstants.addDouble( - "Maximum Velocity (rps)", () -> SwerveConstants.MAXIMUM_ROTATION_SPEED.getRotations()); + "Maximum Velocity (rps)", () -> maximumRotationVelocity().getRotations()); Telemetry.addSwerveModuleStates(tab, "Swerve Module States", this::getModuleStates); Telemetry.addSwerveModuleStates(tab, "Swerve Module Setpoints", this::getModuleSetpoints); @@ -172,13 +179,58 @@ public void setChassisSpeeds(ChassisSpeeds speeds) { * @param lazy if true, optimize the module setpoints. */ public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) { - SwerveDriveKinematics.desaturateWheelSpeeds(setpoints, SwerveConstants.MAXIMUM_SPEED); + SwerveDriveKinematics.desaturateWheelSpeeds(setpoints, maximumTranslationVelocity()); for (int i = 0; i < 4; i++) { swerveModules[i].setSetpoint(setpoints[i], lazy); } } + /** + * Returns the motion profile config. + * + * @return the motion profile config. + */ + public MotionProfileConfig translationMotionProfileConfig() { + return translationMotionProfileConfig; + } + + /** + * Returns the maximum translation velocity. + * + * @return the maximum translation velocity. + */ + public double maximumTranslationVelocity() { + return translationMotionProfileConfig.maximumVelocity(); + } + + /** + * Returns the maximum translation acceleration. + * + * @return the maximum translation acceleration. + */ + public double maximumTranslationAcceleration() { + return translationMotionProfileConfig.maximumAcceleration(); + } + + /** + * Returns the rotation motion profile config. + * + * @return the rotation motion profile config. + */ + public MotionProfileConfig rotationMotionProfileConfig() { + return rotationMotionProfileConfig; + } + + /** + * Returns the maximum rotation velocity. + * + * @return the maximum rotation velocity. + */ + public Rotation2d maximumRotationVelocity() { + return Rotation2d.fromRotations(rotationMotionProfileConfig.maximumVelocity()); + } + /** * Drives the swerve using an Xbox controller. * diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java index 663fed1..da38d12 100644 --- a/src/main/java/frc/robot/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/swerve/SwerveConstants.java @@ -1,6 +1,5 @@ package frc.robot.swerve; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import frc.lib.config.FeedbackControllerConfig; import frc.lib.config.FeedforwardControllerConfig; @@ -25,21 +24,6 @@ public static class MK4iConstants { public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI; } - /** Module X offset in meters. */ - public static final double X_OFFSET = Units.inchesToMeters(10.375); - - /** Module Y offset in meters. */ - public static final double Y_OFFSET = Units.inchesToMeters(10.375); - - /** Maximum speed in meters per second. */ - public static final double MAXIMUM_SPEED = 4.5; - - /** Maximum acceleration in meters per second per second. */ - public static final double MAXIMUM_ACCELERATION = 18; - - /** Maximum rotational speed. */ - public static final Rotation2d MAXIMUM_ROTATION_SPEED = Rotation2d.fromRotations(0.25); - /** Drive motor config. */ public static final MechanismConfig DRIVE_CONFIG = new MechanismConfig()