Fix party button attempt 2

lasarobotics · Mar 23, 2024 · 76c7409 · 76c7409
1 parent d1d7988
commit 76c7409
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -143,7 +143,7 @@ private void configureBindings() {
     PRIMARY_CONTROLLER.povRight().whileTrue(feedThroughCommand());
 
     // DPAD left - PARTY BUTTON!!
-    PRIMARY_CONTROLLER.povLeft().whileTrue(partyButton());
+    PRIMARY_CONTROLLER.povLeft().whileTrue(partyMode());
   }
 
   /**
@@ -273,10 +273,10 @@ private Command aimAtObject() {
    * PARTY BUTTON!!!!
    * @return Command that spins the robot and moves the shooter up and down
    */
-  private Command partyButton() {
+  private Command partyMode() {
     return Commands.parallel(
       DRIVE_SUBSYSTEM.driveCommand(() -> 0.0, () -> 0.0, () -> 1.0),
-      SHOOTER_SUBSYSTEM.shootPartyButton()
+      SHOOTER_SUBSYSTEM.shootPartyMode()
     );
   }
 

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java
@@ -575,18 +575,14 @@ public Command passCommand() {
    * Move shooter up and down
    * @return Command to move the shooter between upper and lower limits
    */
-  public Command shootPartyButton() {
+  public Command shootPartyMode() {
     return run(() -> {
-      State top = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(m_angleConfig.getUpperLimit()));
-      State bottom = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(m_angleConfig.getLowerLimit()));
-      boolean currentTargetTop = true;
-      if (currentTargetTop)
-        setState(top);
-      else
+      State top = new State(ZERO_FLYWHEEL_SPEED, Units.Radians.of(m_angleConfig.getUpperLimit()));
+      State bottom = new State(ZERO_FLYWHEEL_SPEED, Units.Radians.of(m_angleConfig.getLowerLimit()));
+      if (isReady() && m_desiredShooterState == top)
         setState(bottom);
-
-      if (isReady())
-        currentTargetTop = !currentTargetTop;
+      if (isReady() && m_desiredShooterState == bottom)
+        setState(top);
     });
   }