Fix: A whole lot of things with onboard pid

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
     id "com.peterabeles.gversion" version "1.10"
     id "org.sonarqube" version "4.4.1.3373"
 }

src/main/java/frc/robot/RobotContainer.java

@@ -59,10 +59,10 @@ public RobotContainer() {
           // Real robot, instantiate hardware IO implementations
           m_driveSubsystem = new DriveSubsystem(
             new GyroIOPigeon1(12),
-              new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS),
-              new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS),
-              new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS),
-              new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS));
+            new ModuleIOTalonFX(Constants.DriveConstants.FL_MOD_CONSTANTS),
+            new ModuleIOTalonFX(Constants.DriveConstants.FR_MOD_CONSTANTS),
+            new ModuleIOTalonFX(Constants.DriveConstants.BL_MOD_CONSTANTS),
+            new ModuleIOTalonFX(Constants.DriveConstants.BR_MOD_CONSTANTS));
           m_shooter = new ShooterSubsystem(new ShooterIOPrototype());
         }
     case SIM -> {

src/main/java/frc/robot/subsystems/drive/module/Module.java

@@ -90,7 +90,7 @@ public void periodic() {
 
     // Run closed loop turn control
     if (m_angleSetpoint != null) {
-      m_io.setTurnPositionDegs(m_angleSetpoint.getRotations());
+      m_io.setTurnPositionDegs(m_angleSetpoint.minus(m_turnRelativeOffset).getRotations());
 
       // Run closed loop drive control
       // Only allowed if closed loop turn control is running

src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java

@@ -28,10 +28,10 @@
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import lib.properties.phoenix6.Phoenix6PidPropertyBuilder;
 import lib.properties.phoenix6.PidPropertyPublic;
 import org.littletonrobotics.junction.Logger;
-import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
 
 import java.util.Queue;
 
@@ -52,8 +52,8 @@ public class ModuleIOTalonFX implements ModuleIO {
   private final TalonFX m_driveTalon;
   private final TalonFX m_turnTalon;
 
-  private final PidPropertyPublic m_drivePid;
-  private final PidPropertyPublic m_turnPid;
+  private final Phoenix6PidPropertyBuilder m_drivePid;
+  private final Phoenix6PidPropertyBuilder m_turnPid;
 
   private final ModuleConstants m_moduleConstants;
 
@@ -69,6 +69,9 @@ public class ModuleIOTalonFX implements ModuleIO {
   private final StatusSignal<Double> m_turnVelocity;
   private final StatusSignal<Double> m_turnAppliedVolts;
   private final StatusSignal<Double> m_turnCurrent;
+  private final StatusSignal<Double> m_turnClosedLoopOutput;
+  private final StatusSignal<Double> m_turnClosedLoopError;
+  private final StatusSignal<Double> m_turnClosedLoopReference;
 
 
   PositionVoltage m_posRequest;
@@ -99,8 +102,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
             .addI(m_moduleConstants.DRIVE_KI())
             .addD(m_moduleConstants.DRIVE_KD())
             .addKV(m_moduleConstants.DRIVE_KV())
-            .addKS(m_moduleConstants.DRIVE_KS())
-            .build();
+            .addKS(m_moduleConstants.DRIVE_KS());
 
     // run configs on turning motor
     var turnConfig = new TalonFXConfiguration();
@@ -110,7 +112,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
             moduleConstants.TURN_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive
                     : InvertedValue.CounterClockwise_Positive;
     turnConfig.ClosedLoopGeneral.ContinuousWrap = true;
-    turnConfig.Feedback.SensorToMechanismRatio = -m_moduleConstants.TURNING_GEAR_RATIO();
+    turnConfig.Feedback.SensorToMechanismRatio = m_moduleConstants.TURNING_GEAR_RATIO();
     m_turnTalon.getConfigurator().apply(turnConfig);
     setTurnBrakeMode(true);
 
@@ -122,8 +124,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
                     0)
             .addP(m_moduleConstants.TURN_KP())
             .addI(m_moduleConstants.TURN_KI())
-            .addD(m_moduleConstants.TURN_KD())
-            .build();
+            .addD(m_moduleConstants.TURN_KD());
 
     m_posRequest = new PositionVoltage(0, 0, false, 0, 0, false, false, false);
 
@@ -155,6 +156,10 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
     m_turnAppliedVolts = m_turnTalon.getMotorVoltage();
     m_turnCurrent = m_turnTalon.getStatorCurrent();
 
+    m_turnClosedLoopOutput = m_turnTalon.getClosedLoopOutput();
+    m_turnClosedLoopError = m_turnTalon.getClosedLoopError();
+    m_turnClosedLoopReference = m_turnTalon.getClosedLoopReference();
+
     // setup refresh rates on all inputs
     BaseStatusSignal.setUpdateFrequencyForAll(
         Module.ODOMETRY_FREQUENCY, m_drivePosition, m_turnPosition);
@@ -166,7 +171,10 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
         m_turnAbsolutePosition,
         m_turnVelocity,
         m_turnAppliedVolts,
-        m_turnCurrent);
+        m_turnCurrent,
+        m_turnClosedLoopOutput,
+        m_turnClosedLoopError,
+        m_turnClosedLoopReference);
     m_driveTalon.optimizeBusUtilization();
     m_turnTalon.optimizeBusUtilization();
   }
@@ -182,7 +190,10 @@ public void updateInputs(ModuleIOInputs inputs) {
             m_turnPosition,
             m_turnVelocity,
             m_turnAppliedVolts,
-            m_turnCurrent);
+            m_turnCurrent,
+            m_turnClosedLoopOutput,
+            m_turnClosedLoopError,
+            m_turnClosedLoopReference);
 
     m_drivePid.updateIfChanged();
     m_turnPid.updateIfChanged();
