Subsystems

FrcCatalyst provides three complex subsystem wrappers that integrate multiple components.

Table of contents

  1. SwerveSubsystem
    1. driveFieldCentric or driveFieldAbsolute
    2. PathPlanner support
    3. pathfindToPose (v0.3.6.1+)
    4. Choreo paths (v0.8.0+)
    5. driveToPiece — vision pursuit (v0.8.0+)
    6. WheelRadiusCalibration (v0.8.0+)
    7. SlipCurrentCalibration (Unreleased)
    8. SwerveSetpointGenerator (v0.4.0+)
  2. VisionSubsystem
    1. LimelightTriggers (v0.4.0+)
  3. LEDSubsystem

SwerveSubsystem

Wraps CTRE Tuner X generated swerve code. Teams generate their drivetrain using Tuner X, then wrap it with SwerveSubsystem to get:

  • Field-centric and robot-centric drive commands
  • Heading lock — auto-holds heading when driver isn’t rotating
  • Point-at-target — always face a scoring target while translating
  • Drive-with-heading — lock to a specific heading angle
  • PathPlanner integration — one-line AutoBuilder configuration
  • Vision pose estimation — addVisionMeasurement() bridge
  • Automatic telemetry — pose, heading, speed to NetworkTables
  • Skew correction — pose exponential discretization
  • Slew rate limiting — smooth acceleration with asymmetric profiles
  • Snap-to-angle — auto-snap heading to predefined angles
  • Advanced drive — combined deadband, slew, heading lock, skew correction, snap-to-angle
  • Slow mode — toggleable speed multiplier for precision
  • Auto-align — drive while auto-rotating to a target heading
SwerveSubsystem drive = new SwerveSubsystem(
    TunerConstants.createDrivetrain(),
    4.5,
    SwerveSubsystem.PathPlannerConfig.builder()
        .translationPID(5.0, 0, 0)
        .rotationPID(5.0, 0, 0)
        .build()
);

// Advanced drive (recommended default command)
// Combines deadband, slew limiting, heading lock, snap-to-angle, and skew correction
drive.setSkewCorrectionEnabled(true);
drive.enableSlewRateLimiting(2.0, 5.0); // translation m/s per second, rotation rad/s per second
drive.setSnapToAngles(new double[]{0.0, 90.0, 180.0, 270.0}, 5.0);
drive.setDefaultCommand(drive.advancedDrive(
    () -> -driver.getLeftY(),
    () -> -driver.getLeftX(),
    () -> -driver.getRightX(),
    0.05
));

// Slow mode for precision alignment
driver.leftBumper().whileTrue(drive.slowModeWhileHeld(0.3));

// Point at speaker while driving
driver.rightBumper().whileTrue(drive.pointAtTarget(
    () -> -driver.getLeftY(),
    () -> -driver.getLeftX(),
    () -> new Translation2d(0.0, 5.55),
    0.05
));

See the Advanced Features section for detailed documentation on skew correction, slew rate limiting, snap-to-angle, and auto-align.

driveFieldCentric or driveFieldAbsolute

Both take a field velocity and a turn rate (m/s, m/s, rad/s). They differ in which field:

Method Use it for Frame
driveFieldCentric(vx, vy, omega) A driver’s sticks The operator’s perspective: Phoenix turns the request half a turn on the Red alliance, so “forward” is always away from the driver
driveFieldAbsolute(vx, vy, omega) Anything computed from the field pose The field itself: +x away from the Blue Alliance wall, +y to its left, whichever alliance the robot is on

The rule: a velocity computed from the field pose goes through driveFieldAbsolute. A pose controller, or an assist that steers toward a field point, works in field coordinates; sent through driveFieldCentric its correction points the wrong way on Red.

// vx, vy from the field pose error (target minus drive.getPose()):
drive.driveFieldAbsolute(vx, vy, omega);

// vx, vy from the driver's sticks:
drive.driveFieldCentric(vx, vy, omega);

driveToPose and driveToPiece were wrong on the Red alliance before 2.0.0-rc.3-a6: they sent a field-pose velocity through driveFieldCentric, so on Red they drove away from the target. Both now use driveFieldAbsolute and are fixed.

PathPlanner support

SwerveSubsystem configures PathPlanner’s AutoBuilder for you when you pass a PathPlannerConfig (as above). It wires:

  • getPose / resetPose — pose source + reset
  • getChassisSpeeds — robot-relative speeds (what PathPlanner expects)
  • a robot-relative ChassisVelocities consumer for path output
  • a PPHolonomicDriveController from your translation/rotation PID
  • RobotConfig.fromGUISettings() — mass, MOI, module config from the PathPlanner GUI
  • alliance flipping (mirrors paths for the red alliance automatically)

Once configured, named autos load with one call:

autonomousCommand = AutoBuilder.buildAuto("MyAuto");
// or follow a single path:
new PathPlannerAuto("MyAuto").schedule();

Both pathfindToPose(...) (below) and the behavior framework autos build on this. If AutoBuilder isn’t configured (you didn’t pass a PathPlannerConfig), pathfinding commands fall back to PID-only and print a DS error rather than crashing.

