From fa34ef00fe6d4f4866c5f0a457214a692a63e900 Mon Sep 17 00:00:00 2001
From: Tylr-J42 <84476781+Tylr-J42@users.noreply.github.com>
Date: Mon, 19 May 2025 19:06:48 -0400
Subject: [PATCH] exponents on joystick now implemented correctly

---
 src/main/java/frc/robot/RobotContainer.java        | 4 ++--
 src/main/java/frc/robot/constants/OIConstants.java | 2 ++
 src/main/java/frc/robot/subsystems/Drivetrain.java | 6 ++++--
 3 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 177a46f..6fd15ec 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -111,8 +111,8 @@ public class RobotContainer {
 
 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
-				() -> Math.pow(driver.getLeftY(), 3),
-				() -> Math.pow(driver.getLeftX(), 3),
+				() -> driver.getLeftY(),
+				() -> driver.getLeftX(),
 				() -> driver.getRightX(), 
 				() -> true
 			)
diff --git a/src/main/java/frc/robot/constants/OIConstants.java b/src/main/java/frc/robot/constants/OIConstants.java
index a9b0552..3f828d2 100644
--- a/src/main/java/frc/robot/constants/OIConstants.java
+++ b/src/main/java/frc/robot/constants/OIConstants.java
@@ -9,4 +9,6 @@ public class OIConstants {
     public static final String kAutoTab = "Auto Tab";
     public static final String kSensorsTab = "Sensors Tab";
     public static final String kApriltagTab = "Apriltag Tab";
+
+    public static final double kJoystickExponential = 3;
 }
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 89adee5..a47243c 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -323,8 +323,10 @@ public class Drivetrain extends SubsystemBase {
      */
     public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
         // Convert the commanded speeds into the correct units for the drivetrain
-        double xSpeedDelivered = xSpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
-        double ySpeedDelivered = ySpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+        double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
+
+        double xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
+        double ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
         double rotDelivered = rot * DrivetrainConstants.kMaxAngularSpeed;
 
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(