From 437635838b25d970f60cf55166fdf8e747f83988 Mon Sep 17 00:00:00 2001 From: Anyi Lin Date: Thu, 21 Nov 2024 15:35:42 -0500 Subject: [PATCH] added functionality to IMU methods in PoseUpdater --- .../pedroPathing/localization/Localizer.java | 11 +++++++++++ .../pedroPathing/localization/PoseUpdater.java | 14 +++++++++++--- .../localizers/ThreeWheelIMULocalizer.java | 10 ++++++++++ .../localization/localizers/TwoWheelLocalizer.java | 10 ++++++++++ 4 files changed, 42 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java index 8f9687f..059ee31 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.pedroPathing.localization; +import com.qualcomm.robotcore.hardware.IMU; + import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; /** @@ -91,4 +93,13 @@ public abstract class Localizer { * This resets the IMU of the localizer, if applicable. */ public abstract void resetIMU(); + + /** + * This is overridden to return the IMU, if there is one. + * + * @return returns the IMU if it exists + */ + public IMU getIMU() { + return null; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java index ce2a8f2..eff461f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java @@ -60,6 +60,7 @@ public class PoseUpdater { } this.localizer = localizer; + imu = localizer.getIMU(); } /** @@ -300,7 +301,9 @@ public class PoseUpdater { * This resets the heading of the robot to the IMU's heading, using Road Runner's pose reset. */ public void resetHeadingToIMU() { - localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + if (imu != null) { + localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + } } /** @@ -309,7 +312,9 @@ public class PoseUpdater { * method. */ public void resetHeadingToIMUWithOffsets() { - setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + if (imu != null) { + setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading())); + } } /** @@ -318,7 +323,10 @@ public class PoseUpdater { * @return returns the normalized IMU heading. */ public double getNormalizedIMUHeading() { - return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + if (imu != null) { + return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + } + return 0; } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java index bd3c598..5f0724c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java @@ -313,4 +313,14 @@ public class ThreeWheelIMULocalizer extends Localizer { public void resetIMU() { imu.resetYaw(); } + + /** + * This is returns the IMU. + * + * @return returns the IMU + */ + @Override + public IMU getIMU() { + return imu; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java index 50d7ec2..685cebe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java @@ -296,4 +296,14 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w public void resetIMU() { imu.resetYaw(); } + + /** + * This is returns the IMU. + * + * @return returns the IMU + */ + @Override + public IMU getIMU() { + return imu; + } }