add insead of subtract the negative

DeltaDizzy · May 1, 2024 · d876487 · d876487
1 parent 17ad277
commit d876487
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 5 deletions.
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
@@ -711,7 +711,7 @@ public void reset(Rotation3d newAngle) {
         m_simGyroAngleZ.set(0.0);
       }
 
-      m_angleOffset = newAngle.unaryMinus();
+      m_angleOffset = newAngle;
     }
   }
 
@@ -1113,7 +1113,7 @@ private synchronized Rotation3d getGyroOrientation() {
    * @return The accumulated gyro angle in the X axis in degrees.
    */
   public synchronized double getGyroAngleX() {
-    return Units.radiansToDegrees(getGyroOrientation().minus(m_angleOffset).getX());
+    return Units.radiansToDegrees(getGyroOrientation().plus(m_angleOffset).getX());
   }
 
   /**
@@ -1122,7 +1122,7 @@ public synchronized double getGyroAngleX() {
    * @return The accumulated gyro angle in the Y axis in degrees.
    */
   public synchronized double getGyroAngleY() {
-    return Units.radiansToDegrees(getGyroOrientation().minus(m_angleOffset).getY());
+    return Units.radiansToDegrees(getGyroOrientation().plus(m_angleOffset).getY());
   }
 
   /**
@@ -1131,7 +1131,7 @@ public synchronized double getGyroAngleY() {
    * @return The accumulated gyro angle in the Z axis in degrees.
    */
   public synchronized double getGyroAngleZ() {
-    return Units.radiansToDegrees(getGyroOrientation().minus(m_angleOffset).getZ());
+    return Units.radiansToDegrees(getGyroOrientation().plus(m_angleOffset).getZ());
   }
 
   /**

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSimTest.java
@@ -46,7 +46,7 @@ void testOffset() {
 
       sim.setGyroAngleZ(90);
       assertEquals(180, gyro.getAngle(), 0.0001);
-
+      
       gyro.reset(new Rotation3d(0, Units.degreesToRadians(90), Units.degreesToRadians(90)));
       assertEquals(90, gyro.getGyroAngleY(), 0.0001);
       assertEquals(90, gyro.getAngle(), 0.0001);