 
 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);
 
 }
 

#define DEBUG


// Arduino pin setup
#define ESC_A 9
#define ESC_B 6
#define ESC_C 5

  vc = (abs((-100+bal_ac)/100))*v_ac;
  vd = (abs((-100+bal_bd)/100))*v_bd;
  
  if(velocity < ESC_TAKEOFF_OFFSET){
  
    va = ESC_MIN;
    vb = ESC_MIN;
    vc = ESC_MIN;
    vd = ESC_MIN;
  
  }
  
  #ifdef DEBUG
  
  //Serial.print("A: "+String(va)+" B: "+String(vb)+" C: "+String(vc)+" D: "+String(vd));
  /*Serial.print("AC: ");
  Serial.print(bal_ac);
  Serial.print(" BD: ");
  Serial.print(bal_bd);
  Serial.println("");*/
  #endif
  
}

void updateMotors(){
