Add amp align capabilities

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

@@ -13,6 +13,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.amp_align.AmpAlignManager;
 import frc.robot.autos.Autos;
 import frc.robot.climber.AutoClimbManager;
 import frc.robot.climber.ClimberSubsystem;
@@ -90,8 +91,8 @@ public class Robot extends LoggedRobot {
   private final LightsSubsystem lightsSubsystem =
       new LightsSubsystem(
           new CANdle(RobotConfig.get().lights().deviceID(), "rio"), robotManager, vision);
-  private final AutoClimbManager autoClimbManager =
-      new AutoClimbManager(localization, actions, robotManager, swerve);
+  private final AutoClimbManager autoClimbManager = new AutoClimbManager(localization, swerve);
+  private final AmpAlignManager ampAlignManager = new AmpAlignManager(localization, swerve);
 
   public Robot() {
     System.out.println("roboRIO serial number: " + RobotConfig.SERIAL_NUMBER);
@@ -208,7 +209,9 @@ private void configureBindings() {
     driverController.rightBumper().onTrue(actions.outtakeCommand()).onFalse(actions.stowCommand());
 
     operatorController.povUp().onTrue(actions.getClimberForwardCommand());
-    operatorController.povDown().whileTrue(autoClimbManager.getClimbSequenceCommand());
+    operatorController.povLeft().whileTrue(autoClimbManager.getClimbSequenceCommand());
+    operatorController.povRight().whileTrue(ampAlignManager.getAlignForAmpCommand());
+
     operatorController.povDown().onTrue(actions.getClimberBackwardCommand());
     operatorController.a().onTrue(actions.stowCommand());
     operatorController

src/main/java/frc/robot/amp_align/AmpAlignManager.java

@@ -0,0 +1,57 @@
+// 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.amp_align;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.localization.LocalizationSubsystem;
+import frc.robot.swerve.SwerveSubsystem;
+import frc.robot.util.scheduling.LifecycleSubsystem;
+import frc.robot.util.scheduling.SubsystemPriority;
+import java.util.List;
+import org.littletonrobotics.junction.Logger;
+
+public class AmpAlignManager extends LifecycleSubsystem {
+  private final LocalizationSubsystem localization;
+  private final SwerveSubsystem swerve;
+
+  public AmpAlignManager(LocalizationSubsystem localization, SwerveSubsystem swerve) {
+    super(SubsystemPriority.CLIMBER);
+
+    this.localization = localization;
+    this.swerve = swerve;
+  }
+
+  private AmpAlignPath getClosestTarget() {
+    Pose2d current = localization.getPose();
+
+    List<AmpAlignPath> paths = AmpAlignPaths.PATHS;
+
+    AmpAlignPath closest = AmpAlignPaths.PATHS.get(0);
+    double currentDistance = Double.POSITIVE_INFINITY;
+
+    for (AmpAlignPath target : paths) {
+      double distance =
+          target.behindAmpPose().getTranslation().getDistance(current.getTranslation());
+      if (distance < currentDistance) {
+        closest = target;
+        currentDistance = distance;
+      }
+    }
+    return closest;
+  }
+
+  public Command getAlignForAmpCommand() {
+    return swerve.driveToPoseCommand(
+        () -> getClosestTarget().behindAmpPose(), localization::getPose);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    Logger.recordOutput("AmpAlign/ClosestPose", getClosestTarget().behindAmpPose());
+    Logger.recordOutput(
+        "AmpAlign/ClosestPathRotation", getClosestTarget().behindAmpPose().getRotation());
+  }
+}

src/main/java/frc/robot/amp_align/AmpAlignPath.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.amp_align;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public record AmpAlignPath(Pose2d behindAmpPose, Rotation2d robotHeading) {}

src/main/java/frc/robot/amp_align/AmpAlignPaths.java

@@ -0,0 +1,20 @@
+// 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.amp_align;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.util.List;
+
+public class AmpAlignPaths {
+  public static final AmpAlignPath RED_AMP =
+      new AmpAlignPath(
+          new Pose2d(14.66, 7.32, Rotation2d.fromDegrees(-90)), Rotation2d.fromDegrees(-90));
+  public static final AmpAlignPath BLUE_AMP =
+      new AmpAlignPath(
+          new Pose2d(1.8, 7.19, Rotation2d.fromDegrees(-90)), Rotation2d.fromDegrees(-90));
+
+  public static final List<AmpAlignPath> PATHS = List.of(RED_AMP, BLUE_AMP);
+}

src/main/java/frc/robot/climber/AutoClimbManager.java

@@ -8,8 +8,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.fms.FmsSubsystem;
 import frc.robot.localization.LocalizationSubsystem;
-import frc.robot.robot_manager.RobotCommands;
-import frc.robot.robot_manager.RobotManager;
 import frc.robot.swerve.SwerveSubsystem;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
@@ -18,20 +16,12 @@
 
 public class AutoClimbManager extends LifecycleSubsystem {
   private final LocalizationSubsystem localization;
-  private final RobotCommands actions;
-  private final RobotManager robot;
   private final SwerveSubsystem swerve;
 
-  public AutoClimbManager(
-      LocalizationSubsystem localization,
-      RobotCommands actions,
-      RobotManager robot,
-      SwerveSubsystem swerve) {
+  public AutoClimbManager(LocalizationSubsystem localization, SwerveSubsystem swerve) {
     super(SubsystemPriority.CLIMBER);
 
     this.localization = localization;
-    this.actions = actions;
-    this.robot = robot;
     this.swerve = swerve;
   }
 

src/main/java/frc/robot/climber/ClimbAlignPaths.java

@@ -11,37 +11,31 @@
 public class ClimbAlignPaths {
   public static final ClimbPath RED_LEFT =
       new ClimbPath(
-          // PathPlannerPath.bezierFromPoses(
           new Pose2d(12.69, 2.37, Rotation2d.fromDegrees(-60)),
           new Pose2d(12.0, 3.5, Rotation2d.fromDegrees(-60)),
           Rotation2d.fromDegrees(-60));
   public static final ClimbPath RED_FORWARD =
       new ClimbPath(
-          // PathPlannerPath.bezierFromPoses(
           new Pose2d(9.48, 3.94, Rotation2d.fromDegrees(180)),
           new Pose2d(10.80, 3.94, Rotation2d.fromDegrees(180)),
           Rotation2d.fromDegrees(180));
   public static final ClimbPath RED_RIGHT =
       new ClimbPath(
-          //  PathPlannerPath.bezierFromPoses(
           new Pose2d(12.6, 5.74, Rotation2d.fromDegrees(60)),
           new Pose2d(12.14, 4.95, Rotation2d.fromDegrees(60)),
           Rotation2d.fromDegrees(60));
   public static final ClimbPath BLUE_LEFT =
       new ClimbPath(
-          // PathPlannerPath.bezierFromPoses(
           new Pose2d(5.64, 5.5, Rotation2d.fromDegrees(0)),
           new Pose2d(5.00, 4.75, Rotation2d.fromDegrees(0)),
           Rotation2d.fromDegrees(120));
   public static final ClimbPath BLUE_FORWARD =
       new ClimbPath(
-          // PathPlannerPath.bezierFromPoses(
           new Pose2d(6.32, 4.11, Rotation2d.fromDegrees(0)),
           new Pose2d(5.55, 4.11, Rotation2d.fromDegrees(0)),
           Rotation2d.fromDegrees(240));
   public static final ClimbPath BLUE_RIGHT =
       new ClimbPath(
-          // PathPlannerPath.bezierFromPoses(
           new Pose2d(3.85, 2.49, Rotation2d.fromDegrees(-120)),
           new Pose2d(4.36, 3.26, Rotation2d.fromDegrees(-120)),
           Rotation2d.fromDegrees(-120));