Merge pull request #51 from Team4388/add-high-gear-drive-straight

WIP Add high gear drive straight

src/main/java/frc4388/robot/Constants.java

@@ -42,6 +42,8 @@ public final class Constants {
         public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG =
             new SupplyCurrentLimitConfiguration(false, 60, 40, 2);
         public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops
+        public static final double COS_MULTIPLIER_LOW = 1.0;
+        public static final double COS_MULTIPLIER_HIGH = 0.8; 
 
         /* Drive Train Characteristics */
         public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13;

src/main/java/frc4388/robot/RobotContainer.java

@@ -127,7 +127,7 @@ public class RobotContainer {
         /* Default Commands */
         // drives the robot with a two-axis input from the driver controller
         
-        m_robotDrive.setDefaultCommand(new DriveWithJoystickDriveStraight(m_robotDrive, getDriverController()));
+        m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController()));
         //m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController()));
         
         // drives intake with input from triggers on the opperator controller

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

@@ -9,6 +9,7 @@ package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Pneumatics;
 import frc4388.utility.controller.IHandController;
@@ -52,13 +53,13 @@ public class DriveWithJoystick extends CommandBase {
       moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
-    double cosMultiplier = 1.0;
+    double cosMultiplier;
     double deadzone = .1;
 
     if (m_pneumatics.m_isSpeedShiftHigh) {
-      cosMultiplier = 0.8;
+      cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH;
     } else {
-      cosMultiplier = 1.0;
+      cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW;
     }
 
     if (steerInput > 0){

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;    
     double steerOutput = 0;
     double deadzone = .1;
+
+    if (m_pneumatics.m_isSpeedShiftHigh) {
+      cosMultiplier = DriveConstants.COS_MULTIPLIER_HIGH;
+    } else {
+      cosMultiplier = DriveConstants.COS_MULTIPLIER_LOW;
+    }
+
     /* 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.

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -53,7 +53,7 @@ public class Drive extends SubsystemBase {
   public Orchestra m_orchestra;
 
   /* Pneumatics Subsystem */
-  Pneumatics m_pneumaticsSubsystem;
+  public Pneumatics m_pneumaticsSubsystem;
 
   /* Low Gear Gains */
   public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;