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


  // Start GPS and tell it to send GPGGA and GPRMC strings
  ss.begin(GPSBAUD);    // local GPS
//  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.."));
  }
  bno.setExtCrystalUse(true);                     //Use the external clock in the IMU
  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);
  // Temporary
  balance();
}

  }
}


void updateMotors(double aziDegs, double eleDegs){
  if(isnan(aziDegs) || isnan(eleDegs)){
    Serial.println("Moving to 0, 0");
    xAxis.moveTo(0);
    yAxis.moveTo(0);
  } else {
    long a = degreesToSteps(aziDegs);
    long e = -degreesToSteps(eleDegs);
//    Serial.print(F("yaw="));
//    Serial.print(aziDegs);
//    Serial.print(F(", "));
//    Serial.print(a);
//    Serial.print(F(", pitch="));
//    Serial.print(eleDegs);
//    Serial.print(F(", "));
//    Serial.print(e);
//    Serial.print(F(", calibrations: "));
//    Serial.print(sys);
//    Serial.print(F(", "));
//    Serial.print(gyro);
//    Serial.print(F(", "));
//    Serial.print(accel);
//    Serial.print(F(", "));
//    Serial.println(mag);
    xAxis.moveTo(a);
    yAxis.moveTo(e);

  }
}


void balance(){
  while(1){
    // Read IMU 10 times per second
    if(millis() - imuTimer > 250){
      imuTimer = millis(); // reset the timer

      imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
      long x = map(euler.x(), 0, 360, 0, 45900);
      long y = map(euler.y(), 0, 360, 0, 45900);
      long z = map(euler.z(), 0, 360, 0, 45900);
//      Serial.print("x=");     Serial.print(euler.x());      Serial.print(" -> ");     Serial.println(x);
//      Serial.print("y=");     Serial.print(euler.y());      Serial.print(" -> ");     Serial.println(y);
//      Serial.print("z=");     Serial.print(euler.z());      Serial.print(" -> ");     Serial.println(z);
//      Serial.println("------------------");
      xAxis.moveTo(y);
      yAxis.moveTo(-z);
    }
  
    xAxis.run();
