added stator and supply current logging to spark and talonfx

lasarobotics · Feb 1, 2025 · 8f10739 · 8f10739
1 parent 345539d
commit 8f10739
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 25 deletions.
diff --git a/src/main/java/org/lasarobotics/hardware/ctre/TalonFX.java b/src/main/java/org/lasarobotics/hardware/ctre/TalonFX.java
@@ -80,6 +80,7 @@
 import edu.wpi.first.units.measure.Frequency;
 import edu.wpi.first.units.measure.MutAngle;
 import edu.wpi.first.units.measure.MutAngularVelocity;
+import edu.wpi.first.units.measure.MutCurrent;
 
 
 /** TalonFX */
@@ -112,13 +113,13 @@ public static class TalonFXInputs {
     public MutAngularVelocity rotorVelocity = Units.RadiansPerSecond.zero().mutableCopy();
     public MutAngle selectedSensorPosition = Units.Radians.zero().mutableCopy();
     public MutAngularVelocity selectedSensorVelocity = Units.RadiansPerSecond.zero().mutableCopy();
+    public MutCurrent supplyCurrent = Units.Amps.zero().mutableCopy();
+    public MutCurrent statorCurrent = Units.Amps.zero().mutableCopy();
   }
 
   private static final String VALUE1_LOG_ENTRY = "/OutputValue1";
   private static final String VALUE2_LOG_ENTRY = "/OutputValue2";
   private static final String MODE_LOG_ENTRY = "/OutputMode";
-  private static final String SUPPLY_CURRENT_LOG_ENTRY = "/SupplyCurrent";
-  private static final String STATOR_CURRENT_LOG_ENTRY = "/StatorCurrent";
 
   private com.ctre.phoenix6.hardware.TalonFX m_talon;
 
@@ -177,15 +178,14 @@ protected void updateInputs() {
       m_inputs.rotorVelocity.mut_replace(m_talon.getRotorVelocity().getValue());
       m_inputs.selectedSensorPosition.mut_replace(m_talon.getPosition().getValue());
       m_inputs.selectedSensorVelocity.mut_replace(m_talon.getVelocity().getValue());
+      m_inputs.supplyCurrent.mut_replace(m_talon.getSupplyCurrent().getValue());
+      m_inputs.statorCurrent.mut_replace(m_talon.getStatorCurrent().getValue());
     }
   }
 
   @Override
   protected void periodic() {
     synchronized (m_inputs) { Logger.processInputs(m_id.name, m_inputs); }
-
-    Logger.recordOutput(m_id.name + SUPPLY_CURRENT_LOG_ENTRY, m_talon.getSupplyCurrent().getValueAsDouble());
-    Logger.recordOutput(m_id.name + STATOR_CURRENT_LOG_ENTRY, m_talon.getStatorCurrent().getValueAsDouble());
   }
 
   @Override

diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
@@ -41,6 +41,7 @@
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Frequency;
+import edu.wpi.first.units.measure.MutCurrent;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.Timer;
@@ -120,6 +121,8 @@ public static class SparkInputs {
     public double absoluteEncoderVelocity = 0.0;
     public boolean forwardLimitSwitch = false;
     public boolean reverseLimitSwitch = false;
+    public MutCurrent supplyCurrent = Units.Amps.zero().mutableCopy();
+    public MutCurrent statorCurrent = Units.Amps.zero().mutableCopy();
   }
 
   private static final String LOG_TAG = "Spark";
@@ -136,8 +139,6 @@ public static class SparkInputs {
   private static final String ARB_FF_LOG_ENTRY = "/ArbitraryFF";
   private static final String ARB_FF_UNITS_LOG_ENTRY = "/ArbitraryFFUnits";
   private static final String IDLE_MODE_LOG_ENTRY = "/IdleMode";
-  private static final String INPUT_CURRENT_LOG_ENTRY = "/InputCurrent";
-  private static final String OUTPUT_CURRENT_LOG_ENTRY = "/OutputCurrent";
   private static final String HEALTH_STATUS_LOG_ENTRY = "/IsHealthy";
   private static final String TEMPERATURE_LOG_ENTRY = "/Temperature";
   private static final ClosedLoopSlot PID_SLOT = ClosedLoopSlot.kSlot0;
@@ -346,6 +347,22 @@ private boolean getReverseLimitSwitch() {
     return (RobotBase.isReal()) ? m_spark.getReverseLimitSwitch().isPressed() : m_sparkSim.getReverseLimitSwitchSim().getPressed();
   }
 
+  /**
+   * Spark approximate supply current
+   * @return Supply current
+   */
+  private Current getSupplyCurrent() {
+    return getStatorCurrent().times(m_spark.getAppliedOutput());
+  }
+
+  /**
+   * Spark stator  current
+   * @return stator current
+   */
+  private Current getStatorCurrent() {
+    return Units.Amps.of(m_spark.getOutputCurrent());
+  }
+
   /**
    * Update sensor input readings
    */
@@ -359,6 +376,8 @@ protected void updateInputs() {
       m_inputs.absoluteEncoderVelocity = getAbsoluteEncoderVelocity();
       m_inputs.forwardLimitSwitch = getForwardLimitSwitch();
       m_inputs.reverseLimitSwitch = getReverseLimitSwitch();
+      m_inputs.supplyCurrent.mut_replace(getSupplyCurrent());
+      m_inputs.statorCurrent.mut_replace(getStatorCurrent());
 
       // Get motor encoder
       if (!getMotorType().equals(MotorType.kBrushed)) {
@@ -371,9 +390,6 @@ protected void updateInputs() {
   @Override
   protected void periodic() {
     synchronized (m_inputs) { Logger.processInputs(m_id.name, m_inputs); }
-
-    Logger.recordOutput(m_id.name + INPUT_CURRENT_LOG_ENTRY, getInputCurrent());
-    Logger.recordOutput(m_id.name + OUTPUT_CURRENT_LOG_ENTRY, getOutputCurrent());
     Logger.recordOutput(m_id.name + HEALTH_STATUS_LOG_ENTRY, isHealthy());
 
     if (getMotorType() == MotorType.kBrushed) return;
@@ -750,21 +766,6 @@ public REVLibError follow(Spark leader) {
     }
   }
 
-  /**
-   * Spark approximate input current
-   * @return
-   */
-  public Current getInputCurrent() {
-    return getOutputCurrent().times(m_spark.getAppliedOutput());
-  }
-
-  /**
-   * Spark output current
-   * @return Output current
-   */
-  public Current getOutputCurrent() {
-  return Units.Amps.of(m_spark.getOutputCurrent());
-  }
 
   /**
    * Get applied output of Spark