java

DeltaDizzy · Jan 22, 2024 · 4b26977 · 4b26977
1 parent 1f46814
commit 4b26977
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 21 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
@@ -14,6 +14,7 @@
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
@@ -192,6 +193,7 @@ public enum IMUAxis {
   private volatile boolean m_thread_idle = false;
   private boolean m_auto_configured = false;
   private boolean m_start_up_mode = true;
+  private Rotation3d m_angleOffset;
 
   /* Resources */
   private SPI m_spi;
@@ -673,6 +675,11 @@ public void reset() {
     }
   }
 
+  public void reset(Rotation3d offset) {
+    reset();
+    m_angleOffset = offset;
+  }
+
   /** Delete (free) the spi port used for the IMU. */
   @Override
   public void close() {
@@ -1049,9 +1056,9 @@ public IMUAxis getYawAxis() {
    */
   public synchronized double getGyroAngleX() {
     if (m_simGyroAngleX != null) {
-      return m_simGyroAngleX.get();
+      return m_simGyroAngleX.get() + m_angleOffset.getX();
     }
-    return m_integ_gyro_angle_x;
+    return m_integ_gyro_angle_x + m_angleOffset.getX();
   }
 
   /**
@@ -1061,9 +1068,9 @@ public synchronized double getGyroAngleX() {
    */
   public synchronized double getGyroAngleY() {
     if (m_simGyroAngleY != null) {
-      return m_simGyroAngleY.get();
+      return m_simGyroAngleY.get() + m_angleOffset.getY();
     }
-    return m_integ_gyro_angle_y;
+    return m_integ_gyro_angle_y + m_angleOffset.getY();
   }
 
   /**
@@ -1073,9 +1080,9 @@ public synchronized double getGyroAngleY() {
    */
   public synchronized double getGyroAngleZ() {
     if (m_simGyroAngleZ != null) {
-      return m_simGyroAngleZ.get();
+      return m_simGyroAngleZ.get() + m_angleOffset.getZ();
     }
-    return m_integ_gyro_angle_z;
+    return m_integ_gyro_angle_z + m_angleOffset.getZ();
   }
 
   /**

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
@@ -13,6 +13,7 @@
 
 // import java.lang.FdLibm.Pow;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
@@ -253,6 +254,7 @@ public enum IMUAxis {
   private volatile boolean m_thread_idle = false;
   private boolean m_auto_configured = false;
   private double m_scaled_sample_rate = 2500.0;
+  private Rotation3d m_angleOffset = new Rotation3d();
 
   // Resources
   private SPI m_spi;
@@ -1023,6 +1025,18 @@ public void reset() {
     }
   }
 
+  /**
+   * Reset the gyro.
+   * 
+   * <p>Resets the gyro accumulations to a heading of zero, with an offset rotation added. This can be used if there is significant
+   * drift in the gyro and it needs to be recalibrated after running.
+   * @param offset 3d angle to offset readings by
+   */
+  public void reset(Rotation3d offset) {
+    reset();
+    m_angleOffset = offset;
+  }
+
   /**
    * Allow the designated gyro angle to be set to a given value. This may happen with unread values
    * in the buffer, it is suggested that the IMU is not moving when this method is run.
@@ -1117,19 +1131,19 @@ public synchronized double getAngle(IMUAxis axis) {
     switch (axis) {
       case kX:
         if (m_simGyroAngleX != null) {
-          return m_simGyroAngleX.get();
+          return m_simGyroAngleX.get() + m_angleOffset.getX();
         }
-        return m_integ_angle_x;
+        return m_integ_angle_x + m_angleOffset.getX();
       case kY:
         if (m_simGyroAngleY != null) {
-          return m_simGyroAngleY.get();
+          return m_simGyroAngleY.get() + m_angleOffset.getY();
         }
-        return m_integ_angle_y;
+        return m_integ_angle_y + m_angleOffset.getY();
       case kZ:
         if (m_simGyroAngleZ != null) {
-          return m_simGyroAngleZ.get();
+          return m_simGyroAngleZ.get() + m_angleOffset.getZ();
         }
-        return m_integ_angle_z;
+        return m_integ_angle_z + m_angleOffset.getZ();
       default:
     }
 
@@ -1145,19 +1159,19 @@ public synchronized double getAngle() {
     switch (m_yaw_axis) {
       case kX:
         if (m_simGyroAngleX != null) {
-          return m_simGyroAngleX.get();
+          return m_simGyroAngleX.get() + m_angleOffset.getX();
         }
-        return m_integ_angle_x;
+        return m_integ_angle_x + m_angleOffset.getX();
       case kY:
         if (m_simGyroAngleY != null) {
-          return m_simGyroAngleY.get();
+          return m_simGyroAngleY.get() + m_angleOffset.getY();
         }
-        return m_integ_angle_y;
+        return m_integ_angle_y + m_angleOffset.getY();
       case kZ:
         if (m_simGyroAngleZ != null) {
-          return m_simGyroAngleZ.get();
+          return m_simGyroAngleZ.get() + m_angleOffset.getZ();
         }
-        return m_integ_angle_z;
+        return m_integ_angle_z + m_angleOffset.getZ();
       default:
     }
     return 0.0;

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
@@ -48,6 +48,7 @@ public class ADXRS450_Gyro implements Sendable, AutoCloseable {
   private SimBoolean m_simConnected;
   private SimDouble m_simAngle;
   private SimDouble m_simRate;
+  private Rotation2d m_angleOffset;
 
   /** Constructor. Uses the onboard CS0. */
   public ADXRS450_Gyro() {
@@ -183,6 +184,11 @@ public void reset() {
     }
   }
 
+  public void reset(Rotation2d offset) {
+    reset();
+    m_angleOffset = offset;
+  }
+
   /** Delete (free) the spi port used for the gyro and stop accumulating. */
   @Override
   public void close() {
@@ -213,12 +219,12 @@ public void close() {
    */
   public double getAngle() {
     if (m_simAngle != null) {
-      return m_simAngle.get();
+      return m_simAngle.get() + m_angleOffset.getDegrees();
     }
     if (m_spi == null) {
       return 0.0;
     }
-    return m_spi.getAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
+    return m_spi.getAccumulatorIntegratedValue() * kDegreePerSecondPerLSB + m_angleOffset.getDegrees();
   }
 
   /**
@@ -256,7 +262,7 @@ public double getRate() {
    * @return the current heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
    */
   public Rotation2d getRotation2d() {
-    return Rotation2d.fromDegrees(-getAngle());
+    return Rotation2d.fromDegrees(-getAngle()).plus(m_angleOffset);
   }
 
   @Override