From a716506369b3e5737d543ee020395e7c37404d81 Mon Sep 17 00:00:00 2001
From: Carly <23janscc@elmbrookstudents.org>
Date: Mon, 28 Feb 2022 20:45:32 -0600
Subject: [PATCH] mMagazine Command

---
 src/main/java/frc/robot/RobotContainer.java   |  8 ++-
 .../java/frc/robot/commands/MaintainMag.java  | 51 +++++++++++++++++++
 2 files changed, 58 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc/robot/commands/MaintainMag.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 88cd0310..684a6a4c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -8,6 +8,7 @@
 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;
@@ -15,6 +16,7 @@
 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) );
     }
diff --git a/src/main/java/frc/robot/commands/MaintainMag.java b/src/main/java/frc/robot/commands/MaintainMag.java
new file mode 100644
index 00000000..62ac02be
--- /dev/null
+++ b/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;
+  }
+}