From f9452eb562243aa342f2df16258c8ef37bcf2cba Mon Sep 17 00:00:00 2001
From: Matt Soucy <git@msoucy.me>
Date: Sat, 1 Jun 2024 11:47:28 -0400
Subject: [PATCH] feat: Polar scaling

---
 src/main/java/frc/robot/subsystems/Drive.java | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
index f12ca9f..5d2230a 100644
--- 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);
     }