 
   // Start GPS and tell it to send GPGGA and GPRMC strings
   ss.begin(GPSBAUD);    // local GPS
-//  ss.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
+  //ss.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
 
   // Set up BNO055 IMU
   if(!bno.begin()){
     Serial.println(F("Ooops, could not find BNO055... Check your wiring or I2C ADDR!"));
-//    while(1);
   } else {
     Serial.println(F("BNO055 IMU detected.."));
   }
+  delay(100); // give BNO055 time to start up
   bno.setExtCrystalUse(true);                     //Use the external clock in the IMU
-  bno.setMode(bno.OPERATION_MODE_NDOF);
+//  bno.setMode(bno.OPERATION_MODE_NDOF);
 
   // Set stepper motor acceleration and top speeds
-  xAxis.setMaxSpeed(10000);
-  xAxis.setAcceleration(5000);
-  yAxis.setMaxSpeed(10000);
-  yAxis.setAcceleration(5000);
+  xAxis.setMaxSpeed(8000);
+  xAxis.setAcceleration(4000);
+  yAxis.setMaxSpeed(8000);
+  yAxis.setAcceleration(4000);
   // Temporary
   balance();
 }

  }
}

void updateMotors(double aziDegs, double eleDegs){
  azimuth_steps = degreesToSteps(-aziDegs);
  elevation_steps = degreesToSteps(-eleDegs);
  Serial.print(F("yaw="));
  Serial.print(aziDegs);
  Serial.print(F(", pitch="));
  Serial.print(eleDegs);
  Serial.print(F(", "));
  Serial.print(sys);
  Serial.print(F(", "));
  Serial.print(gyro);
  Serial.print(F(", "));
  Serial.print(accel);
  Serial.print(F(", "));
  Serial.println(mag);
  xAxis.moveTo(azimuth_steps);
  yAxis.moveTo(elevation_steps);
}


void balance(){
  while(1){
    // Read IMU 10 times per second
    if(millis() - imuTimer > 100){
      imuTimer = millis(); // reset the timer
      //Read the current calibration values from the IMU
      bno.getCalibration(&sys, &gyro, &accel, &mag);
      //Read the current positional values
      bno.getEvent(&event);
    
      // Using quaternions
      imu::Quaternion q = bno.getQuat();
      q.normalize();
      float temp = q.x();  q.x() = -q.y();  q.y() = temp;
      q.z() = -q.z();
      // Converted back to eulers
      imu::Vector<3> euler = q.toEuler();
      
      updateMotors(
        radsToDegrees(euler.x()),
        radsToDegrees(euler.y())
      );
    }
  
    xAxis.run();

}


long degreesToSteps(double deg){
  // Takes degrees, returns steps (assuming 16*3060 = 48960 total steps)
  // deg / 360 = steps / (microsteps * motor steps)
  // 3000/0.12*15.3 = 45900
  // deg * 45900 / 360
  return round(deg * 127.5);
}

double radsToDegrees(double rad){
