Added backfeed!

Author: dracco1993

src/main/java/frc/robot/controls/OperatorControls.java

@@ -48,5 +48,9 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur
     controller.a().whileTrue(
         superstructure.feedAllCommand()
             .finallyDo(() -> superstructure.stopFeedingAllCommand().schedule()));
+
+    controller.b().whileTrue(
+        superstructure.backFeedAllCommand()
+            .finallyDo(() -> superstructure.stopFeedingAllCommand().schedule()));
   }
 }

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

@@ -58,6 +58,10 @@ public Command feedCommand() {
     return hopper.set(HOPPER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Hopper.Feed");
   }
 
+  public Command backFeedCommand() {
+    return hopper.set(-HOPPER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Hopper.BackFeed");
+  }
+
   /**
    * Command to run the hopper in reverse while held.
    */

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

@@ -124,6 +124,16 @@ public Command deployAndRollCommand() {
     }).withName("Intake.DeployAndRoll");
   }
 
+  public Command backFeedAndRollCommand() {
+    return Commands.run(() -> {
+      setIntakeDeployed();
+      smc.setDutyCycle(-INTAKE_SPEED);
+    }, this).finallyDo(() -> {
+      smc.setDutyCycle(0);
+      setIntakeHold();
+    }).withName("Intake.BackFeedAndRoll");
+  }
+
   private void setIntakeStow() {
     intakePivotController.setPosition(Degrees.of(0));
   }

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

@@ -249,6 +249,12 @@ public Command feedAllCommand() {
         intake.setPivotAngle(Degrees.of(46)).asProxy()).withName("Superstructure.feedAll");
   }
 
+  public Command backFeedAllCommand() {
+    return Commands.parallel(
+        hopper.backFeedCommand().asProxy(),
+        intake.backFeedAndRollCommand().asProxy()).withName("Superstructure.backFeedAll");
+  }
+
   // public Command intakeBounceCommand() {
   // return Commands.sequence(
   // Commands.runOnce(() -> intake.setPivotAngle(Degrees.of(115))).asProxy()