From 9ad00e47d81e2d18c3bc133fb6217f565f3ec8c5 Mon Sep 17 00:00:00 2001 From: cyberarm Date: Tue, 24 Jan 2023 10:00:23 -0600 Subject: [PATCH] Preserve i2c driver implementation attempts --- .../drivers/EncoderAdafruitP4991.java | 95 ++++++++++++++++ .../cyberarm/drivers/IMUAdafruitBNO085.java | 104 ++++++++++++++++++ .../TeleOp/engine/AdafruitEncoder.java | 16 +++ .../TeleOp/states/AdafruitEncoderState.java | 24 ++++ .../TeleOp/states/AdafruitIMUState.java | 16 +-- 5 files changed, 248 insertions(+), 7 deletions(-) create mode 100644 TeamCode/src/main/java/org/cyberarm/drivers/EncoderAdafruitP4991.java create mode 100644 TeamCode/src/main/java/org/cyberarm/drivers/IMUAdafruitBNO085.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TeleOp/engine/AdafruitEncoder.java create mode 100644 TeamCode/src/main/java/org/timecrafters/TeleOp/states/AdafruitEncoderState.java diff --git a/TeamCode/src/main/java/org/cyberarm/drivers/EncoderAdafruitP4991.java b/TeamCode/src/main/java/org/cyberarm/drivers/EncoderAdafruitP4991.java new file mode 100644 index 0000000..144dd15 --- /dev/null +++ b/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 { + 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(); + } + } + +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/cyberarm/drivers/IMUAdafruitBNO085.java b/TeamCode/src/main/java/org/cyberarm/drivers/IMUAdafruitBNO085.java new file mode 100644 index 0000000..c5d541e --- /dev/null +++ b/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 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() { + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/AdafruitEncoder.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/engine/AdafruitEncoder.java new file mode 100644 index 0000000..53c0d06 --- /dev/null +++ b/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()); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/AdafruitEncoderState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/AdafruitEncoderState.java new file mode 100644 index 0000000..288f2a1 --- /dev/null +++ b/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(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/AdafruitIMUState.java b/TeamCode/src/main/java/org/timecrafters/TeleOp/states/AdafruitIMUState.java index 3fe331b..9dc5f3d 100644 --- a/TeamCode/src/main/java/org/timecrafters/TeleOp/states/AdafruitIMUState.java +++ b/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