From 03d95d791e98f2c22ad9d5f22d2b80d0248d087d Mon Sep 17 00:00:00 2001 From: Ryan Brott Date: Tue, 20 Feb 2024 22:07:37 -0800 Subject: [PATCH] Rename `LazyImu` drive members Also makes sure the mecanum drive localizer fetches the IMU on construction. --- .../org/firstinspires/ftc/teamcode/MecanumDrive.java | 10 +++++++--- .../java/org/firstinspires/ftc/teamcode/TankDrive.java | 4 ++-- .../ftc/teamcode/tuning/TuningOpModes.java | 4 ++-- 3 files changed, 11 insertions(+), 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 index 26576a9..688e881 100644 --- 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.DcMotor; 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 @@ public final class MecanumDrive { public final VoltageSensor voltageSensor; - public final LazyImu imu; + public final LazyImu lazyImu; public final Localizer localizer; public Pose2d pose; @@ -123,6 +124,7 @@ public final class MecanumDrive { 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 final class MecanumDrive { 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 final class MecanumDrive { 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 final class MecanumDrive { // 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 index 6ed5e3f..c7f0ba2 100644 --- 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 @@ public final class TankDrive { public final List leftMotors, rightMotors; - public final LazyImu imu; + public final LazyImu lazyImu; public final VoltageSensor voltageSensor; @@ -237,7 +237,7 @@ public final class TankDrive { // 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 index ce6031f..b95e872 100644 --- 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 final class TuningOpModes { 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 final class TuningOpModes { rightEncs, parEncs, perpEncs, - td.imu, + td.lazyImu, td.voltageSensor, () -> new MotorFeedforward(TankDrive.PARAMS.kS, TankDrive.PARAMS.kV / TankDrive.PARAMS.inPerTick,