Clean up command chaos

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.2.1"
+    id "edu.wpi.first.GradleRIO" version "2024.3.1"
     id "com.peterabeles.gversion" version "1.10.2"
     id "com.diffplug.spotless" version "6.12.0"
 }

src/main/java/com/cyberknights4911/control/ButtonAction.java

@@ -20,13 +20,7 @@ public enum ButtonAction {
   FireNote,
   StowClimber,
   ExtendClimber,
-  AimSpeaker,
+  AimSubwoofer,
   AimPodium,
-  AimAmp,
-  Test1,
-  Test2,
-  Test3,
-  Test4,
-  Test5,
-  Test6
+  AimAmp
 }

src/main/java/com/cyberknights4911/drive/Drive.java

@@ -278,12 +278,8 @@ ChassisSpeeds createChassisSpeeds(
       x = -x;
       y = -y;
     }
-    System.out.println("x: " + x);
-    System.out.println("y: " + y);
     double linearMagnitude =
         MathUtil.applyDeadband(Math.hypot(x, y), controlConstants.stickDeadband());
-    System.out.println("linearMagnitude: " + linearMagnitude);
-    System.out.println("stickDeadband: " + controlConstants.stickDeadband());
     Rotation2d linearDirection = new Rotation2d(x, y);
     double omega =
         applyOmegaDeadbandAndScaling
@@ -300,15 +296,12 @@ ChassisSpeeds createChassisSpeeds(
             .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
             .getTranslation();
 
-    System.out.println("linearVelocity: " + linearVelocity);
-
     // Convert to field relative speeds & send command
     return ChassisSpeeds.fromFieldRelativeSpeeds(
         linearVelocity.getX() * driveConstants.maxLinearSpeed(),
         linearVelocity.getY() * driveConstants.maxLinearSpeed(),
         omega * maxAngularSpeedMetersPerSecond,
         getRotation());
-    // return new ChassisSpeeds();
   }
 
   private ChassisSpeeds createChassisSpeeds(

src/main/java/com/cyberknights4911/robot2024/Robot2024.java

@@ -125,208 +125,27 @@ private void configureControls() {
                 Units.inchesToMeters(x),
                 Units.inchesToMeters(y)));
 
-    binding.triggersFor(ButtonAction.StowCollector).onTrue(stow());
+    binding.triggersFor(ButtonAction.StowCollector).onTrue(stowEverything());
 
     binding.triggersFor(ButtonAction.CollectNote).onTrue(collectNote());
 
-    binding.triggersFor(ButtonAction.AimSpeaker).onTrue(aimSpeaker());
+    binding.triggersFor(ButtonAction.AimSubwoofer).onTrue(shooter.aimSubwoofer());
 
-    binding.triggersFor(ButtonAction.AimPodium).onTrue(aimPodium());
+    binding.triggersFor(ButtonAction.AimPodium).onTrue(shooter.aimPodium());
 
