From 7859fa33185acc9448f99f1e1bb701782c7b2257 Mon Sep 17 00:00:00 2001
From: Aarav Shah <aaravshah18@gmail.com>
Date: Sat, 29 Feb 2020 13:24:22 -0700
Subject: [PATCH] added 80% output only on high gear the right way

---
 .../robot/commands/DriveWithJoystick.java     | 41 +++++++++++--------
 1 file changed, 25 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
index efddb8f..4d9777a 100644
--- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
+++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java
@@ -53,6 +53,13 @@ public class DriveWithJoystick extends CommandBase {
 
     double cosMultiplier = 1.0;
     double deadzone = .1;
+
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      cosMultiplier = 0.8;
+    } else {
+      cosMultiplier = 1.0;
+    }
+
     if (steerInput > 0){
       steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
     } else if (steerInput < 0) {
@@ -60,29 +67,31 @@ public class DriveWithJoystick extends CommandBase {
     } else {
       steerOutput = 0;
     }
-    
+
+    /*
     double outputLimit = 0.8;
 
     boolean isMoveOutputLimited = false;
-    boolean isSteerOutputLimited = true;
+    boolean isSteerOutputLimited = false;
     
-  if (m_pneumatics.m_isSpeedShiftHigh) {
-    if (isMoveOutputLimited) {
-      if (moveOutput > outputLimit) {
-        moveOutput = outputLimit;
-      } else if(moveOutput < -outputLimit) {
-        moveOutput = -outputLimit;
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      if (isMoveOutputLimited) {
+        if (moveOutput > outputLimit) {
+          moveOutput = outputLimit;
+        } else if(moveOutput < -outputLimit) {
+          moveOutput = -outputLimit;
+        }
       }
-    }
 
-    if (isSteerOutputLimited) {
-      if (steerOutput > outputLimit) {
-        steerOutput = outputLimit;
-      } else if(steerOutput < -outputLimit) {
-        steerOutput = -outputLimit;
+      if (isSteerOutputLimited) {
+        if (steerOutput > outputLimit) {
+          steerOutput = outputLimit;
+        } else if(steerOutput < -outputLimit) {
+          steerOutput = -outputLimit;
+        }
       }
-    }
-  }
+    } 
+  */
     
     m_drive.driveWithInput(moveOutput, steerOutput);
   }