   ch2Last = ch2;
   ch4Last = ch4;
   
-  Serial.println(ch2);
-  
   ypr[0] = ypr[0] * 180/M_PI;
   ypr[1] = ypr[1] * 180/M_PI;
   ypr[2] = ypr[2] * 180/M_PI;
   
-  pitchReg.Compute();
+  Serial.print("CH2:\t");
+  Serial.print(ch2);
+  Serial.print("\tPITCH:\t");
+  Serial.print(ypr[1]);
+  
+  bool test = pitchReg.Compute();
   rollReg.Compute();
   yawReg.Compute();
+  
+  Serial.print("\tBAL_BD:\t");
+  Serial.print(bal_bd);
+  Serial.print("\tNEW PID?\t");
+  Serial.println(test);
 
 }
 

 *  filters the rc input
 */

void smoothInput(){

  ch1 = floor(ch1/100)*100;
  ch2 = floor(ch2/100)*100;
  ch3 = floor(ch3/100)*100;
  ch4 = floor(ch4/100)*100;
  ch5 = floor(ch5/100)*100;

}

/*  calculateVelocities function

#define RC_LOW_CH4 2000
#define RC_HIGH_CH5 1000
#define RC_LOW_CH5 2000
#define RC_ROUNDING_BASE 50

/*  PID configuration
 *  
 */

#define PITCH_P_VAL 2
#define PITCH_I_VAL 5
#define PITCH_D_VAL 1

#define ROLL_P_VAL 2
#define ROLL_I_VAL 5
