From 37b9040cf9faac286b94fa2bb6c059a2ea8fd5fb Mon Sep 17 00:00:00 2001
From: Vignesh Balasubramaniam <vigneshb.hp@gmail.com>
Date: Thu, 12 Sep 2024 02:52:10 -0500
Subject: [PATCH] Refactor hardware monitoring

---
 .../hardware/LoggableHardware.java            |  2 +-
 .../lasarobotics/hardware/Monitorable.java    | 19 +++++----
 .../hardware/revrobotics/Spark.java           | 40 -------------------
 3 files changed, 12 insertions(+), 49 deletions(-)

diff --git a/src/main/java/org/lasarobotics/hardware/LoggableHardware.java b/src/main/java/org/lasarobotics/hardware/LoggableHardware.java
index 72ea48d9..8ae5688d 100644
--- a/src/main/java/org/lasarobotics/hardware/LoggableHardware.java
+++ b/src/main/java/org/lasarobotics/hardware/LoggableHardware.java
@@ -6,7 +6,7 @@
 
 import org.littletonrobotics.junction.inputs.LoggableInputs;
 
-public abstract class LoggableHardware implements Monitorable, AutoCloseable {
+public abstract class LoggableHardware extends Monitorable implements AutoCloseable {
   /**
    * Call this method periodically
    */
diff --git a/src/main/java/org/lasarobotics/hardware/Monitorable.java b/src/main/java/org/lasarobotics/hardware/Monitorable.java
index e2f1fa36..1433d15f 100644
--- a/src/main/java/org/lasarobotics/hardware/Monitorable.java
+++ b/src/main/java/org/lasarobotics/hardware/Monitorable.java
@@ -4,15 +4,16 @@
 
 package org.lasarobotics.hardware;
 
-public interface Monitorable {
-  static final int DEFAULT_RETRIES = 3;
+public abstract class Monitorable {
+  private static final int DEFAULT_RETRIES = 3;
+  private int m_errorCount = 0;
 
   /**
    * Return true if the component is healthy. Some components may not have this capability, so a
    * default of returning true is used.
    * @return True if the component is connected.
    */
-  public default boolean isHealthy() {
+  public boolean isHealthy() {
     return true;
   }
 
@@ -21,7 +22,7 @@ public default boolean isHealthy() {
    * Method to call to re-initialize component
    * @return True if component re-initialized successfully
    */
-  public default boolean reinit() {
+  public boolean reinit() {
     return true;
   }
 
@@ -31,20 +32,22 @@ public default boolean reinit() {
    * Defaults to {@value Monitorable#DEFAULT_RETRIES}
    * @return Number of retries
    */
-  public default int getMaxRetries() {
+  public int getMaxRetries() {
     return DEFAULT_RETRIES;
   }
 
   /**
    * Save number of errors that have occured
    */
-  public default void setErrorCount(int num) {};
+  public void setErrorCount(int num) {
+    m_errorCount = num;
+  };
 
   /**
    * Get number of failures that have occured
    * @return Number of failures
    */
-  public default int getErrorCount() {
-    return 0;
+  public int getErrorCount() {
+    return m_errorCount;
   }
 }
diff --git a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
index 6e53cfc0..8998a76b 100644
--- a/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
+++ b/src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
@@ -165,7 +165,6 @@ public static class SparkInputs {
   private Notifier m_inputThread;
   private Measure<Time> m_inputThreadPeriod;
 
-  private int m_errorCount;
   private boolean m_isSmoothMotionEnabled;
   private Debouncer m_smoothMotionFinishedDebouncer;
   private TrapezoidProfile.State m_desiredState;
@@ -211,7 +210,6 @@ public Spark(ID id, MotorKind kind, SparkLimitSwitch.Type limitSwitchType, Measu
     this.m_inputThread = new Notifier(this::updateInputs);
     this.m_inputThreadPeriod = inputThreadPeriod;
     this.m_isSmoothMotionEnabled = false;
-    this.m_errorCount = 0;
     this.m_limitSwitchType = limitSwitchType;
     this.m_parameterChain = new LinkedHashSet<>();
     this.m_invertedRunner = () -> {};
@@ -594,15 +592,6 @@ private void handleSmoothMotion() {
     );
 
     m_isSmoothMotionEnabled = !isSmoothMotionFinished();
-
-    if (!m_isSmoothMotionEnabled) {
-      set(
-        m_desiredState.position,
-        ControlType.kPosition,
-        m_feedforwardSupplier.apply(m_desiredState),
-        SparkPIDController.ArbFFUnits.kVoltage
-      );
-    }
   }
 
   /**
@@ -630,19 +619,11 @@ public SparkInputsAutoLogged getInputs() {
     synchronized (m_inputs) { return m_inputs; }
   }
 
-  /**
-   * Get if Spark is healthy
-   * <p>
-   * Checks if Spark has NOT reset
-   */
   @Override
   public boolean isHealthy() {
     return !m_spark.getStickyFault(FaultID.kHasReset);
   }
 
-  /**
-   * Method to re-initialize Spark after reset
-   */
   @Override
   public boolean reinit() {
     boolean success = true;
@@ -654,32 +635,11 @@ public boolean reinit() {
     return success;
   }
 
-  /**
-   * Get maximum number of times to try to re-initialize Spark
-   */
   @Override
   public int getMaxRetries() {
     return MAX_ATTEMPTS;
   }
 
-  /**
-   * Save number of errors that have occurred
-   * @param num Number of errors
-   */
-  @Override
-  public void setErrorCount(int num) {
-    m_errorCount = num;
-  }
-
-  /**
-   * Get number of failures that have occured
-   * @return number of failures
-   */
-  @Override
-  public int getErrorCount() {
-    return m_errorCount;
-  }
-
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("PIDController");