> For the complete documentation index, see [llms.txt](https://yagsl.gitbook.io/yams/llms.txt). Markdown versions of documentation pages are available by appending `.md` to page URLs; this page is available as [Markdown](https://yagsl.gitbook.io/yams/documentation/details/advantagekit-integration.md). # AdvantageKit Integration YAMS integrates seamlessly with [AdvantageKit](https://docs.advantagekit.org/), Team 6328's logging and replay framework. This guide shows how to use YAMS mechanisms with AdvantageKit's IO layer pattern for deterministic log replay. ## What is AdvantageKit? [AdvantageKit](https://docs.advantagekit.org/getting-started/what-is-advantagekit) is a logging framework that records **all inputs** to your robot code, enabling deterministic replay in simulation. Unlike traditional logging that captures specific values, AdvantageKit captures everything flowing into your code, allowing you to: * Replay matches exactly as they happened * Add new logged fields after the fact * Test code changes against real match data * Debug issues that only occurred on the real robot {% hint style="info" %} AdvantageKit is optional. YAMS works perfectly without it using its built-in telemetry system. Consider AdvantageKit if you want deterministic log replay capabilities. {% endhint %} ## Architecture Overview AdvantageKit uses an [IO interface pattern](https://docs.advantagekit.org/data-flow/recording-inputs/io-interfaces) to separate hardware interaction from business logic. Traditionally, this requires separate IO implementations for real hardware and simulation: ``` ┌─────────────────────────────────────────────────────────────┐ │ Traditional AdvantageKit Pattern (without YAMS) │ │ │ │ Subsystem │ │ │ │ │ IO Interface │ │ / \ │ │ IO Real IO Sim │ │ (Real hardware) (Physics sim) │ │ - Manual motor - DCMotorSim │ │ - Manual encoders - Manual state │ └─────────────────────────────────────────────────────────────┘ ``` ## YAMS Simplification: One IO Class for Real and Sim **With YAMS, you don't need separate Real and Sim IO implementations.** Every `SmartMotorController` wrapper (TalonFXWrapper, SparkWrapper, etc.) and every Mechanism class (Arm, Elevator, FlyWheel, SwerveModule, SwerveDrive, etc.) includes **passive simulation** that runs automatically when `Robot.isSimulation()` returns true. ``` ┌─────────────────────────────────────────────────────────────┐ │ YAMS Pattern: Single IO Implementation │ │ │ │ Subsystem │ │ │ │ │ IO Interface │ │ │ │ │ IO (Single!) ◄── Same class for both! │ │ │ │ │ SmartMotorController │ │ or Mechanism │ │ │ │ │ ┌───────────────┴───────────────┐ │ │ │ │ │ │ Real Hardware Passive Simulation │ │ (when deployed) (when in sim) │ │ │ │ Automatically selected based on Robot.isSimulation() │ └─────────────────────────────────────────────────────────────┘ ``` This means: * **One IO class** handles both real robot and simulation * **No duplicate code** - configuration is written once * **No SimWrapper needed** - real hardware wrappers simulate themselves * **Physics are automatic** - based on your DCMotor, gearing, and mass/MOI configuration {% hint style="success" %} **Key Insight**: When you create a `TalonFXWrapper`, `SparkWrapper`, `NovaWrapper`, or any YAMS Mechanism, the simulation physics run automatically in sim mode. You write your IO class once using real hardware objects, and YAMS handles the rest. {% endhint %} ## Step 1: Define the IO Interface Create an interface that defines what inputs your subsystem reads and what outputs it sends. Following AdvantageKit conventions, inputs are grouped in an `Inputs` class. ### Arm IO Interface ```java package frc.robot.subsystems.akit; import org.littletonrobotics.junction.AutoLog; public interface ArmIO { /** * Inputs that will be logged and replayed. * The @AutoLog annotation generates ArmIOInputsAutoLogged class. */ @AutoLog public static class ArmIOInputs { public double positionRotations = 0.0; public double velocityRotationsPerSec = 0.0; public double appliedVolts = 0.0; public double supplyCurrentAmps = 0.0; public double statorCurrentAmps = 0.0; public double temperatureCelsius = 0.0; public double targetPositionRotations = 0.0; } /** Update the inputs from hardware. Called every loop cycle. */ default void updateInputs(ArmIOInputs inputs) {} /** Set the target angle for the arm. */ default void setTargetAngle(double rotations) {} /** Stop the arm motor. */ default void stop() {} } ``` ### Elevator IO Interface ```java package frc.robot.subsystems.akit; import org.littletonrobotics.junction.AutoLog; public interface ElevatorIO { @AutoLog public static class ElevatorIOInputs { public double positionMeters = 0.0; public double velocityMetersPerSec = 0.0; public double appliedVolts = 0.0; public double supplyCurrentAmps = 0.0; public double statorCurrentAmps = 0.0; public double temperatureCelsius = 0.0; public double targetPositionMeters = 0.0; } default void updateInputs(ElevatorIOInputs inputs) {} default void setTargetHeight(double meters) {} default void stop() {} } ``` ### Shooter IO Interface ```java package frc.robot.subsystems.akit; import org.littletonrobotics.junction.AutoLog; public interface ShooterIO { @AutoLog public static class ShooterIOInputs { public double velocityRotationsPerSec = 0.0; public double appliedVolts = 0.0; public double supplyCurrentAmps = 0.0; public double statorCurrentAmps = 0.0; public double temperatureCelsius = 0.0; public double targetVelocityRotationsPerSec = 0.0; } default void updateInputs(ShooterIOInputs inputs) {} default void setTargetVelocity(double rotationsPerSec) {} default void stop() {} } ``` {% hint style="info" %} The `@AutoLog` annotation from AdvantageKit generates a class (e.g., `ArmIOInputsAutoLogged`) that handles serialization for log replay. See [AdvantageKit AutoLog documentation](https://docs.advantagekit.org/data-flow/recording-inputs/io-interfaces#autolog) for details. {% endhint %} ## Step 2: Implement the IO Class With YAMS, you only need **one IO implementation** that works for both real hardware and simulation. The `SmartMotorController` automatically detects when it's running in simulation and uses physics simulation instead of real hardware. ### Arm IO Implementation (Works for Real and Sim!) ```java package frc.robot.subsystems.akit; import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj2.command.SubsystemBase; import yams.gearing.GearBox; import yams.gearing.MechanismGearing; import yams.mechanisms.config.ArmConfig; import yams.mechanisms.positional.Arm; import yams.motorcontrollers.SmartMotorController; import yams.motorcontrollers.SmartMotorControllerConfig; import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; import yams.motorcontrollers.remote.TalonFXWrapper; /** * Single IO implementation for the arm - works in BOTH real and simulation! * Uses YAMS Arm mechanism class while pulling telemetry from the underlying SmartMotorController. */ public class ArmIOTalonFX implements ArmIO { private final Arm arm; private final SmartMotorController motor; public ArmIOTalonFX(SubsystemBase subsystem, int canId) { TalonFX talonFX = new TalonFX(canId); // Step 1: Create SmartMotorControllerConfig SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(subsystem) .withGearing(new MechanismGearing(GearBox.fromReductionStages(5, 4, 3))) .withClosedLoopController(5, 0, 0.1) .withFeedforward(new ArmFeedforward(0.1, 0.3, 0.5, 0.01)) .withTrapezoidalProfile(RotationsPerSecond.of(1.0), RotationsPerSecondPerSecond.of(2.0)); // Step 2: Create SmartMotorController (TalonFXWrapper) SmartMotorController smc = new TalonFXWrapper(talonFX, DCMotor.getKrakenX60(1), smcConfig); // Step 3: Create ArmConfig with the SmartMotorController ArmConfig armConfig = new ArmConfig(smc) .withLength(Inches.of(18)) // Arm length - used for simulation physics .withMass(Pounds.of(5)) // Arm mass - used for simulation physics .withHardLimit(Rotations.of(-0.3), Rotations.of(0.3)) // Physical hard stops for sim .withSoftLimits(Rotations.of(-0.25), Rotations.of(0.25)) .withStartingPosition(Rotations.of(0)) .withTelemetry("Arm", TelemetryVerbosity.HIGH); // Step 4: Create Arm mechanism - handles simulation automatically! this.arm = new Arm(armConfig); // Get reference to underlying SmartMotorController for telemetry this.motor = arm.getMotor(); } @Override public void updateInputs(ArmIOInputs inputs) { // Pull telemetry data from the underlying SmartMotorController inputs.positionRotations = motor.getMechanismPosition().in(Rotations); inputs.velocityRotationsPerSec = motor.getMechanismVelocity().in(RotationsPerSecond); inputs.appliedVolts = motor.getVoltage().in(Volts); inputs.supplyCurrentAmps = motor.getSupplyCurrent().map(c -> c.in(Amps)).orElse(0.0); inputs.statorCurrentAmps = motor.getStatorCurrent().in(Amps); inputs.temperatureCelsius = motor.getTemperature().in(Celsius); inputs.targetPositionRotations = motor.getMechanismPositionSetpoint() .map(a -> a.in(Rotations)).orElse(0.0); } @Override public void setTargetAngle(double rotations) { // Use SmartMotorController's setPosition method motor.setPosition(Rotations.of(rotations)); } @Override public void stop() { motor.setVoltage(Volts.of(0)); } /** Access the Arm mechanism for command helpers like run() and runTo() */ public Arm getArm() { return arm; } } ``` ### Elevator IO Implementation (Works for Real and Sim!) ```java package frc.robot.subsystems.akit; import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj2.command.SubsystemBase; import yams.gearing.GearBox; import yams.gearing.MechanismGearing; import yams.mechanisms.config.ElevatorConfig; import yams.mechanisms.positional.Elevator; import yams.motorcontrollers.SmartMotorController; import yams.motorcontrollers.SmartMotorControllerConfig; import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; import yams.motorcontrollers.remote.TalonFXWrapper; /** * Single IO implementation - uses YAMS Elevator mechanism with SmartMotorController telemetry. */ public class ElevatorIOTalonFX implements ElevatorIO { private final Elevator elevator; private final SmartMotorController motor; public ElevatorIOTalonFX(SubsystemBase subsystem, int canId) { TalonFX talonFX = new TalonFX(canId); // Step 1: Create SmartMotorControllerConfig SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(subsystem) .withGearing(new MechanismGearing(GearBox.fromReductionStages(5, 4))) .withMechanismCircumference(Inches.of(1.5 * Math.PI)) // Pulley circumference .withClosedLoopController(10, 0, 0.5) .withFeedforward(new ElevatorFeedforward(0.1, 0.2, 0.5, 0.01)) .withTrapezoidalProfile(MetersPerSecond.of(1.0), MetersPerSecondPerSecond.of(2.0)); // Step 2: Create SmartMotorController (TalonFXWrapper) SmartMotorController smc = new TalonFXWrapper(talonFX, DCMotor.getKrakenX60(1), smcConfig); // Step 3: Create ElevatorConfig with the SmartMotorController ElevatorConfig elevatorConfig = new ElevatorConfig(smc) .withDrumRadius(Inches.of(0.75)) // Drum radius for pulley .withMass(Pounds.of(10)) // Carriage mass - used for simulation physics .withHardLimits(Meters.of(0), Meters.of(1.5)) // Physical hard stops for sim .withSoftLimits(Meters.of(0.02), Meters.of(1.2)) .withStartingHeight(Meters.of(0.5)) .withTelemetry("Elevator", TelemetryVerbosity.HIGH); // Step 4: Create Elevator mechanism - handles simulation automatically! this.elevator = new Elevator(elevatorConfig); // Get reference to underlying SmartMotorController for telemetry this.motor = elevator.getMotor(); } @Override public void updateInputs(ElevatorIOInputs inputs) { // Pull telemetry data from the underlying SmartMotorController inputs.positionMeters = motor.getMeasurementPosition().in(Meters); inputs.velocityMetersPerSec = motor.getMeasurementVelocity().in(MetersPerSecond); inputs.appliedVolts = motor.getVoltage().in(Volts); inputs.supplyCurrentAmps = motor.getSupplyCurrent().map(c -> c.in(Amps)).orElse(0.0); inputs.statorCurrentAmps = motor.getStatorCurrent().in(Amps); inputs.temperatureCelsius = motor.getTemperature().in(Celsius); inputs.targetPositionMeters = motor.getMeasurementPositionSetpoint() .map(d -> d.in(Meters)).orElse(0.0); } @Override public void setTargetHeight(double meters) { // Use SmartMotorController's setPosition method with Distance motor.setPosition(Meters.of(meters)); } @Override public void stop() { motor.setVoltage(Volts.of(0)); } /** Access the Elevator mechanism for command helpers like run() and runTo() */ public Elevator getElevator() { return elevator; } } ``` ### Shooter (FlyWheel) IO Implementation (Works for Real and Sim!) ```java package frc.robot.subsystems.akit; import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj2.command.SubsystemBase; import yams.gearing.GearBox; import yams.gearing.MechanismGearing; import yams.mechanisms.config.FlyWheelConfig; import yams.mechanisms.velocity.FlyWheel; import yams.motorcontrollers.SmartMotorController; import yams.motorcontrollers.SmartMotorControllerConfig; import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; import yams.motorcontrollers.remote.TalonFXWrapper; /** * Single IO implementation - uses YAMS FlyWheel mechanism with SmartMotorController telemetry. */ public class ShooterIOTalonFX implements ShooterIO { private final FlyWheel flywheel; private final SmartMotorController motor; public ShooterIOTalonFX(SubsystemBase subsystem, int canId) { TalonFX talonFX = new TalonFX(canId); // Step 1: Create SmartMotorControllerConfig SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(subsystem) .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) // Direct drive .withClosedLoopController(0.5, 0, 0) .withFeedforward(new SimpleMotorFeedforward(0.1, 0.12, 0.01)); // Step 2: Create SmartMotorController (TalonFXWrapper) SmartMotorController smc = new TalonFXWrapper(talonFX, DCMotor.getKrakenX60(1), smcConfig); // Step 3: Create FlyWheelConfig with the SmartMotorController FlyWheelConfig flywheelConfig = new FlyWheelConfig(smc) .withDiameter(Inches.of(4)) // Flywheel diameter .withMass(Pounds.of(0.5)) // Flywheel mass - used for simulation physics .withSoftLimit(RPM.of(0), RPM.of(6000)) // Velocity soft limits .withTelemetry("Shooter", TelemetryVerbosity.HIGH); // Step 4: Create FlyWheel mechanism - handles simulation automatically! this.flywheel = new FlyWheel(flywheelConfig); // Get reference to underlying SmartMotorController for telemetry this.motor = flywheel.getMotor(); } @Override public void updateInputs(ShooterIOInputs inputs) { // Pull telemetry data from the underlying SmartMotorController inputs.velocityRotationsPerSec = motor.getMechanismVelocity().in(RotationsPerSecond); inputs.appliedVolts = motor.getVoltage().in(Volts); inputs.supplyCurrentAmps = motor.getSupplyCurrent().map(c -> c.in(Amps)).orElse(0.0); inputs.statorCurrentAmps = motor.getStatorCurrent().in(Amps); inputs.temperatureCelsius = motor.getTemperature().in(Celsius); inputs.targetVelocityRotationsPerSec = motor.getMechanismSetpointVelocity() .map(v -> v.in(RotationsPerSecond)).orElse(0.0); } @Override public void setTargetVelocity(double rotationsPerSec) { // Use SmartMotorController's setVelocity method motor.setVelocity(RotationsPerSecond.of(rotationsPerSec)); } @Override public void stop() { motor.setVoltage(Volts.of(0)); } /** Access the FlyWheel mechanism for command helpers like run() and runTo() */ public FlyWheel getFlyWheel() { return flywheel; } } ``` {% hint style="success" %} **Why This Works**: YAMS's passive simulation is built into every wrapper. When `Robot.isSimulation()` is true, the `TalonFXWrapper` (and all other wrappers) automatically uses the `DCMotor`, gearing, and mass/MOI you provided to simulate physics. You get accurate simulation without writing any simulation-specific code! {% endhint %} ## Step 3: Create the Subsystem The subsystem uses the IO interface and doesn't know whether it's running on real hardware or in simulation. ```java package frc.robot.subsystems.akit; import static edu.wpi.first.units.Units.*; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; public class ArmSubsystem extends SubsystemBase { private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); public ArmSubsystem(ArmIO io) { this.io = io; } @Override public void periodic() { // Update and log inputs every cycle io.updateInputs(inputs); Logger.processInputs("Arm", inputs); } /** * Command to move the arm to a target angle. * Uses run() for continuous control. */ public Command setAngle(double rotations) { return run(() -> io.setTargetAngle(rotations)) .withName("Arm.setAngle(" + rotations + ")"); } /** * Command to move the arm to a target angle and finish when reached. * Uses runTo() pattern - be careful with default commands! */ public Command goToAngle(double rotations) { return run(() -> io.setTargetAngle(rotations)) .until(() -> isNear(Rotations.of(rotations), Rotations.of(0.01))) .withName("Arm.goToAngle(" + rotations + ")"); } /** Returns true if the arm is within tolerance of a target position using WPILib's isNear(). */ public boolean isNear(Angle target, Angle tolerance) { return Rotations.of(inputs.positionRotations).isNear(target, tolerance); } /** Returns the current arm position in rotations. */ public double getPositionRotations() { return inputs.positionRotations; } /** Command to stop the arm. */ public Command stop() { return runOnce(() -> io.stop()).withName("Arm.stop"); } } ``` {% hint style="warning" %} See [run() vs runTo() Commands](/yams/documentation/details/run-vs-runto.md) for important information about when commands end and how that interacts with default commands. {% endhint %} ## Step 4: Wire It Up in RobotContainer With YAMS's passive simulation, you don't need conditional logic for real vs sim - the same IO class works for both! ```java package frc.robot; import frc.robot.subsystems.akit.*; public class RobotContainer { private final ArmSubsystem arm; private final ElevatorSubsystem elevator; private final ShooterSubsystem shooter; public RobotContainer() { // Same IO implementation works for BOTH real robot and simulation! // No need for if(RobotBase.isReal()) checks - YAMS handles it automatically arm = new ArmSubsystem(new ArmIOTalonFX(arm, 1)); elevator = new ElevatorSubsystem(new ElevatorIOTalonFX(elevator, 2)); shooter = new ShooterSubsystem(new ShooterIOTalonFX(shooter, 3)); configureBindings(); } private void configureBindings() { // Commands work identically on real robot and in simulation controller.a().whileTrue(arm.setAngle(Rotations.of(0.25))); controller.b().whileTrue(arm.setAngle(Rotations.of(-0.25))); controller.x().whileTrue(elevator.setHeight(Meters.of(1.0))); controller.y().whileTrue(shooter.setSpeed(RotationsPerSecond.of(80))); } } ``` {% hint style="info" %} **Compare to Traditional AdvantageKit**: Without YAMS, you'd need separate `ArmIOReal` and `ArmIOSim` classes with duplicate configuration. YAMS eliminates this duplication entirely. {% endhint %} ## Log Replay Mode For [log replay](https://docs.advantagekit.org/getting-started/what-is-advantagekit), AdvantageKit provides a "replay" mode where inputs are read from a log file instead of hardware. This is the **one case** where you need a separate IO implementation - an empty one that does nothing: ```java package frc.robot.subsystems.akit; /** * Empty IO implementation for log replay. * AdvantageKit replays the logged inputs, so no hardware interaction needed. */ public class ArmIOReplay implements ArmIO { // All methods use default implementations (do nothing) // AdvantageKit will inject the logged values into inputs } ``` In RobotContainer, only check for replay mode: ```java public RobotContainer() { if (Constants.currentMode == Mode.REPLAY) { // Replay mode: empty IO, AdvantageKit injects logged values arm = new ArmSubsystem(new ArmIOReplay()); elevator = new ElevatorSubsystem(new ElevatorIOReplay()); shooter = new ShooterSubsystem(new ShooterIOReplay()); } else { // Real AND Sim: same IO class works for both! arm = new ArmSubsystem(new ArmIOTalonFX(arm, 1)); elevator = new ElevatorSubsystem(new ElevatorIOTalonFX(elevator, 2)); shooter = new ShooterSubsystem(new ShooterIOTalonFX(shooter, 3)); } } ``` {% hint style="success" %} **Summary**: With YAMS + AdvantageKit, you only need **2 IO classes** per mechanism: 1. **One real IO class** (e.g., `ArmIOTalonFX`) - works for both real robot and simulation 2. **One empty replay IO class** (e.g., `ArmIOReplay`) - for log replay only Compare this to traditional AdvantageKit which requires **3 IO classes** (Real, Sim, Replay). {% endhint %} ## Pattern Summary: Mechanism Classes with SmartMotorController Telemetry The examples above demonstrate the recommended pattern for using YAMS with AdvantageKit: 1. **Create Mechanism classes** (`Arm`, `Elevator`, `FlyWheel`, etc.) for high-level control 2. **Get the underlying `SmartMotorController`** via `mechanism.getSmartMotorController()` 3. **Pull telemetry from `SmartMotorController`** for AdvantageKit logging 4. **Use Mechanism methods** like `setMechanismPositionSetpoint()` for control 5. **Optionally expose the Mechanism** via a getter for command helpers like `run()` and `runTo()` This pattern gives you the best of both worlds: * **Mechanism classes** provide convenient configuration and command helpers * **SmartMotorController** provides detailed telemetry for AdvantageKit logging * **Passive simulation** works automatically in both {% hint style="info" %} See the [YAMS example code](https://github.com/Yet-Another-Software-Suite/YAMS/tree/master/examples/simple_robot/src/main/java/frc/robot/subsystems/akit) for complete examples of using YAMS Mechanism classes with AdvantageKit's IO pattern. {% endhint %} ### Using WPILib's isNear() for Tolerance Checking When checking if a mechanism is at its target, use the `isNear()` method from WPILib's unit classes (`Angle`, `Distance`, `AngularVelocity`): ```java // In your subsystem, check if position is near target Angle currentPosition = Rotations.of(inputs.positionRotations); Angle targetPosition = Rotations.of(targetRotations); Angle tolerance = Rotations.of(0.01); boolean atTarget = currentPosition.isNear(targetPosition, tolerance); ``` This approach works with any unit type and handles conversions automatically. ## Swerve Drive with AdvantageKit YAMS swerve integrates seamlessly with AdvantageKit. Here's a complete example based on the [official YAMS AdvantageKit swerve example](https://github.com/Yet-Another-Software-Suite/YAMS/blob/master/examples/advantage_kit/src/main/java/frc/robot/subsystems/SwerveSubsystem.java): ```java package frc.robot.subsystems; import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLog; import org.littletonrobotics.junction.Logger; import yams.gearing.GearBox; import yams.gearing.MechanismGearing; import yams.mechanisms.config.SwerveDriveConfig; import yams.mechanisms.config.SwerveModuleConfig; import yams.mechanisms.swerve.SwerveDrive; import yams.mechanisms.swerve.SwerveModule; import yams.mechanisms.swerve.utility.SwerveInputStream; import yams.motorcontrollers.SmartMotorController; import yams.motorcontrollers.SmartMotorControllerConfig; import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; import yams.motorcontrollers.local.SparkWrapper; public class SwerveSubsystem extends SubsystemBase { private AngularVelocity maxAngularVelocity = DegreesPerSecond.of(720); private LinearVelocity maxLinearVelocity = MetersPerSecond.of(4); private Supplier gyroAngleSupplier; private SwerveDriveConfig config; /** AdvantageKit inputs for the swerve drive */ @AutoLog public static class SwerveInputs { public SwerveModulePosition[] positions = new SwerveModulePosition[4]; public SwerveModuleState[] states = new SwerveModuleState[4]; public Angle gyroRotation = Degrees.of(0); public ChassisSpeeds robotRelativeSpeeds = new ChassisSpeeds(0, 0, 0); public Pose2d estimatedPose = new Pose2d(0, 0, Rotation2d.fromDegrees(0)); } private final SwerveInputsAutoLogged swerveInputs = new SwerveInputsAutoLogged(); private final SwerveDrive drive; public SwerveModule createModule(SparkMax driveMotor, SparkMax azimuthMotor, CANcoder absoluteEncoder, String moduleName, Translation2d location) { MechanismGearing driveGearing = new MechanismGearing(GearBox.fromStages("12:1", "2:1")); MechanismGearing azimuthGearing = new MechanismGearing(GearBox.fromStages("21:1")); SmartMotorControllerConfig driveCfg = new SmartMotorControllerConfig(this) .withWheelDiameter(Inches.of(4)) .withClosedLoopController(50, 0, 4) .withGearing(driveGearing) .withStatorCurrentLimit(Amps.of(40)) .withTelemetry("driveMotor", TelemetryVerbosity.HIGH); SmartMotorControllerConfig azimuthCfg = new SmartMotorControllerConfig(this) .withClosedLoopController(50, 0, 4) .withContinuousWrapping(Radians.of(-Math.PI), Radians.of(Math.PI)) .withGearing(azimuthGearing) .withStatorCurrentLimit(Amps.of(20)) .withTelemetry("angleMotor", TelemetryVerbosity.HIGH); SmartMotorController driveSMC = new SparkWrapper(driveMotor, DCMotor.getNEO(1), driveCfg); SmartMotorController azimuthSMC = new SparkWrapper(azimuthMotor, DCMotor.getNEO(1), azimuthCfg); SwerveModuleConfig moduleConfig = new SwerveModuleConfig(driveSMC, azimuthSMC) .withAbsoluteEncoder(absoluteEncoder.getAbsolutePosition().asSupplier()) .withTelemetry(moduleName, TelemetryVerbosity.HIGH) .withLocation(location) .withOptimization(true); return new SwerveModule(moduleConfig); } public SwerveInputStream getChassisSpeedsSupplier(DoubleSupplier x, DoubleSupplier y, DoubleSupplier rot) { return new SwerveInputStream(drive, x, y, rot) .withMaximumAngularVelocity(maxAngularVelocity) .withMaximumLinearVelocity(maxLinearVelocity) .withDeadband(0.01) .withCubeRotationControllerAxis() .withCubeTranslationControllerAxis() .withAllianceRelativeControl(); } public SwerveSubsystem() { Pigeon2 gyro = new Pigeon2(14); gyroAngleSupplier = gyro.getYaw().asSupplier(); var fl = createModule(new SparkMax(1, MotorType.kBrushless), new SparkMax(2, MotorType.kBrushless), new CANcoder(3), "frontleft", new Translation2d(Inches.of(12), Inches.of(12))); var fr = createModule(new SparkMax(4, MotorType.kBrushless), new SparkMax(5, MotorType.kBrushless), new CANcoder(6), "frontright", new Translation2d(Inches.of(12), Inches.of(-12))); var bl = createModule(new SparkMax(7, MotorType.kBrushless), new SparkMax(8, MotorType.kBrushless), new CANcoder(9), "backleft", new Translation2d(Inches.of(-12), Inches.of(12))); var br = createModule(new SparkMax(10, MotorType.kBrushless), new SparkMax(11, MotorType.kBrushless), new CANcoder(12), "backright", new Translation2d(Inches.of(-12), Inches.of(-12))); config = new SwerveDriveConfig(this, fl, fr, bl, br) .withGyro(() -> getGyroAngle().getMeasure()) .withStartingPose(swerveInputs.estimatedPose) .withTranslationController(new PIDController(1, 0, 0)) .withRotationController(new PIDController(1, 0, 0)); drive = new SwerveDrive(config); } private Rotation2d getGyroAngle() { return new Rotation2d(swerveInputs.gyroRotation); } private void updateInputs() { swerveInputs.estimatedPose = drive.getPose(); swerveInputs.states = drive.getModuleStates(); swerveInputs.positions = drive.getModulePositions(); swerveInputs.robotRelativeSpeeds = drive.getRobotRelativeSpeed(); swerveInputs.gyroRotation = gyroAngleSupplier.get(); } public Command setRobotRelativeChassisSpeeds(Supplier speedsSupplier) { return run(() -> { Logger.recordOutput("Swerve/DesiredChassisSpeeds", speedsSupplier.get()); SwerveModuleState[] states = drive.getStateFromRobotRelativeChassisSpeeds(speedsSupplier.get()); Logger.recordOutput("Swerve/DesiredStates", states); drive.setSwerveModuleStates(states); }).withName("Set Robot Relative Chassis Speeds"); } public Command driveToPose(Pose2d pose) { return drive.driveToPose(pose); } public Command lock() { return run(drive::lockPose); } public Pose2d getPose() { return swerveInputs.estimatedPose; } public ChassisSpeeds getRobotRelativeSpeeds() { return swerveInputs.robotRelativeSpeeds; } @Override public void periodic() { drive.updateTelemetry(); // Updates pose estimator - call BEFORE updateInputs updateInputs(); Logger.processInputs("Swerve", swerveInputs); } @Override public void simulationPeriodic() { drive.simIterate(); } } ``` ### Key Points for Swerve with AdvantageKit 1. **Create an `@AutoLog` inputs class** with module states, positions, gyro, and pose 2. **Call `updateTelemetry()` BEFORE `updateInputs()`** - the pose estimator and internal state updates in `updateTelemetry()` 3. **Log desired states and speeds** using `Logger.recordOutput()` for replay debugging 4. **Call `simIterate()` in `simulationPeriodic()`** for physics simulation {% hint style="warning" %} **Important: Replay Data Scope** During AdvantageKit log replay, **only data inside the `@AutoLog` inputs classes is mutated**. YAMS's built-in telemetry (published via `withTelemetry()`) is NOT affected by replay - it will show live simulation values, not replayed values. This means: * **Inputs you define** (in `SwerveInputs`, `ArmInputs`, etc.) - Replayed correctly * **YAMS telemetry** (from `.withTelemetry("Arm")`) - Shows live sim values during replay To ensure accurate replay debugging, always pull the data you need into your `@AutoLog` inputs class rather than relying on YAMS telemetry during replay. {% endhint %} ## Alternative Pattern: Direct Subsystem (No IO Layer) For simpler projects where replay isn't needed, you can use YAMS mechanisms directly as shown in the [official examples](https://github.com/Yet-Another-Software-Suite/YAMS/tree/master/examples/advantage_kit/src/main/java/frc/robot/subsystems): ```java public class ArmSubsystem extends SubsystemBase { @AutoLog public static class ArmInputs { public Angle pivotPosition = Degrees.of(0); public AngularVelocity pivotVelocity = DegreesPerSecond.of(0); public Voltage pivotAppliedVolts = Volts.of(0); public Current pivotCurrent = Amps.of(0); } private final ArmInputsAutoLogged armInputs = new ArmInputsAutoLogged(); private final SmartMotorController armSMC; private final Arm arm; public ArmSubsystem() { SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) .withControlMode(ControlMode.CLOSED_LOOP) .withClosedLoopController(18, 0, 0.2) .withTrapezoidalProfile(DegreesPerSecond.of(458), DegreesPerSecondPerSecond.of(688)) .withFeedforward(new ArmFeedforward(-0.1, 1.2, 0, 0)) .withGearing(new MechanismGearing(GearBox.fromReductionStages(12.5, 1))) .withIdleMode(MotorMode.BRAKE) .withStatorCurrentLimit(Amps.of(120)); armSMC = new TalonFXWrapper(new TalonFX(40), DCMotor.getFalcon500(1), smcConfig); ArmConfig armCfg = new ArmConfig(armSMC) .withHardLimit(Degrees.of(-25), Degrees.of(141)) .withStartingPosition(Degrees.of(141)) .withLength(Feet.of(14.0 / 12)) .withMOI(0.1055) .withTelemetry("Arm", TelemetryVerbosity.HIGH); arm = new Arm(armCfg); } public void updateInputs() { armInputs.pivotPosition = arm.getAngle(); armInputs.pivotVelocity = armSMC.getMechanismVelocity(); armInputs.pivotAppliedVolts = armSMC.getVoltage(); armInputs.pivotCurrent = armSMC.getStatorCurrent(); } public Command setAngle(Angle angle) { return arm.setAngle(angle); } @Override public void periodic() { arm.updateTelemetry(); // Always call BEFORE updateInputs updateInputs(); Logger.processInputs("Arm", armInputs); } @Override public void simulationPeriodic() { arm.simIterate(); } } ``` This pattern is simpler and **does support log replay**, but with a key difference: during replay, the full YAMS simulation runs alongside the logged data. This means: * **Pro**: You get full simulation behavior during replay, which can help debug physics-related issues * **Con**: Replay requires more processing power since every `SmartMotorController` and Mechanism simulates its physics Choose the IO layer pattern if you need lightweight replay; choose this direct pattern if you prefer simplicity and don't mind the extra processing during replay. ## Benefits of YAMS + AdvantageKit | Feature | Benefit | | --------------------------- | --------------------------------------------------------- | | **No Duplicate IO Classes** | Single IO class works for real robot AND simulation | | **Deterministic Replay** | Debug issues from real matches using log replay | | **Automatic Physics** | YAMS uses DCMotor, gearing, and mass/MOI for accurate sim | | **Less Boilerplate** | 2 IO classes instead of 3 per mechanism | | **Consistent Behavior** | Same code paths execute in real and sim modes | | **Flexible Hardware** | Swap vendors by changing one IO class | ## Further Reading * [AdvantageKit Documentation](https://docs.advantagekit.org/) * [What is AdvantageKit?](https://docs.advantagekit.org/getting-started/what-is-advantagekit) * [IO Interfaces](https://docs.advantagekit.org/data-flow/recording-inputs/io-interfaces) * [Template Projects](https://docs.advantagekit.org/getting-started/template-projects) * [YAMS Example Code](https://github.com/Yet-Another-Software-Suite/YAMS/tree/master/examples/simple_robot/src/main/java/frc/robot/subsystems/akit) --- # Agent Instructions This documentation is published with GitBook. GitBook is the documentation platform designed so that both humans and AI agents can read, navigate, and reason over technical content effectively. Learn more at gitbook.com. ## Querying This Documentation If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question. Perform an HTTP GET request on the current page URL with the `ask` query parameter: ``` GET https://yagsl.gitbook.io/yams/documentation/details/advantagekit-integration.md?ask= ``` The question should be specific, self-contained, and written in natural language. The response will contain a direct answer to the question and relevant excerpts and sources from the documentation. Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.