Start adding auto capabilities for Note Map

src/main/java/frc/robot/Robot.java

@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.auto_manager.AutoManager;
 import frc.robot.autos.Autos;
 import frc.robot.climber.ClimberSubsystem;
 import frc.robot.config.RobotConfig;
@@ -97,6 +98,7 @@ public class Robot extends TimedRobot {
           new CANdle(RobotConfig.get().lights().deviceID(), "rio"), robotManager, vision, intake);
   private final NoteTrackingManager noteTrackingManager =
       new NoteTrackingManager(localization, swerve, actions, robotManager, imu);
+  private final AutoManager autoManager = new AutoManager(actions, noteTrackingManager, robotManager);
 
   public Robot() {
     System.out.println("roboRIO serial number: " + RobotConfig.SERIAL_NUMBER);
@@ -239,7 +241,7 @@ private void configureBindings() {
         .onFalse(actions.stowCommand());
     operatorController
         .povRight()
-        .onTrue(noteTrackingManager.intakeNearestMapNote())
+        .onTrue(autoManager.testCommand())
         .onFalse(actions.stowCommand());
     operatorController
         .rightTrigger()

src/main/java/frc/robot/auto_manager/AutoManager.java

@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.auto_manager;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.intake.IntakeSubsystem;
+import frc.robot.note_manager.NoteManager;
+import frc.robot.note_tracking.NoteMapElement;
+import frc.robot.note_tracking.NoteTrackingManager;
+import frc.robot.robot_manager.RobotCommands;
+import frc.robot.robot_manager.RobotManager;
+import frc.robot.robot_manager.RobotState;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class AutoManager {
+  private final RobotCommands actions;
+  private final NoteTrackingManager noteTrackingManager;
+  private final RobotManager robotManager;
+
+  public AutoManager(RobotCommands actions, NoteTrackingManager noteTrackingManager, RobotManager robotManager) {
+    this.actions = actions;
+    this.noteTrackingManager = noteTrackingManager;
+    this.robotManager = robotManager;
+  }
+
+  public Command doManyAutoSteps(List<AutoNoteStep> steps) {
+    return Commands.sequence(steps.stream().map(this::doAutoStep).toArray(Command[]::new));
+  }
+
+  private Command doAutoStep(AutoNoteStep step) {
+    var command = noteTrackingManager.intakeNoteAtPose(step.noteSearchPose().get());
+
+    // TODO: Pathfind to outtake/shoot position
+    if (step.action() == AutoNoteAction.OUTTAKE) {
+      command = command.andThen(actions.shooterOuttakeCommand().unless(()-> !robotManager.getState().hasNote));
+    } else if (step.action() == AutoNoteAction.SCORE) {
+      command = command.andThen(actions.speakerShotCommand().unless(()-> !robotManager.getState().hasNote));
+    }
+
+    return command;
+  }
+
+  public Command testCommand() {
+
+    return Commands.sequence(
+        Commands.runOnce(
+            () -> {
+              var now = Timer.getFPGATimestamp();
+              // 1. reset note map as if it were start of auto
+              noteTrackingManager.resetNoteMap(
+                  new ArrayList<>(
+                      List.of(
+                        //   new NoteMapElement(now + 5, new Pose2d(8.271, 7.458, new Rotation2d(0))),
+                          new NoteMapElement(now + 5, AutoNoteStep.noteIdToPose(4)),
+                          new NoteMapElement(now + 5, AutoNoteStep.noteIdToPose(5)),
+                          new NoteMapElement(now + 5, AutoNoteStep.noteIdToPose(6)))));
+            }),
+        // 2. intakes notes 4 5 6 and scores
+        doManyAutoSteps(List.of(
+            new AutoNoteStep(4, AutoNoteAction.OUTTAKE),
+            new AutoNoteStep(5, AutoNoteAction.SCORE),
+            new AutoNoteStep(6, AutoNoteAction.SCORE)
+        )));
+  }
+}

src/main/java/frc/robot/auto_manager/AutoNoteAction.java

@@ -0,0 +1,10 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.auto_manager;
+
+public enum AutoNoteAction {
+  SCORE,
+  OUTTAKE;
+}

src/main/java/frc/robot/auto_manager/AutoNoteStep.java

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.auto_manager;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.fms.FmsSubsystem;
+import java.util.function.Supplier;
+
+public record AutoNoteStep(Supplier<Pose2d> noteSearchPose, AutoNoteAction action) {
+  public static Pose2d noteIdToPose(int noteId) {
+    switch (noteId) {
+      case 1:
+        if (FmsSubsystem.isRedAlliance()) {
+          return new Pose2d(13.65, 4.106, new Rotation2d(0));
+        } else {
+          return new Pose2d(2.896, 4.106, new Rotation2d(0));
+        }
+      case 2:
+        if (FmsSubsystem.isRedAlliance()) {
+          return new Pose2d(13.65, 5.553, new Rotation2d(0));
+        } else {
+          return new Pose2d(2.896, 5.553, new Rotation2d(0));
+        }
+      case 3:
+        if (FmsSubsystem.isRedAlliance()) {
+          return new Pose2d(13.65, 7.001, new Rotation2d(0));
+        } else {
+          return new Pose2d(2.896, 7.001, new Rotation2d(0));
+        }
+      case 4:
+        return new Pose2d(8.271, 7.458, new Rotation2d(0));
+      case 5:
+        return new Pose2d(8.271, 5.782, new Rotation2d(0));
+      case 6:
+        return new Pose2d(8.271, 4.106, new Rotation2d(0));
+      case 7:
+        return new Pose2d(8.271, 2.429, new Rotation2d(0));
+      case 8:
+        return new Pose2d(8.271, 0.753, new Rotation2d(0));
+      default:
+        throw new IllegalArgumentException("Expected a note ID from between 1 and 8");
+    }
+  }
+
+  public AutoNoteStep(int noteId, AutoNoteAction action) {
+    this(() -> noteIdToPose(noteId), action);
+  }
+}

src/main/java/frc/robot/note_tracking/NoteTrackingManager.java

@@ -30,6 +30,7 @@
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
+import java.util.function.Supplier;
 
 public class NoteTrackingManager extends LifecycleSubsystem {
 
@@ -76,7 +77,7 @@ public void resetNoteMap(ArrayList<NoteMapElement> startingValues) {
    * Get the pose of the note closest to the provided location, within a threshold. Returns
    * optional.empty if no notes are tracked or notes exceed the threshold.
    */
-  public Optional<NoteMapElement> getNearestNotePoseRelative(
+  private Optional<NoteMapElement> getNearestNotePoseRelative(
       Pose2d searchLocation, double thresholdMeters) {
 
     var maybeElement =
@@ -232,13 +233,18 @@ private void removeNote(NoteMapElement note) {
     noteMap.remove(note);
   }
 
-  public Command intakeNearestMapNote() {
+  public Command intakeNoteAtPose(Pose2d searchPose) {
+    return intakeNoteAtPose(() -> searchPose);
+  }
+
+  public Command intakeNoteAtPose(Supplier<Pose2d> searchPose) {
+    // TODO: If no note found, exit the command (somehow)
     return actions
         .intakeCommand()
         .alongWith(
             swerve.driveToPoseCommand(
                 () -> {
-                  var nearestNote = getNearestNotePoseRelative(getPose(), 5);
+                  var nearestNote = getNearestNotePoseRelative(searchPose.get(), 1.5);
 
                   if (nearestNote.isPresent()) {
                     snaps.setAngle(nearestNote.get().notePose().getRotation());
@@ -252,18 +258,21 @@ public Command intakeNearestMapNote() {
                 },
                 this::getPose,
                 false))
-        .until(() -> robot.getState() == RobotState.IDLE_WITH_GP)
+        .until(() -> robot.getState() == RobotState.IDLE_WITH_GP || getNearestNotePoseRelative(searchPose.get(), 1.5).isEmpty())
         .andThen(
             Commands.runOnce(
                 () -> {
-                  var intakedNote = getNearestNotePoseRelative(getPose(), 0.5);
+                  var intakedNote = getNearestNotePoseRelative(searchPose.get(), 0.5);
                   if (intakedNote.isPresent()) {
                     removeNote(intakedNote.get());
                   }
                 }))
         .withName("IntakeNearestNoteCommand");
   }
 
+  public Command intakeNearestMapNote() {
+    return intakeNoteAtPose(this::getPose);
+  }
 
   private Pose2d getPose() {
     return localization.getPose();