diff --git a/src/main/java/org/murraybridgebunyips/bunyipslib/Encoder.java b/src/main/java/org/murraybridgebunyips/bunyipslib/Encoder.java new file mode 100644 index 000000000..f8e2477ce --- /dev/null +++ b/src/main/java/org/murraybridgebunyips/bunyipslib/Encoder.java @@ -0,0 +1,106 @@ +package org.murraybridgebunyips.bunyipslib; + +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import java.util.function.Supplier; + +/** + * Represents a motor or external encoder that takes in suppliers of position and velocity to calculate motor ticks. + * The velocity methods of this class have been adjusted to include acceleration and corrected velocity receivers. + */ +public class Encoder { + private static final int CPS_STEP = 0x10000; + private final Supplier position; + private final Supplier velocity; + private int resetVal, lastPosition; + private DcMotorSimple.Direction direction; + private double lastTimestamp, veloEstimate, accel, lastVelo; + + /** + * The encoder object for the motor. + * + * @param position the position supplier which points to the + * current position of the motor in ticks ({@code obj::getCurrentPosition}) + * @param velocity the velocity supplier which points to the + * current velocity the motor in ticks per second ({@code obj::getVelocity}) + */ + public Encoder(Supplier position, Supplier velocity) { + this.position = position; + this.velocity = velocity; + resetVal = 0; + lastPosition = 0; + veloEstimate = 0; + direction = DcMotorSimple.Direction.FORWARD; + lastTimestamp = System.nanoTime() / 1.0E9; + } + + /** + * @return the current position of the encoder + */ + public int getPosition() { + int currentPosition = position.get(); + if (currentPosition != lastPosition) { + double currentTime = System.nanoTime() / 1.0E9; + double dt = currentTime - lastTimestamp; + veloEstimate = (currentPosition - lastPosition) / dt; + lastPosition = currentPosition; + lastTimestamp = currentTime; + } + return direction == DcMotorSimple.Direction.FORWARD ? 1 : -1 * currentPosition - resetVal; + } + + /** + * Resets the encoder without having to stop the motor. + */ + public void reset() { + resetVal += getPosition(); + } + + /** + * Sets the direction of the encoder to forward or reverse + * + * @param direction the desired direction + */ + public void setDirection(DcMotorSimple.Direction direction) { + this.direction = direction; + } + + /** + * @return the estimated acceleration of the motor in ticks per second squared. this method will internally + * call {@link #getRawVelocity()} to update the velocity information which is required. + */ + public double getAcceleration() { + getRawVelocity(); + return accel; + } + + /** + * @return the raw velocity of the motor reported by the encoder, may overflow if ticks/sec exceed 32767/sec + */ + public double getRawVelocity() { + double velo = velocity.get(); + if (velo != lastVelo) { + double currentTime = System.nanoTime() / 1.0E9; + double dt = currentTime - lastTimestamp; + accel = (velo - lastVelo) / dt; + lastVelo = velo; + lastTimestamp = currentTime; + } + return velo; + } + + /** + * Gets the current encoder velocity while also correcting for velocity overflow + * (REV hardware limitation workaround) + * + * @see #getRawVelocity() + * @return the corrected velocity + */ + public double getCorrectedVelocity() { + double real = getRawVelocity(); + while (Math.abs(veloEstimate - real) > CPS_STEP / 2.0) { + real += Math.signum(veloEstimate - real) * CPS_STEP; + } + return real; + } +} \ No newline at end of file