Remove unecessary if satements in getHeading() and add getRawHeading()

diff --git a/src/org/usfirst/frc/team3501/robot/utils/BNO055.java b/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
index 08ca79fa136fcbabd8fb13629fce91dca99a5dd3..8d530541cb17d9071587e796933cb00063beca2e 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
+++ b/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
@@ -316,6 +316,8 @@ public class BNO055 {
     executor.schedule(new BNO055UpdateTask(this), 0L, THREAD_PERIOD);
 
     zeroReferenceConst = getHeading();
+
+    System.out.println("Initital Heading: " + zeroReferenceConst);
   }
 
   /**
@@ -744,6 +746,16 @@ public class BNO055 {
     return xyz;
   }
 
+  /**
+   * Heading not relative to the starting angle of the robot.
+   * Exists only for debugging purposes.
+   *
+   * @return
+   */
+  public double getRawHeading() {
+    return xyz[0] + turns * 360;
+  }
+
   /**
    * The heading of the sensor (x axis) in continuous format. Eg rotating the
    * sensor clockwise two full rotations will return a value of 720 degrees.
@@ -753,12 +765,9 @@ public class BNO055 {
    * @return heading in degrees
    */
   public double getHeading() {
-    double reading = (xyz[0] + turns * 360);
-
-    if (zeroReferenceConst == 0)
-      return (xyz[0] + turns * 360);
+    double reading = getRawHeading();
 
-    return (xyz[0] + turns * 360) - zeroReferenceConst;
+    return reading - zeroReferenceConst;
   }
 
   /**