add java doc comments

diff --git a/src/org/usfirst/frc/team3501/robot/utils/BNO055.java b/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
index 2baeafd2121d792c9004c9c72acaed44e89b226e..b6640fed99571c98db0e88f5dcb3bde641035327 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
+++ b/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
@@ -93,7 +93,7 @@ public class BNO055 {
   private volatile long turns = 0;
   private volatile double[] xyz = new double[3];
 
-  private float zeroReferenceConst = 0;;
+  private double zeroReferenceConst = 0;
 
   public class SystemStatus {
     public int system_status;
@@ -315,13 +315,15 @@ public class BNO055 {
     executor = new java.util.Timer();
     executor.schedule(new BNO055UpdateTask(this), 0L, THREAD_PERIOD);
 
-    Timer.delay(2);
-
     setZeroReferenceConst();
   }
 
-  public void setZeroReferenceConst() {
-    double zeroReferenceConst = getHeading();
+  /***
+   * Sets the starting heading to the zero reference angle of the readings
+   * This method should be left private and only be ran once
+   */
+  private void setZeroReferenceConst() {
+    zeroReferenceConst = getHeading();
   }
 
   /**
@@ -338,6 +340,7 @@ public class BNO055 {
   public static BNO055 getInstance(opmode_t mode, vector_type_t vectorType,
       I2C.Port port, byte address) {
     if (instance == null) {
+      System.out.println("gyro called");
       instance = new BNO055(port, address);
     }
     requestedMode = mode;
@@ -374,6 +377,7 @@ public class BNO055 {
       // blocking manner. This sequence of events follows the process
       // defined in the original adafruit source as closely as possible.
       // XXX: It's likely some of these delays can be optimized out.
+      System.out.println("State: " + state);
       switch (state) {
       case 0:
         // Wait for the sensor to be present
@@ -446,6 +450,7 @@ public class BNO055 {
         break;
       case 8:
         if (currentTime >= nextTime) {
+          nextTime = Timer.getFPGATimestamp() + 1.05;
           state++;
         }
       case 9:
@@ -754,6 +759,10 @@ public class BNO055 {
    * @return heading in degrees
    */
   public double getHeading() {
+    double reading = (xyz[0] + turns * 360);
+    if (zeroReferenceConst == 0)
+      return (xyz[0] + turns * 360);
+
     return (xyz[0] + turns * 360) - zeroReferenceConst;
   }