Flywheel auto tuning

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

.claude/settings.local.json

@@ -0,0 +1,10 @@
+{
+  "permissions": {
+    "allow": [
+      "Bash(./gradlew tasks *)",
+      "Bash(./gradlew compileJava)",
+      "Bash(ls \"/c/Users/Public/wpilib/\" 2>&1 | head -5; echo \"---\"; find /c -maxdepth 4 -name \"jdk\" -path \"*wpilib*\" 2>/dev/null | head -5)",
+      "Read(//c/Users/Public/wpilib//**)"
+    ]
+  }
+}

CLAUDE.md

@@ -9,7 +9,7 @@ This file provides guidance to Claude Code (claude.ai/code) when working with co
 ./gradlew deploy         # Build and deploy to RoboRIO over USB or network
 ./gradlew spotlessApply  # Format code (Google Java Format via Spotless)
 ./gradlew spotlessCheck  # Check formatting without modifying
-./gradlew test           # Run JUnit 5 tests
+./gradlew test           # JUnit 5 is configured but no tests exist yet (no src/test/java)
 ./gradlew replayWatch    # AdvantageKit log replay for debugging
 ```
 
@@ -20,7 +20,7 @@ The CI runs `./gradlew build` on every push/PR via `.github/workflows/build.yml`
 
 ## Architecture Overview
 
-This is a **WPILib command-based** robot using **AdvantageKit** for structured logging and replay and **Yet Another Mechanism System** for mechanism/motor control abstraction & simulation. The three runtime modes are selected in `Robot.java`:
+This is a **WPILib command-based** robot using **AdvantageKit** for structured logging and replay, and **YAMS (Yet Another Mechanism System)** for gearing definitions and mechanism simulation (`yams.gearing.GearBox`, `yams.gearing.MechanismGearing` are used in the per-mechanism `*Constants.java` files). The three runtime modes are selected in `Robot.java`:
 - `REAL` — hardware with real sensors
 - `SIM` — physics simulation (run on desktop)
 - `REPLAY` — replay from an AdvantageKit `.wpilog` file
@@ -36,7 +36,9 @@ Every mechanism follows this pattern (example: Intake):
 | `IntakeIOTalonFX.java` | Hardware impl — uses Phoenix 6 TalonFX / CTRE devices |
 | `IntakeConstants.java` | CAN IDs, gear ratios, PID gains, physical constants |
 
-The subsystem never talks to hardware directly — it only calls `io.*` methods. This enables REAL/SIM/REPLAY switching at construction time in `RobotContainer`.
+The subsystem never talks to hardware directly — it only calls `io.*` methods. This enables REAL/SIM/REPLAY switching at construction time in `RobotContainer`. Most TalonFX IO layers internally switch between real-hardware and sim implementations based on `Constants.currentMode`, so `RobotContainer` only constructs the TalonFX impls (see [RobotContainer.java:195-204](src/main/java/org/team157/robot/RobotContainer.java#L195-L204)).
+
+Two subsystems are not in their own subdirectory: [LEDs.java](src/main/java/org/team157/robot/subsystems/LEDs.java) and [SunstoneMechanism3D.java](src/main/java/org/team157/robot/subsystems/SunstoneMechanism3D.java) live directly under `subsystems/`. `SunstoneMechanism3D` publishes 3D pose data for AdvantageScope visualization of the robot's mechanisms.
 
 ### Subsystems
 
@@ -55,9 +57,12 @@ The subsystem never talks to hardware directly — it only calls `io.*` methods.
 
 ### Key Central Files
 
-- **`RobotContainer.java`** — instantiates all subsystems, wires default commands, registers PathPlanner named commands (`DeployIntake`, `RunIntake`, `RunHopper`, `ShootBalls`, `Wiggle`), and configures both Xbox controller button bindings.
+- **`RobotContainer.java`** — instantiates all subsystems, wires default commands, registers PathPlanner named commands (`DeployIntake`, `RunIntake`, `RunHopper`, `ShootBalls`, `Wiggle`, `WiggleCubed`), and configures both Xbox controller button bindings.
 - **`Constants.java`** — global constants (field dimensions, CAN bus names: `"rio"` and `"canivore"`).
-- **`commands/DriveCommands.java`** — factory methods for field-relative teleop drive, SysId characterization routines, and wheel radius measurement.
+- **`commands/DriveCommands.java`** — factory methods for field-relative teleop drive (`joystickDrive`, `joystickDriveAtAngle`), `feedforwardCharacterization()`, and `wheelRadiusCharacterization()`.
+- **`parsing/PositionDetails.java`** — parses [src/main/deploy/positionDetails.json](src/main/deploy/positionDetails.json) for field-relative target locations used by turret/vision tracking.
+- **`util/`** — `LocalADStarAK.java` (PathPlanner A* wrapped for AdvantageKit replay) and `PhoenixUtil.java` (CTRE retry helpers).
+- **`utilities/`** — `PosUtils.java` and `PriorityMap.java` (used by LED patterns and pose math); note this package lives at `org.team157.utilities`, outside the `org.team157.robot` tree.
 
 ### Motor & Sensor Hardware
 
@@ -70,12 +75,28 @@ The subsystem never talks to hardware directly — it only calls `io.*` methods.
 
 ### Vision
 
-Three PhotonVision cameras (`camera0`, `camera1`, `camera2`). `VisionIO` interface has `VisionIOPhotonVision` (real), `VisionIOPhotonVisionSim` (simulation), and `VisionIOLimelight` (alternative). Pose estimates are fed into `Drive`'s `SwerveDrivePoseEstimator`.
+Three PhotonVision cameras configured in [VisionConstants.java](src/main/java/org/team157/robot/subsystems/vision/VisionConstants.java): `frontLeftCam`, `frontRightCam`, `backCam` (the variables are still named `camera0Name`/`camera1Name`/`camera2Name`). `VisionIO` has two implementations: `VisionIOPhotonVision` (real) and `VisionIOPhotonVisionSim` (simulation). Pose estimates are fed into `Drive`'s `SwerveDrivePoseEstimator`.
 
 ### Autonomous
 
 PathPlanner v2026 manages paths. Named commands are registered in `RobotContainer` via `NamedCommands.registerCommand(...)`. Path files live in `src/main/deploy/pathplanner/`.
 
+### SysId Tuning
+
+`Drive` and `Flywheel` expose SysId characterization routines (Quasistatic/Dynamic × Forward/Reverse) wired into the auto chooser. They are gated behind `!DriverStation.isFMSAttached()` so they don't appear at competition (see [RobotContainer.java](src/main/java/org/team157/robot/RobotContainer.java#L223-L255)).
+
+The flywheel routine writes voltage directly to the master TalonFX via `FlywheelIO.setVoltage()`; the follower mirrors automatically because YAMS' `.withFollowers(...)` configures Phoenix 6's persistent `Follower` request at construction. State transitions are logged to `Flywheel/SysIdState`. To run: pick a SysId entry in the auto chooser, enable autonomous, disable when the wheel saturates (quasistatic ~8–12 s) or shortly after the step response (dynamic ~2–3 s). Repeat for all four directions/types.
+
+Analyze the resulting `.wpilog` in the WPILib **`sysid` tool** (`C:\Users\Public\wpilib\2026\tools\sysid.exe`, or via VS Code → "WPILib: Start Tool" → SysId — AdvantageScope itself has no SysId tab). Map Voltage → `Flywheel/AppliedVolts`, Velocity → `Flywheel/MechanismVelocityRPM`, State → `Flywheel/SysIdState`. Set Analysis Type to "Simple".
+
+**Unit gotcha:** sysid treats the velocity field as rotations-per-second when units-per-rotation is left at the default `1`. Because `MechanismVelocityRPM` is rotations per *minute*, the raw `kV`, `kA`, and `kP` outputs come out 60× too small for what YAMS' `SimpleMotorFeedforward` expects (it feeds velocity in rotations per second internally). Multiply `kV`, `kA`, and `kP` by 60 — `kS` is unaffected (pure volts). Alternative: export a `MechanismVelocityRPS = RPM / 60` channel from AdvantageScope and load that instead, but inline multiplication is the path of least resistance.
+
+**Inline-multiplier convention:** constants in [FlywheelConstants.java](src/main/java/org/team157/robot/subsystems/flywheel/FlywheelConstants.java) sourced from external tools (SysId, Reca.lc) keep their raw tool-output value as the first literal and chain corrections as inline multipliers — e.g., `KV = 0.00197327 * 60 * 0.975` (raw sysid output × unit fix × empirical 2.5% trim). Preserves provenance when re-tuning later.
+
 ### Logging
 
-AdvantageKit `Logger` records all `@AutoLog`-annotated inputs each loop. On the robot, logs write to a USB stick. Metadata (Git SHA, branch, build date) is logged at startup from `BuildConstants`.
+AdvantageKit `Logger` records all `@AutoLog`-annotated inputs each loop. On the robot (`REAL`), logs are written via `WPILOGWriter` (defaults to `/U/logs` USB stick) and republished to NT via `NT4Publisher`; in `SIM`, logs go only to NT. Metadata (Git SHA, branch, build date) is logged at startup from `BuildConstants`.
+
+### Dashboards
+
+An Elastic dashboard layout is checked into [src/main/deploy/ElasticLayout/elastic-layout.json](src/main/deploy/ElasticLayout/elastic-layout.json) and deployed with the robot code.

src/main/java/org/team157/robot/BuildConstants.java

@@ -5,13 +5,13 @@ public final class BuildConstants {
     public static final String MAVEN_GROUP = "";
     public static final String MAVEN_NAME = "FRC2026";
     public static final String VERSION = "unspecified";
-    public static final int GIT_REVISION = 366;
-    public static final String GIT_SHA = "2bdd41328abdfc8999eab7d5cad6e80f4c55241c";
-    public static final String GIT_DATE = "2026-05-15 11:56:30 EDT";
-    public static final String GIT_BRANCH = "task/reexport-as-model";
-    public static final String BUILD_DATE = "2026-05-15 15:57:29 EDT";
-    public static final long BUILD_UNIX_TIME = 1778875049705L;
-    public static final int DIRTY = 0;
+    public static final int GIT_REVISION = 370;
+    public static final String GIT_SHA = "280275c40f6542433273b0f1945a6bff02c57f20";
+    public static final String GIT_DATE = "2026-05-19 19:03:05 EDT";
+    public static final String GIT_BRANCH = "flywheel-auto-tuning";
+    public static final String BUILD_DATE = "2026-05-19 20:11:37 EDT";
+    public static final long BUILD_UNIX_TIME = 1779235897649L;
+    public static final int DIRTY = 1;
 
     private BuildConstants() {}
 }

src/main/java/org/team157/robot/RobotContainer.java

@@ -220,25 +220,39 @@ public RobotContainer() {
         // Set up auto routines
         autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
 
-        // Set up SysId routines
-        autoChooser.addOption(
-                "Drive Wheel Radius Characterization",
-                DriveCommands.wheelRadiusCharacterization(drive));
-        autoChooser.addOption(
-                "Drive Simple FF Characterization",
-                DriveCommands.feedforwardCharacterization(drive));
-        autoChooser.addOption(
-                "Drive SysId (Quasistatic Forward)",
-                drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-        autoChooser.addOption(
-                "Drive SysId (Quasistatic Reverse)",
-                drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
-        autoChooser.addOption(
-                "Drive SysId (Dynamic Forward)",
-                drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
-        autoChooser.addOption(
-                "Drive SysId (Dynamic Reverse)",
-                drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+        if (!DriverStation.isFMSAttached()) {
+            // Set up SysId routines only when not connected to FMS
+            autoChooser.addOption(
+                    "Drive Wheel Radius Characterization",
+                    DriveCommands.wheelRadiusCharacterization(drive));
+            autoChooser.addOption(
+                    "Drive Simple FF Characterization",
+                    DriveCommands.feedforwardCharacterization(drive));
+            autoChooser.addOption(
+                    "Drive SysId (Quasistatic Forward)",
+                    drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+            autoChooser.addOption(
+                    "Drive SysId (Quasistatic Reverse)",
+                    drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+            autoChooser.addOption(
+                    "Drive SysId (Dynamic Forward)",
+                    drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
+            autoChooser.addOption(
+                    "Drive SysId (Dynamic Reverse)",
+                    drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+            autoChooser.addOption(
+                    "Flywheel SysId (Quasistatic Forward)",
+                    flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+            autoChooser.addOption(
+                    "Flywheel SysId (Quasistatic Reverse)",
+                    flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+            autoChooser.addOption(
+                    "Flywheel SysId (Dynamic Forward)",
+                    flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward));
+            autoChooser.addOption(
+                    "Flywheel SysId (Dynamic Reverse)",
+                    flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+        }
 
         // Configure the button bindings
         configureBindings();

src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java

@@ -4,11 +4,13 @@
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.RPM;
 import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.Volts;
 
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import org.littletonrobotics.junction.Logger;
 import org.team157.robot.Constants.FieldConstants;
 import org.team157.robot.RobotContainer;
@@ -27,6 +29,21 @@ public class Flywheel extends SubsystemBase {
     // Inputs from the motors and mechanism, to be updated periodically and logged.
     private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged();
 
+    // SysId routine for characterizing kS / kV / kA. Uses default ramp (1 V/s quasistatic) and step
+    // (7 V dynamic). State changes are logged to AdvantageKit so AdvantageScope's SysId tab can
+    // analyze the run alongside the existing `Flywheel/MechanismVelocityRPM` and
+    // `Flywheel/AppliedVolts` inputs.
+    private final SysIdRoutine sysId =
+            new SysIdRoutine(
+                    new SysIdRoutine.Config(
+                            null,
+                            null,
+                            null,
+                            (state) ->
+                                    Logger.recordOutput("Flywheel/SysIdState", state.toString())),
+                    new SysIdRoutine.Mechanism(
+                            (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
+
     /** The calculated ball velocity in meters per second required for the current shot. */
     public static double ballVelocity = 0;
 
@@ -91,6 +108,25 @@ public Command setDynamicVelocity() {
         return io.setVelocity(this::getDesiredFlywheelVelocity);
     }
 
+    ///////////////////////////////
+    /// SYSID CHARACTERIZATION ///
+    /////////////////////////////
+
+    /** Applies an open-loop voltage directly to the flywheel master motor. */
+    public void runCharacterization(double volts) {
+        io.setVoltage(volts);
+    }
+
+    /** Returns a command to run a quasistatic SysId test in the specified direction. */
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return sysId.quasistatic(direction);
+    }
+
+    /** Returns a command to run a dynamic SysId test in the specified direction. */
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return sysId.dynamic(direction);
+    }
+
     ////////////////////////
     /// FLYWHEEL METHODS ///
     ////////////////////////

src/main/java/org/team157/robot/subsystems/flywheel/FlywheelConstants.java

@@ -25,8 +25,15 @@ public final class FlywheelConstants {
     // IDs of both motors powering the flywheel.
     public static final int MOTOR_ID = 22, FOLLOWER_MOTOR_ID = 23;
     // Closed-loop control values for the flywheel.
-    public static final double KP = 2, KI = 0, KD = 0;
-    public static final double KS = 0.0, KV = 0.0, KA = 0.0;
+    /**
+     * Tuned values using sysID. Multiples of 60 are used due to unit conversion from sysID outputs
+     * Further multiples are added to refine tuning through observation of
+     * Flywheel/MechanismVelocityRPM and comparing to Flywheel/TargetVelocityRPM in AdvantageScope
+     * and running at variable RPM setpoints
+     */
+    public static final double KP = 0.00066882 * 60 * 4, KI = 0, KD = 0.02;
+
+    public static final double KS = 0.05094, KV = 0.00197327 * 60 * 0.975, KA = 0.00013465 * 60;
     public static final double SIM_KP = 1.57, SIM_KI = 0, SIM_KD = 0.157;
     public static final double SIM_KS = 0.0,
             SIM_KV = 0.37,

src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIO.java

@@ -42,6 +42,14 @@ default void simIterate() {}
     /** Stops the flywheel. */
     default void stop() {}
 
+    /**
+     * Applies an open-loop voltage to the flywheel master motor. Used by the SysId routine to
+     * characterize feedforward constants ({@code kS}, {@code kV}, {@code kA}).
+     *
+     * @param volts Voltage to apply, clamped to the battery bus.
+     */
+    default void setVoltage(double volts) {}
+
     /**
      * Directly sets the output duty cycle of the flywheel motor.
      *

src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java

@@ -5,6 +5,7 @@
 import static edu.wpi.first.units.Units.RPM;
 import static edu.wpi.first.units.Units.Volts;
 
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -29,9 +30,13 @@ public class FlywheelIOTalonFX implements FlywheelIO {
 
     private final FlyWheel flywheel;
     private final SmartMotorController motor;
+    // motor object for sysID voltage control
+    private final TalonFX talonFX;
+    // initial voltage for sysID voltage control
+    private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(false);
 
     public FlywheelIOTalonFX(SubsystemBase subsystem) {
-        TalonFX talonFX = new TalonFX(FlywheelConstants.MOTOR_ID, Constants.RIO_CAN_BUS);
+        this.talonFX = new TalonFX(FlywheelConstants.MOTOR_ID, Constants.RIO_CAN_BUS);
         TalonFX followerTalonFX =
                 new TalonFX(FlywheelConstants.FOLLOWER_MOTOR_ID, Constants.RIO_CAN_BUS);
 
@@ -97,6 +102,11 @@ public void stop() {
         flywheel.setDutyCycleSetpoint(0);
     }
 
+    @Override
+    public void setVoltage(double volts) {
+        talonFX.setControl(voltageRequest.withOutput(volts));
+    }
+
     @Override
     public Command set(double dutyCycle) {
         return flywheel.set(dutyCycle);