From f91cf1a8bf719c08e4db9fb6fd66336e17617656 Mon Sep 17 00:00:00 2001 From: brotherhobo Date: Sat, 7 Sep 2024 17:36:52 -0400 Subject: [PATCH] you can now reset the IMU! --- .../ftc/teamcode/pedroPathing/follower/Follower.java | 7 +++++++ .../teamcode/pedroPathing/localization/Localizer.java | 5 +++++ .../teamcode/pedroPathing/localization/PoseUpdater.java | 9 +++++++++ .../localization/localizers/DriveEncoderLocalizer.java | 6 ++++++ .../localization/localizers/OTOSLocalizer.java | 6 ++++++ .../localization/localizers/ThreeWheelIMULocalizer.java | 7 +++++++ .../localization/localizers/ThreeWheelLocalizer.java | 6 ++++++ .../localization/localizers/TwoWheelLocalizer.java | 7 +++++++ 8 files changed, 53 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 8407732b..fe9d153f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java @@ -989,4 +989,11 @@ public Path getCurrentPath() { public DashboardPoseTracker getDashboardPoseTracker() { return dashboardPoseTracker; } + + /** + * This resets the IMU, if applicable. + */ + public void resetIMU() { + poseUpdater.resetIMU(); + } } 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 2fa2173f..8f9687f8 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 @@ -86,4 +86,9 @@ public abstract class Localizer { * @return returns the turning ticks to radians multiplier */ public abstract double getTurningMultiplier(); + + /** + * This resets the IMU of the localizer, if applicable. + */ + public abstract void resetIMU(); } 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 415b5a05..ce2a8f2e 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 @@ -5,7 +5,9 @@ import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelIMULocalizer; import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.ThreeWheelLocalizer; +import org.firstinspires.ftc.teamcode.pedroPathing.localization.localizers.TwoWheelLocalizer; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector; @@ -336,4 +338,11 @@ public double getTotalHeading() { public Localizer getLocalizer() { return localizer; } + + /** + * + */ + public void resetIMU() { + localizer.resetIMU(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java index c414057e..a8e3902d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/DriveEncoderLocalizer.java @@ -263,4 +263,10 @@ public double getLateralMultiplier() { public double getTurningMultiplier() { return TURN_TICKS_TO_RADIANS; } + + /** + * This does nothing since this localizer does not use the IMU. + */ + public void resetIMU() { + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index afe19547..cdacd523 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java @@ -209,4 +209,10 @@ public double getLateralMultiplier() { public double getTurningMultiplier() { return otos.getAngularScalar(); } + + /** + * This does nothing since this localizer does not use the IMU. + */ + public void resetIMU() { + } } \ No newline at end of file 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 44a03c64..1e9fd7b2 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 @@ -306,4 +306,11 @@ public double getLateralMultiplier() { public double getTurningMultiplier() { return TURN_TICKS_TO_RADIANS; } + + /** + * This resets the IMU. + */ + public void resetIMU() { + imu.resetYaw(); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java index a955946c..6814b862 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelLocalizer.java @@ -283,4 +283,10 @@ public double getLateralMultiplier() { public double getTurningMultiplier() { return TURN_TICKS_TO_RADIANS; } + + /** + * This does nothing since this localizer does not use the IMU. + */ + public void resetIMU() { + } } 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 bef838e6..62415d6f 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 @@ -289,4 +289,11 @@ public double getLateralMultiplier() { public double getTurningMultiplier() { return 1; } + + /** + * This resets the IMU. + */ + public void resetIMU() { + imu.resetYaw(); + } }