Add isConnected method to encoders

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.
      *

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

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;

src/main/java/com/team766/hal/wpilib/CANcoderEncoder.java

@@ -22,6 +22,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<Angle> position = cancoder.getPosition();

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;
+    }
 }

src/main/java/com/team766/hal/wpilib/REVThroughBoreDutyCycleEncoder.java

@@ -10,6 +10,7 @@ public class REVThroughBoreDutyCycleEncoder extends DutyCycleEncoder implements
     public REVThroughBoreDutyCycleEncoder(int channel) {
         super(channel);
         setDutyCycleRange(1. / 1025., 1024. / 1025.);
+        setConnectedFrequencyThreshold(975 /* 975.6 on spec sheet */);
     }
 
     @Override

src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java

@@ -40,10 +40,9 @@ public double getAngle() {
 
     private double targetAngle;
     private static final double NUDGE_AMOUNT = 1; // degrees
-    private static final double ENCODER_INITIALIZATION_LOOPS = 350;
 
     private final REVThroughBoreDutyCycleEncoder absoluteEncoder;
-    private int encoderInitializationCount = 0;
+    private boolean encoderInitialized = false;
     private static final double SUPPLY_CURRENT_LIMIT = 30.0; // max efficiency from spec sheet
     private static final double STATOR_CURRENT_LIMIT = 80.0; // TUNE THIS!
     private static final double DEFAULT_POSITION = 77.0;
@@ -141,15 +140,13 @@ public boolean isFinished() {
     public void run() {
         // encoder takes some time to settle.
         // this threshold was determined very scientifically around 3:20am.
-        if (encoderInitializationCount < ENCODER_INITIALIZATION_LOOPS
-                && absoluteEncoder.isConnected()) {
+        if (!encoderInitialized && absoluteEncoder.isConnected()) {
             double absPos = absoluteEncoder.get() - 0.071;
             double convertedPos = absoluteEncoderToMotorRotations(absPos);
-            // TODO: only set the sensor position after this has settled?
-            // can try in the next round of testing.
             leftMotor.setSensorPosition(convertedPos);
-            encoderInitializationCount++;
+            encoderInitialized = true;
         }
+
         SmartDashboard.putNumber("[SHOULDER] Angle", getAngle());
         SmartDashboard.putNumber("[SHOULDER] Target Angle", targetAngle);
         // SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations());