Made autoscore work from a larger area with Path on the Fly

src/main/java/frc/robot/RobotContainer.java

@@ -474,7 +474,8 @@ public RobotVisualizer getRobotVisualizer() {
   }
 
   public void putShuffleboardCommands() {
-
+    SmartDashboard.putData(
+        "Align", new SuperAutoScore(drivetrain, elevatorSubsystem, coralSubsystem, vision));
     if (Constants.CORAL_DEBUG) {
       SmartDashboard.putData(
           "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED));

src/main/java/frc/robot/commands/SuperAutoScore.java

@@ -1,6 +1,7 @@
 package frc.robot.commands;
 
 import frc.robot.commands.alignment.AlignClosestBranch;
+import frc.robot.commands.alignment.RoughAlignClosestBranch;
 import frc.robot.commands.coral.ShootCoral;
 import frc.robot.commands.elevator.ElevatorToStoredPosition;
 import frc.robot.constants.Constants;
@@ -20,8 +21,10 @@ public SuperAutoScore(
       Vision vision) {
     super(
         new SetSuperAutoScorePosition(drivetrain, elevator, vision),
-        new LoggableParallelCommandGroup(
-            new AlignClosestBranch(drivetrain), new ElevatorToStoredPosition(elevator)),
+        new LoggableSequentialCommandGroup(
+            new RoughAlignClosestBranch(drivetrain),
+            new LoggableParallelCommandGroup(
+                new AlignClosestBranch(drivetrain), new ElevatorToStoredPosition(elevator))),
         new LoggableWaitCommand(0.2),
         new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED));
   }

src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java

@@ -0,0 +1,42 @@
+package frc.robot.commands.alignment;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.constants.AlignmentPosition;
+import frc.robot.constants.Constants;
+import frc.robot.subsystems.swervev3.SwerveDrivetrain;
+import frc.robot.utils.logging.PathPlannerUtils;
+import frc.robot.utils.logging.commands.LoggableCommand;
+import org.littletonrobotics.junction.Logger;
+
+public class RoughAlignClosestBranch extends LoggableCommand {
+  private final SwerveDrivetrain drivetrain;
+  private final Timer timer = new Timer();
+  private Pose2d position;
+
+  public RoughAlignClosestBranch(SwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain);
+  }
+
+  @Override
+  public void initialize() {
+    timer.restart();
+    AlignmentPosition alignmentTarget =
+        AlignmentPosition.getClosest(drivetrain.getPose().getTranslation());
+    Logger.recordOutput("TargetReefRoughAlignment", alignmentTarget);
+    Pose2d position = alignmentTarget.getPathPlannerPosition();
+    Logger.recordOutput("TargetReefRoughPose", position);
+    PathPlannerUtils.pathToPose(position, 0).schedule();
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (drivetrain.getPose().getTranslation().getDistance(position.getTranslation())
+        < Constants.ALIGNMENT_DISTANCE_THRESHOLD) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+}

src/main/java/frc/robot/constants/AlignmentPosition.java

@@ -12,41 +12,48 @@
 public enum AlignmentPosition {
   // spotless:off Although this makes it harder to read it makes it easy to copy and paste from sheets
 
-  A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN),
+  A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN, new Pose2d(3.0076000000000, 4.0614305050000, Rotation2d.fromDegrees(0))),
 
-  B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN),
+  B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN, new Pose2d(3.0076000000000, 3.7328134950000, Rotation2d.fromRadians(0))),
 
-  C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT),
+  C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT, new Pose2d(3.7176981000607, 2.7604435073367, Rotation2d.fromDegrees(60))),
 
-  D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT),
+  D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT, new Pose2d(4.0022887788364, 2.5961350023367, Rotation2d.fromDegrees(60))),
 
-  E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE),
+  E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE, new Pose2d(5.1994349400607, 2.7249130023367  , Rotation2d.fromDegrees(120))),
 
-  F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE),
+  F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE, new Pose2d(5.4840256188364, 2.8892215073367, Rotation2d.fromDegrees(120))),
 
-  G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN),
+  G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN, new Pose2d(5.9710736800000, 3.9903694950000, Rotation2d.fromDegrees(180))),
 
-  H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN),
+  H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN, new Pose2d(5.9710736800000, 4.3189865050000, Rotation2d.fromDegrees(180))),
 
