fix: corrected the angular velocity axes

See SDK issue FIRST-Tech-Challenge/FtcRobotController#251
Lunerwalker2 · Dec 9, 2021 · 5143277 · 5143277
1 parent 1a52698
commit 5143277
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 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
@@ -298,7 +298,20 @@ 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;
+        /*TODO: IMPORTANT!
+        If you are using the X axis from the above diagram,
+        ensure that you use the negative Z axis for angular velocity ONLY.
+        Likewise, if you are using the Z axis from above, ensure that you use the
+        negative X axis for angular velocity ONLY (this does NOT affect orientation).
+
+        //For angular velocity around the X axis:
+        return (double) -imu.getAngularVelocity().zRotationRate;
+
+        The Y axis is not affected.
+         */
+
+        //See comment above, following is for angular velocity around the Z axis
+        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
@@ -302,7 +302,20 @@ 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;
+        /*TODO: IMPORTANT!
+        If you are using the X axis from the above diagram,
+        ensure that you use the negative Z axis for angular velocity ONLY.
+        Likewise, if you are using the Z axis from above, ensure that you use the
+        negative X axis for angular velocity ONLY (this does NOT affect orientation).
+
+        //For angular velocity around the X axis:
+        return (double) -imu.getAngularVelocity().zRotationRate;
+
+        The Y axis is not affected.
+         */
+
+        //See comment above, following is for angular velocity around the Z axis
+        return (double) -imu.getAngularVelocity().xRotationRate;
     }
 
     public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {