[Robot] (Theoretically) properly index reverse note

SouthwestRoboticsProgramming · Mar 22, 2024 · 9d62c01 · 9d62c01
1 parent f4be9a3
commit 9d62c01
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 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
@@ -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
@@ -16,14 +16,20 @@ 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;
 
         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;
     }
 }