diff --git a/src/main/java/frc/robot/auto/Auto.java b/src/main/java/frc/robot/auto/Auto.java index 5a7ba9a..cd85112 100644 --- a/src/main/java/frc/robot/auto/Auto.java +++ b/src/main/java/frc/robot/auto/Auto.java @@ -45,7 +45,7 @@ private Auto() { new HolonomicPathFollowerConfig( new PIDConstants(1.0), new PIDConstants(1.0), - SwerveConstants.MAXIMUM_SPEED, + SwerveConstants.MAXIMUM_ATTAINABLE_SPEED, SwerveConstants.NORTH_EAST_MODULE_CONFIG.position().getNorm(), new ReplanningConfig()); diff --git a/src/main/java/frc/robot/swerve/Drive.java b/src/main/java/frc/robot/swerve/Drive.java index 1b54fd5..0d01997 100644 --- a/src/main/java/frc/robot/swerve/Drive.java +++ b/src/main/java/frc/robot/swerve/Drive.java @@ -36,7 +36,12 @@ public class Drive extends Command { /* Heading feedback controller. */ private final ProfiledRotationPIDController headingFeedback = new ProfiledRotationPIDController( - 6, 0, 0.1, new Constraints(0.25 * SwerveConstants.MAXIMUM_ROTATION_SPEED, 8)); + 6, + 0, + 0.1, + new Constraints( + SwerveConstants.MAXIMUM_ROTATION_SPEED, + SwerveConstants.MAXIMUM_ROTATION_ACCELERATION)); private final DoubleEntry headingEntry, headingVelocityEntry, @@ -123,8 +128,9 @@ public void execute() { } double maxOmegaRadiansPerSecond = - Units.rotationsToRadians(SwerveConstants.MAXIMUM_ROTATION_SPEED); + Units.rotationsToRadians(SwerveConstants.MAXIMUM_ATTAINABLE_ROTATION_SPEED); + // Clamp angular velocity chassisSpeeds.omegaRadiansPerSecond = Math.signum(chassisSpeeds.omegaRadiansPerSecond) * Math.min(maxOmegaRadiansPerSecond, Math.abs(chassisSpeeds.omegaRadiansPerSecond)); diff --git a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java index affd36d..bbd7e7e 100644 --- a/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java +++ b/src/main/java/frc/robot/swerve/DriveMotorIOTalonFXPID.java @@ -68,7 +68,7 @@ private VoltageOut calculateVelocityVoltage(double velocityMetersPerSecond, bool * @return the voltage to apply. */ private VoltageOut calculateOpenLoopVelocityVoltage(double velocityMetersPerSecond) { - double velocityPercent = velocityMetersPerSecond / SwerveConstants.MAXIMUM_SPEED; + double velocityPercent = velocityMetersPerSecond / SwerveConstants.MAXIMUM_ATTAINABLE_SPEED; double velocityVolts = velocityPercent * RobotConstants.BATTERY_VOLTAGE; diff --git a/src/main/java/frc/robot/swerve/DriveRequest.java b/src/main/java/frc/robot/swerve/DriveRequest.java index 4125586..c87a791 100644 --- a/src/main/java/frc/robot/swerve/DriveRequest.java +++ b/src/main/java/frc/robot/swerve/DriveRequest.java @@ -88,7 +88,7 @@ public Translation2d getTranslationVelocity() { } public double getRotationVelocity() { - return SwerveConstants.MAXIMUM_ROTATION_SPEED * 0.25 * this.rotationVector.getY(); + return SwerveConstants.MAXIMUM_ROTATION_SPEED * this.rotationVector.getY(); } private Rotation2d snapToNearest(Rotation2d angle, Rotation2d multiple) { diff --git a/src/main/java/frc/robot/swerve/Swerve.java b/src/main/java/frc/robot/swerve/Swerve.java index 1ce2660..2899ee8 100644 --- a/src/main/java/frc/robot/swerve/Swerve.java +++ b/src/main/java/frc/robot/swerve/Swerve.java @@ -155,7 +155,8 @@ public void setChassisSpeeds(ChassisSpeeds speeds) { * @param lazy if true, optimize the module setpoint. */ public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) { - SwerveDriveKinematics.desaturateWheelSpeeds(setpoints, SwerveConstants.MAXIMUM_SPEED); + SwerveDriveKinematics.desaturateWheelSpeeds( + setpoints, SwerveConstants.MAXIMUM_ATTAINABLE_SPEED); for (int i = 0; i < 4; i++) { swerveModules[i].setSetpoint(setpoints[i], lazy); @@ -163,7 +164,7 @@ public void setSetpoints(SwerveModuleState[] setpoints, boolean lazy) { } /** - * Set the steer motor setpoints for each of the swerve modules. + * Set the steer motor setpoints for each of the swerve modules. * * @param steerSetpoints the steer motor setpoints for each swerve module. */ @@ -199,7 +200,10 @@ public void pointSideways() { Rotation2d.fromDegrees(90.0)); } - /** Sets the steer motor setpoints to point the swerve modules inwards, like a cross or "X" pattern. */ + /** + * Sets the steer motor setpoints to point the swerve modules inwards, like a cross or "X" + * pattern. + */ public void pointInwards() { setSteerSetpoints( Rotation2d.fromDegrees(45.0), diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/src/main/java/frc/robot/swerve/SwerveConstants.java index 93b8c8a..18ecf78 100644 --- a/src/main/java/frc/robot/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/swerve/SwerveConstants.java @@ -75,11 +75,22 @@ public static class MK4iConstants { Rotation2d.fromRotations(-0.954346)); /** Maximum attainable speed in meters per second. */ - public static final double MAXIMUM_SPEED = Units.feetToMeters(18.38719); + public static final double MAXIMUM_ATTAINABLE_SPEED = Units.feetToMeters(18.38719); + + /** Maximum speed in meters per second. */ + public static final double MAXIMUM_SPEED = MAXIMUM_ATTAINABLE_SPEED; /** Maximum attainable rotational speed in rotations per second. */ - public static final double MAXIMUM_ROTATION_SPEED = - NORTH_WEST_MODULE_CONFIG.position().getNorm() * MAXIMUM_SPEED; + public static final double MAXIMUM_ATTAINABLE_ROTATION_SPEED = + NORTH_WEST_MODULE_CONFIG.position().getNorm() * MAXIMUM_ATTAINABLE_SPEED; + + /** Maximum rotational speed while snapping to heading in rotations per second. */ + public static final double MAXIMUM_ROTATION_SPEED = 0.5; + + /** + * Maximum rotational acceleration while snapping to heading in rotations per second per second. + */ + public static final double MAXIMUM_ROTATION_ACCELERATION = 8.0; public static class DriveMotorConstants { /** If true, use open-loop control. */