WIP.

.Glass/glass.json

@@ -0,0 +1,29 @@
+{
+  "NetworkTables": {
+    "transitory": {
+      "Shuffleboard": {
+        "Swerve": {
+          "Module 0": {
+            "open": true
+          },
+          "open": true
+        },
+        "open": true
+      },
+      "steerMotorIOTalonFX": {
+        "open": true
+      }
+    },
+    "types": {
+      "/FMSInfo": "FMSInfo",
+      "/Shuffleboard/Auto/Auto Chooser": "String Chooser"
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  },
+  "NetworkTables Settings": {
+    "mode": "Client (NT4)",
+    "serverTeam": "5112"
+  }
+}

simgui.json

@@ -13,6 +13,9 @@
     }
   },
   "NetworkTables": {
+    "Retained Values": {
+      "open": false
+    },
     "transitory": {
       "Shuffleboard": {
         "Swerve": {
@@ -30,8 +33,7 @@
           },
           "North East Module": {
             "open": true
-          },
-          "open": true
+          }
         },
         "open": true
       },

src/main/java/frc/robot/odometry/GyroscopeIOPigeon2.java

@@ -42,9 +42,9 @@ public void update(GyroscopeIOValues values) {
     pitch.refresh();
     yaw.refresh();
 
-    values.rollRotations = roll.getValue();
-    values.pitchRotations = pitch.getValue();
-    values.yawRotations = yaw.getValue();
+    values.rollRotations = Units.degreesToRotations(roll.getValue());
+    values.pitchRotations = Units.degreesToRotations(pitch.getValue());
+    values.yawRotations = Units.degreesToRotations(yaw.getValue());
   }
 
   @Override

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

@@ -3,6 +3,11 @@
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.networktables.BooleanEntry;
+import edu.wpi.first.networktables.DoubleEntry;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import frc.lib.CAN;
 
 /** TalonFX steer motor. */
@@ -17,6 +22,10 @@ public abstract class SteerMotorIOTalonFX implements SteerMotorIO {
   /** CAN identifier for the steer motor's corresponding azimuth encoder. */
   protected final CAN azimuthEncoderCAN;
 
+  /** NetworkTable entry for setting voltage. */
+  protected final DoubleEntry setVoltageEntry;
+  protected final BooleanEntry setVoltageEnableEntry;
+
   /**
    * Creates a new TalonFX steer motor.
    *
@@ -31,6 +40,20 @@ public SteerMotorIOTalonFX(CAN steerMotorCAN, CAN azimuthEncoderCAN) {
     velocityRotationsPerSecond = talonFX.getVelocity();
 
     this.azimuthEncoderCAN = azimuthEncoderCAN;
+
+    NetworkTable table = NetworkTableInstance.getDefault().getTable("steerMotorIOTalonFX");
+
+    setVoltageEntry =
+            table.getDoubleTopic("setVoltage")
+            .getEntry(0);
+
+    setVoltageEntry.set(0);
+
+    setVoltageEnableEntry =
+            table.getBooleanTopic("setVoltageEnable")
+            .getEntry(false);
+
+    setVoltageEnableEntry.set(false);
   }
 
   @Override

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

@@ -46,12 +46,16 @@ public void configure() {
 
   @Override
   public void setSetpoint(double positionRotations) {
-    positionFeedback.setSetpoint(positionRotations);
-
-    if (positionFeedback.atSetpoint()) {
-      talonFX.setControl(new CoastOut());
+    if (setVoltageEnableEntry.get()) {
+      talonFX.setControl(new VoltageOut(setVoltageEntry.get()));
     } else {
-      talonFX.setControl(calculatePositionVoltage());
+      positionFeedback.setSetpoint(positionRotations);
+
+      if (positionFeedback.atSetpoint()) {
+        talonFX.setControl(new CoastOut());
+      } else {
+        talonFX.setControl(calculatePositionVoltage());
+      }
     }
   }
 

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

@@ -69,6 +69,9 @@ public void addToShuffleboard(ShuffleboardTab tab) {
       swerveModuleColumn.addDouble("Angle (deg)", () -> swerveModule.getState().angle.getDegrees());
       swerveModuleColumn.addDouble(
           "Velocity (mps)", () -> swerveModule.getState().speedMetersPerSecond);
+      swerveModuleColumn.addDouble("Setpoint Angle (deg)", () -> swerveModule.getSetpoint().angle.getDegrees());
+      swerveModuleColumn.addDouble(
+          "Setpoint Velocity (mps)", () -> swerveModule.getSetpoint().speedMetersPerSecond);
     }
   }
 

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

@@ -71,7 +71,7 @@ public static class MK4iConstants {
   /** Constants for steer feedback controllers. */
   public static class SteerMotorConstants {
     /** Feedback tolerance. */
-    public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(1.0);
+    public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(3);
 
     /** Feedback proportional gain in volts per rotation. */
     public static final double FEEDBACK_KP = 32.0;

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

@@ -94,7 +94,16 @@ public void setSetpoint(SwerveModuleState setpoint, boolean lazy) {
    * @return the optimized setpoint.
    */
   private SwerveModuleState optimize(SwerveModuleState setpoint, SwerveModuleState state) {
-    return SwerveModuleState.optimize(setpoint, state.angle);
+    setpoint = SwerveModuleState.optimize(setpoint, state.angle);
+
+    final double kDejitterThreshold = 0.01;
+
+    if (Math.abs(setpoint.speedMetersPerSecond) < kDejitterThreshold) {
+      setpoint.angle = state.angle;
+      setpoint.speedMetersPerSecond = 0.0;
+    }
+
+    return setpoint;
   }
 
   @Override