From 358a119ff95332e2e283236f5e073c7ab69c6646 Mon Sep 17 00:00:00 2001
From: Lucas Bubner <lkbubner@proton.me>
Date: Fri, 9 Aug 2024 20:16:10 +0930
Subject: [PATCH] Encoder class

---
 .../bunyipslib/Encoder.java                   | 106 ++++++++++++++++++
 1 file changed, 106 insertions(+)
 create mode 100644 src/main/java/org/murraybridgebunyips/bunyipslib/Encoder.java

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<Integer> position;
+    private final Supplier<Double> 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<Integer> position, Supplier<Double> 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