diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4b397d1..2f9a976 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -35,8 +35,8 @@ public static class DrivetrainConstants { } public static class PneumaticConstants { - public static final int kForwardChannel = 0; // TODO: Update these channel values - public static final int kReverseChannel = 2; + public static final int kForwardChannel = 2; // TODO: Update these channel values + public static final int kReverseChannel = 0; } public static class AutoConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5aa6512..6cb19a5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,8 +30,6 @@ public class RobotContainer { public static final Pneumatics m_piston = new Pneumatics(); public static double speedUp = 0.0; - public static DoubleSolenoid m_doubleSolenoid = new DoubleSolenoid(15, PneumaticsModuleType.CTREPCM, - PneumaticConstants.kForwardChannel, PneumaticConstants.kReverseChannel); // public static Compressor m_compressor = new // Compressor(PneumaticsModuleType.CTREPCM); @@ -61,8 +59,8 @@ private void configureBindings() { // Set the default command for the drivetrain to drive using the joysticks m_drivetrain.setDefaultCommand( - new RunCommand(() -> m_drivetrain.arcadeDrive(m_driverController.getLeftY() * (0.5 + speedUp), - m_driverController.getRightX() * (0.5 + speedUp)), m_drivetrain)); + new RunCommand(() -> m_drivetrain.tankDrive(m_driverController.getLeftY() * (0.5 + speedUp), + m_driverController.getRightY() * (0.5 + speedUp)), m_drivetrain)); /* * Extends piston when operator presses the "b" button Retracts piston when @@ -71,8 +69,8 @@ private void configureBindings() { * pushing and pulling air in the piston when the operator presses the "y" * button */ - m_operatorController.b().onTrue(PneumaticCommands.pistonExtend()); - m_operatorController.a().onTrue(PneumaticCommands.pistonRetract()); + m_operatorController.leftTrigger().onTrue(PneumaticCommands.pistonExtend()); + m_operatorController.rightTrigger().onTrue(PneumaticCommands.pistonRetract()); m_operatorController.x().onTrue(PneumaticCommands.solenoidOff()); m_operatorController.y().onTrue(PneumaticCommands.solenoidToggle()); } diff --git a/src/main/java/frc/robot/subsystems/Auto.java b/src/main/java/frc/robot/subsystems/Auto.java index cdeeaa2..0360612 100644 --- a/src/main/java/frc/robot/subsystems/Auto.java +++ b/src/main/java/frc/robot/subsystems/Auto.java @@ -5,12 +5,16 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.RobotContainer; +import frc.robot.commands.PneumaticCommands; public class Auto { public static Command getAuto(RobotContainer robot) { return new SequentialCommandGroup( // basic auto to leave zone. - new WaitCommand(2), new InstantCommand(() -> RobotContainer.m_drivetrain.tankDrive(0.5, .5)), - new WaitCommand(2), new InstantCommand(() -> RobotContainer.m_drivetrain.tankDrive(0, 0))); + PneumaticCommands.pistonExtend(), + new WaitCommand(2), new InstantCommand(() -> RobotContainer.m_drivetrain.tankDrive(-0.5, -0.5)), + PneumaticCommands.pistonRetract(), + new WaitCommand(2), new InstantCommand(() -> RobotContainer.m_drivetrain.tankDrive(0, 0)) + ); } } diff --git a/src/main/java/frc/robot/subsystems/Pneumatics.java b/src/main/java/frc/robot/subsystems/Pneumatics.java index e4883a1..25607f1 100644 --- a/src/main/java/frc/robot/subsystems/Pneumatics.java +++ b/src/main/java/frc/robot/subsystems/Pneumatics.java @@ -1,6 +1,8 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import frc.robot.Constants.PneumaticConstants; import frc.robot.RobotContainer; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -8,19 +10,21 @@ public class Pneumatics extends SubsystemBase { // public Pneumatics() { // RobotContainer.m_compressor.enableDigital(); // } + public DoubleSolenoid m_doubleSolenoid = new DoubleSolenoid(15, PneumaticsModuleType.CTREPCM, + PneumaticConstants.kForwardChannel, PneumaticConstants.kReverseChannel); public void extendPiston() { - RobotContainer.m_doubleSolenoid.set(DoubleSolenoid.Value.kForward); + m_doubleSolenoid.set(DoubleSolenoid.Value.kForward); } public void retractPiston() { - RobotContainer.m_doubleSolenoid.set(DoubleSolenoid.Value.kReverse); + m_doubleSolenoid.set(DoubleSolenoid.Value.kReverse); } public void setSolenoidOff() { - RobotContainer.m_doubleSolenoid.set(DoubleSolenoid.Value.kOff); + m_doubleSolenoid.set(DoubleSolenoid.Value.kOff); } public void toggleSolenoid() { - RobotContainer.m_doubleSolenoid.toggle(); + m_doubleSolenoid.toggle(); } }