From a3522ad57debfa2b516f1a44c85c8de6e80dfd1a Mon Sep 17 00:00:00 2001 From: Aztechs157-Git Date: Fri, 15 May 2026 20:52:39 -0400 Subject: [PATCH 1/3] WAYYYYYY better flywheel tuning thanks to sysid and claude - RG --- .SysId/sysid.json | 1 + .claude/settings.local.json | 10 ++++++ CLAUDE.md | 23 ++++++++---- .../org/team157/robot/BuildConstants.java | 14 ++++---- .../org/team157/robot/RobotContainer.java | 12 +++++++ .../robot/subsystems/flywheel/Flywheel.java | 36 +++++++++++++++++++ .../flywheel/FlywheelConstants.java | 4 +-- .../robot/subsystems/flywheel/FlywheelIO.java | 8 +++++ .../flywheel/FlywheelIOTalonFX.java | 9 +++++ 9 files changed, 101 insertions(+), 16 deletions(-) create mode 100644 .SysId/sysid.json create mode 100644 .claude/settings.local.json diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1 @@ +{} diff --git a/.claude/settings.local.json b/.claude/settings.local.json new file mode 100644 index 0000000..1db9ffa --- /dev/null +++ b/.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//**)" + ] + } +} diff --git a/CLAUDE.md b/CLAUDE.md index af984b7..e90ef95 100644 --- a/CLAUDE.md +++ b/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 (recently renamed from `SunstoneV2`). ### 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,7 +75,7 @@ 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 @@ -78,4 +83,8 @@ PathPlanner v2026 manages paths. Named commands are registered in `RobotContaine ### 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. diff --git a/src/main/java/org/team157/robot/BuildConstants.java b/src/main/java/org/team157/robot/BuildConstants.java index 76e54c5..494cda6 100644 --- a/src/main/java/org/team157/robot/BuildConstants.java +++ b/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 = 368; + public static final String GIT_SHA = "86616d12a7747a55925f3732096dac5d76dbbfca"; + public static final String GIT_DATE = "2026-05-15 16:39:07 EDT"; + public static final String GIT_BRANCH = "flywheel-auto-tuning"; + public static final String BUILD_DATE = "2026-05-15 20:50:30 EDT"; + public static final long BUILD_UNIX_TIME = 1778892630434L; + public static final int DIRTY = 1; private BuildConstants() {} } diff --git a/src/main/java/org/team157/robot/RobotContainer.java b/src/main/java/org/team157/robot/RobotContainer.java index 6b855fe..cb1e020 100644 --- a/src/main/java/org/team157/robot/RobotContainer.java +++ b/src/main/java/org/team157/robot/RobotContainer.java @@ -239,6 +239,18 @@ public RobotContainer() { 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(); diff --git a/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java b/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java index 85fc226..78886cf 100644 --- a/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java +++ b/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 /// //////////////////////// diff --git a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelConstants.java b/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelConstants.java index a9c37f5..7f525ca 100644 --- a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelConstants.java +++ b/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelConstants.java @@ -25,8 +25,8 @@ 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; + 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, diff --git a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIO.java index a4d5bb9..9e6a02b 100644 --- a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIO.java +++ b/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. * diff --git a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java index c564a35..dec5360 100644 --- a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/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,11 +30,14 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final FlyWheel flywheel; private final SmartMotorController motor; + private final TalonFX masterTalonFX; + private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(true); public FlywheelIOTalonFX(SubsystemBase subsystem) { TalonFX talonFX = new TalonFX(FlywheelConstants.MOTOR_ID, Constants.RIO_CAN_BUS); TalonFX followerTalonFX = new TalonFX(FlywheelConstants.FOLLOWER_MOTOR_ID, Constants.RIO_CAN_BUS); + this.masterTalonFX = talonFX; SmartMotorControllerConfig flywheelMotorConfig = new SmartMotorControllerConfig(subsystem) @@ -97,6 +101,11 @@ public void stop() { flywheel.setDutyCycleSetpoint(0); } + @Override + public void setVoltage(double volts) { + masterTalonFX.setControl(voltageRequest.withOutput(volts)); + } + @Override public Command set(double dutyCycle) { return flywheel.set(dutyCycle); From 280275c40f6542433273b0f1945a6bff02c57f20 Mon Sep 17 00:00:00 2001 From: Aztechs157 Date: Tue, 19 May 2026 19:03:05 -0400 Subject: [PATCH 2/3] improved commenting - RG --- src/main/java/org/team157/robot/BuildConstants.java | 10 +++++----- .../team157/robot/subsystems/flywheel/Flywheel.java | 4 ++-- .../robot/subsystems/flywheel/FlywheelConstants.java | 7 +++++++ .../robot/subsystems/flywheel/FlywheelIOTalonFX.java | 4 +++- 4 files changed, 17 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/team157/robot/BuildConstants.java b/src/main/java/org/team157/robot/BuildConstants.java index 494cda6..f3e8da1 100644 --- a/src/main/java/org/team157/robot/BuildConstants.java +++ b/src/main/java/org/team157/robot/BuildConstants.java @@ -5,12 +5,12 @@ 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 = 368; - public static final String GIT_SHA = "86616d12a7747a55925f3732096dac5d76dbbfca"; - public static final String GIT_DATE = "2026-05-15 16:39:07 EDT"; + public static final int GIT_REVISION = 369; + public static final String GIT_SHA = "a3522ad57debfa2b516f1a44c85c8de6e80dfd1a"; + public static final String GIT_DATE = "2026-05-15 20:52:39 EDT"; public static final String GIT_BRANCH = "flywheel-auto-tuning"; - public static final String BUILD_DATE = "2026-05-15 20:50:30 EDT"; - public static final long BUILD_UNIX_TIME = 1778892630434L; + public static final String BUILD_DATE = "2026-05-19 18:53:23 EDT"; + public static final long BUILD_UNIX_TIME = 1779231203924L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java b/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java index 78886cf..4262928 100644 --- a/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team157/robot/subsystems/flywheel/Flywheel.java @@ -108,9 +108,9 @@ 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) { diff --git a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelConstants.java b/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelConstants.java index 7f525ca..1788e3a 100644 --- a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelConstants.java +++ b/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelConstants.java @@ -25,7 +25,14 @@ 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. + /** + * 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, diff --git a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java index dec5360..38e8798 100644 --- a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -30,8 +30,10 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final FlyWheel flywheel; private final SmartMotorController motor; + // motor object for sysID voltage control private final TalonFX masterTalonFX; - private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(true); + // 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); From f5397621e0cf3362a30550facbf3195e1e8a535f Mon Sep 17 00:00:00 2001 From: Aztechs157-Git Date: Tue, 19 May 2026 20:21:40 -0400 Subject: [PATCH 3/3] updated per PR comments and updated CLAUDE.md with sysID steps - RG --- CLAUDE.md | 14 +++- .../org/team157/robot/BuildConstants.java | 10 +-- .../org/team157/robot/RobotContainer.java | 64 ++++++++++--------- .../flywheel/FlywheelIOTalonFX.java | 7 +- 4 files changed, 54 insertions(+), 41 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index e90ef95..c5c8ef8 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -38,7 +38,7 @@ Every mechanism follows this pattern (example: Intake): 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 (recently renamed from `SunstoneV2`). +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 @@ -81,6 +81,18 @@ Three PhotonVision cameras configured in [VisionConstants.java](src/main/java/or 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 (`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`. diff --git a/src/main/java/org/team157/robot/BuildConstants.java b/src/main/java/org/team157/robot/BuildConstants.java index f3e8da1..33b7d43 100644 --- a/src/main/java/org/team157/robot/BuildConstants.java +++ b/src/main/java/org/team157/robot/BuildConstants.java @@ -5,12 +5,12 @@ 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 = 369; - public static final String GIT_SHA = "a3522ad57debfa2b516f1a44c85c8de6e80dfd1a"; - public static final String GIT_DATE = "2026-05-15 20:52:39 EDT"; + 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 18:53:23 EDT"; - public static final long BUILD_UNIX_TIME = 1779231203924L; + 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() {} diff --git a/src/main/java/org/team157/robot/RobotContainer.java b/src/main/java/org/team157/robot/RobotContainer.java index cb1e020..6e5cbb8 100644 --- a/src/main/java/org/team157/robot/RobotContainer.java +++ b/src/main/java/org/team157/robot/RobotContainer.java @@ -220,37 +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)); - 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)); + 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(); diff --git a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java index 38e8798..0ada39b 100644 --- a/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team157/robot/subsystems/flywheel/FlywheelIOTalonFX.java @@ -31,15 +31,14 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final FlyWheel flywheel; private final SmartMotorController motor; // motor object for sysID voltage control - private final TalonFX masterTalonFX; + 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); - this.masterTalonFX = talonFX; SmartMotorControllerConfig flywheelMotorConfig = new SmartMotorControllerConfig(subsystem) @@ -105,7 +104,7 @@ public void stop() { @Override public void setVoltage(double volts) { - masterTalonFX.setControl(voltageRequest.withOutput(volts)); + talonFX.setControl(voltageRequest.withOutput(volts)); } @Override