cleaned up prev merge

src/main/java/frc/robot/Robot.java

@@ -85,8 +85,6 @@ public class Robot extends TimedRobot {
   // global state variables
   private static Piece _currentPiece = Piece.NONE;
 
-  private final Trigger _noPiece = new Trigger(() -> getCurrentPiece() == Piece.NONE);
-
   /** The current piece in the manipulator. */
   public static Piece getCurrentPiece() {
     return _currentPiece;
@@ -124,10 +122,11 @@ public Robot(NetworkTableInstance ntInst) {
     configureOperatorBindings();
 
     new Trigger(_serializer::getBackBeam).onTrue(rumbleControllers(1, 1));
-    _noPiece.onChange(rumbleControllers(1, 1));
+    new Trigger(() -> getCurrentPiece() == Piece.NONE).onChange(rumbleControllers(1, 1));
 
-    _noPiece
-        .and(() -> !_manipulator.getBeam())
+    // a coral was passoff'ed
+    _manipulator
+        .getBeamNoPiece()
         .and(_wristevator::homeSwitch)
         .and(() -> Math.signum(_manipulator.getSpeed()) == -1)
         .onTrue(runOnce(() -> _currentPiece = Piece.CORAL));

src/main/java/frc/robot/subsystems/Manipulator.java

@@ -5,21 +5,25 @@
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.lib.AdvancedSubsystem;
 import frc.robot.Constants.ManipulatorConstants;
 import java.util.function.Consumer;
 
 public class Manipulator extends AdvancedSubsystem {
-  private final Consumer<Piece> _currentPieceSetter; // TODO: this should be used on beam triggers
+  private final Consumer<Piece> _currentPieceSetter;
 
   private final DigitalInput _manipulatorBeam =
       new DigitalInput(ManipulatorConstants.manipulatorBeamPort);
-  private final DigitalInput _manipulatorSwitch =
+  private final DigitalInput _algaeSwitch =
       new DigitalInput(ManipulatorConstants.manipulatorSwitchPort);
 
   private final Trigger _beamBroken = new Trigger(this::getBeam);
-  private final Trigger _switchPressed = new Trigger(this::getSwitch); // Might have to switches
+  private final Trigger _switchPressed = new Trigger(this::getSwitch);
+
+  private final Trigger _beamWithPiece = _beamBroken.and(() -> getCurrentPiece() != Piece.NONE);
+  private final Trigger _beamNoPiece = _beamBroken.and(() -> getCurrentPiece() == Piece.NONE);
 
   public Manipulator(Consumer<Piece> currentPieceSetter) {
     _currentPieceSetter = currentPieceSetter;
@@ -29,13 +33,15 @@ public Manipulator(Consumer<Piece> currentPieceSetter) {
     new Trigger(() -> getCurrentPiece() == Piece.CORAL).whileTrue(holdCoral());
     new Trigger(() -> getCurrentPiece() == Piece.ALGAE).whileTrue(holdAlgae());
 
-    _beamBroken
-        .and(() -> getCurrentPiece() == Piece.NONE)
-        .onFalse(runOnce(() -> _currentPieceSetter.accept(Piece.CORAL)));
-    _beamBroken
-        .and(() -> getCurrentPiece() != Piece.NONE)
-        .onFalse(runOnce(() -> _currentPieceSetter.accept(Piece.NONE)));
-    _switchPressed.onTrue(runOnce(() -> _currentPieceSetter.accept(Piece.ALGAE)));
+    _beamNoPiece.onFalse(
+        Commands.runOnce(
+            () -> currentPieceSetter.accept(Piece.CORAL))); // coral pick up not from passoff
+
+    _beamWithPiece.onFalse(
+        Commands.runOnce(() -> currentPieceSetter.accept(Piece.NONE))); // any piece came out
+
+    _switchPressed.onTrue(
+        Commands.runOnce(() -> _currentPieceSetter.accept(Piece.ALGAE))); // algae picked up
   }
 
   public static enum Piece {
@@ -44,6 +50,16 @@ public static enum Piece {
     NONE
   }
 
+  /** A trigger that is true when the beam is broken and current piece is not none. */
+  public Trigger getBeamWithPiece() {
+    return _beamWithPiece;
+  }
+
+  /** A trigger that is true when the beam is broken and current piece is none. */
+  public Trigger getBeamNoPiece() {
+    return _beamNoPiece;
+  }
+
   @Logged(name = "Speed")
   public double getSpeed() {
     return 0;
@@ -56,7 +72,8 @@ public boolean getBeam() {
 
   @Logged(name = "Manipulator Switch")
   public boolean getSwitch() {
-    return _manipulatorSwitch.get();
+    // TODO: might have two switches
+    return _algaeSwitch.get();
   }
 
   /** Set the speed of the manipulator feed motor in rad/s. */