@@ -212,12 +223,15 @@ public void updateInputs(ModuleIOInputs inputs) {
     m_drivePositionQueue.clear();
     m_turnPositionQueue.clear();
 
-    Logger.recordOutput("Module " + m_moduleConstants.MODULE_INDEX() +
-            "/Closed Loop Output", m_turnTalon.getClosedLoopOutput().refresh().getValueAsDouble());
-    Logger.recordOutput("Module " + m_moduleConstants.MODULE_INDEX() +
-            "/Closed Loop Error", m_turnTalon.getClosedLoopError().refresh().getValueAsDouble());
-    Logger.recordOutput("Module " + m_moduleConstants.MODULE_INDEX() +
-            "/Closed Loop Reference", m_turnTalon.getClosedLoopReference().refresh().getValueAsDouble());
+    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
+            "/Closed Loop Output", m_turnClosedLoopOutput.getValueAsDouble());
+    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
+            "/Closed Loop Error", m_turnClosedLoopError.getValueAsDouble());
+    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
+            "/Closed Loop Reference", m_turnClosedLoopReference.getValueAsDouble());
+
+    SmartDashboard.putNumber("Module " + m_moduleConstants.MODULE_INDEX() +
+            "/Recorded kP", m_turnPid.getSlotConfigs().kP);
   }
 
   @Override

src/main/java/lib/properties/phoenix6/Phoenix6PidPropertyBuilder.java

@@ -6,6 +6,7 @@
 import com.ctre.phoenix6.configs.SlotConfigs;
 import com.ctre.phoenix6.hardware.core.CoreTalonFX;
 import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.gos.lib.properties.BaseHeavyProperty;
 import com.gos.lib.properties.GosDoubleProperty;
 import com.gos.lib.properties.HeavyDoubleProperty;
 
@@ -105,6 +106,17 @@ public Phoenix6PidPropertyBuilder addKA(double defaultValue) {
     return this;
   }
 
+  public void updateIfChanged(boolean forceUpdate) {
+    m_props.forEach((HeavyDoubleProperty prop) -> prop.updateIfChanged(forceUpdate));
+  }
+
+  public void updateIfChanged() {
+    updateIfChanged(false);
+  }
+
+  public SlotConfigs getSlotConfigs() {
+    return m_slot;
+  }
   public PidPropertyPublic build() {
     return new PidPropertyPublic(m_props);
   }

vendordeps/AdvantageKit.json

@@ -1,7 +1,7 @@
 {
   "fileName": "AdvantageKit.json",
   "name": "AdvantageKit",
-  "version": "3.0.0",
+  "version": "3.0.2",
   "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
   "frcYear": "2024",
   "mavenUrls": [],
@@ -10,24 +10,24 @@
     {
       "groupId": "org.littletonrobotics.akit.junction",
       "artifactId": "wpilib-shim",
-      "version": "3.0.0"
+      "version": "3.0.2"
     },
     {
       "groupId": "org.littletonrobotics.akit.junction",
       "artifactId": "junction-core",
-      "version": "3.0.0"
+      "version": "3.0.2"
     },
     {
       "groupId": "org.littletonrobotics.akit.conduit",
       "artifactId": "conduit-api",
-      "version": "3.0.0"
+      "version": "3.0.2"
     }
   ],
   "jniDependencies": [
     {
       "groupId": "org.littletonrobotics.akit.conduit",
       "artifactId": "conduit-wpilibio",
-      "version": "3.0.0",
+      "version": "3.0.2",
       "skipInvalidPlatforms": false,
       "isJar": false,
       "validPlatforms": [
@@ -39,4 +39,4 @@
     }
   ],
   "cppDependencies": []
-}
+}