From c972577066cd93d84f50ecae0b239ba5cac5cfb9 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Wed, 16 Mar 2022 21:20:18 -0600
Subject: [PATCH] turret manual boilerplate NOT COMPLETE

---
 .../java/frc4388/robot/RobotContainer.java    | 25 ++++----
 .../ButtonBoxCommands/TurretManual.java       | 62 +++++++++++++++++++
 2 files changed, 73 insertions(+), 14 deletions(-)
 create mode 100644 src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index ebd9715..ce9f0c1 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -63,6 +63,7 @@ import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
+import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
 import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
 import frc4388.robot.commands.ShooterCommands.AimToCenter;
 import frc4388.robot.commands.ShooterCommands.Shoot;
@@ -137,8 +138,6 @@ public class RobotContainer {
   private static final Function<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern
       .compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
   
-  
-  public boolean manual = false;
   public static boolean softLimits = true;
 
   /**
@@ -166,10 +165,10 @@ public class RobotContainer {
             getOperatorController().getRightTriggerAxis()),
             m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
     
-    m_robotStorage.setDefaultCommand(
-        new ManageStorage(m_robotStorage, 
-                          m_robotBoomBoom, 
-                          m_robotTurret).withName("Storage ManageStorage defaultCommand"));
+    // m_robotStorage.setDefaultCommand(
+    //     new ManageStorage(m_robotStorage, 
+    //                       m_robotBoomBoom, 
+    //                       m_robotTurret).withName("Storage ManageStorage defaultCommand"));
     
     // m_robotClimber.setDefaultCommand(
     //   new RunCommand(() -> m_robotClimber.runWithInput(getOperatorController().getRightY() * 0.6), m_robotClimber)
@@ -352,11 +351,13 @@ public class RobotContainer {
     //     .whileHeld(new RunCommand(() -> RunMiddleSwitch.setManual(true)))
     //     .whenReleased(new RunCommand(() -> RunMiddleSwitch.setManual(false)));
     
-    // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
-    //     .whileHeld(new RunMiddleSwitch());
+    new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
+        .whileHeld(new TurretManual(m_robotTurret));
       
-    new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
-        .whileHeld(new RunCommand(() -> System.out.println("RightSwitch")));
+    // control turret manual mode
+    // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
+    //     .whileHeld(new RunCommand(() -> TurretManual.setManual(true)))
+    //     .whenReleased(new RunCommand(() -> TurretManual.setManual(false)));
 
     new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value)
     .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
@@ -407,10 +408,6 @@ public class RobotContainer {
     return m_buttonBox;
   }
 
-  public void setManual(boolean set) {
-    this.manual = set;
-  }
-
   public static void setSoftLimits(boolean set) {
     softLimits = set;
   }
diff --git a/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java
new file mode 100644
index 0000000..53d772a
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/ButtonBoxCommands/TurretManual.java
@@ -0,0 +1,62 @@
+// 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 frc4388.robot.commands.ButtonBoxCommands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Turret;
+
+public class TurretManual extends CommandBase {
+
+  // subsystems
+  private Turret turret;
+
+  // booleans
+  private static boolean manual = false;
+  private boolean newManual = false;
+  private boolean changes = false;
+
+  /** Creates a new TurretManual. */
+  public TurretManual(Turret turret) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.turret = turret;
+
+    addRequirements(this.turret);
+  }
+
+  // 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() {
+    changes = (newManual != manual);
+
+    if (manual) {
+      System.out.println("Manual Turret");  // TODO: turret manual controls
+    } else {
+      System.out.println("Auto Turret");    // TODO: turret auto controls;
+    }
+
+    newManual = manual;
+  }
+
+  public static void setManual(boolean set)
+  {
+    manual = set;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    manual = !manual;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return changes;
+  }
+}