From d8764876501e215a57ef4275042b51b5ab3e8eeb Mon Sep 17 00:00:00 2001 From: DeltaDizzy Date: Wed, 1 May 2024 01:45:26 -0400 Subject: [PATCH] add insead of subtract the negative --- .../main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java | 8 ++++---- .../first/wpilibj/simulation/ADIS16448_IMUSimTest.java | 2 +- 2 files changed, 5 insertions(+), 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 index 5d02f5de015..1cac5234bab 100644 --- 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 index bd192487ee7..721612ef846 100644 --- 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);