Robot- vs field-relative: getChassisSpeeds() is robot-relative for PathPlanner. For Shoot-On-The-Fly use getFieldRelativeSpeeds(), which rotates it into the field frame.

pathfindToPose (v0.3.6.1+)

Pathfind to a pose with PathPlanner’s AutoBuilder.pathfindToPose, then hand off to the existing precision-align PID for the last metre.

swerve.pathfindToPose(() -> ScoringPoses.RED_L2);                 // defaults: kP=4.0, tolerance=2 cm
swerve.pathfindToPose(() -> target, 5.0, 0.015);                  // tighter tolerance
swerve.pathfindToPose(() -> target, new PathConstraints(3, 2, …)); // your own constraints

If AutoBuilder isn’t configured the command falls back to PID-only align and prints a driver-station error explaining why — no crash.

Choreo paths (v0.8.0+)

Follow Choreo’s time-optimal trajectories — loaded through PathPlanner, so no extra vendordep (Choreo exports, PathPlanner follows). Put the .traj files in src/main/deploy/choreo/:

autonomousCommand = swerve.followChoreoPath("FourPieceFar");

Needs AutoBuilder configured (the PathPlannerConfig constructor). If the trajectory can’t be loaded it reports to the driver station and returns a no-op instead of crashing.

driveToPiece — vision pursuit (v0.8.0+)

The primitive the Autopilot “acquire” action wants. Give it a supplier of the detected piece’s field position (empty when your coprocessor sees nothing) and it drives onto it:

swerve.driveToPiece(() -> vision.nearestFuelPose());          // defaults kP=3, tol=0.2m
swerve.driveToPiece(() -> vision.nearestFuelPose(), 4.0, 0.15);

Stops when it arrives or the piece disappears. vision.nearestFuelPose() is your team-side detection method (Supplier<Optional<Translation2d>>).

WheelRadiusCalibration (v0.8.0+)

Measures the actual wheel radius by spinning the robot — correcting the CAD value, a documented source of autonomous inaccuracy (tread wears and compresses). Compares the gyro arc to the distance odometry thinks each wheel rolled:

test.a().onTrue(WheelRadiusCalibration.builder(swerve)
    .currentWheelRadius(0.0508)   // your configured radius (m)
    .driveBaseRadius(0.42)        // center → module distance (m)
    .rotations(4)
    .build());

The corrected radius + a copy-paste constant publish to /Catalyst/Calibration/WheelRadius/....

SlipCurrentCalibration (Unreleased)

Measures Phoenix’s slip current — the drive’s stator current limit (kSlipCurrent) — instead of keeping a number copied from another robot. A light robot’s wheels break traction far below a heavy one’s, and a wheel spinning on the carpet is motion the odometry believes. CTRE’s method: front bumper squarely against a wall, modules straight, the drive voltage ramped slowly until a wheel spins up; that wheel’s peak current is the answer.

test.b().onTrue(SlipCurrentCalibration.builder(swerve)
    .currentSlipAmps(120)         // what your constants say today, for the snippet
    .build());

It stops by itself at the slip, at its voltage cap (6 V), or when a wheel turns on a few amps — the robot is rolling, not held, and nothing is measured — and leaves the drive at 0 V however it ends, cancellation included. Status, Volts, PeakAmps, SlipAmps, RecommendedAmps (rounded down to 5 A), Result and Snippet publish to /Catalyst/Calibration/SlipCurrent/....

On this line it is compiled and, with Phoenix 6 26.70.0-alpha-2 and 26.70.x device firmware, can drive a CTRE drivetrain — it just has not yet been driven on a robot.

SwerveSetpointGenerator (v0.4.0+)

Light chassis-aware accel/skid clamp. Wraps a ChassisVelocities and returns one limited by max wheel speed, max angular rate, and a per-second delta-v cap.

SwerveSetpointGenerator gen = new SwerveSetpointGenerator(
    drive.getMaxSpeedMPS(), drive.getMaxAngularRate(), 8.0); // 8 m/s² accel cap

ChassisVelocities limited = gen.generate(requestedSpeeds);
drivetrain.setControl(req.withVelocityX(limited.vx)
                        .withVelocityY(limited.vy)
                        .withRotationalRate(limited.omega));

Catches the most common driver-induced skid (jerking the stick from full-forward to full-right) without doing a full per-wheel feasibility solve. Cheap.

Keeping the rotation (unreleased, on this branch). When translation and rotation together ask a module for more than its top speed, the drivetrain’s own desaturation shrinks both in proportion, and that stays the default. Priority.ROTATION shrinks only the translation, along its direction, by the largest factor that fits every module beside the rotation. That is the one wheel-by-wheel question a robot aiming itself needs answered:

SwerveSetpointGenerator aimGen = new SwerveSetpointGenerator(
    drive.getMaxSpeedMPS(), 3.0, 8.0, Math.PI * 8,
    SwerveSetpointGenerator.Priority.ROTATION,
    drive.getDrivetrain().getModuleLocations());

