feat: Polar scaling

chopshop-166 · Jul 2, 2024 · f9452eb · f9452eb
1 parent 9ed3340
commit f9452eb
Showing 1 changed file with 6 additions and 6 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
@@ -124,12 +124,12 @@ private void deadbandMove(final double xSpeed, final double ySpeed,
         double xInput = deadband.applyAsDouble(xSpeed);
         double yInput = deadband.applyAsDouble(ySpeed);
 
-        final double translateXSpeed = xInput
-                * maxDriveSpeedMetersPerSecond * speedCoef;
-        final double translateYSpeed = yInput
-                * maxDriveSpeedMetersPerSecond * speedCoef;
-        final double rotationSpeed = rotationInput
-                * maxRotationRadiansPerSecond * rotationCoef;
+        double r = Math.hypot(xInput, yInput) * maxDriveSpeedMetersPerSecond * speedCoef;
+        double angle = Math.atan2(yInput, xInput);
+
+        final double translateXSpeed = r * Math.cos(angle);
+        final double translateYSpeed = r * Math.sin(angle);
+        final double rotationSpeed = rotationInput * maxRotationRadiansPerSecond * rotationCoef;
         move(translateXSpeed, translateYSpeed, rotationSpeed, isRobotCentric);
     }