diff --git a/src/main/java/org/lasarobotics/hardware/ctre/CANcoder.java b/src/main/java/org/lasarobotics/hardware/ctre/CANcoder.java index 899520b..1c230e0 100644 --- a/src/main/java/org/lasarobotics/hardware/ctre/CANcoder.java +++ b/src/main/java/org/lasarobotics/hardware/ctre/CANcoder.java @@ -12,6 +12,7 @@ import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CANcoderConfigurator; +import com.ctre.phoenix6.signals.MagnetHealthValue; import com.ctre.phoenix6.sim.CANcoderSimState; import edu.wpi.first.units.Units; @@ -150,6 +151,11 @@ public CANcoderInputsAutoLogged getInputs() { synchronized (m_inputs) { return m_inputs; } } + @Override + public boolean isHealthy() { + return getMagnetHealth().equals(MagnetHealthValue.Magnet_Green); + } + /** * Get device ID * @return Device ID @@ -208,6 +214,21 @@ public CANcoderSimState getSimState() { return m_canCoder.getSimState(); } + /** + * Magnet health as measured by CANcoder. + *
+ * Red indicates too close or too far, Orange is adequate but with + * reduced accuracy, green is ideal. Invalid means the accuracy cannot + * be determined. + * + * This refreshes and returns a cached object. + * + * @return MagnetHealth Object + */ + public MagnetHealthValue getMagnetHealth() { + return m_canCoder.getMagnetHealth().getValue(); + } + /** * Sets the period of the given status frame. *