Rename LazyImu drive members

Also makes sure the mecanum drive localizer fetches the IMU on construction.
wheelerrobotics · Feb 21, 2024 · 03d95d7 · 03d95d7
1 parent 9d2cd48
commit 03d95d7
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 7 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -36,6 +36,7 @@
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.VoltageSensor;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
@@ -109,7 +110,7 @@ kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
 
     public final VoltageSensor voltageSensor;
 
-    public final LazyImu imu;
+    public final LazyImu lazyImu;
 
     public final Localizer localizer;
     public Pose2d pose;
@@ -123,6 +124,7 @@ kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
 
     public class DriveLocalizer implements Localizer {
         public final Encoder leftFront, leftBack, rightBack, rightFront;
+        public final IMU imu;
 
         private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos;
         private Rotation2d lastHeading;
@@ -134,6 +136,8 @@ public DriveLocalizer() {
             rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
             rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
 
+            imu = lazyImu.get();
+
             // TODO: reverse encoders if needed
             //   leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
         }
@@ -145,7 +149,7 @@ public Twist2dDual<Time> update() {
             PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
             PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
 
-            YawPitchRollAngles angles = imu.get().getRobotYawPitchRollAngles();
+            YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
 
             FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage(
                     leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles));
@@ -228,7 +232,7 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
 
         // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        imu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
+        lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
                 PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
 
         voltageSensor = hardwareMap.voltageSensor.iterator().next();

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
@@ -110,7 +110,7 @@ kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
 
     public final List<DcMotorEx> leftMotors, rightMotors;
 
-    public final LazyImu imu;
+    public final LazyImu lazyImu;
 
     public final VoltageSensor voltageSensor;
 
@@ -237,7 +237,7 @@ public TankDrive(HardwareMap hardwareMap, Pose2d pose) {
 
         // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
         //   see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
-        imu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
+        lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
                 PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
 
         voltageSensor = hardwareMap.voltageSensor.iterator().next();

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
@@ -97,7 +97,7 @@ public static void register(OpModeManager manager) {
                         rightEncs,
                         parEncs,
                         perpEncs,
-                        md.imu,
+                        md.lazyImu,
                         md.voltageSensor,
                         () -> new MotorFeedforward(MecanumDrive.PARAMS.kS,
                                 MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick,
@@ -140,7 +140,7 @@ public static void register(OpModeManager manager) {
                         rightEncs,
                         parEncs,
                         perpEncs,
-                        td.imu,
+                        td.lazyImu,
                         td.voltageSensor,
                         () -> new MotorFeedforward(TankDrive.PARAMS.kS,
                                 TankDrive.PARAMS.kV / TankDrive.PARAMS.inPerTick,