Remove angle optimization changes and fix velocity scaling

team581 · Oct 18, 2023 · e5534c0 · e5534c0
1 parent 5847868
commit e5534c0
Showing 1 changed file with 12 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/swerve/SwerveModule.java b/src/main/java/frc/robot/swerve/SwerveModule.java
@@ -213,18 +213,21 @@ private SwerveModuleState optimize(SwerveModuleState desiredState) {
     SwerveModuleState currentState = getState();
     var delta = desiredState.angle.minus(currentState.angle);
 
+    Rotation2d newAngle = desiredState.angle;
+    double newVelocity = desiredState.speedMetersPerSecond;
+
+    if (Math.abs(delta.getDegrees()) > 90.0) {
+      newVelocity = newVelocity * -1;
+      newAngle = newAngle.rotateBy(Rotation2d.fromDegrees(180.0));
+    }
+
     double angleErrorPercentage =
-        (currentState.angle.getDegrees() / desiredState.angle.getDegrees());
-    double velocity = desiredState.speedMetersPerSecond;
+        Math.abs(currentState.angle.getDegrees() / newAngle.getDegrees());
+
     if (angleErrorPercentage < 0.9) {
-      velocity = velocity * angleErrorPercentage;
+      newVelocity = newVelocity * angleErrorPercentage;
     }
 
-    if (Math.abs(delta.getDegrees()) > 90.0 && currentState.speedMetersPerSecond < 0.2) {
-      return new SwerveModuleState(
-          -velocity, desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0)));
-    } else {
-      return new SwerveModuleState(velocity, desiredState.angle);
-    }
+    return new SwerveModuleState(newVelocity, newAngle);
   }
 }