From bc6402592622f15b2ced7ea0fd442c28d839e796 Mon Sep 17 00:00:00 2001
From: Ryan Cahoon <ryan.m.cahoon@gmail.com>
Date: Mon, 29 Jul 2024 22:40:43 -0700
Subject: [PATCH] Add isConnected method to encoders

---
 src/main/java/com/team766/hal/EncoderReader.java     |  5 +++++
 src/main/java/com/team766/hal/mock/MockEncoder.java  | 12 ++++++++++++
 src/main/java/com/team766/hal/simulator/Encoder.java |  5 +++++
 .../java/com/team766/hal/wpilib/CANcoderEncoder.java |  5 +++++
 src/main/java/com/team766/hal/wpilib/Encoder.java    |  5 +++++
 5 files changed, 32 insertions(+)

diff --git a/src/main/java/com/team766/hal/EncoderReader.java b/src/main/java/com/team766/hal/EncoderReader.java
index 5a9179cea..697359239 100755
--- a/src/main/java/com/team766/hal/EncoderReader.java
+++ b/src/main/java/com/team766/hal/EncoderReader.java
@@ -12,6 +12,11 @@ enum Type {
      */
     void reset();
 
+    /**
+     * Return true iff the encoder's readings are up-to-date.
+     */
+    boolean isConnected();
+
     /**
      * Get the distance the robot has driven since the last reset.
      *
diff --git a/src/main/java/com/team766/hal/mock/MockEncoder.java b/src/main/java/com/team766/hal/mock/MockEncoder.java
index 8bd06cda5..55b56548d 100755
--- a/src/main/java/com/team766/hal/mock/MockEncoder.java
+++ b/src/main/java/com/team766/hal/mock/MockEncoder.java
@@ -7,6 +7,7 @@ public class MockEncoder implements EncoderReader {
     private double distance = 0;
     private double rate = 0;
     private double distancePerPulse = 1;
+    private boolean isConnected = false;
 
     public MockEncoder() {}
 
@@ -15,6 +16,11 @@ public void reset() {
         distance = 0;
     }
 
+    @Override
+    public boolean isConnected() {
+        return isConnected;
+    }
+
     @Override
     public double getDistance() {
         return this.distance;
@@ -27,10 +33,16 @@ public double getRate() {
 
     public void setDistance(final double distance_) {
         this.distance = distance_;
+        this.isConnected = true;
     }
 
     public void setRate(final double rate_) {
         this.rate = rate_;
+        this.isConnected = true;
+    }
+
+    public void disconnect() {
+        isConnected = false;
     }
 
     @Override
diff --git a/src/main/java/com/team766/hal/simulator/Encoder.java b/src/main/java/com/team766/hal/simulator/Encoder.java
index c3502c270..9759b77ff 100755
--- a/src/main/java/com/team766/hal/simulator/Encoder.java
+++ b/src/main/java/com/team766/hal/simulator/Encoder.java
@@ -17,6 +17,11 @@ public void reset() {
         set(0);
     }
 
+    @Override
+    public boolean isConnected() {
+        return true;
+    }
+
     @Override
     public double getDistance() {
         int distance = (int) ProgramInterface.encoderChannels[channel].distance;
diff --git a/src/main/java/com/team766/hal/wpilib/CANcoderEncoder.java b/src/main/java/com/team766/hal/wpilib/CANcoderEncoder.java
index ab1fe23df..b6d700ebb 100644
--- a/src/main/java/com/team766/hal/wpilib/CANcoderEncoder.java
+++ b/src/main/java/com/team766/hal/wpilib/CANcoderEncoder.java
@@ -20,6 +20,11 @@ public CANcoderEncoder(int deviceId, String canBus) {
         cancoder = new CANcoder(deviceId, canBus);
     }
 
+    @Override
+    public boolean isConnected() {
+        return cancoder.getPosition().getStatus().isOK();
+    }
+
     @Override
     public double getDistance() {
         StatusSignal<Double> position = cancoder.getPosition();
diff --git a/src/main/java/com/team766/hal/wpilib/Encoder.java b/src/main/java/com/team766/hal/wpilib/Encoder.java
index 1718249d8..e723ca891 100755
--- a/src/main/java/com/team766/hal/wpilib/Encoder.java
+++ b/src/main/java/com/team766/hal/wpilib/Encoder.java
@@ -6,4 +6,9 @@ public class Encoder extends edu.wpi.first.wpilibj.Encoder implements EncoderRea
     public Encoder(final int channelA, final int channelB) {
         super(channelA, channelB);
     }
+
+    @Override
+    public boolean isConnected() {
+        return true;
+    }
 }