Add waiting for initialization of IMU before reading heading. Improve method naming...

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index e852b752a1a71246717d909a859ca92f9de28d2e..91a1e726690a6a926bf6e37db6d4a85dff39532e 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -54,7 +54,11 @@ public class Robot extends IterativeRobot {
   @Override
   public void teleopPeriodic() {
     Scheduler.getInstance().run();
-    System.out.println("Heading: " + imu.getHeading() + " Init: "
-        + imu.isInitialized() + " Calib: " + imu.isCalibrated());
+    if (imu.isInitialized()) {
+      System.out.println(imu.getHeading());
+    }
+    // System.out.println("Heading: "
+    // + imu.getHeading() + " Init: "
+    // + imu.isInitialized() + " Calib: " + imu.isCalibrated());
   }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/utils/BNO055.java b/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
index 21f9b05d9d28acb37418cc186d78b3c3c4bd2dbc..e6d1a1521b20ce38a545ae8c444e64bf2080b2aa 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
+++ b/src/org/usfirst/frc/team3501/robot/utils/BNO055.java
@@ -746,16 +746,17 @@ public class BNO055 {
   }
 
   /**
-   * Heading not relative to the starting angle of the robot.
+   * The default sensor heading not relative to the starting angle of the robot.
    *
    * @return
    */
-  public double getRawHeading() {
+  public double getDefaultHeading() {
     return xyz[0] + turns * 360;
   }
 
   /**
-   * The heading of the sensor (x axis) in continuous format. Eg rotating the
+   * The heading of the sensor (x axis) in continuous format relative to the
+   * initial heading of the robot. Eg rotating the
    * sensor clockwise two full rotations will return a value of 720 degrees.
    * The getVector method will return heading in a constrained 0 - 360 deg
    * format if required.
@@ -763,7 +764,7 @@ public class BNO055 {
    * @return heading in degrees
    */
   public double getHeading() {
-    double reading = getRawHeading();
+    double reading = getDefaultHeading();
 
     return reading - zeroReferenceConst;
   }