Servo + accelerometer (MPU 6050)

Good evening,
I’m working in a school project and i’m trying to create an equilibrist robot and i had a problem in it’s code. I am writing you in order to avoid that problem and i order to succeed in my project. I need your help and your opinion of my program.

#include <Servo.h>   // incluimos la biblioteca Servo 

     Servo miservo;       // creamos un objecto servo para controlar el motor 
     Servo miservo2; 
     
// comienza el programa (estableciendo las variables)
#include <Wire.h> //INCLUYE LA LIBRERIA PARA PODER UTILIZAR EL MPU 6050
#define MPU 0x68
#define A_R 16384.0
#define G_R 131.0

int16_t AcX, AcY, AcZ, GyX, GyY, GyZ; // HEMOS DE GUARDAR LOS VALORES QUE NOS DE LA IMU EN 16 BITS YA QUE SI NO PONEMOS INT16_T LO GUARDA EN MENOS BITS

//ahora definimos los angulos
float Acc[2]; // ANGULO X = 0   ANGULO Y =1  EN TODOS   
float Gy[2];
float Angle [2];
// para poder pasar de radianes a grados
#define RAD_A_DEG= 57.295779


void setup() { // esto solo se repite una vez al inciar
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  miservo.attach(9);  // liga el servo al pin 9 
  miservo2.attach(8);
  Serial.begin(9600);
  // hemos activado la IMU enviando el valor de 0 y se inicia el puerto de serie para ver los resultados 
  


}

void loop() {// lo que se repite todas las veces, voy a poner que la variacion de tiempo entre calculo y calculo es de 0.01 pero he encontrado que hay una opcion en Arduino de milis() para hacerlo mas preciso
 //empezamos por leer los valores que nos da el accelerometro de nuestra IMU (MPU 6050)
//creamos variable char para pasarle los datos del bluetooth desde la app
 Wire.beginTransmission(MPU); // la nuestra, 6050
 Wire.write(0x3B); // pedimos el registro del accelerometro angulo x --> AcX
 Wire.endTransmission(false);
 Wire.requestFrom(MPU,6,true);// hemos pedido seis registros a partir del 0x3B
 AcX=Wire.read()<<8|Wire.read();// cada valor ocupa dos registros
 AcY=Wire.read()<<8|Wire.read();
 AcZ=Wire.read()<<8|Wire.read();
 // calculamos el angulo Y a partir del angulo del accelerometro
 Acc[1]=atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R),2)+pow((AcZ+A_R),2)))*RAD_TO_DEG;
 Acc[0]=atan((AcY/A_R)/sqrt(pow((AcX/A_R),2)+pow((AcZ+A_R),2)))*RAD_TO_DEG;
 //leemos los registros que nos da el giroscopio
 Wire.beginTransmission(MPU);
 Wire.write(0x43);
 Wire.endTransmission(false);
 Wire.requestFrom(MPU,4,true);// ahora solo pedimos 4 registros para el giroscopio
 GyX=Wire.read()<<8|Wire.read();
 GyY=Wire.read()<<8|Wire.read();
 // calculamos el angulo que nos da el giroscopio de shit
 Gy[0]= GyX/G_R;
 Gy[1]=GyY/G_R;

 //ahora aplicamos lo que he dicho para eliminar el drift (creo que lo he puesto más abajo lo que es)
 // angle es el angulo que a nosotros nos interesa, osea el angulo que tiene el robot (la MPU6050)
 Angle[0]=0.98*(Angle[0]+Gy[0]*0.010)+0,02*Acc[0];
 
 Angle[1]=0.98*(Angle[1]+Gy[1]*0.010)+0,02*Acc[1];

 //esto nos sirve por si queremos poner los angulos en la pantalla, osea que salgan puestos en la pantalla xd
 /*Serial.print("Angulito X (d): "); 
 Serial.print(Angle[0]); 
 Serial.print("Angulito Y: ");
 Serial.print(Angle[1]);
 */
 delay(10); //esto lo podemos canviar, ya que es el tiempo entre mesuras
//me falta acabar esto

 pinMode(miservo, OUTPUT);
 pinMode(miservo2, OUTPUT);

  for(int angulo = 0; angulo < 360; angulo += 1) {   //  un ciclo para mover el servo entre los 0 y los 360 grados  
        miservo.write(angulo);               //  manda al servo la posicion
        miservo2.write(angulo);
        delay(15);                        //  espera unos milisegundos para que el servo llegue a su posicion
     }

     for(int angulo = 360; angulo >= 1; angulo -= 1)    {   //  un ciclo para mover el servo entre los 360 y los 0 grados                             
        miservo.write(angulo);                 //  manda al servo la posicion
        miservo2.write(angulo);
        delay(15);                          //  espera unos milisegundos para que el servo llegue a su posicion
     }
}
 

//anguloY=atan(x/sqrt(pow(y,2)+pow(z,2)));

//anguloX=atan(y/sqrt(pow(x,2)+pow(z,2)));

You need to tell us what the program actually does and what you want it to do differently.

A description of the program itself would also be useful. Trying to understand how a program is designed by reading the code is very time consuming.

…R