External imu support (#71)

* Added External IMU capabilities to 20403 * Added External IMU capabilities to both bots. Needs testing on hardware before pushing to main. * Tested on both robots
technototes · Nov 16, 2024 · 9bbdf94 · 9bbdf94
1 parent 2f5c151
commit 9bbdf94
Show file tree

Hide file tree

Showing 6 changed files with 33 additions and 16 deletions.
diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Hardware.java
@@ -5,6 +5,8 @@
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.technototes.library.hardware.motor.EncodedMotor;
+import com.technototes.library.hardware.sensor.AdafruitIMU;
+import com.technototes.library.hardware.sensor.IGyro;
 import com.technototes.library.hardware.sensor.IMU;
 import com.technototes.library.hardware.sensor.encoder.MotorEncoder;
 import com.technototes.library.hardware.servo.Servo;
@@ -16,7 +18,7 @@ public class Hardware implements Loggable {
 
     public List<LynxModule> hubs;
 
-    public IMU imu;
+    public IGyro imu;
     public EncodedMotor<DcMotorEx> fl, fr, rl, rr;
     public Servo clawservo;
     public Servo wristservo;
@@ -30,12 +32,15 @@ public class Hardware implements Loggable {
 
     public Hardware(HardwareMap hwmap) {
         hubs = hwmap.getAll(LynxModule.class);
-        imu = new IMU(
-            Setup.HardwareNames.IMU,
-            RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
-            RevHubOrientationOnRobot.UsbFacingDirection.UP
-        );
-
+        if (Setup.Connected.EXTERNAL_IMU) {
+            imu = new AdafruitIMU(Setup.HardwareNames.EXTERNAL_IMU, AdafruitIMU.Orientation.Pitch);
+        } else {
+            imu = new IMU(
+                Setup.HardwareNames.IMU,
+                RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
+                RevHubOrientationOnRobot.UsbFacingDirection.UP
+            );
+        }
         if (Setup.Connected.DRIVEBASE) {
             fl = new EncodedMotor<DcMotorEx>(Setup.HardwareNames.FL_DRIVE_MOTOR);
             fr = new EncodedMotor<DcMotorEx>(Setup.HardwareNames.FR_DRIVE_MOTOR);

diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/Setup.java
@@ -13,6 +13,7 @@ public static class Connected {
         public static boolean BUCKET = false;
         public static boolean ODOSUBSYSTEM = true;
         public static boolean SAFETYSUBSYSTEM = false;
+        public static boolean EXTERNAL_IMU = true;
     }
 
     @Config
@@ -23,6 +24,7 @@ public static class HardwareNames {
         public static String RL_DRIVE_MOTOR = "rl";
         public static String RR_DRIVE_MOTOR = "rr";
         public static String IMU = "imu";
+        public static String EXTERNAL_IMU = "adafruit-imu";
         public static String ODOF = "odof";
         public static String ODOR = "odor";
 

diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/DrivebaseSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/DrivebaseSubsystem.java
@@ -9,6 +9,7 @@
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 import com.technototes.library.hardware.motor.EncodedMotor;
+import com.technototes.library.hardware.sensor.IGyro;
 import com.technototes.library.hardware.sensor.IMU;
 import com.technototes.library.logger.Log;
 import com.technototes.library.logger.Loggable;
@@ -153,7 +154,7 @@ public DrivebaseSubsystem(
         EncodedMotor<DcMotorEx> fr,
         EncodedMotor<DcMotorEx> rl,
         EncodedMotor<DcMotorEx> rr,
-        IMU i,
+        IGyro i,
         TwoTrackingWheelLocalizer l
     ) {
         super(fl, fr, rl, rr, i, () -> DriveConstants.class);

diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java
@@ -10,7 +10,9 @@
 import com.technototes.library.hardware.motor.EncodedMotor;
 import com.technototes.library.hardware.motor.Motor;
 import com.technototes.library.hardware.motor.Motor;
+import com.technototes.library.hardware.sensor.AdafruitIMU;
 import com.technototes.library.hardware.sensor.ColorSensor;
+import com.technototes.library.hardware.sensor.IGyro;
 import com.technototes.library.hardware.sensor.IMU;
 import com.technototes.library.hardware.sensor.Rev2MDistanceSensor;
 import com.technototes.library.hardware.sensor.encoder.MotorEncoder;
@@ -24,7 +26,7 @@ public class Hardware implements Loggable {
 
     public List<LynxModule> hubs;
 
-    public IMU imu;
+    public IGyro imu;
     public EncodedMotor<DcMotorEx> fl, fr, rl, rr, armL, armR;
     public MotorEncoder odoF, odoR;
     public Servo retainer, jaw, wrist;
@@ -39,11 +41,15 @@ public class Hardware implements Loggable {
 
     public Hardware(HardwareMap hwmap) {
         hubs = hwmap.getAll(LynxModule.class);
-        imu = new IMU(
-            Setup.HardwareNames.IMU,
-            RevHubOrientationOnRobot.LogoFacingDirection.UP,
-            RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD
-        );
+        if (Setup.Connected.EXTERNALIMU) {
+            imu = new AdafruitIMU(Setup.HardwareNames.EXTERNALIMU, AdafruitIMU.Orientation.Pitch);
+        } else {
+            imu = new IMU(
+                Setup.HardwareNames.IMU,
+                RevHubOrientationOnRobot.LogoFacingDirection.UP,
+                RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD
+            );
+        }
         if (Setup.Connected.DRIVEBASE) {
             fl = new EncodedMotor<>(Setup.HardwareNames.FLMOTOR);
             fr = new EncodedMotor<>(Setup.HardwareNames.FRMOTOR);

diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java
@@ -13,6 +13,7 @@ public static class Connected {
         public static boolean KIDSSHAMPOOSUBSYSTEM = false;
         public static boolean HANGSUBSYSTEM = false;
         public static boolean ARMSUBSYSTEM = true;
+        public static boolean EXTERNALIMU = true;
     }
 
     @Config
@@ -23,6 +24,7 @@ public static class HardwareNames {
         public static String RLMOTOR = "rl";
         public static String RRMOTOR = "rr";
         public static String IMU = "imu";
+        public static String EXTERNALIMU = "adafruit-imu";
         public static String OCTOQUAD = "octoquad";
         public static String ODOF = "odof";
         public static String ODOR = "odor";

diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/DrivebaseSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/DrivebaseSubsystem.java
@@ -9,6 +9,7 @@
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 import com.technototes.library.hardware.motor.EncodedMotor;
+import com.technototes.library.hardware.sensor.IGyro;
 import com.technototes.library.hardware.sensor.IMU;
 import com.technototes.library.logger.Log;
 import com.technototes.library.logger.LogConfig;
@@ -165,7 +166,7 @@ public DrivebaseSubsystem(
         EncodedMotor<DcMotorEx> fr,
         EncodedMotor<DcMotorEx> rl,
         EncodedMotor<DcMotorEx> rr,
-        IMU i,
+        IGyro i,
         TwoTrackingWheelLocalizer l
     ) {
         // The localizer is not quite working. Bot drives a little crazy
@@ -189,7 +190,7 @@ public DrivebaseSubsystem(
         EncodedMotor<DcMotorEx> fr,
         EncodedMotor<DcMotorEx> rl,
         EncodedMotor<DcMotorEx> rr,
-        IMU i
+        IGyro i
     ) {
         super(fl, fr, rl, rr, i, () -> DriveConstants.class);
         fl2 = fl;