void gimbal(){
  sBmx160SensorData_t Omagn, Ogyro, Oaccel;
  bmx160.getAllData(NULL, NULL, &Oaccel);
  
//  dP = ((atan((Oaccel.y) / sqrt(pow((Oaccel.x), 2) + pow((Oaccel.z), 2))) * 4068 / 71));
  dP = ((atan((Oaccel.y) / sqrt(pow((Oaccel.x), 2) + pow((Oaccel.z), 2)))) * 180)/ PI;
//  dR= ((atan(-1 * (Oaccel.x) / sqrt(pow((Oaccel.y), 2) + pow((Oaccel.z), 2))) * 4068 / 71));
  dR= ((atan(-1 * (Oaccel.x) / sqrt(pow((Oaccel.y), 2) + pow((Oaccel.z), 2)))) * 180) / PI;
  s_Roll.write(dR+90);
  s_Pitch.write(dP+90);
}