move setting of zeroReferenceConst into the initialization switch statement

diff --git a/src/org/usfirst/frc/team3501/robot/utils/BNO055.java b/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
index 8d530541cb17d9071587e796933cb00063beca2e..21f9b05d9d28acb37418cc186d78b3c3c4bd2dbc 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
+++ b/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
@@ -314,10 +314,6 @@ public class BNO055 {
 
     executor = new java.util.Timer();
     executor.schedule(new BNO055UpdateTask(this), 0L, THREAD_PERIOD);
-
-    zeroReferenceConst = getHeading();
-
-    System.out.println("Initital Heading: " + zeroReferenceConst);
   }
 
   /**
@@ -445,12 +441,15 @@ public class BNO055 {
         break;
       case 8:
         if (currentTime >= nextTime) {
-
           nextTime = Timer.getFPGATimestamp() + 1.05;
           state++;
         }
       case 9:
-        initialized = true;
+        if (currentTime >= nextTime) {
+          calculateVector();
+          zeroReferenceConst = getRawHeading();
+          initialized = true;
+        }
         break;
       default:
         // Should never get here - Fail safe
@@ -748,7 +747,6 @@ public class BNO055 {
 
   /**
    * Heading not relative to the starting angle of the robot.
-   * Exists only for debugging purposes.
    *
    * @return
    */