From 0fd202d2adff71e053ab0ea795f6fa9064eacf29 Mon Sep 17 00:00:00 2001 From: BlueZeeKing Date: Wed, 3 Jan 2024 21:11:02 -0500 Subject: [PATCH] Change algorithm Switch to using preexisting swerve util functions --- .../java/frc/constants/DriveConstants.java | 5 ++- .../frc/robot/subsystems/DriveSubsystem.java | 38 ++++++++++++------- .../java/frc/utils/CustomSlewRateLimiter.java | 28 -------------- src/main/java/frc/utils/SwerveUtils.java | 27 ------------- 4 files changed, 27 insertions(+), 71 deletions(-) delete mode 100644 src/main/java/frc/utils/CustomSlewRateLimiter.java diff --git a/src/main/java/frc/constants/DriveConstants.java b/src/main/java/frc/constants/DriveConstants.java index f99a67a..a5fd1d5 100644 --- a/src/main/java/frc/constants/DriveConstants.java +++ b/src/main/java/frc/constants/DriveConstants.java @@ -14,8 +14,9 @@ public class DriveConstants { public static final double MAX_SPEED_METERS_PER_SECOND = 4.8; public static final double MAX_ANGULAR_SPEED = 2 * Math.PI; // radians per second - public static final double MAX_ACCELERATION = 1; // meters per second per second - public static final double MAX_DRIVE_ANGULAR_VELOCITY = 1; // radians per second + // TODO: Tune these values + public static final double MAX_ACCELERATION = 9.6; // meters per second per second + public static final double MAX_DRIVE_ANGULAR_VELOCITY = 8; // radians per second public static final double MAX_ROTATIONAL_ACCELERATION = 2; // percent per second // Chassis configuration diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 2e48a31..fb3569d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -5,6 +5,7 @@ package frc.robot.subsystems; import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -13,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.constants.CANMappings; import frc.constants.DriveConstants; -import frc.utils.CustomSlewRateLimiter; import frc.utils.SwerveUtils; public class DriveSubsystem extends SubsystemBase { @@ -51,8 +51,8 @@ public class DriveSubsystem extends SubsystemBase { private double currentTranslationDir = 0.0; // radian private double currentTranslationMag = 0.0; // meter per second private SlewRateLimiter magLimiter = new SlewRateLimiter(DriveConstants.MAX_ACCELERATION); - private CustomSlewRateLimiter dirLimiter = - new CustomSlewRateLimiter(DriveConstants.MAX_DRIVE_ANGULAR_VELOCITY, 0); + + private double prevTime = MathSharedStore.getTimestamp(); private SlewRateLimiter rotLimiter = new SlewRateLimiter(DriveConstants.MAX_ROTATIONAL_ACCELERATION); @@ -145,8 +145,10 @@ public void drive( // Convert XY to polar(theta and magnitude) for rate limiting double inputTranslationDir = SwerveUtils.wrapAngle(Math.atan2(ySpeed, xSpeed)); double inputTranslationMag = - Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)) - * DriveConstants.MAX_SPEED_METERS_PER_SECOND; + Math.min( + Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)) + * DriveConstants.MAX_SPEED_METERS_PER_SECOND, + DriveConstants.MAX_SPEED_METERS_PER_SECOND); if (currentTranslationMag < 0.1) { currentTranslationDir = inputTranslationDir; @@ -154,25 +156,33 @@ public void drive( inputTranslationDir = currentTranslationDir; } - inputTranslationDir = SwerveUtils.optimizeAngle(inputTranslationDir, currentTranslationDir); - - if (Math.abs(inputTranslationDir - currentTranslationDir) > Math.PI / 2.0) { + if (SwerveUtils.angleDifference(inputTranslationDir, currentTranslationDir) > Math.PI / 2.0) { inputTranslationDir += Math.PI; inputTranslationMag *= -1; } - inputTranslationDir = SwerveUtils.optimizeAngle(inputTranslationDir, currentTranslationDir); + inputTranslationDir = SwerveUtils.wrapAngle(inputTranslationDir); inputTranslationMag *= - 1 - Math.abs(inputTranslationDir - currentTranslationDir) / (Math.PI / 2); + 1 + - SwerveUtils.angleDifference(inputTranslationDir, currentTranslationDir) + / (Math.PI / 2); + + double dirLimit = + Math.max( + 0, + DriveConstants.MAX_DRIVE_ANGULAR_VELOCITY + * ((1 - currentTranslationMag / DriveConstants.MAX_SPEED_METERS_PER_SECOND) * 0.90 + + 0.10)); - dirLimiter.setRateLimit( - DriveConstants.MAX_DRIVE_ANGULAR_VELOCITY - * (1 - currentTranslationMag / DriveConstants.MAX_SPEED_METERS_PER_SECOND)); + double currentTime = MathSharedStore.getTimestamp(); this.currentTranslationMag = magLimiter.calculate(inputTranslationMag); - this.currentTranslationDir = dirLimiter.calculate(inputTranslationDir); + this.currentTranslationDir = + SwerveUtils.stepTowardsCircular( + currentTranslationDir, inputTranslationDir, dirLimit * (currentTime - prevTime)); + prevTime = currentTime; xSpeedCommanded = this.currentTranslationMag * Math.cos(currentTranslationDir); ySpeedCommanded = this.currentTranslationMag * Math.sin(currentTranslationDir); this.currentRotation = rotLimiter.calculate(rot) * DriveConstants.MAX_ANGULAR_SPEED; diff --git a/src/main/java/frc/utils/CustomSlewRateLimiter.java b/src/main/java/frc/utils/CustomSlewRateLimiter.java deleted file mode 100644 index 02b27c6..0000000 --- a/src/main/java/frc/utils/CustomSlewRateLimiter.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.utils; - -import edu.wpi.first.math.MathSharedStore; -import edu.wpi.first.math.MathUtil; - -public class CustomSlewRateLimiter { - private double rateLimit; - private double prevVal; - private double prevTime; - - public CustomSlewRateLimiter(double rateLimit, double initialVal) { - this.rateLimit = rateLimit; - this.prevVal = initialVal; - this.prevTime = MathSharedStore.getTimestamp(); - } - - public void setRateLimit(double rateLimit) { - this.rateLimit = rateLimit; - } - - public double calculate(double input) { - double currentTime = MathSharedStore.getTimestamp(); - double elapsedTime = currentTime - prevTime; - prevVal += MathUtil.clamp(input - prevVal, -rateLimit * elapsedTime, rateLimit * elapsedTime); - prevTime = currentTime; - return prevVal; - } -} diff --git a/src/main/java/frc/utils/SwerveUtils.java b/src/main/java/frc/utils/SwerveUtils.java index 36fac63..5f32b67 100644 --- a/src/main/java/frc/utils/SwerveUtils.java +++ b/src/main/java/frc/utils/SwerveUtils.java @@ -1,33 +1,6 @@ package frc.utils; public class SwerveUtils { - /** - * Take two angles in radians and find the combonation with the shortest distance - * - * @param self this angle - * @param other the other comparison angle - * @return An equivelent angle to self - */ - public static double optimizeAngle(double self, double other) { - double a = wrapAngle(other); - double b = wrapAngle(self); - - double b1 = b + 2.0 * Math.PI; - double b2 = b - 2.0 * Math.PI; - - double diff = Math.abs(a - b); - double diff1 = Math.abs(a - b1); - double diff2 = Math.abs(a - b2); - - if (diff < diff1 && diff < diff2) { - return b; - } else if (diff1 < diff && diff1 < diff2) { - return b1; - } else { - return b2; - } - } - /** * Steps a value towards a target with a specified step size. *