Add command to align to source

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

@@ -61,6 +61,7 @@ public class RobotContainer {
     private final Trigger slideShotButton = driver.x();
     private final Trigger climberExtendButton = driver.y();
     private final Trigger ampShotButton = driver.povDown();
+    private final Trigger sourceAlignButton = driver.povUp();
 
     /* Subsystems */
     private final Swerve s_Swerve = new Swerve();
@@ -297,6 +298,7 @@ private void configureButtonBindings() {
         ejectButton.whileTrue(new EjectCommand(s_Intake, s_Index, s_Shooter));
 
         ampShotButton.whileTrue(ampPathCommand().withName("Amp path & shoot"));
+        sourceAlignButton.whileTrue(sourcePathCommand().withName("Source align"));
     }
 
     /**
@@ -414,6 +416,19 @@ private Pose2d getAmpPose() {
         return pose;
     }
 
+    private Pose2d getSourcePose() {
+        // Get pose from Vision
+        if (!s_Vision.haveSourceTarget()) {
+            return s_Swerve.getPose();
+        }
+        Pose2d pose = s_Vision.lastPose();
+
+        // Update pose in case we lose the source target
+        s_Swerve.setPose(pose);
+
+        return pose;
+    }
+
     private Command ampPathCommand() {
         PathPlannerPath path = PathPlannerPath.fromPathFile("To Amp");
 
@@ -439,4 +454,28 @@ private Command ampPathCommand() {
             new ShootCommand(s_Shooter, s_Index, false)
         ).handleInterrupt(s_Vision::disableRotationAmpOverride);
     }
+
+    private Command sourcePathCommand() {
+        PathPlannerPath path = PathPlannerPath.fromPathFile("To Source");
+
+        return Commands.sequence(
+            Commands.runOnce(s_Vision::enableRotationSourceOverride),
+            new FollowPathHolonomic(
+                path,
+                this::getSourcePose, // Robot pose supplier
+                s_Swerve::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+                s_Swerve::driveRobotRelativeAuto, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds
+                new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class
+                    new PIDConstants(8.0, 0.0, 0.0), // Translation PID constants
+                    new PIDConstants(2.0, 0.0, 0.0), // Rotation PID constants
+                    Constants.Swerve.maxSpeed, // Max module speed, in m/s
+                    Constants.Swerve.driveRadius, // Drive base radius in meters. Distance from robot center to furthest module.
+                    new ReplanningConfig() // Default path replanning config. See the API for the options here
+                ),
+                Robot::isRed,
+                s_Swerve // Reference to this subsystem to set requirements
+            ),
+            Commands.runOnce(s_Vision::disableRotationSourceOverride)
+        ).handleInterrupt(s_Vision::disableRotationSourceOverride);
+    }
 }

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

@@ -37,9 +37,11 @@ public class VisionSubsystem extends SubsystemBase {
   private boolean haveTarget = false;
   private boolean haveSpeakerTarget = false;
   private boolean haveAmpTarget = false;
+  private boolean haveSourceTarget = false;
   private Pose2d lastPose  = new Pose2d();
   private boolean overrideRotation = false;
   private boolean overrideAmpRotation = false;
+  private boolean overrideSourceRotation = false;
 
   public VisionSubsystem() {
     assert(instance == null);
@@ -63,6 +65,9 @@ public VisionSubsystem() {
   public void enableRotationAmpOverride() { overrideAmpRotation = true; }
   public void disableRotationAmpOverride() { overrideAmpRotation = false; }
 
+  public void enableRotationSourceOverride() { overrideSourceRotation = true; }
+  public void disableRotationSourceOverride() { overrideSourceRotation = false; }
+
   public Optional<Rotation2d> getRotationTargetOverride() {
       if (!overrideRotation || !haveSpeakerTarget()) {
           return Optional.empty();
@@ -84,6 +89,16 @@ public Optional<Rotation2d> getRotationAmpOverride() {
       return Optional.of(rotation);
   }
 
+  public Optional<Rotation2d> getRotationSpeakerOverride() {
+      if (!overrideSourceRotation || !haveSourceTarget()) {
+          return Optional.empty();
+      }
+      Rotation2d adjustment = new Rotation2d(); //Units.degreesToRadians(180.0)); // TODO What about when on Red Alliance?
+      Rotation2d rotation = angleToSpeaker().plus(adjustment); // Adjust angle to PathPlanner coordinates
+      // System.out.println("Overriding amp rotation target to be " + rotation.getDegrees());
+      return Optional.of(rotation);
+  }
+
   public static VisionSubsystem getInstance() {
     return instance;
   }
@@ -100,6 +115,10 @@ public boolean haveAmpTarget() {
     return haveAmpTarget;
   }
 
+  public boolean haveSourceTarget() {
+    return haveSourceTarget;
+  }
+
   private Translation2d speakerOffset() {
     return lastPose.getTranslation().minus(speakerLocation());
   }
@@ -168,6 +187,14 @@ private boolean isAmpId(int id) {
     }
   }
 
+  private boolean isSourceId(int id) {
+    if (Robot.isRed()) {
+      return (id == 9 || id == 10);
+    } else {
+      return (id == 1 || id == 2);
+    }
+  }
+
   public Pose2d lastPose() {
     return lastPose;
   }
@@ -204,16 +231,19 @@ public void periodic() {
       haveTarget = result.hasTargets();
       haveSpeakerTarget = false;
       haveAmpTarget = false;
+      haveSourceTarget = false;
       if (haveTarget) {
         result.getTargets().forEach((t) -> {
           haveSpeakerTarget = haveSpeakerTarget || isSpeakerId(t.getFiducialId());
           haveAmpTarget = haveAmpTarget || isAmpId(t.getFiducialId());
+          haveSourceTarget = haveSourceTarget || isSourceId(t.getFiducialId());
         } );
       }
       if (updateDashboard) {
         SmartDashboard.putBoolean("vision/Have target(s)", haveTarget);
         SmartDashboard.putBoolean("vision/Have speaker target", haveSpeakerTarget);
         SmartDashboard.putBoolean("vision/Have amp target", haveAmpTarget);
+        SmartDashboard.putBoolean("vision/Have source target", haveSourceTarget);
       }
     }
   }