diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6fb19d4..6accd17 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -97,6 +97,14 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); + // sets solenoids into high gear + new JoystickButton(getDriverJoystick(), XboxController.START_BUTTON) + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + + // sets solenoids into low gear + new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON) + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + //new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) // .whenPressed(new RunCommand(() -> m_robotDrive.runMotionMagicPID(5000, 0), m_robotDrive)); // interrupts any running command diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index ee725ad..07ba57a 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -20,6 +20,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; +import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -49,6 +50,8 @@ public class Drive extends SubsystemBase { public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS; public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS; + public DoubleSolenoid speedShift; + /** * Add your docs here. */ @@ -61,6 +64,8 @@ public class Drive extends SubsystemBase { m_pigeon.configFactoryDefault(); resetGyroYaw(); + speedShift = new DoubleSolenoid(7,0,1); + /* set back motors as followers */ m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); @@ -414,4 +419,17 @@ public class Drive extends SubsystemBase { m_pigeon.setYaw(0); m_pigeon.setAccumZAngle(0); } + + /** + * Set to high or low gear based on boolean state, true = high, false = low + * @param state Chooses between high or low gear + */ + public void setShiftState(boolean state) { + if (state == true) { + speedShift.set(DoubleSolenoid.Value.kForward); + } + if (state == false) { + speedShift.set(DoubleSolenoid.Value.kReverse); + } + } }