From 8240f8f3c4b7aa30af659f70c78f51457e3ba1db Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Wed, 15 Mar 2023 09:57:01 -0600
Subject: [PATCH] drift fix (not tuned)

---
 src/main/java/frc4388/robot/Constants.java              | 7 ++++---
 src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 6 +++---
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 83ac014..2d8438b 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -24,9 +24,10 @@ import frc4388.utility.LEDPatterns;
 public final class Constants {
   public static final class SwerveDriveConstants {
 
-    public static final double MAX_ROT_SPEED = 1.5;
-    public static final double MIN_ROT_SPEED = 0.8;
-    public static       double ROTATION_SPEED = MAX_ROT_SPEED;
+    public static final double MAX_ROT_SPEED        = 1.5;
+    public static final double MIN_ROT_SPEED        = 0.8;
+    public static       double ROTATION_SPEED       = MAX_ROT_SPEED;
+    public static       double ROT_CORRECTION_SPEED = MIN_ROT_SPEED;
 
     public static final class IDs {
       public static final int LEFT_FRONT_WHEEL_ID = 2;
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index fcaadb9..1f28f3e 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -58,9 +58,9 @@ public class SwerveDrive extends SubsystemBase {
       double rot = 0;
       if (rightStick.getNorm() > 0.1) {
         rotTarget = gyro.getRotation2d();
-        rot = rightStick.getX();
+        rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
       } else {
-        rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
+        rot = rotTarget.minus(gyro.getRotation2d()).getRadians() * SwerveDriveConstants.ROT_CORRECTION_SPEED;
       }
 
       // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
@@ -68,7 +68,7 @@ public class SwerveDrive extends SubsystemBase {
       Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot, gyro.getRotation2d().times(-1));
     } else {
       // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);