-  I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN),
+  I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN, new Pose2d(5.2609755799393, 5.2913564926633, Rotation2d.fromDegrees(240))),
 
-  J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN),
+  J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN, new Pose2d(4.9763849011636, 5.4556649976633, Rotation2d.fromDegrees(240))),
 
-  K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX),
+  K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX, new Pose2d(3.7792387399393, 5.3268869976633, Rotation2d.fromDegrees(300))),
 
-  L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX);
+  L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX, new Pose2d(3.4946480611636, 5.1625784926633, Rotation2d.fromDegrees(300)));
     // spotless:on
 
   private final Pose2d bluePosition;
   private final Pose2d redPosition;
   private final Apriltag blueTag;
   private final Apriltag redTag;
+  private final Pose2d bluePathPlannerPosition;
+  private final Pose2d redPathPlannerPosition;
 
-  AlignmentPosition(Pose2d position, Apriltag blueTag, Apriltag redTag) {
+  AlignmentPosition(
+      Pose2d position, Apriltag blueTag, Apriltag redTag, Pose2d pathPlannerPosition) {
     this.bluePosition = position;
+    this.bluePathPlannerPosition = pathPlannerPosition;
     Rotation2d redRotation = Rotation2d.fromRadians(Math.PI + position.getRotation().getRadians());
     double redX = FieldLocation.LENGTH_OF_FIELD - bluePosition.getX();
     double redY = FieldLocation.HEIGHT_OF_FIELD - bluePosition.getY();
+    double redPathPlannerX = FieldLocation.LENGTH_OF_FIELD - pathPlannerPosition.getX();
+    double redPathPlannerY = FieldLocation.HEIGHT_OF_FIELD - pathPlannerPosition.getY();
+    this.redPathPlannerPosition = new Pose2d(redPathPlannerX, redPathPlannerY, redRotation);
     this.redPosition = new Pose2d(redX, redY, redRotation);
     this.blueTag = blueTag;
     this.redTag = redTag;
@@ -60,6 +67,14 @@ public Pose2d getRedPosition() {
     return redPosition;
   }
 
+  public Pose2d getBluePathPlannerPosition() {
+    return bluePathPlannerPosition;
+  }
+
+  public Pose2d getRedPathPlannerPosition() {
+    return redPathPlannerPosition;
+  }
+
   public Pose2d getPosition() {
     Optional<DriverStation.Alliance> allianceColor = Robot.getAllianceColor();
     return allianceColor
@@ -69,6 +84,17 @@ public Pose2d getPosition() {
         .orElse(new Pose2d());
   }
 
+  public Pose2d getPathPlannerPosition() {
+    Optional<DriverStation.Alliance> allianceColor = Robot.getAllianceColor();
+    return allianceColor
+        .map(
+            alliance ->
+                alliance == DriverStation.Alliance.Blue
+                    ? getBluePathPlannerPosition()
+                    : getRedPathPlannerPosition())
+        .orElse(new Pose2d());
+  }
+
   public static AlignmentPosition getClosest(Translation2d currentPosition) {
     int closestIndex = 0;
     double closestDistance =

src/main/java/frc/robot/constants/GameConstants.java

@@ -90,7 +90,7 @@ public class GameConstants {
 
   // Treshholds
   public static final double VISION_CONSISTANCY_THRESHOLD = 0.25;
-  public static final double AUTO_ALIGN_THRESHOLD = 2.3; // degrees //TODO: change later
+  public static final double AUTO_ALIGN_THRESHOLD = 0.05; // TODO: change later
   public static final double ELEVATOR_ENCODER_THRESHHOLD = 1; // TODO: Change Later
   public static final int HIHI_EXTENDER_TICK_LIMIT = 10;
 

src/main/java/frc/robot/utils/logging/PathPlannerUtils.java

@@ -15,9 +15,9 @@ public class PathPlannerUtils {
   private static final PathConstraints defualtPathConstraints =
       new PathConstraints(
           Constants.MAX_VELOCITY,
-          Constants.MAX_VELOCITY,
-          Math.toRadians(1000),
-          Math.toRadians(1000));
+          Constants.MAX_PATHPLANNER_ACCEL,
+          Constants.MAX_ANGULAR_SPEED,
+          Math.toRadians(Constants.MAX_PATHPLANNER_ANGULAR_ACCEL));
 
   public static PathPlannerPath createManualPath(
       Pose2d startPose, Pose2d targetPos, double endVelocity) {