diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PoseMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PoseMessage.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java index a5b5bbc..90d4e6d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java @@ -30,7 +30,7 @@ public class LocalizationTest extends LinearOpMode { telemetry.addData("x", drive.pose.position.x); telemetry.addData("y", drive.pose.position.y); - telemetry.addData("heading", drive.pose.heading); + telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.log())); telemetry.update(); } } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) { @@ -51,7 +51,7 @@ public class LocalizationTest extends LinearOpMode { telemetry.addData("x", drive.pose.position.x); telemetry.addData("y", drive.pose.position.y); - telemetry.addData("heading", drive.pose.heading); + telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.log())); telemetry.update(); } } else {