86 drive heading

src/main/java/org/team5924/frc2025/BuildConstants.java

@@ -21,12 +21,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "GoldenGateRobotics2025";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 270;
-  public static final String GIT_SHA = "335559c04de83aa24f7aa7c1d4e54041da73cda5";
-  public static final String GIT_DATE = "2025-04-05 20:00:04 EDT";
-  public static final String GIT_BRANCH = "elevator-hotfix";
-  public static final String BUILD_DATE = "2025-04-05 20:25:07 EDT";
-  public static final long BUILD_UNIX_TIME = 1743899107859L;
+  public static final int GIT_REVISION = 278;
+  public static final String GIT_SHA = "8a97b9bcca84ee0e6c261e9b9b75f4c35d489a50";
+  public static final String GIT_DATE = "2025-10-18 17:31:25 EDT";
+  public static final String GIT_BRANCH = "86-drive-heading";
+  public static final String BUILD_DATE = "2025-10-18 17:40:58 EDT";
+  public static final long BUILD_UNIX_TIME = 1760823658603L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

src/main/java/org/team5924/frc2025/Constants.java

@@ -56,6 +56,7 @@ public static enum Mode {
 
   public static final boolean TUNING_MODE = false;
   public static final boolean ALLOW_ASSERTS = false;
+  public static final double SLOW_MODE_MULTI = 0.5;
 
   /* Field */
   public static final double FIELD_BORDER_MARGIN = 0.5;

src/main/java/org/team5924/frc2025/RobotContainer.java

@@ -50,11 +50,11 @@
 import org.team5924.frc2025.subsystems.elevator.Elevator;
 import org.team5924.frc2025.subsystems.elevator.ElevatorIO;
 import org.team5924.frc2025.subsystems.elevator.ElevatorIOTalonFXGamma;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIO;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOKrakenFOC;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOSim;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOutIO;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOutIOKrakenFOC;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOutIOSim;
 import org.team5924.frc2025.subsystems.vision.Vision;
 import org.team5924.frc2025.subsystems.vision.VisionIO;
 import org.team5924.frc2025.subsystems.vision.VisionIOLimelight;
@@ -217,15 +217,15 @@ private void configureButtonBindings() {
             () -> -driveController.getLeftX(),
             () -> -driveController.getRightX()));
 
-    // Nope. It's slow mode now. Quarter speed
+    // Nope. It's slow mode now.
     driveController
         .a()
         .whileTrue(
             DriveCommands.joystickDrive(
                 drive,
-                () -> -driveController.getLeftY() * .25,
-                () -> -driveController.getLeftX() * .25,
-                () -> -driveController.getRightX() * .25));
+                () -> -driveController.getLeftY() * Constants.SLOW_MODE_MULTI,
+                () -> -driveController.getLeftX() * Constants.SLOW_MODE_MULTI,
+                () -> -driveController.getRightX() * Constants.SLOW_MODE_MULTI));
 
     // Switch to X pattern when X button is pressed
     driveController.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
