Preparation for new IMU

src/main/java/competition/injection/components/CompetitionTestComponent.java

@@ -3,6 +3,7 @@
 import javax.inject.Singleton;
 
 import competition.injection.modules.CommonModule;
+import competition.injection.modules.IMUModule;
 import competition.injection.modules.SimulatedRobotModule;
 import competition.injection.modules.UnitTestRobotModule;
 import dagger.Component;
@@ -12,7 +13,7 @@
 
 @Singleton
 @Component(modules = { UnitTestModule.class, MockDevicesModule.class, MockControlsModule.class,
-        CommonModule.class, UnitTestRobotModule.class})
+        CommonModule.class, UnitTestRobotModule.class, IMUModule.class})
 public abstract class CompetitionTestComponent extends BaseRobotComponent {
 
 }

src/main/java/competition/injection/components/RobotComponent.java

@@ -4,14 +4,15 @@
 
 import competition.injection.modules.CommonModule;
 import competition.injection.modules.CompetitionModule;
+import competition.injection.modules.IMUModule;
 import dagger.Component;
 import xbot.common.injection.modules.RealControlsModule;
 import xbot.common.injection.modules.RealDevicesModule;
 import xbot.common.injection.modules.RobotModule;
 
 @Singleton
 @Component(modules = { RobotModule.class, RealDevicesModule.class, RealControlsModule.class,
-        CompetitionModule.class, CommonModule.class })
+        CompetitionModule.class, CommonModule.class, IMUModule.class})
 public abstract class RobotComponent extends BaseRobotComponent {
 
 }

src/main/java/competition/injection/components/RobotComponent2023.java

@@ -1,6 +1,7 @@
 package competition.injection.components;
 
 import competition.injection.modules.CommonModule;
+import competition.injection.modules.IMUModule;
 import competition.injection.modules.Module2023;
 import dagger.Component;
 import xbot.common.injection.modules.RealControlsModule;
@@ -11,7 +12,7 @@
 
 @Singleton
 @Component(modules = { RobotModule.class, RealDevicesModule.class, RealControlsModule.class,
-        Module2023.class, CommonModule.class})
+        Module2023.class, CommonModule.class, IMUModule.class})
 public abstract class RobotComponent2023 extends BaseRobotComponent {
 
 }

src/main/java/competition/injection/components/RobotComponent2025.java

@@ -1,6 +1,7 @@
 package competition.injection.components;
 
 import competition.injection.modules.CommonModule;
+import competition.injection.modules.IMUModule;
 import competition.injection.modules.Module2025;
 import dagger.Component;
 import xbot.common.injection.modules.RealControlsModule;
@@ -11,7 +12,7 @@
 
 @Singleton
 @Component(modules = { RobotModule.class, RealDevicesModule.class, RealControlsModule.class,
-        Module2025.class, CommonModule.class})
+        Module2025.class, CommonModule.class, IMUModule.class})
 public abstract class RobotComponent2025 extends BaseRobotComponent {
 
 }

src/main/java/competition/injection/components/RoboxComponent.java

@@ -1,6 +1,7 @@
 package competition.injection.components;
 
 import competition.injection.modules.CommonModule;
+import competition.injection.modules.IMUModule;
 import competition.injection.modules.RoboxModule;
 import dagger.Component;
 import xbot.common.injection.modules.RealControlsModule;
@@ -11,7 +12,7 @@
 
 @Singleton
 @Component(modules = { RobotModule.class, RealDevicesModule.class, RealControlsModule.class,
-        RoboxModule.class, CommonModule.class})
+        RoboxModule.class, CommonModule.class, IMUModule.class})
 public abstract class RoboxComponent extends BaseRobotComponent {
 
 }

src/main/java/competition/injection/components/SimulationComponent.java

@@ -3,6 +3,7 @@
 import javax.inject.Singleton;
 
 import competition.injection.modules.CommonModule;
+import competition.injection.modules.IMUModule;
 import competition.injection.modules.SimulatedRobotModule;
 import dagger.Component;
 import xbot.common.injection.modules.MockDevicesModule;
@@ -11,7 +12,7 @@
 
 @Singleton
 @Component(modules = { SimulationModule.class, MockDevicesModule.class, RealControlsModule.class,
-        SimulatedRobotModule.class, CommonModule.class })
+        SimulatedRobotModule.class, CommonModule.class, IMUModule.class})
 public abstract class SimulationComponent extends BaseRobotComponent {
 
 }

src/main/java/competition/injection/modules/IMUModule.java

@@ -0,0 +1,20 @@
+package competition.injection.modules;
+
+import javax.inject.Singleton;
+
+import dagger.Module;
+import dagger.Provides;
+import competition.subsystems.pose.XbotIMU;
+import competition.subsystems.pose.XGyroIMU;
+import xbot.common.controls.sensors.XGyro;
+import competition.electrical_contract.ElectricalContract;
+
+@Module
+public class IMUModule {
+
+    @Provides
+    @Singleton
+    public XbotIMU provideIMU(XGyro.XGyroFactory gyroFactory, ElectricalContract contract) {
+        return new XGyroIMU(gyroFactory.create(contract.getIMUInfo()));
+    }
+}

