Note visualizer shows intaking auto notes

Mechanical-Advantage · Mar 30, 2024 · ee2791c · ee2791c
1 parent 85ec8ac
commit ee2791c
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 5 deletions.
diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java
@@ -7,6 +7,7 @@
 
 package org.littletonrobotics.frc2024.subsystems.rollers;
 
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -15,16 +16,28 @@
 import java.util.function.BooleanSupplier;
 import lombok.Getter;
 import lombok.Setter;
+import org.littletonrobotics.frc2024.Constants;
+import org.littletonrobotics.frc2024.FieldConstants;
+import org.littletonrobotics.frc2024.RobotState;
 import org.littletonrobotics.frc2024.subsystems.leds.Leds;
 import org.littletonrobotics.frc2024.subsystems.rollers.backpack.Backpack;
 import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder;
 import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer;
 import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake;
+import org.littletonrobotics.frc2024.util.AllianceFlipUtil;
 import org.littletonrobotics.frc2024.util.NoteVisualizer;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 public class Rollers extends SubsystemBase {
+  private static final double intakeNoteProximity = 0.6;
+  private static final Translation2d[] autoNotes = new Translation2d[8];
+
+  static {
+    System.arraycopy(FieldConstants.StagingLocations.spikeTranslations, 0, autoNotes, 0, 3);
+    System.arraycopy(FieldConstants.StagingLocations.centerlineTranslations, 0, autoNotes, 3, 5);
+  }
+
   private final Feeder feeder;
   private final Indexer indexer;
   private final Intake intake;
@@ -200,16 +213,30 @@ public void periodic() {
 
     Leds.getInstance().hasNote = gamepieceState != GamepieceState.NONE;
     Leds.getInstance().intaking = goal == Goal.FLOOR_INTAKE || goal == Goal.STATION_INTAKE;
-    NoteVisualizer.setHasNote(gamepieceState != GamepieceState.NONE);
+    if (Constants.getMode() == Constants.Mode.REAL) {
+      NoteVisualizer.setHasNote(gamepieceState != GamepieceState.NONE);
+    }
+    if (DriverStation.isAutonomousEnabled()) {
+      Translation2d robot = RobotState.getInstance().getEstimatedPose().getTranslation();
+      int closest = 0;
+      for (int i = 0; i < autoNotes.length; i++) {
+        if (AllianceFlipUtil.apply(autoNotes[i]).getDistance(robot)
+            <= AllianceFlipUtil.apply(autoNotes[closest]).getDistance(robot)) {
+          closest = i;
+        }
+      }
+      if (AllianceFlipUtil.apply(autoNotes[closest]).getDistance(robot) <= intakeNoteProximity) {
+        NoteVisualizer.intakeAutoNote(closest);
+      }
+    }
   }
 
   public Command setGoalCommand(Goal goal) {
     return startEnd(() -> this.goal = goal, () -> this.goal = Goal.IDLE)
         .alongWith(
-            Commands.either(
-                Commands.waitSeconds(0.15).andThen(NoteVisualizer.shoot()),
-                Commands.none(),
-                () -> goal == Goal.FEED_TO_SHOOTER))
+            Commands.waitSeconds(0.15)
+                .andThen(
+                    NoteVisualizer.shoot().onlyIf(() -> feeder.getGoal() == Feeder.Goal.SHOOTING)))
         .withName("Rollers " + goal);
   }
 

diff --git a/src/main/java/org/littletonrobotics/frc2024/util/NoteVisualizer.java b/src/main/java/org/littletonrobotics/frc2024/util/NoteVisualizer.java
@@ -68,6 +68,18 @@ public static void resetAutoNotes() {
     }
   }
 
+  /**
+   * Intakes staged note.
+   *
+   * @param index of staged note (0-2) spikes, (3-7) centerlines
+   */
+  public static void intakeAutoNote(int index) {
+    if (autoNotes.get(index) != null) {
+      setHasNote(true);
+    }
+    autoNotes.set(index, null);
+  }
+
   /** Shows the currently held note if there is one */
   public static void showHeldNotes() {
     if (hasNote) {