From 0fcaf380ab13f4767188390490ccac184111bf25 Mon Sep 17 00:00:00 2001
From: Cyberarm <matthewlikesrobots@gmail.com>
Date: Thu, 25 Jan 2024 18:06:27 -0600
Subject: [PATCH] Implemented I2C driver for custom encoder based on an
 Adafruit KB2040

---
 .../cyberarm/drivers/EncoderCustomKB2040.java | 67 +++++++++++++++++++
 .../testing/CustomEncoderTestEngine.java      | 40 +++++++++++
 2 files changed, 107 insertions(+)
 create mode 100644 TeamCode/src/main/java/dev/cyberarm/drivers/EncoderCustomKB2040.java
 create mode 100644 TeamCode/src/main/java/dev/cyberarm/testing/CustomEncoderTestEngine.java

diff --git a/TeamCode/src/main/java/dev/cyberarm/drivers/EncoderCustomKB2040.java b/TeamCode/src/main/java/dev/cyberarm/drivers/EncoderCustomKB2040.java
new file mode 100644
index 0000000..01efe5b
--- /dev/null
+++ b/TeamCode/src/main/java/dev/cyberarm/drivers/EncoderCustomKB2040.java
@@ -0,0 +1,67 @@
+package dev.cyberarm.drivers;
+
+import com.qualcomm.robotcore.hardware.I2cAddr;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
+import com.qualcomm.robotcore.hardware.I2cWaitControl;
+import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
+import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+
+@I2cDeviceType()
+@DeviceProperties(name = "Encoder Custom KB2040", description = "Non-competition legal i2c encoder", xmlTag = "ENCODER_CUSTOM_KB2040")
+public class EncoderCustomKB2040 extends I2cDeviceSynchDevice<I2cDeviceSynchSimple> {
+    enum Register {
+        REPORT_POSITION,
+        RESET_POSITION
+    }
+
+    private final static I2cAddr ADDRESS_I2C_DEFAULT = I2cAddr.create7bit(0x16);
+    private int position = -1;
+    public EncoderCustomKB2040(I2cDeviceSynchSimple i2cDeviceSynchSimple, boolean deviceClientIsOwned) {
+        super(i2cDeviceSynchSimple, deviceClientIsOwned);
+
+        this.deviceClient.setI2cAddress(ADDRESS_I2C_DEFAULT);
+        super.registerArmingStateCallback(false);
+        engage();
+    }
+
+    @Override
+    protected synchronized boolean doInitialize() {
+        return true;
+    }
+
+    @Override
+    public Manufacturer getManufacturer() {
+        return Manufacturer.Other;
+    }
+
+    @Override
+    public String getDeviceName() {
+        return "Encoder Custom KB2040";
+    }
+
+    public int getCurrentPosition() {
+//        deviceClient.write8(Register.READ.ordinal());
+
+        // Read int32_t
+        byte[] buffer = deviceClient.read(Register.REPORT_POSITION.ordinal(), 4);
+
+        // Reconstruct int32_t from 4 int8_t
+        int newPos = ByteBuffer.wrap(buffer).order(ByteOrder.BIG_ENDIAN).getInt();
+
+        position = newPos;
+
+        return position;
+    }
+
+    public int getLastPosition() {
+        return position;
+    }
+
+    public void reset() {
+        deviceClient.write8(Register.RESET_POSITION.ordinal(), I2cWaitControl.WRITTEN);
+    }
+}
diff --git a/TeamCode/src/main/java/dev/cyberarm/testing/CustomEncoderTestEngine.java b/TeamCode/src/main/java/dev/cyberarm/testing/CustomEncoderTestEngine.java
new file mode 100644
index 0000000..7f7d64d
--- /dev/null
+++ b/TeamCode/src/main/java/dev/cyberarm/testing/CustomEncoderTestEngine.java
@@ -0,0 +1,40 @@
+package dev.cyberarm.testing;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import dev.cyberarm.drivers.EncoderCustomKB2040;
+import dev.cyberarm.engine.V2.CyberarmEngine;
+import dev.cyberarm.engine.V2.CyberarmState;
+
+//@TeleOp(name = "I2C Driver Test", group = "TESTING")
+public class CustomEncoderTestEngine extends CyberarmEngine {
+    @Override
+    public void setup() {
+        addState(new CyberarmState() {
+            private EncoderCustomKB2040 encoder;
+            private double triggerMS = 0;
+            private int position = -1;
+
+            public void init() {
+                encoder = engine.hardwareMap.get(EncoderCustomKB2040.class, "encoder");
+            }
+
+            @Override
+            public void exec() {
+                if (runTime() - triggerMS >= 0) {
+                    triggerMS = runTime();
+
+                    position = encoder.getCurrentPosition();
+//                    encoder.reset();
+                }
+            }
+
+            @Override
+            public void telemetry() {
+                engine.telemetry.addData("POS", position);
+                engine.telemetry.addData("RunTime", runTime());
+                engine.telemetry.addData("Trigger", triggerMS);
+            }
+        });
+    }
+}