   // Setup motor update interval timer
   MsTimer2::set(motorUpdateRate, updateMotor);
   MsTimer2::start();
+
+  ref = millis();
 }
 
 void loop() {

// ============ Runtime variables ========================
// Orientation data
float positionChange;

// Interrupt flag
int timer = millis();
bool update_flag = false; // interrupt flag for executing motor update

/**
    Sets up system
*/

// ================ SYSTEM PARAMETERS ===================
// Sample rates
const int motorUpdateRate = 50; // gyro sample rate
const int calibrationSamples = 1000;

// Initial calibration value
float calibratedOffset;

// Indicator LED
const int LED = 13;
