Swerve tuning

CLAUDE.md

@@ -1,5 +1,4 @@
-
-# CLAUDE.md
+# [CLAUDE.md](http://CLAUDE.md)
 
 This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository.
 
@@ -10,69 +9,84 @@ This is FRC Team 1310 (Runnymede Robotics) 2026 swerve drive robot code. The pro
 ## Build & Development Commands
 
 ### Building
+
 ```bash
 ./gradlew build
 ```
 
 ### Deploying to Robot
+
 ```bash
 ./gradlew deploy
 ```
 
 ### Running Tests
+
 ```bash
 ./gradlew test
 ```
 
 ### Simulation
+
 ```bash
 ./gradlew simulateJava
 ```
 
 ### Clean Build (Required when updating RunnymedeSwerve library)
+
 ```bash
 ./gradlew clean && ./gradlew compileJava
 ```
 
 ### Single Test
+
 ```bash
 ./gradlew test --tests ClassName.testMethodName
 ```
 
 ## Architecture
 
 ### Command-Based Structure
+
 The codebase follows WPILib's command-based paradigm:
+
 - **Robot.java**: Main robot class, handles mode transitions, port forwarding for Limelights, and telemetry updates (every 150ms)
 - **RobotContainer.java**: Initializes subsystems, sets default commands, configures button bindings, and provides autonomous command
 - **Constants.java**: Centralized configuration including swerve module configs, PID values, field geometry, and telemetry flags
 
 ### Subsystems
-1. **SwerveSubsystem** (`subsystems/swerve/`): Wraps `RunnymedeSwerveDrive` from the external library. Provides field-oriented and robot-oriented drive, rate limiting via `SlewRateLimiter`, heading PID control, and convenience methods like `computeOmega()` and `getClosestReefAngle()`.
+
+1. **SwerveSubsystem** (`subsystems/swerve/`): Wraps `RunnymedeSwerveDrive` from the external library. Provides field-oriented and robot-oriented drive, 2D vector acceleration limiting (`limitTranslationAcceleration` — limits magnitude of velocity change per timestep, resets on dt > 0.1s gaps), omega rate limiting via `SlewRateLimiter`, heading PID control with symmetric clamping (`computeOmega` uses `MathUtil.clamp`), and convenience methods like `getClosestReefAngle()`.
 
 2. **LimelightVisionSubsystem** (`subsystems/vision/`): Manages Limelight vision processing, integrates with swerve odometry via `LimelightAwareSwerveDrive`.
 
 ### Commands
+
 - **TeleopDriveCommand**: Default drive command that reads from `OperatorInput`, supports field-oriented drive, speed modes (fast/slow/normal), and rotation behaviors (manual, 180°, face reef, align to stations)
 - **Auto Commands** (`commands/auto/`): Autonomous routines like `ExitZoneAutoCommand`
 - **Test Commands** (`commands/test/SystemTestCommand`): System testing triggered by Start+Back buttons when not on FMS
 
 ### Operator Input
+
 **OperatorInput.java** manages Xbox controller input with deadband handling, button bindings, rumble patterns (match timing, haptic feedback), and autonomous mode selection via SmartDashboard choosers.
 
 ### Configuration Pattern
+
 Subsystem configs are immutable records:
+
 - `SwerveDriveSubsystemConfig`: Aggregates core swerve, gyro, limelight, translation, and rotation configs
 - `SwerveTranslationConfig`: Max speeds, acceleration limits, velocity PID
 - `SwerveRotationConfig`: Rotation velocity limits, heading PID
 
 ### Hardware Configuration
+
 - **Gyro**: Pigeon 2 (CAN ID 8)
-- **Swerve Modules**: SDS MK4i with NEO/NEO 550 motors, CANCoder absolute encoders
-  - Module CAN IDs: FrontLeft (10-12), FrontRight (20-22), BackRight (30-32), BackLeft (40-42)
-- **Limelights**: "nikola" (primary at 10.13.10.11), "thomas" (secondary at 10.13.10.12)
+- **Swerve Modules**: SDS MK4i with NEO Vortex (Spark Flex) drive motors and NEO (Spark Max) angle motors, CANCoder absolute encoders
+  - Module CAN IDs: FrontLeft (10,11,12), FrontRight (15,16,17), BackRight (20,21,22), BackLeft (25,26,27)
+- **Limelights**: "hugh" (primary), "thomas" (secondary)
 
 ### Telemetry
+
 Custom telemetry system (`telemetry/Telemetry.java`) with per-subsystem enable flags in `Constants.TelemetryConfig`. Posted every 150ms in `Robot.robotPeriodic()`.
 
 ## Code Formatting
