 
 void arm(){
 
-  a.write(22);
-  b.write(22);
-  c.write(22);
-  d.write(22);
-  
-  delay(5000);
+  a.write(ESC_MIN);
+  b.write(ESC_MIN);
+  c.write(ESC_MIN);
+  d.write(ESC_MIN);
+  
+  delay(ESC_ARM_DELAY);
 
 }
 

unsigned long rcLastChange4 = millis();
unsigned long rcLastChange5 = millis();

// MOTOR SETUP

int velocity; //global velocity

float bal_ac, bal_bd; // motor balances can vary between -100 & 100
float bal_axes; // throttle balance between axes -100:ac , +100:bd

Servo a,b,c,d;

// PID SETUP

PID pitchReg(&ypr[1], &bal_bd, &ch2, pKp, pKi, pKd, REVERSE);
PID rollReg(&ypr[2], &bal_ac, &ch1, rKp, rKi, rKd, REVERSE);
PID yawReg(&ypr[0], &bal_axes, &ch4, yKp, yKi, yKd, DIRECT);


void setup(){
  
  initRC();
  initMPU();
  initESCs();
  initBalancing();
  initRegulators();
  
  #ifdef DEBUG
  
  Serial.begin(9600);
  Serial.flush();
  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  
  #endif
}

void loop(){
  
  while(!mpuInterrupt && fifoCount < packetSize){
     //Do nothing
     //This should be a VERY short period
  }
  
  getYPR();
  computePID();
  calculateVelocities();
  updateMotors();
  
}

void initRegulators(){

  pitchReg.SetMode(AUTOMATIC);
  pitchReg.SetOutputLimits(-15, 15);
  
  rollReg.SetMode(AUTOMATIC);
  rollReg.SetOutputLimits(-20, 20);
  
  yawReg.SetMode(AUTOMATIC);
  yawReg.SetOutputLimits(-20, 20);

}

void computePID(){

  ch2 = map(ch2, 100, 200, -45, 45);
  ch1 = map(ch1, 100, 200, -45, 45);
  ch4 = map(ch4, 100, 200, -180, 180);
  
  ypr[0] = ypr[0] * 180/M_PI;
  ypr[1] = ypr[1] * 180/M_PI;


}

void initRegulators(){

  pitchReg.SetMode(AUTOMATIC);
  pitchReg.SetOutputLimits(-PID_PITCH_INFLUENCE, PID_PITCH_INFLUENCE);
  
  rollReg.SetMode(AUTOMATIC);
  rollReg.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);
  
  yawReg.SetMode(AUTOMATIC);
  yawReg.SetOutputLimits(-PID_YAW_INFLUENCE, PID_YAW_INFLUENCE);

}

void rcInterrupt1(){
   ch1 = millis() - rcLastChange1;
   rcLastChange1 = millis(); 
