From 9d62c01192454a840f428325bf2625a7d8e27c2a Mon Sep 17 00:00:00 2001
From: rmheuer <63077980+rmheuer@users.noreply.github.com>
Date: Fri, 22 Mar 2024 08:14:29 -0500
Subject: [PATCH] [Robot] (Theoretically) properly index reverse note

---
 .../robot/control/ControlBoard.java           |  7 +++-
 .../subsystems/speaker/IndexerSubsystem.java  | 39 +++++++++++++------
 2 files changed, 32 insertions(+), 14 deletions(-)

diff --git a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java
index fb31e24..42d1218 100644
--- a/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java
+++ b/Robot/src/main/java/com/swrobotics/robot/control/ControlBoard.java
@@ -154,6 +154,10 @@ public ControlBoard(RobotContainer robot) {
                 .onTrue(Commands.runOnce(() -> robot.intake.set(IntakeSubsystem.State.OFF)));
 
         new Trigger(() -> CHARACTERISE_WHEEL_RADIUS.get()).whileTrue(new CharactarizeWheelCommand(robot.drive));
+
+        new Trigger(operator.start::isPressed)
+                .onTrue(Commands.runOnce(robot.indexer::beginReverse))
+                .onFalse(Commands.runOnce(robot.indexer::endReverse));
     }
 
     private boolean driverWantsSnapCloser() {
@@ -210,7 +214,7 @@ private boolean getRobotRelativeDrive() {
     public void periodic() {
         if (!DriverStation.isTeleop()) {
             climberState = ClimberArm.State.RETRACTED;
-            robot.indexer.setReverse(false);
+            robot.indexer.endReverse();
             robot.intake.setReverse(false);
             robot.shooter.setFlywheelControl(ShooterSubsystem.FlywheelControl.SHOOT); // Always aim with note when not teleop
 
@@ -289,7 +293,6 @@ public void periodic() {
         robot.climber.setState(climberState);
 
         robot.intake.setReverse(operator.back.isPressed());
-        robot.indexer.setReverse(operator.start.isPressed());
 
         boolean shootAmp = operator.y.isPressed();
         if (shootAmp)
diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java
index c5b82f3..d76e10b 100644
--- a/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java
+++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/speaker/IndexerSubsystem.java
@@ -16,7 +16,9 @@ public final class IndexerSubsystem extends SubsystemBase {
 
     private final IntakeSubsystem intake;
     private boolean feedToShooter;
-    private boolean reverse;
+
+    private boolean reverseTake, waitingForPiece;
+    private boolean hadPiece;
 
     public IndexerSubsystem(IntakeSubsystem intake) {
         this.intake = intake;
@@ -24,6 +26,10 @@ public IndexerSubsystem(IntakeSubsystem intake) {
         sidesMotor.setInverted(true);
 
         feedToShooter = false;
+
+        reverseTake = false;
+        waitingForPiece = false;
+        hadPiece = false;
     }
 
     public void setFeedToShooter(boolean feedToShooter) {
@@ -37,29 +43,38 @@ public boolean hasPiece() {
 
     @Override
     public void periodic() {
-        NTData.INDEXER_HAS_PIECE.set(hasPiece());
+        boolean hasPiece = hasPiece();
+        NTData.INDEXER_HAS_PIECE.set(hasPiece);
 
         double sides = 0, top = 0;
         if (feedToShooter) {
             sides = NTData.INDEXER_SIDES_FEED_SPEED.get();
             top = NTData.INDEXER_TOP_FEED_SPEED.get();
-        } else {
-            if (intake.getState() == IntakeSubsystem.State.INTAKE && !hasPiece()) {
-                sides = NTData.INDEXER_SIDES_TAKE_SPEED.get();
-                top = NTData.INDEXER_TOP_TAKE_SPEED.get();
-            }
-        }
-
-        if (reverse) {
+        } else if (reverseTake && waitingForPiece) {
             sides = -NTData.INDEXER_SIDES_FEED_SPEED.get();
             top = -NTData.INDEXER_TOP_FEED_SPEED.get();
+
+            // Stop reversing once the piece has gone fully past the beam break
+            // Then the case below will run indexer forward to get it to normal position
+            if (!hasPiece && hadPiece)
+                waitingForPiece = false;
+        } else if ((intake.getState() == IntakeSubsystem.State.INTAKE || reverseTake) && !hasPiece) {
+            sides = NTData.INDEXER_SIDES_TAKE_SPEED.get();
+            top = NTData.INDEXER_TOP_TAKE_SPEED.get();
         }
 
         sidesMotor.set(sides);
         topMotor.set(top);
+
+        hadPiece = hasPiece;
+    }
+
+    public void beginReverse() {
+        reverseTake = true;
+        waitingForPiece = true;
     }
 
-    public void setReverse(boolean reverse) {
-        this.reverse = reverse;
+    public void endReverse() {
+        reverseTake = false;
     }
 }