Updated swervelib

swervelib/imu/NavXSwerve.java

@@ -0,0 +1,97 @@
+package frc.robot.subsystems.swervedrive2.swervelib.imu;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.SerialPort;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Communicates with the NavX as the IMU.
+ */
+public class NavXSwerve extends SwerveIMU
+{
+
+  /**
+   * NavX IMU.
+   */
+  private AHRS   gyro;
+  /**
+   * Offset for the NavX yaw reading.
+   */
+  private double yawOffset = 0;
+
+  /**
+   * Constructor for the NavX swerve.
+   */
+  public NavXSwerve()
+  {
+    try
+    {
+      /* Communicate w/navX-MXP via the MXP SPI Bus.                                     */
+      /* Alternatively:  I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB     */
+      /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details. */
+      gyro = new AHRS(SerialPort.Port.kMXP);
+      SmartDashboard.putData(gyro);
+    } catch (RuntimeException ex)
+    {
+      DriverStation.reportError("Error instantiating navX-MXP:  " + ex.getMessage(), true);
+    }
+  }
+
+  /**
+   * Reset IMU to factory default.
+   */
+  @Override
+  public void factoryDefault()
+  {
+    // gyro.reset(); // Reported to be slow
+    yawOffset = gyro.getYaw();
+  }
+
+  /**
+   * Clear sticky faults on IMU.
+   */
+  @Override
+  public void clearStickyFaults()
+  {
+  }
+
+  /**
+   * Set the yaw in degrees.
+   *
+   * @param yaw Yaw angle in degrees.
+   */
+  @Override
+  public void setYaw(double yaw)
+  {
+    // gyro.reset(); // Reported to be slow using the offset.
+    if (yaw != 0)
+    {
+      yawOffset = yaw + gyro.getYaw();
+    }
+  }
+
+  /**
+   * Fetch the yaw/pitch/roll from the IMU.
+   *
+   * @param yprArray Array which will be filled with {yaw, pitch, roll} in degrees.
+   */
+  @Override
+  public void getYawPitchRoll(double[] yprArray)
+  {
+    yprArray[0] = gyro.getYaw() - yawOffset;
+    yprArray[1] = gyro.getPitch();
+    yprArray[2] = gyro.getRoll();
+  }
+
+  /**
+   * Get the instantiated IMU object.
+   *
+   * @return IMU object.
+   */
+  @Override
+  public Object getIMU()
+  {
+    return gyro;
+  }
+}