Driver Logic

src/main/java/frc/robot/Constants.java

@@ -11,8 +11,11 @@
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.units.AngularAccelerationUnit;
 import edu.wpi.first.units.AngularVelocityUnit;
@@ -26,6 +29,7 @@
 import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.units.measure.Per;
 import frc.robot.generated.TunerConstants;
+import frc.robot.utils.AlignPoses;
 import frc.robot.utils.VisionPoseEstimator.VisionPoseEstimatorConstants;
 import java.util.HashMap;
 
@@ -50,6 +54,50 @@ public static class Ports {
   public static class FieldConstants {
     public static final AprilTagFieldLayout fieldLayout =
         AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
+
+    public static final Translation2d reefCenter =
+        new Translation2d(Inches.of(176.75).in(Meters), Inches.of(158.5).in(Meters));
+
+    public static final Translation2d fieldCenter =
+        new Translation2d(fieldLayout.getFieldLength() / 2, fieldLayout.getFieldWidth() / 2);
+
+    public static final Translation2d humanCenter =
+        new Translation2d(Inches.of(47.93).in(Meter), Inches.of(158.28).in(Meters));
+
+    public static final AlignPoses reef =
+        new AlignPoses(
+            new Pose2d(Inches.of(101.3).in(Meters), Inches.of(170.5).in(Meters), Rotation2d.kZero),
+            new Pose2d(Inches.of(101.3).in(Meters), Inches.of(158.5).in(Meters), Rotation2d.kZero),
+            new Pose2d(Inches.of(101.3).in(Meters), Inches.of(146.5).in(Meters), Rotation2d.kZero));
+
+    public static final AlignPoses human =
+        new AlignPoses(
+            new Pose2d(
+                Inches.of(23.14).in(Meters),
+                Inches.of(264.66).in(Meters),
+                new Rotation2d(Degrees.of(126))),
+            new Pose2d(
+                Inches.of(41.17).in(Meters),
+                Inches.of(282.69).in(Meters),
+                new Rotation2d(Degrees.of(126))),
+            new Pose2d(
+                Inches.of(64.61).in(Meters),
+                Inches.of(294.86).in(Meters),
+                new Rotation2d(Degrees.of(126))));
+
+    public static final AlignPoses processor =
+        new AlignPoses(
+            new Pose2d(
+                Inches.of(233.7).in(Meters), Inches.of(16.2).in(Meters), Rotation2d.kCW_90deg));
+
+    public static final AlignPoses cage =
+        new AlignPoses(
+            new Pose2d(
+                Inches.of(324.95).in(Meters), Inches.of(285.84).in(Meters), Rotation2d.kCCW_90deg),
+            new Pose2d(
+                Inches.of(324.95).in(Meters), Inches.of(241.89).in(Meters), Rotation2d.kCCW_90deg),
+            new Pose2d(
+                Inches.of(324.95).in(Meters), Inches.of(200.16).in(Meters), Rotation2d.kCCW_90deg));
   }
 
   public static class VisionConstants {

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

@@ -31,6 +31,7 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.lib.FaultLogger;
 import frc.lib.InputStream;
+import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.Ports;
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.Constants.WristevatorConstants;
@@ -44,6 +45,8 @@
 import frc.robot.subsystems.Serializer;
 import frc.robot.subsystems.Swerve;
 import frc.robot.subsystems.Wristevator;
+import frc.robot.utils.AlignPoses;
+import frc.robot.utils.AlignPoses.AlignSide;
 
 /**
  * The methods in this class are called automatically corresponding to each mode, as described in
@@ -187,14 +190,30 @@ private void configureDefaultCommands() {
                     .scale(WristevatorConstants.maxWristSpeed.in(RadiansPerSecond))));
   }
 
-  private void configureDriverBindings() {
-    _driverController.x().whileTrue(_swerve.brake());
-    _driverController.a().onTrue(_swerve.toggleFieldOriented());
-    _driverController.y().onTrue(_swerve.resetHeading());
+  private void alignmentTriggers(Trigger button, AlignPoses poses) {
+    button
+        .and(_driverController.leftTrigger().and(_driverController.rightTrigger().negate()))
+        .whileTrue(_swerve.alignTo(poses, AlignSide.LEFT));
+
+    button
+        .and(
+            _driverController.leftTrigger().negate().and(_driverController.rightTrigger().negate()))
+        .whileTrue(_swerve.alignTo(poses, AlignSide.CENTER));
 
-    // _driverController
-    //     .b()
-    //     .whileTrue(_swerve.driveTo(new Pose2d(10, 3, Rotation2d.fromDegrees(-150))));
+    button
+        .and(_driverController.rightTrigger().and(_driverController.leftTrigger().negate()))
+        .whileTrue(_swerve.alignTo(poses, AlignSide.RIGHT));
+  }
+
+  private void configureDriverBindings() {
+    _driverController.a().whileTrue(_swerve.brake());
+    _driverController.povUp().onTrue(_swerve.toggleFieldOriented());
+    _driverController.povDown().onTrue(_swerve.resetHeading());
+
+    alignmentTriggers(_driverController.x(), FieldConstants.reef);
+    alignmentTriggers(_driverController.y(), FieldConstants.human);
+    alignmentTriggers(_driverController.b(), FieldConstants.processor);
+    alignmentTriggers(_driverController.start(), FieldConstants.cage);
   }
 
   private void configureOperatorBindings() {

src/main/java/frc/robot/subsystems/Swerve.java

@@ -5,7 +5,7 @@
 package frc.robot.subsystems;
 
 import static edu.wpi.first.units.Units.*;
-import static edu.wpi.first.wpilibj2.command.Commands.*;
+import static edu.wpi.first.wpilibj2.command.Commands.sequence;
 
 import choreo.trajectory.SwerveSample;
 import com.ctre.phoenix6.SignalLogger;
@@ -47,13 +47,16 @@
 import frc.robot.Constants.VisionConstants;
 import frc.robot.Robot;
 import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
+import frc.robot.utils.AlignPoses;
+import frc.robot.utils.AlignPoses.AlignSide;
 import frc.robot.utils.HolonomicController;
 import frc.robot.utils.SysId;
 import frc.robot.utils.VisionPoseEstimator;
 import frc.robot.utils.VisionPoseEstimator.VisionPoseEstimate;
 import java.util.ArrayList;
 import java.util.HashSet;
 import java.util.List;
+import java.util.Optional;
 import java.util.Set;
 import org.photonvision.simulation.VisionSystemSim;
 
@@ -146,6 +149,8 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem, SelfChec
   @Logged(name = "Ignore Vision Estimates")
   private boolean _ignoreVisionEstimates = true; // for sim for now
 
+  private AlignPoses _alignGoal = new AlignPoses(Pose2d.kZero);
+
   private HolonomicController _poseController = new HolonomicController();
 
   @Logged(name = VisionConstants.blueArducamName)
@@ -427,6 +432,84 @@ public void followTrajectory(SwerveSample sample) {
             .withWheelForceFeedforwardsY(sample.moduleForcesY()));
   }
 
+  /**
+   * Aligns to a {@link AlignPoses} to the correct side.
+   *
+   * @return Drive to the correct pose.
+   */
+  public Command alignTo(AlignPoses alignGoal, AlignSide side) {
+    return runOnce(
+            () -> {
+              Pose2d pose = getPose();
+              Optional<Alliance> alliance = DriverStation.getAlliance();
+
+              if (alignGoal == FieldConstants.reef) {
+                double minDistance = Double.MAX_VALUE;
+
+                var goal =
+                    alliance.get() == Alliance.Red
+                        ? alignGoal.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg)
+                        : alignGoal;
+
+                var reefCenter =
+                    alliance.get() == Alliance.Red
+                        ? FieldConstants.reefCenter.rotateAround(
+                            FieldConstants.fieldCenter, Rotation2d.k180deg)
+                        : FieldConstants.reefCenter;
+
+                for (int i = 0; i < 6; i++) {
+                  var rotated = goal.rotateAround(reefCenter, Rotation2d.fromDegrees(60).times(i));
+
+                  if (pose.minus(rotated.getCenter()).getTranslation().getNorm() < minDistance) {
+                    _alignGoal = rotated;
+                    minDistance = pose.minus(rotated.getCenter()).getTranslation().getNorm();
+                  }
+                }
+              }
+
+              if (alignGoal == FieldConstants.human) {
+                double minDistance = Double.MAX_VALUE;
+
+                for (int i = 0; i < 2; i++) {
+                  AlignPoses rotated = alignGoal.offset(0, -6.26 * i);
+
+                  rotated =
+                      alliance.get() == Alliance.Red
+                          ? rotated.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg)
+                          : rotated;
+
+                  rotated =
+                      rotated.rotateAround(
+                          rotated.getCenter().getTranslation(),
+                          Rotation2d.fromDegrees(106).times(i));
+
+                  if (pose.minus(rotated.getCenter()).getTranslation().getNorm() < minDistance) {
+                    _alignGoal = rotated;
+                    minDistance = pose.minus(rotated.getCenter()).getTranslation().getNorm();
+                  }
+                }
+              }
+
+              if (alignGoal == FieldConstants.processor) {
+                _alignGoal =
+                    alliance.get() == Alliance.Red
+                        ? alignGoal.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg)
+                        : alignGoal;
+              }
+
+              if (alignGoal == FieldConstants.cage) {
+                _alignGoal =
+                    alliance.get() == Alliance.Red
+                        ? alignGoal.rotateAround(FieldConstants.fieldCenter, Rotation2d.k180deg)
+                        : alignGoal;
+              }
+
+              DogLog.log("Auto/Align Pose", _alignGoal.getPose(side));
+            })
+        .andThen(defer(() -> driveTo(_alignGoal.getPose(side))))
+        .withName("Align To");
+  }
+
   /** Drives the robot in a straight line to some given goal pose. */
   public Command driveTo(Pose2d goalPose) {
     return run(() -> {

src/main/java/frc/robot/utils/AlignPoses.java

@@ -0,0 +1,81 @@
+// 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.utils;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+/** Poses for alignment. */
+public class AlignPoses {
+  private final Pose2d _left;
+  private final Pose2d _center;
+  private final Pose2d _right;
+
+  public static enum AlignSide {
+    LEFT,
+    CENTER,
+    RIGHT
+  }
+
+  public Pose2d getLeft() {
+    return _left;
+  }
+
+  public Pose2d getCenter() {
+    return _center;
+  }
+
+  public Pose2d getRight() {
+    return _right;
+  }
+
+  /** Setup align poses for three positions */
+  public AlignPoses(Pose2d left, Pose2d center, Pose2d right) {
+    _left = left;
+    _center = center;
+    _right = right;
+  }
+
+  /** Setup align pose for center */
+  public AlignPoses(Pose2d center) {
+    this(center, center, center);
+  }
+
+  /** Get pose depending on side. */
+  public Pose2d getPose(AlignSide side) {
+    switch (side) {
+      case LEFT:
+        return getLeft();
+
+      case CENTER:
+        return getCenter();
+
+      case RIGHT:
+        return getRight();
+
+      default:
+        return Pose2d.kZero;
+    }
+  }
+
+  /** Rotates all the poses around a point. */
+  public AlignPoses rotateAround(Translation2d point, Rotation2d rot) {
+    return new AlignPoses(
+        new Pose2d(_left.getTranslation().rotateAround(point, rot), _left.getRotation().plus(rot)),
+        new Pose2d(
+            _center.getTranslation().rotateAround(point, rot), _center.getRotation().plus(rot)),
+        new Pose2d(
+            _right.getTranslation().rotateAround(point, rot), _right.getRotation().plus(rot)));
+  }
+
+  /** Offset the points by a transformation */
+  public AlignPoses offset(double x, double y) {
+    return new AlignPoses(
+        new Pose2d(_left.getX() + x, _left.getY() + y, _left.getRotation()),
+        new Pose2d(_center.getX() + x, _center.getY() + y, _center.getRotation()),
+        new Pose2d(_right.getX() + x, _right.getY() + y, _right.getRotation()));
+  }
+}