Merge branch 'main' into 29-pathfinding

frc-862 · Mar 9, 2024 · 4225c02 · 4225c02
2 parents a03c874 + 5ad1eb6
commit 4225c02
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 28 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -351,22 +351,30 @@ protected void configureSystemTests() {
 	}
 
 	public static Command hapticDriverCommand() {
-		return new StartEndCommand(() -> {
-			driver.setRumble(GenericHID.RumbleType.kRightRumble, 1d);
-			driver.setRumble(GenericHID.RumbleType.kLeftRumble, 1d);
-		}, () -> {
-			driver.setRumble(GenericHID.RumbleType.kRightRumble, 0);
-			driver.setRumble(GenericHID.RumbleType.kLeftRumble, 0);
-		});
+		if (DriverStation.isAutonomous()) {
+			return new StartEndCommand(() -> {
+				driver.setRumble(GenericHID.RumbleType.kRightRumble, 1d);
+				driver.setRumble(GenericHID.RumbleType.kLeftRumble, 1d);
+			}, () -> {
+				driver.setRumble(GenericHID.RumbleType.kRightRumble, 0);
+				driver.setRumble(GenericHID.RumbleType.kLeftRumble, 0);
+			});
+		} else {
+			return new InstantCommand();
+		}
 	}
 
 	public static Command hapticCopilotCommand() {
-		return new StartEndCommand(() -> {
-			coPilot.setRumble(GenericHID.RumbleType.kRightRumble, 1d);
-			coPilot.setRumble(GenericHID.RumbleType.kLeftRumble, 1d);
-		}, () -> {
-			coPilot.setRumble(GenericHID.RumbleType.kRightRumble, 0);
-			coPilot.setRumble(GenericHID.RumbleType.kLeftRumble, 0);
-		});
+		if (DriverStation.isAutonomous()) {
+			return new StartEndCommand(() -> {
+				coPilot.setRumble(GenericHID.RumbleType.kRightRumble, 1d);
+				coPilot.setRumble(GenericHID.RumbleType.kLeftRumble, 1d);
+			}, () -> {
+				coPilot.setRumble(GenericHID.RumbleType.kRightRumble, 0);
+				coPilot.setRumble(GenericHID.RumbleType.kLeftRumble, 0);
+			});
+		} else {
+			return new InstantCommand();
+		}	
 	}
 }
diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java
@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import edu.wpi.first.math.filter.Debouncer;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -18,15 +19,16 @@ public class Indexer extends SubsystemBase {
     private DigitalInput indexerSensorEntry = new DigitalInput(DIO.INDEXER_ENTER_BEAMBREAK);
     private DigitalInput indexerSensorExit = new DigitalInput(DIO.INDEXER_EXIT_BEAMBREAK);
 
-    private int exitIndexerIteration = 0;
-
     private double timeLastTriggered = 0d;
 
     private double targetPower = 0;
 
     private PieceState currentState = PieceState.NONE;
     private boolean didShoot = false;
 
+    private Debouncer entryDebouncer = new Debouncer(0.05);
+    private Debouncer exitDebouncer = new Debouncer(0.05);
+
     public Indexer(Collector collector) {
         this.collector = collector;
 
@@ -108,7 +110,7 @@ public void stop() {
      * @return entry beambreak state
      */
     public boolean getEntryBeamBreakState() {
-        return !indexerSensorEntry.get();
+        return entryDebouncer.calculate(!indexerSensorEntry.get());
     }
 
     /**
@@ -117,14 +119,14 @@ public boolean getEntryBeamBreakState() {
      * @return exit beambreak state
      */
     public boolean getExitBeamBreakState() {
-        return !indexerSensorExit.get();
+        return exitDebouncer.calculate(!indexerSensorExit.get());
     }
 
     /**
      * @return true if piece is exiting the indexer
      */
     public boolean isExiting() {
-        return exitIndexerIteration >= 1;
+        return getExitBeamBreakState() && getPieceState() == PieceState.IN_INDEXER;
     }
 
     /**
@@ -150,10 +152,7 @@ public void clearHasShot() {
     public void periodic() {
         // Update piece state based on beambreaks
         if (getExitBeamBreakState()) {
-            exitIndexerIteration++;
-            if (exitIndexerIteration >= 3) {
-                setPieceState(PieceState.IN_INDEXER);
-            }
+            setPieceState(PieceState.IN_INDEXER);
         } else if (getEntryBeamBreakState()) {
             setPieceState(PieceState.IN_PIVOT);
         } else if (collector.getEntryBeamBreakState()) {
@@ -165,11 +164,6 @@ public void periodic() {
             didShoot = didShoot || hasNote();
             setPieceState(PieceState.NONE);
         }
-
-        // reset exitIndexerIteration
-        if (!getEntryBeamBreakState()) {
-            exitIndexerIteration = 0;
-        }
     }
 
     public boolean hasNote() {