Writing AdvantageKit IO Layers

Example

Below is an example of a sample IO layer for the NavX in AdvantageKit

package frc.robot.subsystems.drive;

import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.SPI;
import java.util.OptionalDouble;
import java.util.Queue;
import org.littletonrobotics.junction.Logger;

/** IO implementation for NavX */
public class GyroIONavX implements GyroIO {
  byte updateRate = 50;
  private final AHRS navx = new AHRS(SPI.Port.kMXP);
  private final Queue<Double> yawPositionQueue;
  private final Queue<Double> yawTimestampQueue;

  public GyroIONavX() {
    if (!navx.isCalibrating()) {
      navx.reset();
    } else {
      // Wait for calibration to finish, then reset
      new Thread(
              () -> {
                try {
                  // 1 second calibration time after factory calibration
                  Thread.sleep((1000));
                  navx.reset();
                } catch (Exception e) {
                }
              })
          .start();
    }
    yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue();
    yawPositionQueue =
        SparkMaxOdometryThread.getInstance()
            .registerSignal(
                () -> {
                  boolean valid = navx.isConnected();
                  if (valid) {
                    return OptionalDouble.of(-navx.getFusedHeading());
                  } else {
                    return OptionalDouble.empty();
                  }
                });
  }

  @Override
  public void updateInputs(GyroIOInputs inputs) {
    inputs.connected = navx.isConnected();
    inputs.yawPosition = Rotation2d.fromDegrees(-navx.getFusedHeading());
    inputs.yawVelocityRadPerSec = Units.degreesToRadians(-navx.getRate());
    inputs.odometryYawTimestamps =
        yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
    inputs.odometryYawPositions =
        yawPositionQueue.stream()
            .map((Double value) -> Rotation2d.fromDegrees(value))
            .toArray(Rotation2d[]::new);
    yawTimestampQueue.clear();
    yawPositionQueue.clear();

    Logger.recordOutput("Gyro/fusedHeading", navx.getFusedHeading());
    Logger.recordOutput("Gyro/compassHeading", navx.getCompassHeading());
    Logger.recordOutput("Gyro/compassX", navx.getRawMagX());
    Logger.recordOutput("Gyro/compassY", navx.getRawMagY());
    Logger.recordOutput("Gyro/compassZ", navx.getRawMagZ());
    Logger.recordOutput("Gyro/accelX", navx.getRawAccelX());
    Logger.recordOutput("Gyro/accelY", navx.getRawAccelY());
    Logger.recordOutput("Gyro/accelZ", navx.getRawAccelZ());
  }

  @Override
  public void resetGyro() {
    navx.reset();
  }
}

An inputs layer must be predefined where all of the data you want to read from the hardware is declared. The updateInputs() method is called periodically and will update the values into the inputs to be used elsewhere.

Ensure to also record all data you wish to visualize for later using the AdvantageKit logger in order to keep track and replay the data.

Writing AdvantageKit IO Layers

Example​

Example