Merge branch 'climber' of https://github.com/SaintsRobotics/Crescendo…

…2024 into climber
SaintsRobotics · Feb 24, 2024 · 28f20a6 · 28f20a6
2 parents b21cbb4 + 07dd8c6
commit 28f20a6
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 30 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -5,15 +5,12 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Button;
@@ -40,8 +37,7 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
   private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
-  public final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
-
+  private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
   private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
 
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
@@ -103,7 +99,7 @@ public RobotContainer() {
    */
   private void configureBindings() {
     new JoystickButton(m_driverController, Button.kStart.value)
-        .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive));
+        .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive));
 
     // new JoystickButton(m_driverController, Button.kA.value).whileTrue(
     //     AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new PathConstraints(
@@ -116,6 +112,11 @@ private void configureBindings() {
         .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
         .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
 
+    new JoystickButton(m_operatorController, Button.kA.value)
+        .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_robotDrive));
+    new JoystickButton(m_operatorController, Button.kB.value)
+        .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive));
+
     new Trigger(() -> {
       return m_driverController.getRightTriggerAxis() > 0.5;
     }).whileTrue(new IntakeCommand(m_intakeSubsystem));

diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -3,7 +3,6 @@
 import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import frc.robot.Constants.ClimberConstants;
 
@@ -13,43 +12,46 @@
 
 public class ClimberSubsystem {
 
-    private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel);
-    private final DoubleSolenoid m_rightSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, ClimberConstants.rightForwardChannel, ClimberConstants.rightReverseChannel);
-
+    private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH,
+            ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel);
+    private final DoubleSolenoid m_rightSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH,
+            ClimberConstants.rightForwardChannel, ClimberConstants.rightReverseChannel);
+
     private final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH);
-    
+
     private boolean enableCompressor = true;
-    
-    public ClimberSubsystem(){
+
+    public ClimberSubsystem() {
         m_leftSolenoid.set(kOff);
         m_rightSolenoid.set(kOff);
         m_compressor.disable();
         m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure);
     }
-    
-    //Runs once every tick (~20ms)
+
+    // Runs once every tick (~20ms)
     public void periodic() {
     }
-    /*
-     * Sets the state of the solenoid to off 
+
+    /**
+     * Sets the state of the solenoid to off
      */
-    public void off(){
+    public void off() {
         m_leftSolenoid.set(kOff);
         m_rightSolenoid.set(kOff);
     }
 
-    //TODO: fix doc
-    /*
+    // TODO: fix doc after testing
+    /**
      * Extends both arms
      */
     public void forward() {
         m_leftSolenoid.set(kForward);
         m_rightSolenoid.set(kForward);
     }
 
-    //TODO:fix doc
-    /*
-     * Retracts the arm
+    // TODO:fix doc
+    /**
+     * Retracts both arms
      */
     public void reverse() {
         m_leftSolenoid.set(kReverse);
@@ -60,17 +62,16 @@ public void reverse() {
      */
 
     public void toggle() {
-        m_leftSolenoid.toggle();;
-        m_rightSolenoid.toggle();;
+        m_leftSolenoid.toggle();
+        m_rightSolenoid.toggle();
     }
 
-    //Toggles the state of the compressor (on/off)
-    public void toggleCompresor(){
+    // Toggles the state of the compressor (on/off)
+    public void toggleCompresor() {
         enableCompressor = !enableCompressor;
-        if (enableCompressor){
+        if (enableCompressor) {
             m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure);
-        }
-        else{
+        } else {
             m_compressor.disable();
         }
     }