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 5089eb4c..23885ca8 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 @@ -1019,7 +1019,7 @@ public DashboardPoseTracker getDashboardPoseTracker() { /** * This resets the IMU, if applicable. */ - public void resetIMU() { + private void resetIMU() throws InterruptedException { 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 059ee318..64710ace 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 @@ -92,7 +92,7 @@ public abstract class Localizer { /** * This resets the IMU of the localizer, if applicable. */ - public abstract void resetIMU(); + public abstract void resetIMU() throws InterruptedException; /** * This is overridden to return the IMU, if there is one. 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 3daf54a2..b718320f 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 @@ -351,7 +351,7 @@ public Localizer getLocalizer() { /** * */ - public void resetIMU() { + public void resetIMU() throws InterruptedException { localizer.resetIMU(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java index b0ef6bc2..73272b88 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java @@ -79,13 +79,11 @@ // //TODO: Set encoder directions // odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); // -// odo.resetPosAndIMU(); -// +// resetPinpoint();; + // setStartPose(setStartPose); // totalHeading = 0; // previousHeading = setStartPose.getHeading(); -// -// resetPinpoint(); // } // // /** @@ -107,7 +105,7 @@ // @Override // public Pose getVelocity() { // Pose2D pose = odo.getVelocity(); -// return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS)); +// return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), odo.getHeadingVelocity()); // } // // /** @@ -142,7 +140,6 @@ // */ // @Override // public void setPose(Pose setPose) { -// resetPinpoint(); // odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading())); // } // @@ -195,17 +192,29 @@ // } // // /** -// * This resets the IMU. +// * This resets the IMU. Note: This does not change the estimated heading orientation. // */ // @Override -// public void resetIMU() { -// odo.recalibrateIMU(); +// public void resetIMU() throws InterruptedException { +// odo.recalibrateIMU(); +// +// try { +// Thread.sleep(300); +// } catch (InterruptedException e) { +// throw new RuntimeException(e); +// } // } // // /** -// * This resets the OTOS. +// * This resets the pinpoint. // */ -// public void resetPinpoint(){ +// private void resetPinpoint() { // odo.resetPosAndIMU(); +// +// try { +// Thread.sleep(300); +// } catch (InterruptedException e) { +// throw new RuntimeException(e); +// } // } //}