From 056b1d958f892ef47e5d6c487b89bc7792e1ab00 Mon Sep 17 00:00:00 2001
From: Lunerwalker2 <55162022+Lunerwalker2@users.noreply.github.com>
Date: Thu, 9 Dec 2021 12:11:11 -0500
Subject: [PATCH] Adjust the IMU angular velocity axes (#159)

Mitigates SDK issue https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/251
---
 .../ftc/teamcode/drive/SampleMecanumDrive.java             | 7 ++++++-
 .../firstinspires/ftc/teamcode/drive/SampleTankDrive.java  | 7 ++++++-
 2 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
index d876be7d..91ab37b7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
@@ -298,7 +298,12 @@ public Double getExternalHeadingVelocity() {
         // Rotate about the z axis is the default assuming your REV Hub/Control Hub is laying
         // flat on a surface
 
-        return (double) imu.getAngularVelocity().zRotationRate;
+        // To work around an SDK bug, use -zRotationRate in place of xRotationRate 
+        // and -xRotationRate in place of zRotationRate (yRotationRate behaves as 
+        // expected). This bug does NOT affect orientation. 
+        //
+        // See https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/251 for details.
+        return (double) -imu.getAngularVelocity().xRotationRate;
     }
 
     public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
index fcecc2fd..ed0e1a6c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
@@ -302,7 +302,12 @@ public Double getExternalHeadingVelocity() {
         // Rotate about the z axis is the default assuming your REV Hub/Control Hub is laying
         // flat on a surface
 
-        return (double) imu.getAngularVelocity().zRotationRate;
+        // To work around an SDK bug, use -zRotationRate in place of xRotationRate
+        // and -xRotationRate in place of zRotationRate (yRotationRate behaves as
+        // expected). This bug does NOT affect orientation.
+        //
+        // See https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/251 for details.
+        return (double) -imu.getAngularVelocity().xRotationRate;
     }
 
     public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {