Preserve i2c driver implementation attempts

TeamCode/src/main/java/org/cyberarm/drivers/EncoderAdafruitP4991.java

@@ -0,0 +1,95 @@
+package org.cyberarm.drivers;
+
+import com.qualcomm.hardware.adafruit.AdafruitI2cColorSensor;
+import com.qualcomm.robotcore.hardware.I2cAddr;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
+import com.qualcomm.robotcore.hardware.I2cWaitControl;
+import com.qualcomm.robotcore.hardware.TimestampedData;
+import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
+import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
+import com.qualcomm.robotcore.util.RobotLog;
+import com.qualcomm.robotcore.util.TypeConversion;
+
+import org.firstinspires.ftc.robotcore.internal.android.dx.util.ByteArray;
+import org.timecrafters.testing.engine.AdafruitIMU;
+
+import java.nio.ByteBuffer;
+import java.util.Arrays;
+
+@I2cDeviceType
+@DeviceProperties(name = "Adafruit P4991 Rotary Encoder QT", xmlTag = "Adafruit_Encoder_P4991")
+public class EncoderAdafruitP4991 extends I2cDeviceSynchDevice<I2cDeviceSynch> {
+    final Object i2cLock = new Object();
+    I2cAddr ADDRESS_I2C_DEFAULT = I2cAddr.create7bit(0x36);
+    private int operation = Register.SEESAW_ENCODER_BASE.bVal;
+
+    public enum Register {
+        SEESAW_ENCODER_BASE(0x11),
+
+        SEESAW_ENCODER_STATUS(0x00),
+        SEESAW_ENCODER_INTEN_SET(0x10),
+        SEESAW_ENCODER_INTEN_CLR(0x20),
+        SEESAW_ENCODER_POSITION(0x30),
+        SEESAW_ENCODER_DELTA(0x40);
+
+        public final int bVal;
+
+        Register(int bVal)
+        {
+            this.bVal = bVal;
+        }
+    }
+
+    public EncoderAdafruitP4991(I2cDeviceSynch i2cDeviceSynch) {
+        // SEE: https://learn.adafruit.com/adafruit-seesaw-atsamd09-breakout?view=all#i2c-transactions-2937115
+
+        super(i2cDeviceSynch, true);
+
+        super.registerArmingStateCallback(false);
+        this.deviceClient.engage();
+    }
+
+    @Override
+    protected boolean doInitialize() {
+        deviceClient.setI2cAddress(ADDRESS_I2C_DEFAULT);
+
+        return deviceClient.isArmed();
+    }
+
+    @Override
+    public Manufacturer getManufacturer() {
+        return Manufacturer.Adafruit;
+    }
+
+    @Override
+    public String getDeviceName() {
+        return "Adafruit P4991 Rotary Encoder QT";
+    }
+
+    public synchronized int getCurrentPosition() {
+//            ByteBuffer buf = ByteBuffer.allocate(4);
+//            buf.putShort((short) Register.SEESAW_ENCODER_BASE.bVal);
+//            buf.putShort((short) Register.SEESAW_ENCODER_POSITION.bVal);
+
+        deviceClient.write8(0x11);
+        deviceClient.write8(0x30);
+
+        delay(100);
+
+        RobotLog.vv("Encoder", Arrays.toString(deviceClient.read(4)));
+        return 0;
+//            TimestampedData data = deviceClient.readTimeStamped(4);
+//            RobotLog.vv("ENCODER", "Operation: " + operation + ", Timestamp: " + data.nanoTime + ", Data: " + Arrays.toString(data.data));
+//            return TypeConversion.byteArrayToInt(data.data);
+    }
+
+    private void delay(long ms) {
+        try {
+            wait(ms);
+        } catch (InterruptedException e) {
+            e.printStackTrace();
+        }
+    }
+
+}

TeamCode/src/main/java/org/cyberarm/drivers/IMUAdafruitBNO085.java

@@ -0,0 +1,104 @@
+package org.cyberarm.drivers;
+
+import com.qualcomm.robotcore.hardware.I2cAddr;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
+import com.qualcomm.robotcore.hardware.IMU;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+public class IMUAdafruitBNO085 extends I2cDeviceSynchDevice<I2cDeviceSynch> implements IMU {
+
+    private final Object i2cLock = new Object();
+    private final I2cAddr DEFAULT_I2C_ADDRESS = new I2cAddr(0x4A);
+    private enum Register {
+        PRODUCT_ID(0x00);
+
+        private final int address;
+
+        Register(int address) {
+            this.address = address;
+        }
+    }
+
+
+    protected IMUAdafruitBNO085(I2cDeviceSynch i2cDeviceSynch, boolean deviceClientIsOwned) {
+        super(i2cDeviceSynch, deviceClientIsOwned);
+
+        this.deviceClient.setI2cAddress(DEFAULT_I2C_ADDRESS);
+    }
+
+    @Override
+    public boolean initialize(Parameters parameters) {
+        return false;
+    }
+
+    @Override
+    public void resetYaw() {
+    }
+
+    @Override
+    public YawPitchRollAngles getRobotYawPitchRollAngles() {
+        return null;
+    }
+
+    @Override
+    public Orientation getRobotOrientation(AxesReference reference, AxesOrder order, AngleUnit angleUnit) {
+        return null;
+    }
+
+    @Override
+    public Quaternion getRobotOrientationAsQuaternion() {
+        return null;
+    }
+
+    @Override
+    public AngularVelocity getRobotAngularVelocity(AngleUnit angleUnit) {
+        return null;
+    }
+
+    @Override
+    public Manufacturer getManufacturer() {
+        return Manufacturer.Adafruit;
+    }
+
+    @Override
+    public String getDeviceName() {
+        return null;
+    }
+
+    @Override
+    public String getConnectionInfo() {
+        return null;
+    }
+
+    @Override
+    public int getVersion() {
+        return 0;
+    }
+
+    @Override
+    protected boolean doInitialize() {
+        synchronized (i2cLock) {
+            deviceClient.read(32);
+        }
+
+        return false;
+    }
+
+    @Override
+    public void resetDeviceConfigurationForOpMode() {
+
+    }
+
+    @Override
+    public void close() {
+
+    }
+}

TeamCode/src/main/java/org/timecrafters/TeleOp/engine/AdafruitEncoder.java

@@ -0,0 +1,16 @@
+package org.timecrafters.testing.engine;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.cyberarm.engine.V2.CyberarmEngine;
+import org.timecrafters.testing.states.AdafruitEncoderState;
+
+@TeleOp(name = "Adafruit Encoder")
+@Disabled
+public class AdafruitEncoder extends CyberarmEngine {
+    @Override
+    public void setup() {
+        addState(new AdafruitEncoderState());
+    }
+}

TeamCode/src/main/java/org/timecrafters/TeleOp/states/AdafruitEncoderState.java

@@ -0,0 +1,24 @@
+package org.timecrafters.testing.states;
+
+import org.cyberarm.drivers.EncoderAdafruitP4991;
+import org.cyberarm.engine.V2.CyberarmState;
+
+public class AdafruitEncoderState extends CyberarmState {
+    EncoderAdafruitP4991 encoder;
+    int position = 0;
+
+    @Override
+    public void init() {
+        encoder = engine.hardwareMap.get(EncoderAdafruitP4991.class, "encoder");
+    }
+
+    @Override
+    public void telemetry() {
+        engine.telemetry.addData("Encoder Position",position);
+    }
+
+    @Override
+    public void exec() {
+        position =  encoder.getCurrentPosition();
+    }
+}

TeamCode/src/main/java/org/timecrafters/TeleOp/states/AdafruitIMUState.java

@@ -2,23 +2,25 @@ package org.timecrafters.TeleOp.states;
 
 import com.qualcomm.hardware.adafruit.AdafruitBNO055IMU;
 import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.ImuOrientationOnRobot;
 
 import org.cyberarm.engine.V2.CyberarmState;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 
 public class AdafruitIMUState extends CyberarmState {
-    AdafruitBNO055IMU imu;
+    IMU imu;
     @Override
     public void init() {
-        imu = engine.hardwareMap.get(AdafruitBNO055IMU.class, "adafruit_imu");
-        BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
-        imu.initialize(parameters);
+        imu = engine.hardwareMap.get(IMU.class, "adafruit_imu");
     }
 
     @Override
     public void telemetry() {
-        engine.telemetry.addData("First Angle", imu.getAngularOrientation().firstAngle);
-        engine.telemetry.addData("Second Angle", imu.getAngularOrientation().secondAngle);
-        engine.telemetry.addData("Third Angle", imu.getAngularOrientation().thirdAngle);
+        engine.telemetry.addData("PITCH", imu.getRobotYawPitchRollAngles().getPitch(AngleUnit.DEGREES));
+        engine.telemetry.addData("ROLL", imu.getRobotYawPitchRollAngles().getRoll(AngleUnit.DEGREES));
+        engine.telemetry.addData("YAW", imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
     }
 
     @Override