From 1503c7f0270c63e6a90140fae2e70077840adabc Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Thu, 28 Oct 2021 17:59:34 -0600
Subject: [PATCH] Fix mode switching - Compact joystick drive math

---
 src/main/java/frc4388/robot/Constants.java    |  8 +++--
 .../commands/drive/DriveWithJoystick.java     | 34 ++++---------------
 2 files changed, 13 insertions(+), 29 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 930549c..90b52bd 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -38,7 +38,7 @@ public final class Constants {
             int i = mode.ordinal() + 1;
             Mode[] values = values();
             i = i >= values.length ? 0 : i;
-            mode = values[i];
+            set(values[i]);
             System.out.println(mode);
         }
     }
@@ -83,7 +83,7 @@ public final class Constants {
         public static final int DRIVE_CRUISE_VELOCITY = 30000;
         public static final int DRIVE_ACCELERATION = 23000;
 
-        private static final double[] DRIVE_WITH_JOYSTICK_FACTOR_MODES = { 1.0, 0.5 };
+        private static final double[] DRIVE_WITH_JOYSTICK_FACTOR_MODES = { 1.0, 0.8 };
         public static double DRIVE_WITH_JOYSTICK_FACTOR;
 
         public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5);
@@ -264,4 +264,8 @@ public final class Constants {
         public static final int XBOX_OPERATOR_ID = 1;
         public static final int BUTTON_FOX_ID = 2;
     }
+
+    public static final class MathConstants {
+        public static final double PI_2 = Math.PI / 2;
+    }
 }
diff --git a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java
index e2949ff..84b8c9a 100644
--- a/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/drive/DriveWithJoystick.java
@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.Constants;
 import frc4388.robot.Robot;
 import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.Constants.MathConstants;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Pneumatics;
 import frc4388.utility.controller.IHandController;
@@ -44,34 +45,13 @@ public class DriveWithJoystick extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double moveInput = m_controller.getLeftYAxis(); 
-    double steerInput = m_controller.getRightXAxis();
+    double moveInput = m_controller.getLeftYAxis() * DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR;
+    double steerInput = m_controller.getRightXAxis() * DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR;
 
-    double moveOutput = 0;
-    double steerOutput = 0;
-    if (moveInput >= 0){
-      moveOutput = -Math.cos(1.571*moveInput)+1;
-    } else {
-      moveOutput = Math.cos(1.571*moveInput)-1;
-    }
-    moveOutput *= DriveConstants.DRIVE_WITH_JOYSTICK_FACTOR;
-    System.out.println(moveOutput);
-    double cosMultiplier;
-    double deadzone = .1;
-
-    if (m_pneumatics.m_isSpeedShiftHigh) {
-      cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH;
-    } else {
-      cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW;
-    }
-
-    if (steerInput > 0){
-      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
-    } else if (steerInput < 0) {
-      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
-    } else {
-      steerOutput = 0;
-    }
+    double moveOutput = (moveInput >= 0 ? -1 : 1) * (Math.cos(MathConstants.PI_2 * moveInput) - 1);
+    double cosMultiplier = m_pneumatics.m_isSpeedShiftHigh ? DriveConstants.COS_MULTIPLIER_HIGH : DriveConstants.COS_MULTIPLIER_LOW;
+    double deadzone = 0.1;
+    double steerOutput = (steerInput == 0 ? 0 : (steerInput > 0 ? -1 : 1)) * (Math.cos(MathConstants.PI_2 * steerInput) * (cosMultiplier - deadzone) + cosMultiplier);
 
     /*
     double outputLimit = 0.8;