-    binding.triggersFor(ButtonAction.FireNote).onTrue(fire());
-
-    // binding
-    //     .triggersFor(ButtonAction.Test1)
-    //     .onTrue(
-    //         Commands.runOnce(
-    //             () -> {
-    //               shooter.setAimerPostion(34);
-    //               collect.extendCollecter();
-    //               collect.runCollectOutput(.4);
-    //               indexer.runOutput(.2);
-    //               shooter.runGuideOutput(.2);
-    //             }));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test2)
-    //     .onTrue(
-    //         Commands.runOnce(
-    //             () -> {
-    //               shooter.setAimerPostion(1);
-    //               collect.retractCollecter();
-    //               collect.runCollectOutput(0);
-    //               indexer.runOutput(0);
-    //               shooter.runGuideOutput(0);
-    //             }));
-    // binding
-    //     .triggersFor(ButtonAction.Test3)
-    //     .onTrue(
-    //         Commands.runOnce(() -> shooter.setAimerPostion(54))
-    //             .andThen(() -> shooter.runGuideOutput(-.1))
-    //             .andThen(Commands.waitSeconds(.05))
-    //             .andThen(() -> shooter.runGuideOutput(0)));
-    // binding
-    //     .triggersFor(ButtonAction.Test4)
-    //     .onTrue(
-    //         Commands.runOnce(() -> shooter.runShooterOutput(.4))
-    //             .andThen(Commands.waitSeconds(2))
-    //             .andThen(() -> shooter.runGuideOutput(.5))
-    //             .andThen(Commands.waitSeconds(2))
-    //             .andThen(
-    //                 () -> {
-    //                   shooter.runShooterOutput(0);
-    //                   shooter.runGuideOutput(0);
-    //                   shooter.setAimerPostion(1);
-    //                 }));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test1)
-    //     .onTrue(Commands.runOnce(() -> indexer.runOutput(.2), indexer));
-
-    // binding.triggersFor(ButtonAction.Test2).onTrue(Commands.runOnce(() -> indexer.runOutput(0)));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test3)
-    //     .onTrue(Commands.runOnce(() -> shooter.runGuideOutput(.1), shooter));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test4)
-    //     .onTrue(Commands.runOnce(() -> shooter.runGuideOutput(0)));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test5)
-    //     .onTrue(Commands.runOnce(() -> collect.runCollectOutput(.6), collect));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test6)
-    //     .onTrue(Commands.runOnce(() -> collect.runCollectOutput(0), collect));
-
-    // aimer test code
-    // binding
-    //     .triggersFor(ButtonAction.Test1)
-    //     .onTrue(Commands.runOnce(() -> shooter.setAimerPostion(1)));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test2)
-    //     .onTrue(Commands.runOnce(() -> shooter.setAimerPostion(54)));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test3)
-    //     .onTrue(Commands.runOnce(() -> shooter.setAimerPostion(45)));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test3)
-    //     .onTrue(Commands.runOnce(() -> shooter.runAimerOutput(0)));
-
-    // collector test code
-    // binding
-    //     .triggersFor(ButtonAction.Test1)
-    //     .onTrue(Commands.runOnce(() -> collect.extendCollecter()));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test2)
-    //     .onTrue(Commands.runOnce(() -> collect.retractCollecter()));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test1)
-    //     .onTrue(
-    //         Commands.runOnce(
-    //             () -> {
-    //               collect.extendCollecter();
-    //               collect.runCollectOutput(.6);
-    //               indexer.runOutput(.2);
-    //               shooter.runGuideOutput(.1);
-    //             }));
-
-    // binding
-    //     .triggersFor(ButtonAction.Test2)
-    //     .onTrue(
-    //         Commands.runOnce(
-    //             () -> {
-    //               collect.retractCollecter();
-    //               collect.runCollectOutput(0);
-    //               indexer.runOutput(0);
-    //               shooter.runGuideOutput(0);
-    //             }));
+    binding.triggersFor(ButtonAction.FireNote).onTrue(shooter.fire());
   }
 
   private Command collectNote() {
-    return Commands.runOnce(
-            () -> {
-              shooter.stopShooter();
-              collect.collectAtTunableOutput();
-              collect.extendCollecter();
-              indexer.runIndexAtTunableOutput();
-              shooter.guideAtTunableOutput();
-              shooter.setAimerForCollect();
-              shooter.setAimerPostion(34);
-            },
-            shooter,
-            collect,
-            indexer)
-        .andThen(Commands.waitUntil(() -> shooter.isBeamBreakBlocked()))
-        .andThen(stow());
-  }
-
-  private Command stow() {
-    return Commands.runOnce(
-        () -> {
-          collect.retractCollecter();
-          collect.stopCollector();
-          indexer.stop();
-          shooter.stopGuide();
-          shooter.stopShooter();
-          shooter.setAimerForCollect();
-        },
-        collect,
-        indexer,
-        shooter);
-  }
-
-  private Command backNoteFromShooter() {
-    //   return Commands.runOnce(
-    //           () -> {
-    //             shooter.guideReverseAtTunableOutput();
-    //             shooter.setAimerForCollect();
-    //           })
-    //       .andThen(Commands.waitUntil(() -> !shooter.isBeamBreakBlocked()));
-    return Commands.runOnce(
-        () -> {
-          shooter.setAimerForCollect();
-        });
-  }
-
-  private Command aimSpeaker() {
-    return backNoteFromShooter()
-        .andThen(() -> shooter.setAimerForSpeaker())
-        .andThen(Commands.waitSeconds(shooter.aimTime()))
-        .andThen(
-            () -> {
-              shooter.runShooterAtTunableSpeed();
-              shooter.stopGuide();
-            });
-  }
-
-  private Command aimPodium() {
-    return backNoteFromShooter()
-        .andThen(() -> shooter.setAimerForPodium())
-        .andThen(Commands.waitSeconds(shooter.aimTime()))
-        .andThen(
-            () -> {
-              shooter.runShooterAtTunableSpeed();
-              shooter.stopGuide();
-            });
+    return collect
+        .extendCollecter()
+        .andThen(indexer.runIndexAtTunableOutput())
+        .andThen(shooter.collectAndWaitForNote())
+        .andThen(stowEverything());
   }
 
