chore: fixed logic and refactored

SaintsRobotics · Feb 27, 2024 · faee82d · faee82d
1 parent fce6055
commit faee82d
Show file tree

Hide file tree

Showing 2 changed files with 59 additions and 53 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -113,8 +113,10 @@ private void configureBindings() {
         .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(
-    //         DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5)));
+    // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new
+    // PathConstraints(
+    // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5,
+    // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5)));
 
     new JoystickButton(m_operatorController, Button.kX.value)
         .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
@@ -124,9 +126,9 @@ private void configureBindings() {
         .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
 
     new JoystickButton(m_operatorController, Button.kA.value)
-        .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_robotDrive));
+        .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_climberSubsystem));
     new JoystickButton(m_operatorController, Button.kB.value)
-        .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive));
+        .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_climberSubsystem));
 
     new Trigger(() -> {
       return m_driverController.getLeftTriggerAxis() > 0.5;

diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java
@@ -3,74 +3,78 @@
 import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
-
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ClimberConstants;
 
 import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
 import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff;
 import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
 
-public class ClimberSubsystem {
+public class ClimberSubsystem extends SubsystemBase{
+
+  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 final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH);
+  private boolean enableCompressor = true;
 
-    private boolean enableCompressor = true;
+  private Value m_state;
 
-    public ClimberSubsystem() {
-        m_leftSolenoid.set(kOff);
-        m_rightSolenoid.set(kOff);
+  public ClimberSubsystem() {
+        solenoidOff();
         m_compressor.disable();
         m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure);
     }
 
-    // Runs once every tick (~20ms)
-    public void periodic() {
-    }
+  // Runs once every tick (~20ms)
+  public void periodic() {
+    m_leftSolenoid.set(m_state);
+    m_rightSolenoid.set(m_state);
+  }
 
-    /**
-     * Sets the state of the solenoid to off
-     */
-    public void off() {
-        m_leftSolenoid.set(kOff);
-        m_rightSolenoid.set(kOff);
-    }
+  /**
+   * Sets the state of the solenoid to off
+   */
+  public void solenoidOff() {
+    m_state = kOff;
+  }
 
-    /**
-     * Extends both arms
-     */
-    public void forward() {
-        m_leftSolenoid.set(kForward);
-        m_rightSolenoid.set(kForward);
-    }
+  /**
+   * Extends both arms
+   */
+  public void forward() {
+    m_state = kForward;
+  }
 
-    /**
-     * Retracts both arms
-     */
-    public void reverse() {
-        m_leftSolenoid.set(kReverse);
-        m_rightSolenoid.set(kReverse);
-    }
-    /*
-     * Toggles the state of the climber
-     */
+  /**
+   * Retracts both arms
+   */
+  public void reverse() {
+    m_state = kReverse;
+  }
+  /*
+   * Toggles the state of the climber
+   */
 
-    public void toggle() {
-        m_leftSolenoid.toggle();
-        m_rightSolenoid.toggle();
+  public void toggle() {
+    if(m_state == kForward){
+      m_state = kReverse;
+    }else if(m_state == kReverse){
+      m_state = kForward;
     }
+  }
 
-    // Toggles the state of the compressor (on/off)
-    public void toggleCompresor() {
-        enableCompressor = !enableCompressor;
-        if (enableCompressor) {
-            m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure);
-        } else {
-            m_compressor.disable();
-        }
+  // Toggles the state of the compressor (on/off)
+  public void toggleCompresor() {
+    enableCompressor = !enableCompressor;
+    if (enableCompressor) {
+      m_compressor.enableAnalog(ClimberConstants.minPressure, ClimberConstants.maxPressure);
+    } else {
+      m_compressor.disable();
     }
+  }
 }