@@ -81,13 +95,13 @@ This project uses Google Java Format. Format code before committing. IntelliJ pl
 
 ## Dependencies
 
-- **WPILib**: Standard FRC library (2025 season)
-- **RunnymedeSwerve Library**: `ca.team1310:swerve:3.3.5` (Maven Central or local maven)
+- **WPILib**: Standard FRC library (2026 season)
+- **RunnymedeSwerve Library**: `ca.team1310:swerve` (Maven Central or local maven)
 - **Vendor Libraries**: Phoenix 6 (CTRE), REVLib, Studica
 - **Testing**: JUnit 5
 
 When updating the RunnymedeSwerve library version in `build.gradle`, run `./gradlew clean && ./gradlew compileJava` to fetch the new version.
 
 ## Field Coordinates
 
-Uses 2025 FRC field with AprilTag locations defined in `Constants.FieldConstants.TAGS`. Coordinate system: positive X away from blue alliance wall, positive Y to the left. Field dimensions: 17.55m x 8.052m.
+Uses 2025 FRC field (Reefscape) with AprilTag locations defined in `Constants.FieldConstants.TAGS`. Coordinate system: positive X away from blue alliance wall, positive Y to the left. Field dimensions: 17.55m x 8.052m.

build.gradle

@@ -65,7 +65,7 @@ dependencies {
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
 
-    implementation 'ca.team1310:swerve:4.1.1'
+    implementation 'ca.team1310:swerve:4.1.2'
 
     roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
     roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)

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

@@ -162,7 +162,7 @@ public static final class Swerve {
             /* min speed (m/s) */ 1.0,
             /* max speed (m/s) */ 4.8,
             /* max module speed (m/s) */ 5.36,
-            /* max acceleration (m/s/s) */ 42.0,
+            /* max acceleration (m/s/s) */ 10.0,
             /* velocity PID p */ 1.2,
             /* velocity PID i */ 0,
             /* velocity PID d */ 0);
