added stopAiming and resetShooter

Patribots4738 · Jan 24, 2024 · 31f168a · 31f168a
1 parent 3940508
commit 31f168a
Showing 1 changed file with 10 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/commands/ShooterCalc.java b/src/main/java/frc/robot/commands/ShooterCalc.java
@@ -105,6 +105,12 @@ public Command sendBackCommand() {
               .andThen(() -> shooterSetSpeed(ShooterConstants.SHOOTER_BACK_SPEED));
   }
 
+  public Command resetShooter() {
+      return Commands.runOnce(() -> stopAiming())
+              .andThen(Commands.runOnce(() -> shooterStop()))
+                        .alongWith(Commands.runOnce(() -> pivotSetRestAngle()));
+  }
+
   public void shooterSetSpeed(double speed) {
     shooter.setSpeed(speed);
   }
@@ -126,6 +132,10 @@ private void toggleAiming() {
     this.aiming = (() -> !aiming.getAsBoolean());
   }
 
+  private void stopAiming() {
+    this.aiming = (() -> false);
+  }
+
   // Gets a SpeedAnglePair by interpolating values from a map of already 
   // known required speeds and angles for certain poses
   private SpeedAnglePair calculateSpeed(Pose2d robotPose, boolean shootingAtSpeaker) {