added setSensorValue to customEncoder

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

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();

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

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