 //    mCompassHeading = (float) vector.z();
     
   bno.getEvent(&event);
-  Serial.print("X: ");
-  Serial.print(event.orientation.x, 4);
-  Serial.print("\tY: ");
-  Serial.print(event.orientation.y, 4);
-  Serial.print("\tZ: ");
-  Serial.print(event.orientation.z, 4);
-  Serial.print("\tT: ");
-  Serial.print(event.timestamp);
-  Serial.println("");
-  mCompassHeading = event.orientation.x; 
+//  vector = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
+//  Serial.print("X: ");
+//  Serial.print(event.orientation.x, 4);
+//  Serial.print("\tY: ");
+//  Serial.print(event.orientation.y, 4);
+//  Serial.print("\tZ: ");
+//  Serial.print(event.orientation.z, 4);
+//  Serial.print("\tT: ");
+//  Serial.print(event.timestamp);
+//  Serial.println("");
+//float Pi = 3.14159;
+//  mCompassHeading = (atan2(vector.y(), vector.x()) * 180) / Pi;
+//  if(mCompassHeading < 0){
+//     mCompassHeading += 360; 
+//  }
+  
+  mCompassHeading = event.orientation.x + mHeadingOffset;
+  if(mCompassHeading > 360){
+    mCompassHeading -= 360;
+  } else if (mCompassHeading < 0){
+     mCompassHeading += 360;
+  }
 //  float heading = atan2(event.orientation.y, event.orientation.x);
 //  
 //  // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.

 * Send forward command
 */
 void forward(){
  dataStruct.tar = 20;
  dataStruct.val = mSpeed / 2;
  etData.sendData();
  dataStruct.tar = 21;
  dataStruct.val = -mSpeed / 2;
  etData.sendData();
  Serial.print("forward ");
 }
 

*/

#define PIN_GPS 2
#include <Adafruit_NeoPixel.h>
#include <TargetRegistration.h>
#include <TinyGPS.h>
#include <SoftwareSerial.h>
#include <Wire.h> //Needed by the Adafruit BNO055 library
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <EasyTransfer.h>
#include <Servo.h> 

/** Edit these to tune navigation */
//Minimumn distance in meters to waypoint