// A field-relative request, judged where the robot faces:
ChassisVelocities send = aimGen.generate(requested, dt, drive.getHeading());
double kept = aimGen.getTranslationScale();  // 1 unless translation gave way

Shoot on the Move shows it in use.

VisionSubsystem

Multi-camera pose estimation with Kalman filter integration. Limelight-first: MegaTag1 and MegaTag2, several cameras at once. PhotonVision is not built on this branch - there is no 2027 vendordep for it.

Features:

  • Distance-scaled standard deviations — trusts close targets more
  • Ambiguity-scaled std devs — higher ambiguity = less trust
  • Spin rejection — ignores vision during fast rotation, as the gyro measures it (VisionPoseSink.getYawRateRadPerSec()), not as the wheels report it
  • High-speed rejection — ignores vision while driving fast
  • Heading divergence filtering — rejects single-tag poses that disagree with the gyro
  • Kalman innovation tracking — logs innovation norms for tuning
  • Latency filtering — rejects stale measurements
  • Configurable field bounds — custom field dimensions for bounds checking
  • Per-cycle telemetry — see which estimates are accepted/rejected with reasons
VisionSubsystem vision = new VisionSubsystem(
    VisionConfig.builder()
        .driveSubsystem(drive)                                    // wires pose fusion
        .addLimelight("limelight-front", frontCameraPose)
        .addPhotonCamera("cam-rear", rearCameraPose, fieldLayout) // Photon needs the tag layout
        .baseXYStdDev(0.5)                // expected XY error at 1 m off one tag
        .baseRotStdDev(0.9)               // expected heading error in radians
        .xyDistanceScaling(1.0)           // xyStdDev = base * (1 + distance^2 * scaling)
        .rotDistanceScaling(1.5)          // heading degrades faster with distance than XY does
        .singleTagRotDistanceThreshold(4.0)  // past 4 m, one tag's heading is ignored outright
        .rejectDuringSpin(2.0)
        .rejectDuringHighSpeed(3.0)       // reject when > 3 m/s
        .maxHeadingDivergence(15.0)       // reject if heading disagrees > 15 deg
        .fieldDimensions(16.54, 8.07)     // REBUILT 2026 bounds
        .maxLatency(0.5)
        .build()
);

Pass 4+ cameras and they’re fused deterministically — each is filtered independently and added to the pose estimator in timestamp order with quality/index tiebreaks, so the result is reproducible run-to-run.

LimelightTriggers (v0.4.0+)

Wrap a Limelight’s NetworkTables keys as WPILib Triggers. Point at the table name, bind, done.

LimelightTriggers front = new LimelightTriggers("limelight-front");

front.hasTarget().onTrue(leds.solid(Color.GREEN));
front.tagInView(7).whileTrue(swerve.pathfindToPose(() -> SCORE_7));
front.detectorClass("note").onTrue(intake.intakeCommand());
front.horizontalErrorBelow(2.0).onTrue(rumble.fire(Pattern.DOUBLE_TAP, Channel.DRIVER));
front.targetWithinArea(2.0).onTrue(climber.armCommand());
Trigger Backed by
hasTarget() tv > 0.5
tagInView(int) tv > 0.5 && tid == id
detectorClass(String) tv > 0.5 && tclass == name
horizontalErrorBelow(double) \|tx\| <= degrees
targetWithinArea(double) ta >= percent

Plus diagnostic readers: tx(), ty(), ta(), tid(), latencyMs() for direct polling when you don’t want a trigger.

LEDSubsystem

Addressable LED pattern controller with 14 pre-built effects.

Basic patterns: solid, blink, rainbow, chase, breathe, alternating

Advanced patterns: fire, gradient, scrolling gradient, strobe, larson scanner, dynamic progress, status indicator, alignment indicator

LEDSubsystem leds = new LEDSubsystem(
    LEDConfig.builder()
        .pwmPort(0)          // PWM port the strip is on
        .ledCount(60)        // LEDs in the strip
        .build()
);

// Alliance color by default
leds.setDefaultCommand(leds.solid(Color.BLUE));

// Rainbow when scoring
scoring.whileTrue(leds.rainbow());

// Blink green when game piece acquired
intake.hasPieceTrigger().whileTrue(leds.blink(Color.GREEN, 0.1));

// Fire effect for celebration
scoring.whileTrue(leds.fire());

// Alignment indicator for driver (Color + 0..1 progress supplier)
aligning.whileTrue(leds.alignmentIndicator(Color.GREEN, () -> alignProgress));

// Progress bar for elevator height
leds.dynamicProgress(Color.GREEN, () -> elevator.getPosition() / 1.2);

See the Advanced Features section for details on all new LED patterns.

Back to top

Documents Catalyst 2.0.0-beta.2 (Systemcore OS image 14, WPILib 2027.0.0-alpha-7). Other versions: Versions and compatibility. Built for the FRC community. Go build something awesome.

This site uses Just the Docs, a documentation theme for Jekyll.