actually update the pose

wheelerrobotics · Jul 2, 2024 · 4cf4292 · 4cf4292
1 parent 5a39761
commit 4cf4292
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 2 deletions.
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
@@ -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/...Code/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSHeadingOffsetTuner.java b/...Code/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/...ode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSPositionOffsetTuner.java b/...ode/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);