   return imu.calcGyro(imu.gz) - calibratedOffset;
 }
 
+/**
+    Blinks an LED on pin 13
+    @param delay in milliseconds
+    @param number of blinks
+*/
 void blinkLED(int del, int n) {
   for (int i = 0; i < n; i++) {
     digitalWrite(LED, HIGH);

  }
}

void updateMotor() {
  update_flag = true;
}

void setupIMU() {
  imu.settings.device.commInterface = IMU_MODE_I2C;
  imu.settings.device.mAddress = LSM9DS1_M;

  azimuth.write(ZERO_SPEED);
}

/**
    Reads the IMU's gyro
    @return rate in degrees / ms
*/
float readGyro() {
  if (imu.gyroAvailable()) {
    imu.readGyro();
