diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index a05c4f9a5799..5b0215a6e9f1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -291,12 +291,12 @@ public boolean run(@NonNull TelemetryPacket p) { p.put("x", pose.position.x); p.put("y", pose.position.y); - p.put("heading (deg)", Math.toDegrees(pose.heading.log())); + p.put("heading (deg)", Math.toDegrees(pose.heading.toDouble())); Pose2d error = txWorldTarget.value().minusExp(pose); p.put("xError", error.position.x); p.put("yError", error.position.y); - p.put("headingError (deg)", Math.toDegrees(error.heading.log())); + p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble())); // only draw when active; only one drive action should be active at a time Canvas c = p.fieldOverlay(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java index d92df77cb6c8..5a393d7f13ae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java @@ -304,12 +304,12 @@ public boolean run(@NonNull TelemetryPacket p) { p.put("x", pose.position.x); p.put("y", pose.position.y); - p.put("heading (deg)", Math.toDegrees(pose.heading.log())); + p.put("heading (deg)", Math.toDegrees(pose.heading.toDouble())); Pose2d error = txWorldTarget.value().minusExp(pose); p.put("xError", error.position.x); p.put("yError", error.position.y); - p.put("headingError (deg)", Math.toDegrees(error.heading.log())); + p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble())); targetPoseWriter.write(new PoseMessage(txWorldTarget.value())); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java index 8d540547c146..c87bfa02b6cc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java @@ -12,7 +12,7 @@ public PoseMessage(Pose2d pose) { this.timestamp = System.nanoTime(); this.x = pose.position.x; this.y = pose.position.y; - this.heading = pose.heading.log(); + this.heading = pose.heading.toDouble(); } } 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 90d4e6ddc37f..b75f768d9d46 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 void runOpMode() throws InterruptedException { telemetry.addData("x", drive.pose.position.x); telemetry.addData("y", drive.pose.position.y); - telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.log())); + telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble())); telemetry.update(); } } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) { @@ -51,7 +51,7 @@ public void runOpMode() throws InterruptedException { telemetry.addData("x", drive.pose.position.x); telemetry.addData("y", drive.pose.position.y); - telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.log())); + telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble())); telemetry.update(); } } else {