diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f93dcce..109aaff 100644 --- 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 index 0efaee5..d875135 100644 --- 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); }); }