 //1: serial logging,
 //2: SD card logging
 
-#define LOOP_LOGGING 1
+#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;

float rollFrequencyCutoff = 0.5;
float pitchFrequencyCutoff = 0.5;
float yawFrequencyCutoff = 0.5;
FilterOnePole rollHighpass(HIGHPASS, rollFrequencyCutoff);
FilterOnePole pitchHighpass(HIGHPASS, pitchFrequencyCutoff);
FilterOnePole yawHighpass(HIGHPASS, yawFrequencyCutoff);

//led output pin
const int kLedPin =  13;

///
//

float degree2rad(float degree) {
  return degree*2.0*PI/360.0;
}

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

//float yawRollover(float yaw) {
//  static float prev = 0;
//  static float mod = 0;
//  if(abs() > ) 
//}

void setup() {
  pinMode(kLedPin, OUTPUT);
  dataLoggingSetup();
  imuSetup();
  motControlSetup();
  motSetup(1, 100,0,0);
  motSetup(2, 100,0,0);
  motSetup(3, 100,0,0);
  
  if(!LOOP_LOGGING) {
    setLogPermission(false);
