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 2f1dd274c..8b37e2d5e 100644 --- a/src/main/java/com/team766/hal/wpilib/CANcoderEncoder.java +++ b/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 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; + } } diff --git a/src/main/java/com/team766/hal/wpilib/REVThroughBoreDutyCycleEncoder.java b/src/main/java/com/team766/hal/wpilib/REVThroughBoreDutyCycleEncoder.java index 2b64b2805..477ddb091 100644 --- a/src/main/java/com/team766/hal/wpilib/REVThroughBoreDutyCycleEncoder.java +++ b/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 diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java index 47f4647e6..63f92d896 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java +++ b/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());