Merge pull request #2 from grt192/stats-update

Stats update: Publishing to NT and using Phoenix 6 Signals
grt192 · Jan 12, 2025 · d4985f0 · d4985f0
2 parents 250bad3 + ecbc7a0
commit d4985f0
Show file tree

Hide file tree

Showing 2 changed files with 92 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/KrakenDriveMotor.java
@@ -1,6 +1,8 @@
 package frc.robot.subsystems.swerve;
 
+import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -9,6 +11,13 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
 
 public class KrakenDriveMotor {
 
@@ -18,6 +27,21 @@ public class KrakenDriveMotor {
 
     private final TalonFXConfiguration motorConfig = new TalonFXConfiguration();
 
+    private NetworkTableInstance ntInstance;
+    private NetworkTable swerveStatsTable;
+    private NetworkTableEntry deviceTempEntry;
+    private NetworkTableEntry processorTempEntry;
+    private NetworkTableEntry ampDrawEntry;
+    private NetworkTableEntry statorCurrentEntry;
+    private NetworkTableEntry supplyCurrentEntry;
+    private NetworkTableEntry supplyVoltageEntry;
+
+    private StatusSignal<Angle> positionSignal;
+    private StatusSignal<AngularVelocity> velocitySignal;
+    private StatusSignal<Voltage> appliedVoltsSignal;
+    private StatusSignal<Current> supplyCurrentSignal;
+    private StatusSignal<Current> statorCurrentSignal; //torqueCurrent is Pro
+
     /** A kraken drive motor for swerve.
      *
      * @param canId The canId of the motor.
@@ -36,8 +60,51 @@ public KrakenDriveMotor(int canId) {
         for (int i = 0; i < 4; i++) {
             boolean error = motor.getConfigurator().apply(motorConfig, 0.1) == StatusCode.OK;
             if (!error) break;
+        }
+
+        initNT(canId);
+        initSignals();
+    }
+
+    /**
+     * initializes network table and entries
+     * @param canId motor's CAN ID
+     */
+    private void initNT(int canId){
+        ntInstance = NetworkTableInstance.getDefault();
+        swerveStatsTable = ntInstance.getTable("swerveStats");
+        deviceTempEntry = swerveStatsTable.getEntry(canId + "deviceTemp");
+        processorTempEntry = swerveStatsTable.getEntry(canId + "processorTemp");
+        ampDrawEntry = swerveStatsTable.getEntry(canId + "ampDraw");
+        statorCurrentEntry = swerveStatsTable.getEntry(canId + "statorCurrent");
+        supplyCurrentEntry = swerveStatsTable.getEntry(canId + "supplyCurrena");
+        supplyVoltageEntry = swerveStatsTable.getEntry(canId + "supplyVoltage");
     }
 
+    private void initSignals(){
+        positionSignal = motor.getPosition();
+        velocitySignal = motor.getVelocity();
+        appliedVoltsSignal = motor.getMotorVoltage();
+        statorCurrentSignal = motor.getStatorCurrent();
+        supplyCurrentSignal = motor.getSupplyCurrent();
+
+        BaseStatusSignal.setUpdateFrequencyForAll(
+            250.0, positionSignal, velocitySignal,
+            appliedVoltsSignal, statorCurrentSignal, supplyCurrentSignal
+        );
+        motor.optimizeBusUtilization(0, 1.0);
+    }
+
+    /**
+     * updates motor stats to network tables
+     */
+    public void updateNT(){
+        deviceTempEntry.setDouble(getDeviceTemp());
+        processorTempEntry.setDouble(getProcessorTemp());
+        ampDrawEntry.setDouble(getAmpDraw());
+        statorCurrentEntry.setDouble(getStatorCurrent());
+        supplyCurrentEntry.setDouble(getSupplyCurrent());
+        supplyVoltageEntry.setDouble(getSupplyVoltage());
     }
 
     public void setVelocity(double metersPerSec) {
@@ -79,4 +146,25 @@ public double getSetpoint() {
     public double getAmpDraw() {
         return motor.getSupplyCurrent().getValueAsDouble();
     }
+
+    public double getDeviceTemp() {
+        return motor.getDeviceTemp().getValueAsDouble();
+    }
+
+    public double getProcessorTemp() {
+        return motor.getProcessorTemp().getValueAsDouble();
+    }
+
+    public double getStatorCurrent() {
+        return motor.getStatorCurrent().getValueAsDouble();
+    }
+
+    public double getSupplyCurrent() {
+        return motor.getSupplyCurrent().getValueAsDouble();
+    }
+
+    public double getSupplyVoltage() {
+        return motor.getSupplyVoltage().getValueAsDouble();
+    }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
@@ -169,4 +169,8 @@ public double getDistanceDriven() {
     public double getDriveVelocity() {
         return driveMotor.getVelocity();
     }
+
+    public void updateNT(){
+        driveMotor.updateNT();
+    }
 }