src/main/java/competition/simulation/MapleSimulator.java

@@ -141,7 +141,7 @@ protected void updateDriveSimulation() {
         aKitLog.record("RobotVelocity", swerveDriveSimulation.getActualSpeedsFieldRelative());
 
         // update gyro reading from sim
-        ((MockGyro) pose.imu).setYaw(this.swerveDriveSimulation.getOdometryEstimatedPose().getRotation().getMeasure());
+        ((MockGyro) pose.getImu()).setYaw(this.swerveDriveSimulation.getOdometryEstimatedPose().getRotation().getMeasure());
     }
 
     @Override

src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java

@@ -48,7 +48,7 @@ public SwerveDriveWithJoysticksCommand(
     ) {
         pf.setPrefix(this);
         this.drive = drive;
-        this.xGyro = pose.imu;
+        this.xGyro = (XGyro) pose.getImu();
         this.pose = pose;
         this.oi = oi;
         this.headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid());

src/main/java/competition/subsystems/pose/PoseSubsystem.java

@@ -7,6 +7,7 @@
 import javax.inject.Singleton;
 
 import competition.electrical_contract.ElectricalContract;
+import competition.subsystems.pose.XbotIMU;
 import competition.subsystems.drive.DriveSubsystem;
 import competition.subsystems.vision.AprilTagVisionSubsystemExtended;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
@@ -27,6 +28,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import xbot.common.controls.sensors.XGyro;
 import xbot.common.controls.sensors.XGyro.XGyroFactory;
 import xbot.common.math.WrappedRotation2d;
 import xbot.common.properties.BooleanProperty;
@@ -46,6 +48,7 @@ public class PoseSubsystem extends BasePoseSubsystem {
     private final BooleanProperty useVisionAssistedPose;
     private final BooleanProperty reportCameraPoses;
     private final DoubleProperty isFacingTargetMarginOfError;
+    private final XbotIMU imu;
     private AprilTagFieldLayout aprilTagFieldLayout;
 
     private boolean preferOdometryToVision = false;
@@ -56,13 +59,14 @@ public class PoseSubsystem extends BasePoseSubsystem {
     protected Optional<SwerveModulePosition[]> simulatedModulePositions = Optional.empty();
 
     @Inject
-    public PoseSubsystem(ElectricalContract electricalContract,
-                         XGyroFactory gyroFactory,
+    public PoseSubsystem(XbotIMU imu,
                          PropertyFactory propManager,
                          DriveSubsystem drive,
                          AprilTagVisionSubsystemExtended aprilTagVisionSubsystem,
                          AprilTagFieldLayout aprilTagFieldLayout) {
-        super(gyroFactory.create(electricalContract.getIMUInfo()), propManager);
+        super((XGyro) imu, propManager);
+        this.imu = imu;
+
         this.drive = drive;
         this.aprilTagVisionSubsystem = aprilTagVisionSubsystem;
         this.aprilTagFieldLayout = aprilTagFieldLayout;
@@ -310,6 +314,11 @@ public WrappedRotation2d getCurrentHeading() {
         }
     }
 
+    public XbotIMU getImu() {
+        return imu;
+    }
+
+
     // used by the physics simulator to mock what the swerve modules are doing
     // currently for pose estimation
     public void ingestSimulatedSwerveModulePositions(SwerveModulePosition[] positions) {

src/main/java/competition/subsystems/pose/SystemCoreIMU.java

@@ -0,0 +1,26 @@
+package competition.subsystems.pose;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class SystemCoreIMU implements XbotIMU {
+
+    @Override
+    public Rotation2d getRotation() {
+        return Rotation2d.fromDegrees(0);
+    }
+
+    @Override
+    public double getAngularVelocity() {
+        return 0;
+    }
+
+    @Override
+    public double getPitch() {
+        return 0;
+    }
+
+    @Override
+    public double getRoll() {
+        return 0;
+    }
+}

src/main/java/competition/subsystems/pose/XGyroIMU.java

@@ -0,0 +1,36 @@
+package competition.subsystems.pose;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import xbot.common.controls.sensors.XGyro;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+
+public class XGyroIMU implements XbotIMU {
+
+    private final XGyro gyro;
+
+    public XGyroIMU(XGyro gyro) {
+        this.gyro = gyro;
+    }
+
+    @Override
+    public Rotation2d getRotation() {
+        return Rotation2d.fromDegrees(gyro.getHeading().in(Degrees));
+    }
+
+    @Override
+    public double getAngularVelocity() {
+        return gyro.getYawAngularVelocity().in(DegreesPerSecond);
+    }
+
+    @Override
+    public double getPitch() {
+        return gyro.getPitch().in(Degrees);
+    }
+
+    @Override
+    public double getRoll() {
+        return gyro.getRoll().in(Degrees);
+    }
+}

src/main/java/competition/subsystems/pose/XbotIMU.java

@@ -0,0 +1,10 @@
+package competition.subsystems.pose;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public interface XbotIMU {
+    Rotation2d getRotation();
+    double getAngularVelocity();
+    double getPitch();
+    double getRoll();
+}