feat(swerve): Add swerve module states and setpoints to Shuffleboard.

src/main/java/frc/lib/Telemetry.java

@@ -1,11 +1,13 @@
 package frc.lib;
 
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import java.util.HashMap;
+import java.util.function.Supplier;
 
 /** Helper class for managing robot telemetry. */
 public class Telemetry {
@@ -68,4 +70,26 @@ public static ShuffleboardLayout addColumn(ShuffleboardTab tab, String columnTit
   public static void addFullscreen(ShuffleboardTab tab, String title, Sendable sendable) {
     tab.add(title, sendable).withPosition(0, 0).withSize(10, 4);
   }
+
+  /**
+   * Adds swerve module states to a Shuffleboard tab.
+   * 
+   * @param tab the Shuffleboard tab to add the swerve module states to.
+   * @param title the title of the swerve module states.
+   * @param swerveModuleStatesSupplier a supplier for swerve module states.
+   */
+  public static void addSwerveModuleStates(ShuffleboardTab tab, String title, Supplier<SwerveModuleState[]> swerveModuleStatesSupplier) {
+    tab.addDoubleArray(title, () -> {
+      SwerveModuleState[] states = swerveModuleStatesSupplier.get();
+      double[] doubles = new double[8];
+
+      for (int i = 0; i < 4; i++) {
+        SwerveModuleState state = states[i];
+        doubles[2 * i] = state.angle.getDegrees();
+        doubles[2 * i + 1] = state.speedMetersPerSecond;
+      }
+
+      return doubles;
+    });
+  }
 }

src/main/java/frc/robot/swerve/SteerMotorIOSim.java

@@ -19,6 +19,9 @@ public class SteerMotorIOSim implements SteerMotorIO {
   /** Represents the position of the steer motor. */
   private double positionRotations;
 
+  /** Represents the velocity of the steer motor. */
+  private double velocityRotationsPerSecond;
+
   /** Feedback controller for the position. */
   private final PIDController positionFeedback = new PIDController(1.0, 0, 0);
 
@@ -33,14 +36,6 @@ public void configure() {}
 
   @Override
   public void update(SteerMotorIOValues values) {
-    double voltage = positionFeedback.calculate(positionRotations);
-
-    wheelSim.setInputVoltage(voltage);
-    wheelSim.update(RobotConstants.PERIODIC_DURATION);
-
-    double velocityRotationsPerSecond = wheelSim.getAngularVelocityRPM() / 60.0;
-    positionRotations += velocityRotationsPerSecond;
-
     values.positionRotations = positionRotations;
     values.velocityRotationsPerSecond = velocityRotationsPerSecond;
   }
@@ -52,6 +47,12 @@ public void setPosition(double positionRotations) {
 
   @Override
   public void setSetpoint(double positionRotations) {
-    positionFeedback.setSetpoint(positionRotations);
+    double voltage = positionFeedback.calculate(this.positionRotations, positionRotations);
+
+    wheelSim.setInputVoltage(voltage);
+    wheelSim.update(RobotConstants.PERIODIC_DURATION);
+
+    this.velocityRotationsPerSecond = wheelSim.getAngularVelocityRPM() / 60.0;
+    this.positionRotations += this.velocityRotationsPerSecond;
   }
 }

src/main/java/frc/robot/swerve/Swerve.java

@@ -57,6 +57,9 @@ public void periodic() {}
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
+    Telemetry.addSwerveModuleStates(tab, "Swerve Module States", this::getModuleStates);
+    Telemetry.addSwerveModuleStates(tab, "Swerve Module Setpoints", this::getModuleSetpoints);
+
     for (int i = 0; i < 4; i++) {
       SwerveModuleIO swerveModule = swerveModules[i];
 
@@ -92,6 +95,21 @@ public SwerveModuleState[] getModuleStates() {
     return moduleStates;
   }
 
+  /**
+   * Gets the setpoint of each of the swerve's modules.
+   *
+   * @return the setpoint of each of the swerve's modules.
+   */
+  public SwerveModuleState[] getModuleSetpoints() {
+    SwerveModuleState[] moduleSetpoints = new SwerveModuleState[4];
+
+    for (int i = 0; i < 4; i++) {
+      moduleSetpoints[i] = swerveModules[i].getSetpoint();
+    }
+
+    return moduleSetpoints;
+  }
+
   /**
    * Gets the position of each of the swerve's modules.
    *

src/main/java/frc/robot/swerve/SwerveModuleIO.java

@@ -21,6 +21,13 @@ public interface SwerveModuleIO {
    */
   public SwerveModuleState getState();
 
+  /**
+   * Gets the setpoint of the swerve module.
+   *
+   * @return the setpoint of the swerve module.
+   */
+  public SwerveModuleState getSetpoint();
+
   /**
    * Gets the position of the swerve module.
    *

src/main/java/frc/robot/swerve/SwerveModuleIOCustom.java

@@ -29,6 +29,9 @@ public class SwerveModuleIOCustom implements SwerveModuleIO {
   /** Driver motor values. */
   private final DriveMotorIOValues driveMotorValues = new DriveMotorIOValues();
 
+  /** Module setpoint */
+  private SwerveModuleState setpoint;
+
   /**
    * Creates a custom swerve module.
    *
@@ -44,6 +47,8 @@ public SwerveModuleIOCustom(SwerveModuleConfig config) {
     driveMotor = SwerveFactory.createDriveMotor(config);
     driveMotor.configure();
 
+    setpoint = new SwerveModuleState();
+
     azimuthEncoder.update(azimuthEncoderValues);
     steerMotor.setPosition(azimuthEncoderValues.angleRotations);
   }
@@ -56,6 +61,8 @@ public void setSetpoint(SwerveModuleState setpoint, boolean lazy) {
 
     steerMotor.setSetpoint(setpoint.angle.getRotations());
     driveMotor.setSetpoint(setpoint.speedMetersPerSecond);
+
+    this.setpoint = setpoint;
   }
 
   /**
@@ -92,6 +99,11 @@ public SwerveModuleState getState() {
         Rotation2d.fromRotations(steerMotorValues.positionRotations));
   }
 
+  @Override
+  public SwerveModuleState getSetpoint() {
+    return setpoint;
+  }
+
   @Override
   public SwerveModulePosition getPosition() {
     azimuthEncoder.update(azimuthEncoderValues);