diff --git a/custom/sensors/CANEncoder.java b/custom/sensors/CANEncoder.java index dbbe4803..2fecf6e7 100644 --- a/custom/sensors/CANEncoder.java +++ b/custom/sensors/CANEncoder.java @@ -164,12 +164,9 @@ public void reset() { } } - public void resetViaOffset(double setpoint) { - this.offset += setpoint - getDistance(); - } - - public void resetViaOffset() { - resetViaOffset(0.0); + @Override + public void setSensorValue(double setpoint) { + this.offset += setpoint - pidGet(); } @Override diff --git a/custom/sensors/CANTalonEncoder.java b/custom/sensors/CANTalonEncoder.java index 48ffd968..2a048144 100644 --- a/custom/sensors/CANTalonEncoder.java +++ b/custom/sensors/CANTalonEncoder.java @@ -11,15 +11,21 @@ public class CANTalonEncoder implements CustomEncoder { protected PIDSourceType pidSource; protected double distancePerPulse; protected boolean reverseDirection; + protected double offset ; - public CANTalonEncoder(String name, TalonSRX talon, boolean reverseDirection, double distancePerPulse) { + public CANTalonEncoder(String name, TalonSRX talon, boolean reverseDirection, double distancePerPulse, double offset) { this.talon = talon; talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 0); + this.offset = offset; this.reverseDirection = reverseDirection; this.distancePerPulse = distancePerPulse; setPIDSourceType(PIDSourceType.kDisplacement); } + public CANTalonEncoder(String name, TalonSRX talon, boolean reverseDirection, double distancePerPulse) { + this(name, talon, reverseDirection, distancePerPulse, 0.0); + } + public CANTalonEncoder(String name, TalonSRX talon, boolean reverseDirection) { this(name, talon, reverseDirection, 1.0); } @@ -70,9 +76,9 @@ public int get() { @Override public double getDistance() { if (reverseDirection) { - return distancePerPulse * talon.getSelectedSensorPosition(0) * -1.0; + return distancePerPulse * talon.getSelectedSensorPosition(0) * -1.0 + offset; } else { - return distancePerPulse * talon.getSelectedSensorPosition(0); + return distancePerPulse * talon.getSelectedSensorPosition(0) + offset; } } @@ -89,10 +95,10 @@ public boolean getStopped() { @Override public double getRate() { if (reverseDirection) { - return talon.getSelectedSensorVelocity(0) * -10.0 * distancePerPulse; + return talon.getSelectedSensorVelocity(0) * -10.0 * distancePerPulse + offset; // getSpeed must be converted from ticks to 100ms to ticks per second, so *10. } else { - return talon.getSelectedSensorVelocity(0) * 10.0 * distancePerPulse; + return talon.getSelectedSensorVelocity(0) * 10.0 * distancePerPulse + offset; } } @@ -121,6 +127,11 @@ public void reset() { talon.setSelectedSensorPosition(0, 0, 0); } + @Override + public void setSensorValue(double setpoint) { + this.offset += setpoint - pidGet(); + } + @Override public double pidGetSafely() { return pidGet(); diff --git a/custom/sensors/CustomDigitalEncoder.java b/custom/sensors/CustomDigitalEncoder.java index 30b49735..7efd957f 100644 --- a/custom/sensors/CustomDigitalEncoder.java +++ b/custom/sensors/CustomDigitalEncoder.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.CounterBase; import edu.wpi.first.wpilibj.DigitalSource; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PIDSourceType; /** * A RoboRIO encoder that implements @@ -13,6 +14,7 @@ public class CustomDigitalEncoder extends Encoder implements CustomEncoder { private double distancePerPulse; private boolean reverseDirection; + protected double offset = 0.0; public CustomDigitalEncoder(DigitalSource aSource, DigitalSource bSource) { super(aSource, bSource); @@ -80,17 +82,17 @@ public void setReverseDirection(boolean reverseDirection) { @Override public double pidGetSafely() { - return pidGet(); + return pidGet() + offset; } @Override public int getSafely() { - return get(); + return get() + (int) offset; } @Override public double getDistanceSafely() { - return getDistance(); + return getDistance() + offset; } @Override @@ -107,4 +109,8 @@ public boolean getStoppedSafely() { public double getRateSafely() { return getRate(); } + + public void setSensorValue(double setpoint) { + this.offset += setpoint - pidGetSafely(); + } } \ No newline at end of file diff --git a/custom/sensors/CustomEncoder.java b/custom/sensors/CustomEncoder.java index 8c0e9fc8..e29d81a8 100644 --- a/custom/sensors/CustomEncoder.java +++ b/custom/sensors/CustomEncoder.java @@ -80,4 +80,16 @@ public interface CustomEncoder extends PIDSensor, NativeDerivativeSensor { * Resets the encoder */ void reset(); + + /** + * Set the value from encoder get methods to a setpoint + */ + void setSensorValue(double setpoint); + + /** + * Set the value from encoder get methods 0.0, essentially resetting the encoder + */ + default void setSensorValue() { + setSensorValue(0.0); + } } \ No newline at end of file diff --git a/custom/sensors/EncoderPair.java b/custom/sensors/EncoderPair.java index a6003b0f..3c60ee24 100644 --- a/custom/sensors/EncoderPair.java +++ b/custom/sensors/EncoderPair.java @@ -23,7 +23,7 @@ public class EncoderPair implements CustomEncoder { private final double rateTolerance; protected static final double DEFAULT_DISTANCE_TOLERANCE = 10; protected static final double DEFAULT_RATE_TOLERANCE = 10; - + /** * Amalgamates the data of two encoders for the purpose * of controlling a single motion controller. @@ -33,11 +33,11 @@ public class EncoderPair implements CustomEncoder { * in the same direction with the same rate (before setDistancePerTick). * * @param encoders - * The encoders to amalgamate. + * The encoders to amalgamate. * @param distanceTolerance - * The distance by which the encoders can be different before isInSync() returns false + * The distance by which the encoders can be different before isInSync() returns false * @param rateTolerance - * The rate by which the encoders can be different before isInSync() returns false + * The rate by which the encoders can be different before isInSync() returns false */ public EncoderPair(CustomEncoder encoder1, CustomEncoder encoder2, double distanceTolerance, double rateTolerance) { encoders = new CustomEncoder[] {encoder1, encoder2}; @@ -47,7 +47,7 @@ public EncoderPair(CustomEncoder encoder1, CustomEncoder encoder2, double distan pidSource = PIDSourceType.kDisplacement; reverseDirection = false; } - + /** * Amalgamates the data of two encoders for the purpose * of controlling a single motion controller. @@ -57,17 +57,17 @@ public EncoderPair(CustomEncoder encoder1, CustomEncoder encoder2, double distan * in the same direction with the same rate (before setDistancePerTick). * * @param encoders - * The encoders to amalgamate. + * The encoders to amalgamate. */ public EncoderPair(CustomEncoder encoder1, CustomEncoder encoder2) { this(encoder1, encoder2, EncoderPair.DEFAULT_DISTANCE_TOLERANCE, EncoderPair.DEFAULT_RATE_TOLERANCE); } - + @Override public PIDSourceType getPIDSourceType() { return pidSource; } - + @Override public double pidGet() { if (pidSource == PIDSourceType.kDisplacement) { @@ -75,7 +75,7 @@ public double pidGet() { } return getRate(); } - + @Override public double pidGetSafely() throws InvalidSensorException { if (pidSource == PIDSourceType.kDisplacement) { @@ -83,19 +83,20 @@ public double pidGetSafely() throws InvalidSensorException { } return getRateSafely(); } - + @Override public void setPIDSourceType(PIDSourceType pidSource) { if (pidSource != null) { this.pidSource = pidSource; } } - + @Override public int getSafely() throws InvalidSensorException { - return (int) Math.round(((double) (encoders[0].getSafely() - offset[0]) + (double) (encoders[1].getSafely() - offset[1])) / 2.0); + return (int) Math + .round(((double) (encoders[0].getSafely() - offset[0]) + (double) (encoders[1].getSafely() - offset[1])) / 2.0); } - + @Override public int get() { try { @@ -106,12 +107,13 @@ public int get() { return 0; } } - + @Override public double getDistanceSafely() throws InvalidSensorException { - return ((encoders[0].getDistanceSafely() - offset[0] * encoders[0].getDistancePerPulse()) + (encoders[1].getDistanceSafely() - offset[1] * encoders[1].getDistancePerPulse())) / 2.0; + return ((encoders[0].getDistanceSafely() - offset[0] * encoders[0].getDistancePerPulse()) + + (encoders[1].getDistanceSafely() - offset[1] * encoders[1].getDistancePerPulse())) / 2.0; } - + @Override public double getDistance() { try { @@ -122,32 +124,32 @@ public double getDistance() { return 0; } } - + @Override public boolean getDirection() { return getRate() > 0; } - + @Override public boolean getDirectionSafely() throws InvalidSensorException { return getRateSafely() > 0; } - + @Override public boolean getStopped() { return Util.isZero(getRate()); } - + @Override public boolean getStoppedSafely() throws InvalidSensorException { return Util.isZero(getRateSafely()); } - + @Override public double getRateSafely() throws InvalidSensorException { return (encoders[0].getRateSafely() + encoders[1].getRateSafely()) / 2.0; } - + @Override public double getRate() { try { @@ -158,7 +160,7 @@ public double getRate() { return 0; } } - + /** * Get whether this entire encoder is inverted. * @@ -169,14 +171,14 @@ public double getRate() { public boolean getReverseDirection() { return reverseDirection; } - + /** * Sets the direction inversion of all encoder substituents. * This respects the original inversion state of each encoder when constructed, * and will only invert encoders if this.getReverseDirection() != the input. * * @param reverseDirection - * The state of inversion, true is inverted. + * The state of inversion, true is inverted. */ @Override public void setReverseDirection(boolean reverseDirection) { @@ -186,25 +188,31 @@ public void setReverseDirection(boolean reverseDirection) { } this.reverseDirection = reverseDirection; } - + @Override public double getDistancePerPulse() { return distancePerPulse; } - + @Override public void setDistancePerPulse(double distancePerPulse) { this.distancePerPulse = distancePerPulse; encoders[0].setDistancePerPulse(distancePerPulse); encoders[1].setDistancePerPulse(distancePerPulse); } - + @Override public void reset() { offset[0] = encoders[0].get(); offset[1] = encoders[1].get(); } - + + @Override + public void setSensorValue(double setpoint) { + offset[0] += setpoint - encoders[0].get(); + offset[1] += setpoint - encoders[1].get(); + } + public double getDifference() { try { return getDifferenceSafely(); @@ -214,11 +222,12 @@ public double getDifference() { return 0.0; // TODO: is this a reasonable default } } - + public double getDifferenceSafely() throws InvalidSensorException { - return (encoders[0].getDistanceSafely() - offset[0] * encoders[0].getDistancePerPulse()) - (encoders[1].getDistanceSafely() - offset[1] * encoders[1].getDistancePerPulse()); + return (encoders[0].getDistanceSafely() - offset[0] * encoders[0].getDistancePerPulse()) + - (encoders[1].getDistanceSafely() - offset[1] * encoders[1].getDistancePerPulse()); } - + public double getRateDifference() { try { return getRateDifferenceSafely(); @@ -228,11 +237,11 @@ public double getRateDifference() { return 0.0; // TODO: is this a reasonable default } } - + public double getRateDifferenceSafely() throws InvalidSensorException { return encoders[0].getRateSafely() - encoders[1].getRateSafely(); } - + public boolean isInSync() { try { return isInSyncSafely(); @@ -242,28 +251,28 @@ public boolean isInSync() { return false; // If a sensor is broken, it is not in sync. } } - + public boolean isInSyncSafely() throws InvalidSensorException { return Math.abs(getDifferenceSafely()) < distanceTolerance && Math.abs(getRateDifferenceSafely()) < rateTolerance; } - + public class EncoderDifference implements PIDSensor { private PIDSourceType pidSource; // Needs to be seperate in case of multiple threads changing source type - + public EncoderDifference() { pidSource = PIDSourceType.kDisplacement; } - + @Override public void setPIDSourceType(PIDSourceType pidSource) { this.pidSource = pidSource; } - + @Override public PIDSourceType getPIDSourceType() { return pidSource; } - + @Override public double pidGet() { try { @@ -274,7 +283,7 @@ public double pidGet() { return 0.0; } } - + @Override public double pidGetSafely() throws InvalidSensorException { if (pidSource == PIDSourceType.kDisplacement) {