   motor_left.writeMicroseconds(mL - 100);
   motor_front.writeMicroseconds(mF - 100);
   motor_back.writeMicroseconds(mB - 100);
-  print_pitch_and_roll();
+  //print_pitch_and_roll();
 
 }
 

// MOTORS
int mR, mL, mF, mB;

//rx
int throttle = THROTTLE_RMIN;
volatile int input[4];

unsigned long timer[4];
byte last_channel[4];


float roll_angle;
float pitch_angle;
float yaw_angle;

Adafruit_BNO055 bno = Adafruit_BNO055();

PID roll_controller(&pid_roll_in, &pid_roll_out, &pid_roll_setpoint, 5.0, 1.0, 1.0, REVERSE);
PID pitch_controller(&pid_pitch_in, &pid_pitch_out, &pid_pitch_setpoint, 5.0, 1.0, 1.0, REVERSE);
PID yaw_controller(&pid_yaw_in, &pid_yaw_out, &pid_yaw_setpoint, 5.0, 1.0, 1.0, REVERSE);


void bno_initialisation() {
  bno.begin();
  bno.setExtCrystalUse(true);

#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

Servo motor_right;
Servo motor_left;
Servo motor_front;
Servo motor_back;

// PID variables
double pid_roll_in,   pid_roll_out,   pid_roll_setpoint = 0;
