 #define LOOP_LOGGING 0
 //1: enable logging in loop
 //0: disable logging in loop
-
-#define RUN_MOTORS 1
 
 #define MAX_FREQUENCY 50
 const long MIN_LOOP_TIME = 1000/MAX_FREQUENCY;

  return rad * 360.0/(2.0*PI/360);
}

float yawRollover(float yaw) {
  static float prev = 0;
  static float mod = 0;
  float delta = yaw + mod - prev
  if(delta < prev-360.0){
    //rollover 360 
    mod = mod + 360.0;
  } else if(delta > prev) {
    //rollover 0
    mod = mod - 360.0;
  }
  prev = yaw+mod;
  return prev;
}

void setup() {

}

void loop() {
  //filter setup
  FilterOnePole rollHighpass(HIGHPASS, rollFrequencyCutoff);
  FilterOnePole pitchHighpass(HIGHPASS, pitchFrequencyCutoff);
  FilterOnePole yawHighpass(HIGHPASS, yawFrequencyCutoff);

  //rollover storage
  float roll_mod = 0.0;
  float pitch_mod = 0.0;
  float yaw_mod = 0.0;
  
  float roll_prev = 0.0;
  float pitch_prev = 0.0;
  float yaw_prev = 0.0;

  float roll_rollover = 0.0;
  float pitch_rollover = 0.0;
  float yaw_rollover = 0.0;
  
  while(1) {
    
    //imu or failover
    if(!updateImu()) {
      logln("Something went wrong with the IMU.");
      return; //skip the cycle if the imu doesn't update correctly
    }
    
    //limit cycle frequency to MAX_FREQUENCY
    static long time_last = 0;
    while(millis()-time_last < MIN_LOOP_TIME);
    long loop_time = millis() - time_last;
    time_last += loop_time;
    
    //update rollover
    roll_rollover = rollover(&roll_prev, &roll_mod, imu.roll);
    pitch_rollover = rollover(&pitch_prev, &pitch_mod, imu.pitch);
    yaw_rollover = rollover(&yaw_prev, &yaw_mod, imu.yaw);
    
    //update washout filters
    rollHighpass.input(roll_rollover);
    pitchHighpass.input(pitch_rollover);
    yawHighpass.input(yaw_rollover);
    
    //set motor commands
    setVal(2, ANGLE, degree2rad(rollHighpass.output()) + 3.14); //roll
    setVal(1, ANGLE, degree2rad(pitchHighpass.output()) + 1.5); //pitch
    setVal(3, ANGLE, degree2rad(-yawHighpass.output()) + 1.5);  //yaw
    
    //sync motor info
    syncMotor(2); //roll
    syncMotor(1); //pitch
    syncMotor(3); //yaw
  
    logln("Compiling output...");
  
