diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSAngularScalar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSAngularScalar.java index 19035296e526..747a217adf32 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSAngularScalar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSAngularScalar.java @@ -20,6 +20,7 @@ public void runOpMode() throws InterruptedException { telemetry.update(); waitForStart(); while (opModeIsActive()) { + drive.updatePoseEstimate(); radsTurned += drive.pose.heading.minus(lastHeading); lastHeading = drive.pose.heading; telemetry.addData("Uncorrected Degrees Turned", Math.toDegrees(radsTurned)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSHeadingOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSHeadingOffsetTuner.java index ed0b405a7a96..447b8b5af5ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSHeadingOffsetTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSHeadingOffsetTuner.java @@ -18,8 +18,9 @@ public void runOpMode() throws InterruptedException { telemetry.update(); waitForStart(); while (opModeIsActive()) { - telemetry.addData("Heading Offset (radians, enter this one into SparkFunOTOSDrive!)",Math.atan2(drive.pose.position.x,drive.pose.position.y)); - telemetry.addData("Heading Offset (degrees)",Math.toRadians(Math.atan2(drive.pose.position.x,drive.pose.position.y))); + drive.updatePoseEstimate(); + telemetry.addData("Heading Offset (radians, enter this one into SparkFunOTOSDrive!)",Math.atan2(drive.pose.position.y,drive.pose.position.x)); + telemetry.addData("Heading Offset (degrees)",Math.toDegrees(Math.atan2(drive.pose.position.y,drive.pose.position.x))); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSPositionOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSPositionOffsetTuner.java index 3e722da308ab..0450b6d6ab9e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSPositionOffsetTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSPositionOffsetTuner.java @@ -18,6 +18,7 @@ public void runOpMode() throws InterruptedException { telemetry.update(); waitForStart(); while (opModeIsActive()) { + drive.updatePoseEstimate(); telemetry.addData("Heading (deg)",Math.toDegrees(drive.pose.heading.toDouble())); if (Math.abs(Math.toDegrees(drive.pose.heading.toDouble())) > 175) { telemetry.addData("X Offset", drive.pose.position.x / 2);