diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java index ef9c1b9..8de8d2f 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java +++ b/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. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 059d3c6..599e5e7 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/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;