Change algorithm

Switch to using preexisting swerve util functions
Ocebots · Jan 4, 2024 · 0fd202d · 0fd202d
1 parent 71d2a6d
commit 0fd202d
Show file tree

Hide file tree

Showing 4 changed files with 27 additions and 71 deletions.
diff --git 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
@@ -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,34 +145,44 @@ 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;
       } else if (inputTranslationMag < 0.1) {
         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
diff --git 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.
    *