From 6fea7e69aa0404cbcae7d777f9762c97b9372a72 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sun, 22 Jan 2023 08:47:57 -0600 Subject: [PATCH] Make Robot#facing return 360 degree rotation --- .../org/timecrafters/minibots/cyberarm/chiron/Robot.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java index c98d84e..1f9cf74 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java @@ -368,10 +368,10 @@ public class Robot { throw new RuntimeException("Failed to find variable with name: " + variableName + " in group: Robot, action: Tuning"); } - // TODO: Convert to 360 degree range with +90 degrees being on the RIGHT public double facing() { - // FIXME: Apply imuAngleOffset - return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + double imuDegrees = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + + return (imuDegrees + imuAngleOffset + 360.0) % 360.0; } public double heading() {