Merge pull request #46 from Team334/operator-logic-redesign

Redesign operator logic.

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

@@ -26,11 +26,11 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.ScheduleCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.lib.FaultLogger;
 import frc.lib.InputStream;
-import frc.robot.Constants.ManipulatorConstants;
 import frc.robot.Constants.Ports;
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.Constants.WristevatorConstants;
@@ -66,7 +66,7 @@ public class Robot extends TimedRobot {
   private final Intake _intake = new Intake();
 
   @Logged(name = "Serializer")
-  private final Serializer _serializer = new Serializer();
+  private final Serializer _serializer = new Serializer((Piece piece) -> _currentPiece = piece);
 
   @Logged(name = "Manipulator")
   private final Manipulator _manipulator = new Manipulator((Piece piece) -> _currentPiece = piece);
@@ -120,24 +120,8 @@ public Robot(NetworkTableInstance ntInst) {
     configureDriverBindings();
     configureOperatorBindings();
 
-    // piece comes into the robot from ground intake
-    new Trigger(_serializer::getBackBeam)
-        .onTrue(
-            either(
-                runOnce(() -> _currentPiece = Piece.NONE),
-                rumbleControllers(1, 1),
-                () -> getCurrentPiece() == Piece.CORAL));
-
-    // piece in manipulator changes
     new Trigger(() -> getCurrentPiece() == Piece.NONE).onChange(rumbleControllers(1, 1));
 
-    // a coral was passoff'ed
-    _manipulator
-        .getBeamNoPiece()
-        .and(_wristevator::homeSwitch)
-        .and(() -> _manipulator.getFeedDirection() == -1)
-        .onTrue(runOnce(() -> _currentPiece = Piece.CORAL));
-
     SmartDashboard.putData(
         "Robot Self Check",
         sequence(
@@ -213,6 +197,7 @@ private void configureDriverBindings() {
   }
 
   private void configureOperatorBindings() {
+    // wristevator setpoint control
     _operatorController.back().whileTrue(_wristevator.setSetpoint(PROCESSOR));
     _operatorController.start().whileTrue(_wristevator.setSetpoint(HUMAN));
     _operatorController.rightStick().whileTrue(_wristevator.setSetpoint(HOME));
@@ -237,29 +222,35 @@ private void configureOperatorBindings() {
 
     _operatorController.x().whileTrue(_wristevator.setSetpoint(L4));
 
+    // ground intake / passoff
     _operatorController
         .rightBumper()
-        .and(() -> _wristevator.homeSwitch())
+        .and(_wristevator::homeSwitch)
         .whileTrue(Superstructure.passoff(_intake, _serializer, _manipulator));
 
     _operatorController
         .rightBumper()
         .and(() -> !_wristevator.homeSwitch())
-        .whileTrue(Superstructure.groundIntake(_intake, _serializer));
+        .whileTrue(
+            Superstructure.groundIntake(_intake, _serializer)
+                .andThen(new ScheduleCommand(rumbleControllers(1, 1))));
 
+    // ground outtake
     _operatorController.leftBumper().whileTrue(Superstructure.groundOuttake(_intake, _serializer));
 
+    // intake / inverse passoff
     _operatorController
         .rightTrigger()
-        .whileTrue(
-            either(
-                Superstructure.inversePassoff(_serializer, _manipulator),
-                _manipulator.setSpeed(ManipulatorConstants.feedSpeed.in(RadiansPerSecond)),
-                _wristevator::homeSwitch));
+        .and(_wristevator::homeSwitch)
+        .whileTrue(Superstructure.inversePassoff(_serializer, _manipulator));
 
     _operatorController
-        .leftTrigger()
-        .whileTrue(_manipulator.setSpeed(-ManipulatorConstants.feedSpeed.in(RadiansPerSecond)));
+        .rightTrigger()
+        .and(() -> !_wristevator.homeSwitch())
+        .whileTrue(_manipulator.intake());
+
+    // outtake
+    _operatorController.leftTrigger().whileTrue(_manipulator.outtake());
   }
 
   /** Rumble the driver and operator controllers for some amount of seconds. */

src/main/java/frc/robot/commands/Superstructure.java

@@ -10,7 +10,6 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.IntakeConstants;
-import frc.robot.Constants.ManipulatorConstants;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Manipulator;
 import frc.robot.subsystems.Serializer;
@@ -27,50 +26,34 @@ private Superstructure() {
   /** Passoff from intake (if needed) -> serializer -> manipulator. */
   public static Command passoff(Intake intake, Serializer serializer, Manipulator manipulator) {
     return parallel(
-            intake
-                .set(
-                    IntakeConstants.actuatorOut.in(Radians),
-                    IntakeConstants.feedSpeed.in(RadiansPerSecond))
-                .unless(serializer::getBackBeam),
-            serializer.setSpeed(0),
-            manipulator.setSpeed(-ManipulatorConstants.feedSpeed.in(RadiansPerSecond)))
+            intake.intake().until(serializer::hasCoral).asProxy(),
+            serializer.passoff(),
+            manipulator.passoff())
         .withName("Passoff");
   }
 
   /** Coral passoff from manipulator -> serializer. */
   public static Command inversePassoff(Serializer serializer, Manipulator manipulator) {
-    return parallel(
-            manipulator.setSpeed(ManipulatorConstants.feedSpeed.in(RadiansPerSecond)),
-            serializer.setSpeed(0))
-        .until(serializer::getBackBeam)
+    return deadline(serializer.inversePassoff(), manipulator.inversePassoff())
         .withName("Inverse Passoff");
   }
 
   /** Outtake from serializer -> intake. */
   public static Command groundOuttake(Intake intake, Serializer serializer) {
     return sequence(
             intake
-                .set(
-                    IntakeConstants.actuatorOut.in(Radians),
-                    -IntakeConstants.feedSpeed.in(RadiansPerSecond))
+                .outtake()
                 .until(
                     () ->
                         MathUtil.isNear(
                             IntakeConstants.actuatorOut.in(Radians), intake.getAngle(), 0.01)),
-            serializer.setSpeed(0))
+            serializer.outtake())
         .withName("Ground Outtake");
   }
 
   /** Intake from intake -> serializer. */
   public static Command groundIntake(Intake intake, Serializer serializer) {
-    return parallel(
-            intake
-                .set(
-                    IntakeConstants.actuatorOut.in(Radians),
-                    IntakeConstants.feedSpeed.in(RadiansPerSecond))
-                .unless(serializer::getBackBeam),
-            serializer.setSpeed(0))
-        .until(serializer::getFrontBeam)
+    return deadline(serializer.intake(), intake.intake().until(serializer::hasCoral).asProxy())
         .withName("Ground Intake");
   }
 }

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

@@ -60,7 +60,7 @@ public class Intake extends AdvancedSubsystem {
   private Notifier _simNotifier;
 
   public Intake() {
-    setDefaultCommand(set(IntakeConstants.actuatorStowed.in(Radians), 0));
+    setDefaultCommand(stow());
 
     var feedMotorConfigs = new TalonFXConfiguration();
     var actuatorMotorConfigs = new TalonFXConfiguration();
@@ -147,18 +147,33 @@ public double getSpeed() {
     return _feedVelocityGetter.refresh().getValue().in(RadiansPerSecond);
   }
 
-  /**
-   * Set the actuator angle and feed speed.
-   *
-   * @param actuatorAngle Actuator angle in rad.
-   * @param feedSpeed Feed wheel speed in rad/s.
-   */
-  public Command set(double actuatorAngle, double feedSpeed) {
-    return run(() -> {
+  // set the actuator angle and feed speed.
+  private Command set(double actuatorAngle, double feedSpeed) {
+    return run(
+        () -> {
           _actuatorMotor.setControl(_actuatorPositionSetter.withPosition(actuatorAngle));
           _feedMotor.setControl(_feedVelocitySetter.withVelocity(feedSpeed));
-        })
-        .withName("Set");
+        });
+  }
+
+  /** Stow the intake into the robot. */
+  public Command stow() {
+    return set(IntakeConstants.actuatorStowed.in(Radians), 0).withName("Stow");
+  }
+
+  /** Intake a coral off of the ground. */
+  public Command intake() {
+    return set(
+            IntakeConstants.actuatorOut.in(Radians), IntakeConstants.feedSpeed.in(RadiansPerSecond))
+        .withName("Intake");
+  }
+
+  /** Outtake onto the ground. */
+  public Command outtake() {
+    return set(
+            IntakeConstants.actuatorOut.in(Radians),
+            -IntakeConstants.feedSpeed.in(RadiansPerSecond))
+        .withName("Outtake");
   }
 
   @Override