Added high gear for shtuff

src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java

@@ -7,14 +7,18 @@
 
 package frc4388.robot.commands;
 
+import java.security.PublicKey;
+
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpiutil.math.MathUtil;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Pneumatics;
 import frc4388.utility.controller.IHandController;
 
 public class DriveWithJoystickDriveStraight extends CommandBase {
   Drive m_drive;
+  Pneumatics m_pneumatics;
   double m_targetGyro, m_currentGyro;
   double m_stopPos;
   long m_currTime, m_deltaTime;
@@ -22,6 +26,8 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
   long m_deadTimeout = 100;
   IHandController m_controller;
   boolean m_isInterrupted;
+  double highGearMultiplier = 1;
+  double lowGearMultiplier = 1;
 
   /**
    * Creates a new DriveWithJoystickDriveStraight to control the drivetrain with an Xbox controller.
@@ -35,6 +41,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
   public DriveWithJoystickDriveStraight(Drive subsystem, IHandController controller) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
+    m_pneumatics = m_drive.m_pneumaticsSubsystem;
     m_controller = controller;
     addRequirements(m_drive);
   }
@@ -84,9 +91,17 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
   }
 
   private void runDriveWithInput(double move, double steer) {
-    double cosMultiplier = .7;
+
+    double cosMultiplier = 0.7;    
     double steerOutput = 0;
     double deadzone = .1;
+
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      cosMultiplier = 0.7;
+    } else {
+      cosMultiplier = 0.7;
+    }
+
     /* Curves the steer output to be similarily gradual */
     if (steer > 0) {
       steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steer) + cosMultiplier;
@@ -107,8 +122,13 @@ public class DriveWithJoystickDriveStraight extends CommandBase {
    */
   private void resetGyroTarget() {
     //m_targetGyro = m_currentGyro;
-    m_targetGyro =  m_currentGyro
-                    + m_drive.getTurnRate();
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      m_targetGyro =  m_currentGyro
+                    + highGearMultiplier * m_drive.getTurnRate();
+    } else {
+      m_targetGyro =  m_currentGyro
+                    + lowGearMultiplier * m_drive.getTurnRate();
+    }
   }
 
   // Called once the command ends or is interrupted.