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 8407732..fe9d153 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 class Follower { 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 2fa2173..8f9687f 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 415b5a0..ce2a8f2 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.HardwareMap; 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 class PoseUpdater { 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 c414057..a8e3902 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 class DriveEncoderLocalizer extends Localizer { 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 afe1954..cdacd52 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 class OTOSLocalizer extends Localizer { 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 44a03c6..1e9fd7b 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 class ThreeWheelIMULocalizer extends Localizer { 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 a955946..6814b86 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 class ThreeWheelLocalizer extends Localizer { 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 bef838e..62415d6 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 class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w public double getTurningMultiplier() { return 1; } + + /** + * This resets the IMU. + */ + public void resetIMU() { + imu.resetYaw(); + } }