diff --git a/src/main/java/com/team766/hal/wpilib/REVThroughBoreDutyCycleEncoder.java b/src/main/java/com/team766/hal/wpilib/REVThroughBoreDutyCycleEncoder.java index 06b5bf375..890d3a5a0 100644 --- a/src/main/java/com/team766/hal/wpilib/REVThroughBoreDutyCycleEncoder.java +++ b/src/main/java/com/team766/hal/wpilib/REVThroughBoreDutyCycleEncoder.java @@ -8,6 +8,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 84a92ffa0..a28b3831b 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java @@ -38,10 +38,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! @@ -138,21 +137,21 @@ 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.getPosition(); 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()); SmartDashboard.putNumber("[SHOULDER] Target Rotations", targetRotations); SmartDashboard.putNumber( "[SHOULDER] Absolute Encoder Position", getAbsoluteEncoderPosition()); + SmartDashboard.putNumber( + "[SHOULDER] Absolute Encoder Frequency", absoluteEncoder.getFrequency()); SmartDashboard.putNumber( "[SHOULDER] Left Motor Supply Current", MotorUtil.getCurrentUsage(leftMotor)); SmartDashboard.putNumber(