-  private Command fire() {
-    return Commands.runOnce(() -> shooter.guideAtTunableOutput())
-        .andThen(Commands.waitSeconds(shooter.feedTime()))
-        .andThen(
-            () -> {
-              shooter.stopShooter();
-              shooter.stopGuide();
-              shooter.setAimerForCollect();
-            });
+  private Command stowEverything() {
+    return shooter.stow().alongWith(indexer.stop()).alongWith(collect.retractCollecter());
   }
 
   @Override
@@ -346,7 +165,7 @@ public void onRobotPeriodic(LoggedRobot robot) {
   public void setupAutos(AutoCommandHandler handler) {
     Autos autos = new Autos(Robot2024Constants.DRIVE_CONSTANTS, climb, collect, shooter, drive);
     autos.addAllAutos(handler);
-    NamedCommands.registerCommand("SHOOT_SUB", aimSpeaker().andThen(fire()));
+    NamedCommands.registerCommand("SHOOT_SUB", shooter.aimSubwoofer().andThen(shooter.fire()));
     NamedCommands.registerCommand("COLLECT", collectNote());
   }
 

src/main/java/com/cyberknights4911/robot2024/collect/Collect.java

@@ -16,6 +16,8 @@
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import org.littletonrobotics.junction.Logger;
@@ -83,14 +85,6 @@ public Collect(CollectConstants constants, CollectIO collectIO) {
                 (voltage) -> runCollectVolts(voltage.in(Volts)), null, this));
   }
 
-  public boolean isBeamBreakBlocked() {
-    return inputs.beamBreakVoltage > beamThreshold.get();
-  }
-
-  public void runCollectOutput(double percent) {
-    collectIO.setCollectOutput(percent);
-  }
-
   /** Run collector open loop at the specified voltage. */
   public void runCollectVolts(double volts) {
     collectIO.setCollectVoltage(volts);
@@ -104,14 +98,6 @@ public void runCollectVelocity(double velocityRpm) {
     Logger.recordOutput("Collect/SetpointRPM", velocityRpm);
   }
 
-  public void extendCollecter() {
-    collectIO.setCollecterPosition(true);
-  }
-
-  public void retractCollecter() {
-    collectIO.setCollecterPosition(false);
-  }
-
   @Override
   public void periodic() {
     collectIO.updateInputs(inputs);
@@ -134,7 +120,7 @@ public void periodic() {
     // Logger.recordOutput("Collect/Mechanism", mechanism);
 
     if (DriverStation.isDisabled()) {
-      stopCollector();
+      collectIO.stopCollector();
     }
   }
 
@@ -161,12 +147,21 @@ public double getCharacterizationVelocity() {
     return inputs.collectVelocityRadPerSec;
   }
 
-  /** Stops the collector. */
-  public void stopCollector() {
-    collectIO.stopCollector();
+  public Command extendCollecter() {
+    return Commands.runOnce(
+        () -> {
+          collectIO.setCollecterPosition(true);
+          collectIO.setCollectOutput(collectOutput.get());
+        },
+        this);
   }
 
-  public void collectAtTunableOutput() {
-    runCollectOutput(collectOutput.get());
+  public Command retractCollecter() {
+    return Commands.runOnce(
+        () -> {
+          collectIO.stopCollector();
+          collectIO.setCollecterPosition(false);
+        },
+        this);
   }
 }

src/main/java/com/cyberknights4911/robot2024/control/ControllerBinding.java

@@ -67,7 +67,7 @@ public Triggers triggersFor(ButtonAction action) {
         return new Triggers(operator.b());
       case AimPodium:
         return new Triggers(operator.povLeft());
-      case AimSpeaker:
+      case AimSubwoofer:
         return new Triggers(operator.povRight());
       case FireNote:
         return new Triggers(operator.rightTrigger());

src/main/java/com/cyberknights4911/robot2024/indexer/Indexer.java

@@ -10,6 +10,8 @@
 import com.cyberknights4911.logging.LoggedTunableNumber;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 
@@ -73,11 +75,11 @@ public void runVelocity(double velocityRpm) {
     Logger.recordOutput("Indexer/SetpointRPM", velocityRpm);
   }
 
-  public void stop() {
-    indexerIO.stop();
+  public Command runIndexAtTunableOutput() {
+    return Commands.runOnce(() -> runOutput(percent.get()), this);
   }
 
-  public void runIndexAtTunableOutput() {
-    runOutput(percent.get());
+  public Command stop() {
+    return Commands.runOnce(() -> indexerIO.stop(), this);
   }
 }