mMagazine Command

src/main/java/frc/robot/RobotContainer.java

@@ -8,13 +8,15 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 //import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.Constants.Autonomous;
 import frc.robot.Constants.DriverPrefs;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.IntakeCommand.IntakeMode;
 import static frc.robot.commands.MoveIntake.DeployMode;
 import frc.robot.commands.MoveIntake;
 import frc.robot.commands.MagazineCommand;
+import frc.robot.commands.MaintainMag;
 import frc.robot.commands.MagazineCommand.MagazineMode;
 import frc.robot.commands.MovePositioner.PositionerMode;
 import frc.robot.commands.MovePositioner;
@@ -183,7 +185,11 @@ void setAssistantButtons() {
     if(Constants.HAS_INTAKE) {
       driverControls.bind(Id.Assistant, XboxButton.LB).whenPressed(new MoveIntake(DeployMode.Toggle));
       // IntakeCommand takes a DoubleSupplier f() which could be tied to our UX instead of const f() given here.
-      driverControls.bind(Id.Assistant, XboxButton.A).whileHeld(new IntakeCommand((()-> 0.47), ()-> 0.20,  IntakeMode.LoadCargo) );
+      //driverControls.bind(Id.Assistant, XboxButton.A).whileHeld(new IntakeCommand((()-> 0.47), ()-> 0.20,  IntakeMode.LoadCargo) );
+      driverControls.bind(Id.Assistant,XboxButton.A).whileHeld(new ParallelCommandGroup(
+        new IntakeCommand((()-> 0.47), ()-> 0.20,  IntakeMode.LoadCargo),
+        new MaintainMag(magazine, ()-> 0.2)
+      ));
       // IntakeCommand motor direction
       driverControls.bind(Id.Assistant, XboxButton.B).whileHeld(new IntakeCommand((()-> 0.35), ()-> 0.20, IntakeMode.ExpellCargo) );
     }

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

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Magazine_Subsystem;
+
+public class MaintainMag extends CommandBase {
+
+  private Magazine_Subsystem mMagazine;
+  private DoubleSupplier topWheelSpeed;
+
+  public MaintainMag(Magazine_Subsystem mMagazine, DoubleSupplier topWheelSpeed) {
+    this.mMagazine = mMagazine;
+    this.topWheelSpeed = topWheelSpeed;
+
+    addRequirements(mMagazine);
+  }
+
+  /** Creates a new MaintainMag. */
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (mMagazine.isGate1Blocked()){
+      mMagazine.driveWheelOn(topWheelSpeed.getAsDouble());
+    } else {
+      mMagazine.driveWheelOff();
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    mMagazine.driveWheelOff();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}