From 98af64d69ab79c01fb63ee6edcbaa13fbb97c38d Mon Sep 17 00:00:00 2001
From: WindowsVistaisCool <kyle@quackings.com>
Date: Thu, 15 Aug 2024 17:02:24 -0400
Subject: [PATCH] fix for smartcollecttele

---
 src/main/java/frc/robot/RobotContainer.java       |  4 ++--
 src/main/java/frc/robot/command/SmartCollect.java | 11 +++++++++--
 .../java/frc/robot/command/shoot/NotePass.java    | 15 +++++++++------
 3 files changed, 20 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 68af2a30..fa55e1ed 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -179,7 +179,7 @@ protected void initializeNamedCommands() {
 				new AutonSmartCollect(() -> 0.5, () -> 0.6, collector, indexer)
 						.deadlineWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1)));
 		NamedCommands.registerCommand("Smart-Collect-Tele",
-				new SmartCollect(() -> 0.5, () -> 0.6, collector, indexer, pivot, flywheel)
+				new SmartCollect(() -> 0.5, () -> 0.6, collector, indexer, pivot, flywheel, true)
 						.deadlineWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1)));
 		NamedCommands.registerCommand("Index-Up", new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer));
 		NamedCommands.registerCommand("PathFind", new PathToPose(PathFindingConstants.TEST_POSE, drivetrain));
@@ -195,7 +195,7 @@ protected void initializeNamedCommands() {
 		NamedCommands.registerCommand("End-Kama", new InstantCommand(() -> flywheel.endKama()));
 		NamedCommands.registerCommand("Start-Kama", new InstantCommand(() -> flywheel.startKama()));
 		NamedCommands.registerCommand("Note-Pass", new InstantCommand()); // Why is this here?? IDK but im gonna keep it here so we don't break things
-		// NamedCommands.registerCommand("Reverse-Cheese-Paste", new ReverseChasePieces(drivetrain, collector, indexer, limelights));
+		NamedCommands.registerCommand("Reverse-Cheese-Paste", new InstantCommand()); //new ReverseChasePieces(drivetrain, collector, indexer, limelights));
 
 		// make sure named commands are initialized before autobuilder!
 		autoChooser = AutoBuilder.buildAutoChooser();
diff --git a/src/main/java/frc/robot/command/SmartCollect.java b/src/main/java/frc/robot/command/SmartCollect.java
index 0430477f..1ce8e40e 100644
--- a/src/main/java/frc/robot/command/SmartCollect.java
+++ b/src/main/java/frc/robot/command/SmartCollect.java
@@ -39,9 +39,10 @@ public class SmartCollect extends Command {
      * @param indexer        subsystem
      * @param pivot          subsystem (read only)
      * @param flywheel       subsystem
+     * @param isAutonomous   whether or not this command is being used in autonomous
      */
     public SmartCollect(DoubleSupplier collectorPower, DoubleSupplier indexerPower, Collector collector,
-            Indexer indexer, Pivot pivot, Flywheel flywheel) {
+            Indexer indexer, Pivot pivot, Flywheel flywheel, boolean isAutonomous) {
         this.collector = collector;
         this.indexer = indexer;
         this.pivot = pivot;
@@ -49,13 +50,19 @@ public SmartCollect(DoubleSupplier collectorPower, DoubleSupplier indexerPower,
         this.collectorPower = collectorPower;
         this.indexerPower = indexerPower;
 
-        if (DriverStation.isAutonomous()) {
+        if (isAutonomous || DriverStation.isAutonomous()) {
             addRequirements(collector, indexer);
         } else {
             addRequirements(collector, indexer, flywheel);
         }
     }
 
+    // Overloaded constructor for use in teleop
+    public SmartCollect(DoubleSupplier collectorPower, DoubleSupplier indexerPower, Collector collector,
+            Indexer indexer, Pivot pivot, Flywheel flywheel) {
+        this(collectorPower, indexerPower, collector, indexer, pivot, flywheel, false);
+    }
+
     @Override
     public void initialize() {
         reversedFromExit = false;
diff --git a/src/main/java/frc/robot/command/shoot/NotePass.java b/src/main/java/frc/robot/command/shoot/NotePass.java
index a882e8f8..f75228d2 100644
--- a/src/main/java/frc/robot/command/shoot/NotePass.java
+++ b/src/main/java/frc/robot/command/shoot/NotePass.java
@@ -1,7 +1,5 @@
 package frc.robot.command.shoot;
 
-import java.sql.Driver;
-import java.util.function.BooleanSupplier;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -18,7 +16,6 @@
 import frc.robot.Constants.PassConstants;
 import frc.robot.Constants.ShooterConstants;
 import frc.robot.subsystems.Flywheel;
-import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Pivot;
 import frc.robot.subsystems.Swerve;
 import frc.thunder.command.TimedCommand;
@@ -52,9 +49,9 @@ public class NotePass extends Command {
 	 * Creates a new NotePass.
 	 *
 	 * @param drivetrain subsystem
-	 * @param pivot subsystem
-	 * @param flywheel subsystem
-	 * @param driver the driver's controller, used for drive input
+	 * @param pivot      subsystem
+	 * @param flywheel   subsystem
+	 * @param driver     the driver's controller, used for drive input
 	 */
 	public NotePass(Swerve drivetrain, Flywheel flywheel, Pivot pivot, XboxControllerFilter driver) {
 		this.drivetrain = drivetrain;
@@ -67,6 +64,12 @@ public NotePass(Swerve drivetrain, Flywheel flywheel, Pivot pivot, XboxControlle
 
 	@Override
 	public void initialize() {
+		/*
+		 * Because the flywheels are on one side, the note will spin slightly
+		 * which is why we want to use the corner pose on blue and
+		 * the speaker pose on red.
+		 * I may be going crazy but whatever!!
+		 */
 		if (isBlueAlliance()) {
 			targetPose = DrivetrainConstants.BLUE_CORNER_POSE;
 		} else {