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 b0ef6bc..f6ee733 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(); // } // // /** @@ -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); +// } // } //}