@@ -182,7 +182,7 @@ public static final class Swerve {
             /* inverted? */ true,
             /* current limit (A) */ 20,
             /* nominal voltage (V) */ 12,
-            /* ramp rate 0 to full power (s)*/ 0.25,
+            /* ramp rate 0 to full power (s)*/ 0.1,
             /* angle motor gear ratio */ 150.0 / 7 /* SDS MK4i 150/7:1 */,
             /* angle motor PID p */ 0.009,
             /* angle motor PID i */ 0,
@@ -196,7 +196,7 @@ public static final class Swerve {
             /* inverted? */ false,
             /* current limit (A) */ 40,
             /* nominal voltage (V) */ 12,
-            /* ramp rate 0 to full power (s)*/ 0.25, // TODO: FIXME: TRY LOWERING THIS A LOT
+            /* ramp rate 0 to full power (s)*/ 0.02,
             /* drive motor gear ratio */ 6.75 /* SDS MK4i L2 --> 6.75:1 */,
             /* drive motor PID p */ 0.075,
             /* drive motor PID i */ 0,

src/main/java/frc/robot/commands/swervedrive/DriveToFieldLocationCommand.java

@@ -61,9 +61,9 @@ public void execute() {
     double angleDif =
         SwerveUtils.normalizeDegrees(targetHeadingDeg - currentPose.getRotation().getDegrees());
 
-    double maxOmega = Math.max((Math.toRadians(angleDif) / dif.getNorm()) * transV.getNorm(), .1);
+    double maxOmega =
+        Math.max((Math.toRadians(Math.abs(angleDif)) / dif.getNorm()) * transV.getNorm(), .1);
     double omega = swerve.computeOmega(targetHeadingDeg, maxOmega);
-    System.out.println(maxOmega);
 
     swerve.driveFieldOriented(transV.getX(), transV.getY(), omega);
   }

src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java

@@ -3,6 +3,8 @@
 import ca.team1310.swerve.RunnymedeSwerveDrive;
 import ca.team1310.swerve.utils.SwerveUtils;
 import ca.team1310.swerve.vision.LimelightAwareSwerveDrive;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -19,8 +21,8 @@ public class SwerveSubsystem extends SubsystemBase {
 
   private final RunnymedeSwerveDrive drive;
   private final SwerveDriveSubsystemConfig config;
-  private final SlewRateLimiter xLimiter;
-  private final SlewRateLimiter yLimiter;
+  private Translation2d prevLimitedVelocity = new Translation2d();
+  private double prevLimitTime;
   private final SlewRateLimiter omegaLimiter;
   private final PIDController headingPIDController;
 
@@ -29,8 +31,7 @@ public SwerveSubsystem(SwerveDriveSubsystemConfig config) {
         new LimelightAwareSwerveDrive(
             config.coreConfig(), config.gyroConfig(), config.limelightConfig());
     this.config = config;
-    this.xLimiter = new SlewRateLimiter(this.config.translationConfig().maxAccelMPS2());
-    this.yLimiter = new SlewRateLimiter(this.config.translationConfig().maxAccelMPS2());
+    this.prevLimitTime = MathSharedStore.getTimestamp();
     this.omegaLimiter = new SlewRateLimiter(config.rotationConfig().maxAccelerationRadPS2());
     headingPIDController =
         new PIDController(
@@ -59,16 +60,48 @@ public void periodic() {
    */
 
   /**
-   * Add limiters to the change in drive values. Note this may not scale evenly - one may reach
-   * desired speed before another.
+   * 2D vector acceleration limiter. Limits the magnitude of the velocity change vector per
+   * timestep, ensuring acceleration never exceeds maxAccelMPS2 in any direction. This eliminates
+   * the sqrt(2) diagonal problem of independent X/Y limiting.
+   */
+  private Translation2d limitTranslationAcceleration(double x, double y) {
+    double currentTime = MathSharedStore.getTimestamp();
+    double dt = currentTime - prevLimitTime;
+    prevLimitTime = currentTime;
+
+    if (dt > 0.1) {
+      // Robot was likely disabled or there was a long gap — reset to current target
+      prevLimitedVelocity = new Translation2d(x, y);
+      return prevLimitedVelocity;
+    }
+
+    double dx = x - prevLimitedVelocity.getX();
+    double dy = y - prevLimitedVelocity.getY();
+    double changeMag = Math.sqrt(dx * dx + dy * dy);
+    double maxChange = config.translationConfig().maxAccelMPS2() * dt;
+
+    if (changeMag > maxChange && changeMag > 1e-9) {
+      double scale = maxChange / changeMag;
+      prevLimitedVelocity =
+          new Translation2d(
+              prevLimitedVelocity.getX() + dx * scale, prevLimitedVelocity.getY() + dy * scale);
+    } else {
+      prevLimitedVelocity = new Translation2d(x, y);
+    }
+    return prevLimitedVelocity;
+  }
+
+  /**
+   * Add limiters to the change in drive values.
    *
    * @param x m/s
    * @param y m/s
    * @param omega rad/s
    */
   private void driveSafely(double x, double y, double omega) {
-    x = xLimiter.calculate(x);
-    y = yLimiter.calculate(y);
+    Translation2d limited = limitTranslationAcceleration(x, y);
+    x = limited.getX();
+    y = limited.getY();
     omega = omegaLimiter.calculate(omega);
 
     if (this.config.enabled()) {
@@ -77,16 +110,16 @@ private void driveSafely(double x, double y, double omega) {
   }
 
   /**
-   * Add limiters to the change in drive values. Note this may not scale evenly - one may reach
-   * desired speed before another.
+   * Add limiters to the change in drive values.
    *
    * @param x m/s
    * @param y m/s
    * @param omega rad/s
    */
   private void driveSafelyFieldOriented(double x, double y, double omega) {
-    x = xLimiter.calculate(x);
-    y = yLimiter.calculate(y);
+    Translation2d limited = limitTranslationAcceleration(x, y);
+    x = limited.getX();
+    y = limited.getY();
     omega = omegaLimiter.calculate(omega);
 
     if (this.config.enabled()) {
@@ -118,7 +151,7 @@ public final void driveRobotOriented(double x, double y, double omega) {
     driveSafely(x, y, omega);
   }
 
-  /** Stop all motors as fast as possible */
+  /** Stop the robot with controlled deceleration through the acceleration limiter. */
   public void stop() {
     driveRobotOriented(0, 0, 0);
   }
@@ -273,7 +306,7 @@ public double computeOmega(double desiredHeadingDegrees) {
    */
   public double computeOmega(double desiredHeadingDegrees, double maxOmegaRadPerSec) {
     double omega = headingPIDController.calculate(drive.getYaw(), desiredHeadingDegrees);
-    return Math.min(omega, maxOmegaRadPerSec);
+    return MathUtil.clamp(omega, -maxOmegaRadPerSec, maxOmegaRadPerSec);
   }
 
   public double oldComputeTranslateVelocity(double distance, double maxSpeedMPS, double tolerance) {