Keep at range for source

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

@@ -25,6 +25,7 @@
 import frc.robot.commands.AdaptiveAutoAlignCommands;
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.ManualAlignCommands;
+import frc.robot.commands.SourceStrafingCommands;
 import frc.robot.commands.controllers.JoystickInputController;
 import frc.robot.commands.controllers.SpeedLevelController;
 import frc.robot.subsystems.dashboard.DriverDashboard;
@@ -456,7 +457,11 @@ private void configureDriverControllerBindings(
       intakeAlignmentCommands.setEndCommand(() -> rumbleController(xbox, 0.3).withTimeout(0.1));
 
       xbox.leftTrigger()
-          .onTrue(intakeAlignmentCommands.driveToClosest(drive).withName("Align INTAKE"))
+          .onTrue(
+              intakeAlignmentCommands
+                  .driveToClosest(drive)
+                  .withName("Align INTAKE")
+                  .andThen(SourceStrafingCommands.takeAction(drive, xbox)))
           .onFalse(intakeAlignmentCommands.stop(drive));
 
       xbox.leftTrigger()

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

@@ -26,6 +26,7 @@
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 
+@Deprecated
 public class DriveToPose extends Command {
   private static final LoggedTunableNumber drivekP = new LoggedTunableNumber("DriveToPose/DrivekP");
   private static final LoggedTunableNumber drivekD = new LoggedTunableNumber("DriveToPose/DrivekD");

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

@@ -0,0 +1,107 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.FieldConstants;
+import frc.robot.commands.controllers.JoystickInputController;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.utility.AllianceFlipUtil;
+
+public class SourceStrafingCommands {
+  private static final PIDController xController = new PIDController(0, 0, 0); // TODO
+  private static final PIDController angleController = new PIDController(0, 0, 0);
+
+  /** IF auto alignment is finished and placed us by a source */
+  public static Command takeAction(Drive drive, CommandXboxController xbox) {
+    if (bySourceLeft(drive.getRobotPose().getY())) {
+      return strafeAlongLine(
+              drive,
+              xbox,
+              AllianceFlipUtil.apply(
+                  FieldConstants.CoralStation.leftCenterFace
+                      .getRotation()
+                      .minus(Rotation2d.k180deg)),
+              Translation2d.kZero, // TODO
+              Translation2d.kZero)
+          .until(xbox.leftBumper().negate());
+    } else { // Since autoalign just ran we must be on the right
+      return strafeAlongLine(
+          drive,
+          xbox,
+          AllianceFlipUtil.apply(
+              FieldConstants.CoralStation.rightCenterFace.getRotation().minus(Rotation2d.k180deg)),
+          Translation2d.kZero, // TODO
+          Translation2d.kZero);
+    }
+  }
+
+  private static boolean bySourceLeft(double y) {
+    Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue);
+    boolean isTopHalf = y >= 4;
+
+    return (alliance == Alliance.Blue && isTopHalf) || (alliance == Alliance.Red && !isTopHalf);
+  }
+
+  /** Do not flip input translations, they will be handled by the function */
+  public static Command strafeAlongLine(
+      Drive drivetrain,
+      CommandXboxController xbox,
+      Rotation2d desiredHeading,
+      Translation2d a,
+      Translation2d b) {
+    final JoystickInputController input = getInput(drivetrain, xbox);
+
+    Pose2d startingPose = // TODO offset this
+        AllianceFlipUtil.apply(FieldConstants.CoralStation.leftCenterFace);
+
+    Runnable keepAtRange =
+        () -> {
+          Translation2d inputTranslation = input.getTranslationMetersPerSecond();
+          Pose2d robotPose = drivetrain.getRobotPose();
+          double targetHeadingRadians = desiredHeading.getRadians();
+
+          double omega = angleController.calculate(desiredHeading.getRadians());
+
+          double targetX = findTargetX(a, b, robotPose.getY());
+
+          boolean notAtDesiredAngle =
+              !MathUtil.isNear(
+                  robotPose.getRotation().getRadians(), targetHeadingRadians, Math.PI / 4);
+
+          drivetrain.setRobotSpeeds(
+              new ChassisSpeeds(
+                  xController.calculate(robotPose.getX(), targetX),
+                  inputTranslation.getY(),
+                  notAtDesiredAngle ? omega : 0),
+              true);
+        };
+
+    return Commands.sequence(
+        DriveCommands.pathfindToPoseCommand(drivetrain, startingPose, 1, 0),
+        drivetrain.run(keepAtRange));
+  }
+
+  private static double findTargetX(Translation2d a, Translation2d b, double y) {
+    double m = (b.getY() - a.getY()) / (b.getX() - a.getX());
+
+    return ((y - a.getY()) / m) + a.getX();
+  }
+
+  private static JoystickInputController getInput(Drive drive, CommandXboxController xbox) {
+    return new JoystickInputController(
+        drive,
+        () -> -xbox.getLeftY(),
+        () -> -xbox.getLeftX(),
+        () -> -xbox.getRightY(),
+        () -> -xbox.getRightX());
+  }
+}

src/main/java/frc/robot/subsystems/rangefinder/Rangefinder.java

@@ -0,0 +1,25 @@
+package frc.robot.subsystems.rangefinder;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.Optional;
+import org.littletonrobotics.junction.Logger;
+
+public class Rangefinder extends SubsystemBase {
+  private final RangefinderIO io;
+  private final RangefinderIOInputsAutoLogged inputs = new RangefinderIOInputsAutoLogged();
+
+  public Rangefinder(RangefinderIO io) {
+    this.io = io;
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("Rangefinder", inputs);
+  }
+
+  /** Get the measurement in millimetres */
+  public Optional<Integer> getMeasurement() {
+    return io.getMeasurementMillimetres();
+  }
+}

src/main/java/frc/robot/subsystems/rangefinder/RangefinderIO.java

@@ -0,0 +1,18 @@
+package frc.robot.subsystems.rangefinder;
+
+import java.util.Optional;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface RangefinderIO {
+  @AutoLog
+  public static class RangefinderIOInputs {
+    public int measurementMillimetres = 0;
+  }
+
+  /** Updates the set of loggable inputs. */
+  public default void updateInputs(RangefinderIOInputs inputs) {}
+
+  public default Optional<Integer> getMeasurementMillimetres() {
+    return Optional.empty();
+  }
+}

src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java

@@ -0,0 +1,36 @@
+package frc.robot.subsystems.rangefinder;
+
+import au.grapplerobotics.ConfigurationFailedException;
+import au.grapplerobotics.LaserCan;
+import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode;
+import java.util.Optional;
+
+public class RangefinderIOHardware implements RangefinderIO {
+  private final LaserCan sensor;
+
+  public RangefinderIOHardware(int id) {
+    sensor = new LaserCan(id);
+
+    try {
+      sensor.setRangingMode(RangingMode.SHORT);
+    } catch (ConfigurationFailedException e) {
+      e.printStackTrace();
+    }
+  }
+
+  @Override
+  public void updateInputs(RangefinderIOInputs inputs) {
+    inputs.measurementMillimetres = getMeasurementMillimetres().orElse(-1);
+  }
+
+  @Override
+  public Optional<Integer> getMeasurementMillimetres() {
+    LaserCan.Measurement measurement = sensor.getMeasurement();
+
+    if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) {
+      return Optional.of(measurement.distance_mm);
+    } else {
+      return Optional.empty();
+    }
+  }
+}

vendordeps/libgrapplefrc2025.json

@@ -0,0 +1,74 @@
+{
+    "fileName": "libgrapplefrc2025.json",
+    "name": "libgrapplefrc",
+    "version": "2025.0.8",
+    "frcYear": "2025",
+    "uuid": "8ef3423d-9532-4665-8339-206dae1d7168",
+    "mavenUrls": [
+        "https://storage.googleapis.com/grapple-frc-maven"
+    ],
+    "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json",
+    "javaDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcjava",
+            "version": "2025.0.8"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcdriver",
+            "version": "2025.0.8",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrccpp",
+            "version": "2025.0.8",
+            "libName": "grapplefrc",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcdriver",
+            "version": "2025.0.8",
+            "libName": "grapplefrcdriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ]
+}