chrck-in

Team2470 · Oct 4, 2023 · 64f0265 · 64f0265
1 parent 98b2876
commit 64f0265
Showing 1 changed file with 9 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/commands/DriveWithController.java b/src/main/java/frc/robot/commands/DriveWithController.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.CircularBuffer;
 import edu.wpi.first.wpilibj.XboxController;
@@ -103,17 +104,16 @@ public void execute() {
         if (moving) {
             // xMove = Math.copySign(xMove * xMove, xMove);
             // yMove = Math.copySign(yMove * yMove, yMove);
-            // rotate = Math.copySign(rotate * rotate, rotate);
-
-
-            double amplitudeSquared = Math.pow(xMove, 2) + Math.pow(yMove, 2);
+            rotate = Math.copySign(rotate * rotate, rotate);
+
+            Translation2d moveTranslation = new Translation2d(xMove, yMove);
+            double moveSpeed = moveTranslation.getNorm() * moveTranslation.getNorm();
+            Rotation2d angle = moveTranslation.getAngle();
 
-            xMove = Math.copySign(amplitudeSquared * Math.sin(rotate), xMove);
-            yMove = Math.copySign(amplitudeSquared * Math.sin(rotate), yMove);
+            xMove = moveSpeed * angle.getCos();
+            yMove = moveSpeed * angle.getSin();
 
-
-
-
+
             if (drive.getSlowMode() || controller.getXButton()){
                 xMove *= 0.3;
                 yMove *= 0.3;