@@ -251,17 +251,27 @@ private void configureButtonBindings() {
         .whileTrue(
             new DeferredCommand(() -> DriveCommands.driveToReef(drive, false), Set.of(drive)));
 
+    // driveController
+    //     .rightTrigger()
+    //     .whileTrue(
+    //         DriveCommands.turnToRightCoralStation(
+    //             drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX()));
+
+    // driveController
+    //     .leftTrigger()
+    //     .whileTrue(
+    //         DriveCommands.turnToLeftCoralStation(
+    //             drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX()));
+
+    driveController.rightTrigger().onTrue(DriveCommands.lockOnCoralStation(drive, true));
+    driveController.leftTrigger().onTrue(DriveCommands.lockOnCoralStation(drive, false));
     driveController
         .rightTrigger()
-        .whileTrue(
-            DriveCommands.turnToRightCoralStation(
-                drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX()));
+        .or(driveController.leftTrigger())
+        .onFalse(DriveCommands.unlockRotation(drive));
+
+    driveController.rightStick().onTrue(Commands.runOnce(() -> drive.toggleSnapToHeading()));
 
-    driveController
-        .leftTrigger()
-        .whileTrue(
-            DriveCommands.turnToLeftCoralStation(
-                drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX()));
     // Coral In and Out
 
     driveController.y().onTrue(new TeleopShoot(coralInAndOut).withTimeout(Seconds.of(1)));

src/main/java/org/team5924/frc2025/RobotState.java

@@ -24,8 +24,8 @@
 import org.team5924.frc2025.subsystems.climber.Climber.ClimberState;
 import org.team5924.frc2025.subsystems.elevator.Elevator.ElevatorState;
 import org.team5924.frc2025.subsystems.pivot.AlgaePivot.AlgaePivotState;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState;
 import org.team5924.frc2025.subsystems.rollers.algae.AlgaeRoller.AlgaeRollerState;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState;
 import org.team5924.frc2025.util.VisionFieldPoseEstimate;
 
 @Getter

src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunIntake.java

@@ -18,8 +18,8 @@
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.*;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.*;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState;
 
 public class RunIntake extends Command {
   private CoralInAndOut coralInAndOut;

src/main/java/org/team5924/frc2025/commands/coralInAndOut/RunShooter.java

@@ -18,8 +18,8 @@
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.*;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.*;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState;
 
 public class RunShooter extends Command {
   private CoralInAndOut coralInAndOut;

src/main/java/org/team5924/frc2025/commands/coralInAndOut/TeleopShoot.java

@@ -20,8 +20,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import org.team5924.frc2025.RobotState;
 import org.team5924.frc2025.subsystems.elevator.Elevator.ElevatorState;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut;
-import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut;
+import org.team5924.frc2025.subsystems.rollers.coralInAndOut.CoralInAndOut.CoralState;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class TeleopShoot extends Command {

src/main/java/org/team5924/frc2025/commands/drive/DriveCommands.java

@@ -72,6 +72,52 @@ private static Translation2d getLinearVelocityFromJoysticks(double x, double y)
         .getTranslation();
   }
 
+  /** returns whether rotations should be flipped (same as if the alliance is red) */
+  public static boolean isFlipped() {
+    return DriverStation.getAlliance().isPresent()
+        && DriverStation.getAlliance().get() == Alliance.Red;
+  }
+
+  /**
+   * gets the needed rotation to align to the corresponding coral station
+   *
+   * @param right true = right coral station; false = left coral station
+   */
+  public static double getCoralStationRotation(boolean right) {
+    double heading;
+
+    if (isFlipped()) {
+      heading = Constants.CORAL_STATION_RADIANS_NORMAL - Math.PI;
+    } else {
+      heading = Constants.CORAL_STATION_RADIANS_NORMAL;
+    }
+
+    return right ? heading : -heading;
+  }
+
+  /**
+   * Locks the robot's rotation in the direction of the desired coral station
+   *
+   * @param drive a reference to the {@link Drive} subsystem
+   * @param right true = right coral station; false = left coral station
+   */
+  public static Command lockOnCoralStation(Drive drive, boolean right) {
+    return Commands.runOnce(
+        () -> {
+          double heading = getCoralStationRotation(right);
+          drive.setDesiredHeading(heading);
+          drive.setSnapToHeading(true);
+        });
+  }
+
+  /** Unlocks the robots rotation, stops it from snapping to the desired heading */
+  public static Command unlockRotation(Drive drive) {
+    return Commands.runOnce(
+        () -> {
+          drive.setSnapToHeading(false);
+        });
+  }
+
   /**
    * Field relative drive command using two joysticks (controlling linear and angular velocities).
    */

src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java

@@ -31,7 +31,9 @@
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -142,6 +144,24 @@ public class Drive extends SubsystemBase {
 
   private final Field2d field = new Field2d();
 
+  // in radians
+  private double desiredHeading = 0.0;
+  private boolean snapToHeading = false;
+
+  PIDController headingPid = new PIDController(3, 0, 0);
+
+  public boolean toggleSnapToHeading() {
+    return snapToHeading = !snapToHeading;
+  }
+
+  public void setSnapToHeading(boolean snap) {
+    snapToHeading = snap;
+  }
+
+  public void setDesiredHeading(double heading) {
+    desiredHeading = heading;
+  }
+
   public Drive(
       GyroIO gyroIO,
       ModuleIO flModuleIO,
@@ -228,6 +248,8 @@ public void initSendable(SendableBuilder builder) {
             builder.addDoubleProperty("Robot Angle", () -> getRotation().getRadians(), null);
           }
         });
+
+    headingPid.enableContinuousInput(-Math.PI, Math.PI);
   }
 
   @Override
@@ -352,13 +374,45 @@ public void periodic() {
     }
   }
 
+  /**
+   * rotates the speeds towards the desired heading with a ±3 degree tolerance
+   *
+   * @param speeds input speeds that will be updated
+   * @param targetHeading the desired heading (rotation)
+   * @return updated speeds
+   */
+  public ChassisSpeeds updateSpeedsWithDesiredHeading(ChassisSpeeds speeds, double targetHeading) {
+    // tolerance of ±3 deg
+    double currentHeading = getRotation().getRadians();
+    double error = MathUtil.angleModulus(targetHeading - currentHeading);
+    boolean isWithinTolerance = Math.abs(error) <= Math.toRadians(3.0);
+
+    if (isWithinTolerance)
+      return new ChassisSpeeds(
+          speeds.vxMetersPerSecond,
+          speeds.vyMetersPerSecond,
+          0.0); // within tolerance; don't rotate
+
+    // calculate omega
+    double omega = headingPid.calculate(currentHeading, targetHeading);
+    omega = MathUtil.clamp(omega, -getMaxAngularSpeedRadPerSec(), getMaxAngularSpeedRadPerSec());
+    headingPid.close();
+
+    return new ChassisSpeeds(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, omega);
+  }
+
   /**
    * Runs the drive at the desired velocity.
    *
    * @param speeds Speeds in meters/sec
    */
   public void runVelocity(ChassisSpeeds speeds) {
     // Calculate module setpoints
+
+    if (snapToHeading) {
+      speeds = updateSpeedsWithDesiredHeading(speeds, desiredHeading);
+    }
+
     ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02);
     previousSetpoint =
         setpointGenerator.generateSetpoint(

src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java

@@ -14,7 +14,7 @@
  * If you did not, see <https://www.gnu.org/licenses>.
  */
 
-package org.team5924.frc2025.subsystems.rollers.CoralInAndOut;
+package org.team5924.frc2025.subsystems.rollers.coralInAndOut;
 
 import java.util.function.DoubleSupplier;
 import lombok.Getter;

src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIO.java

@@ -14,7 +14,7 @@
  * If you did not, see <https://www.gnu.org/licenses>.
  */
 
-package org.team5924.frc2025.subsystems.rollers.CoralInAndOut;
+package org.team5924.frc2025.subsystems.rollers.coralInAndOut;
 
 import org.littletonrobotics.junction.AutoLog;
 import org.team5924.frc2025.subsystems.rollers.GenericRollerSystemIO;

src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIOKrakenFOC.java

@@ -14,7 +14,7 @@
  * If you did not, see <https://www.gnu.org/licenses>.
  */
 
-package org.team5924.frc2025.subsystems.rollers.CoralInAndOut;
+package org.team5924.frc2025.subsystems.rollers.coralInAndOut;
 
 import au.grapplerobotics.LaserCan;
 import edu.wpi.first.wpilibj.Alert;

src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOutIOSim.java

@@ -14,7 +14,7 @@
  * If you did not, see <https://www.gnu.org/licenses>.
  */
 
-package org.team5924.frc2025.subsystems.rollers.CoralInAndOut;
+package org.team5924.frc2025.subsystems.rollers.coralInAndOut;
 
 import edu.wpi.first.math.system.plant.DCMotor;
 import org.team5924.frc2025.Constants;

vendordeps/AdvantageKit.json

@@ -1,7 +1,7 @@
 {
   "fileName": "AdvantageKit.json",
   "name": "AdvantageKit",
-  "version": "4.1.0",
+  "version": "4.1.2",
   "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
   "frcYear": "2025",
   "mavenUrls": [
@@ -12,14 +12,14 @@
     {
       "groupId": "org.littletonrobotics.akit",
       "artifactId": "akit-java",
-      "version": "4.1.0"
+      "version": "4.1.2"
     }
   ],
   "jniDependencies": [
     {
       "groupId": "org.littletonrobotics.akit",
       "artifactId": "akit-wpilibio",
-      "version": "4.1.0",
+      "version": "4.1.2",
       "skipInvalidPlatforms": false,
       "isJar": false,
       "validPlatforms": [

vendordeps/PathplannerLib-2025.2.3.json → vendordeps/PathplannerLib-2025.2.7.json

@@ -1,7 +1,7 @@
 {
-  "fileName": "PathplannerLib-2025.2.3.json",
+  "fileName": "PathplannerLib-2025.2.7.json",
   "name": "PathplannerLib",
-  "version": "2025.2.3",
+  "version": "2025.2.7",
   "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
   "frcYear": "2025",
   "mavenUrls": [
@@ -12,15 +12,15 @@
     {
       "groupId": "com.pathplanner.lib",
       "artifactId": "PathplannerLib-java",
-      "version": "2025.2.3"
+      "version": "2025.2.7"
     }
   ],
   "jniDependencies": [],
   "cppDependencies": [
     {
       "groupId": "com.pathplanner.lib",
       "artifactId": "PathplannerLib-cpp",
-      "version": "2025.2.3",
+      "version": "2025.2.7",
       "libName": "PathplannerLib",
       "headerClassifier": "headers",
       "sharedLibrary": false,