diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e09b86a..bf12a83 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -52,16 +52,16 @@ public final class Constants { /* PID Constants Drive*/ public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); - public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55); - public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); - public static final int DRIVE_CRUISE_VELOCITY = 25000; - public static final int DRIVE_ACCELERATION = 21000; + public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.05, 0.0, 1.0, 0.025, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 2.0, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0, 0.025, 0, 1.0); + public static final int DRIVE_CRUISE_VELOCITY = 30000; + public static final int DRIVE_ACCELERATION = 23000; - public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5); - public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.55); - public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.5); + public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.2, 0.0, 0.0, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; public static final int DRIVE_ACCELERATION_HIGH = 7000; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 4f894c9..9e827dd 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -77,7 +77,7 @@ public class Robot extends TimedRobot { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - //m_robotContainer.setDriveGearState(true); + m_robotContainer.setDriveGearState(false); m_robotContainer.resetOdometry(); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); @@ -105,7 +105,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - //m_robotContainer.setDriveGearState(true); + m_robotContainer.setDriveGearState(false); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); // This makes sure that the autonomous stops running when diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 718abcb..4793fc2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -29,12 +29,15 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.AutoPath1FromCenter; +import frc4388.robot.commands.AutoPath2FromRight; import frc4388.robot.commands.CalibrateShooter; import frc4388.robot.commands.DrivePositionMPAux; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystick; +import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; @@ -123,7 +126,10 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - m_robotDrive.setDefaultCommand(new DriveWithJoystick(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 m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // runs the turret with joystick @@ -150,15 +156,15 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 192)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, m_robotPneumatics, 24.0)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 6000)); + .whenPressed(new TurnDegrees(m_robotDrive, 90)); // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new GotoCoordinates(m_robotDrive, 12, 12, 0)); + .whenPressed(new Wait(m_robotDrive, 0, 0)); // X driver test button new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) @@ -241,31 +247,9 @@ public class RobotContainer { RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); - - //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those - //This assumes that we are positioned against the right wall with our shooter facing the target. - return new SequentialCommandGroup(new Wait(0, m_robotDrive), - //add aim command - //add shooter command - //new DriveStraightToPositionMM(m_robotDrive, 48.0), - /*new ParallelCommandGroup( - new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new DriveStraightToPositionMM(m_robotDrive, 36.0)), - new ParallelCommandGroup( - new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new DriveStraightToPositionMM(m_robotDrive, 36.0)), - new StorageIntakeGroup(m_robotIntake, m_robotStorage),*/ - //add aim command - //add shooter command -//Below this would be the picking up additional balls outside of those in the trench directly behind us + return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics); + //return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics); - //new GotoCoordinates(m_robotDrive, 36, 36), - new GotoCoordinates(m_robotDrive, 36, 36, -90));//, - //new StorageIntakeGroup(m_robotIntake, m_robotStorage), - //new TurnDegrees(m_robotDrive, 75), - //new DriveStraightToPositionMM(m_robotDrive, 18.0), - //new TurnDegrees(m_robotDrive, -45), - //new DriveStraightToPositionMM(m_robotDrive, 12.0)); } TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( @@ -365,4 +349,4 @@ public class RobotContainer { { return m_driverXbox.getJoyStick(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java new file mode 100644 index 0000000..4ec080a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class AutoPath1FromCenter extends SequentialCommandGroup { + Drive m_drive; + Pneumatics m_pneumatics; + + /** + * Creates a new AutoPath1FromCenter. + */ + public AutoPath1FromCenter(Drive subsystem, Pneumatics subsystem2) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + m_drive = subsystem; + m_pneumatics = subsystem2; + + addCommands( new Wait(m_drive, 0, 1), + //shoot pre-loaded 3 balls + new GotoCoordinates(m_drive, m_pneumatics, 75, 44, -90), + //Start Intake Ball 1 + new GotoCoordinates(m_drive, m_pneumatics, 0, 12), + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + //Start Intake Ball 2 + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + //Start Intake Ball 3 + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new Wait(m_drive, 0, 2) + //Shoot 3 Balls + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java new file mode 100644 index 0000000..43657c3 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AutoPath2FromRight.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class AutoPath2FromRight extends SequentialCommandGroup { + Drive m_drive; + Pneumatics m_pneumatics; + + /** + * Creates a new AutoPath2FromRight. + */ + public AutoPath2FromRight(Drive subsystem, Pneumatics subsystem2) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + m_drive = subsystem; + m_pneumatics = subsystem2; + + addCommands( new Wait(m_drive, 0, 1), + new GotoCoordinates(m_drive, m_pneumatics, 0, 77), + //Start Intake Ball 1 + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + //Start Intake Ball 2 + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + //Shoot 5 Balls + new GotoCoordinates(m_drive, m_pneumatics, 0, 28), + //Start Intake Ball 1 (second round) + new GotoCoordinates(m_drive, m_pneumatics, 0, 8), + //Start Moving to 4th Ball + new GotoCoordinates(m_drive, m_pneumatics, 60, -50), + new Wait(m_drive, 0, 2) + ); + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java index 925f07a..c9aa927 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java @@ -11,9 +11,11 @@ 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; public class DriveStraightToPositionMM extends CommandBase { Drive m_drive; + Pneumatics m_pneumatics; double m_targetPosIn; double m_targetPosOut; double m_targetGyro; @@ -25,10 +27,19 @@ public class DriveStraightToPositionMM extends CommandBase { * @param subsystem drive subsystem * @param targetPos distance to travel in inches */ - public DriveStraightToPositionMM(Drive subsystem, double targetPos) { + public DriveStraightToPositionMM(Drive subsystem, Pneumatics subsystem2, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + m_pneumatics = subsystem2; + try { + if (m_pneumatics.m_isSpeedShiftHigh) { + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2; + } else { + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + } + } catch (Exception e) { + System.err.println("Error In Motion Magic Switch Gains."); + } addRequirements(m_drive); //SmartDashboard.putNumber("Distance Target Inches", targetPos); } diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index c31944e..a457e26 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -11,9 +11,11 @@ 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; public class DriveStraightToPositionPID extends CommandBase { Drive m_drive; + Pneumatics m_pneumatics; double m_targetPosIn; double m_targetPosOut; double m_targetGyro; @@ -24,10 +26,19 @@ public class DriveStraightToPositionPID extends CommandBase { * @param subsystem drive subsystem * @param targetPos distance to travel in inches */ - public DriveStraightToPositionPID(Drive subsystem, double targetPos) { + public DriveStraightToPositionPID(Drive subsystem, Pneumatics subsystem2, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + m_pneumatics = subsystem2; + try { + if (m_pneumatics.m_isSpeedShiftHigh) { + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2; + } else { + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + } + } catch (Exception e) { + System.err.println("Error In Motion Magic Switch Gains."); + } addRequirements(m_drive); //SmartDashboard.putNumber("Distance Target Inches", targetPos); } diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 54bc129..3c6e88f 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -10,11 +10,13 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; import frc4388.utility.controller.IHandController; public class DriveWithJoystick extends CommandBase { private Drive m_drive; private IHandController m_controller; + private Pneumatics m_pneumatics; /** * Creates a new DriveWithJoystick to control the drivetrain with an Xbox controller. @@ -24,9 +26,10 @@ public class DriveWithJoystick extends CommandBase { * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} */ - public DriveWithJoystick(Drive subsystem, IHandController controller) { + public DriveWithJoystick(Drive subsystem, Pneumatics subsystem2, IHandController controller) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; + m_pneumatics = subsystem2; m_controller = controller; addRequirements(m_drive); } @@ -51,6 +54,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) { @@ -58,23 +68,31 @@ public class DriveWithJoystick extends CommandBase { } else { steerOutput = 0; } - double tempOutputLimit = 0.8; - boolean isOutputLimited = false; + /* + double outputLimit = 0.8; - if (isOutputLimited) { - if (moveOutput > tempOutputLimit) { - moveOutput = tempOutputLimit; - } else if(moveOutput < -tempOutputLimit) { - moveOutput = -tempOutputLimit; + boolean isMoveOutputLimited = false; + boolean isSteerOutputLimited = false; + + if (m_pneumatics.m_isSpeedShiftHigh) { + if (isMoveOutputLimited) { + if (moveOutput > outputLimit) { + moveOutput = outputLimit; + } else if(moveOutput < -outputLimit) { + moveOutput = -outputLimit; + } } - if (steerOutput > tempOutputLimit) { - steerOutput = tempOutputLimit; - } else if(steerOutput < -tempOutputLimit) { - steerOutput = -tempOutputLimit; + if (isSteerOutputLimited) { + if (steerOutput > outputLimit) { + steerOutput = outputLimit; + } else if(steerOutput < -outputLimit) { + steerOutput = -outputLimit; + } } - } + } + */ m_drive.driveWithInput(moveOutput, steerOutput); } diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java index 5580cfc..6ca3deb 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java @@ -10,12 +10,14 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; // NOTE: Consider using this command inline, rather than writing a subclass. For more // information, see: // https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html public class GotoCoordinates extends SequentialCommandGroup { Drive m_drive; + Pneumatics m_pneumatics; double m_xTarget; double m_yTarget; @@ -26,10 +28,11 @@ public class GotoCoordinates extends SequentialCommandGroup { /** * Creates a new GotoPosition. */ - public GotoCoordinates(Drive subsystem, double xTarget, double yTarget, double endAngle) { + public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); m_drive = subsystem; + m_pneumatics = subsystem2; m_xTarget = xTarget; m_yTarget = yTarget; @@ -38,13 +41,30 @@ public class GotoCoordinates extends SequentialCommandGroup { m_currentAngle = calcAngle(); - SmartDashboard.putNumber("Current Angle Coordinates", m_currentAngle); - m_endAngle = endAngle; + addCommands( new TurnDegrees(m_drive, m_currentAngle), + new Wait(m_drive, 0, 0), + new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), + new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); + } - addCommands( new TurnDegrees(m_drive, m_currentAngle), - new DriveStraightToPositionMM(m_drive, m_hypotDist), + public GotoCoordinates(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) { + m_drive = subsystem; + m_pneumatics = subsystem2; + + m_xTarget = xTarget; + m_yTarget = yTarget; + + m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); + + m_currentAngle = calcAngle(); + + m_endAngle = m_currentAngle; + + addCommands( new TurnDegrees(m_drive, m_currentAngle), + new Wait(m_drive, 0, 0), + new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); } diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java index 934b508..580d279 100644 --- a/src/main/java/frc4388/robot/commands/Wait.java +++ b/src/main/java/frc4388/robot/commands/Wait.java @@ -17,15 +17,19 @@ public class Wait extends CommandBase { long m_waitTime; long m_currentTime; SubsystemBase m_subsystem; + int m_waitNum; + + int counter = 0; /** * Creates a new WaitCommand. */ - public Wait(float seconds, SubsystemBase subsystem) { + public Wait(SubsystemBase subsystem, double seconds, int waitNum) { // Use addRequirements() here to declare subsystem dependencies. m_waitTime = (long) (seconds * 1000); m_subsystem = subsystem; + m_waitNum = waitNum; addRequirements(m_subsystem); } @@ -40,8 +44,15 @@ public class Wait extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + + if (counter == 0) { + SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime); + } + m_currentTime = System.currentTimeMillis(); SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime)); + + counter ++; } // 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 096feff..edcbac5 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -127,7 +127,15 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); /* PID for Front Motor Control in Teleop */ - setRightMotorGains(false); + try { + if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { + setRightMotorGains(true); + } else { + setRightMotorGains(false); + } + } catch (Exception e) { + System.err.println("Error while trying to switch gains."); + } /* PID for Back Motor Control in Tank Drive Vel */ m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);