Robot autobilanciante

Ciao a tutti,ho bisogno di aiuto per il mio robot autobilanciante,utilizzo l’MPU650 per avere i dati di Roll.Non riesco a farlo stare in piedi nemmeno mezzo secondo.Credo sia un problema software,magari potreste aiutarmi.Grazie

#include <SPI.h>
#include <Wire.h>
#include <L298N.h>
#define MPU 0x68  // I2C address of the MPU-6050
 
double AcX,AcY,AcZ;
int Pitch, Roll;



const int ENA = 9;
const int IN1 = 2;
const int IN2 = 3;
const int IN3 = 4;
const int IN4 = 5;
const int ENB = 10;
L298N driver(ENA,IN1,IN2,IN3,IN4,ENB); 
int speed;
 
void setup(){
  Serial.begin(9600);
  init_MPU(); // Inizializzazione MPU6050
}
 
void loop()
{
  FunctionsMPU(); // Acquisisco assi AcX, AcY, AcZ.
    
  Roll = FunctionsPitchRoll(AcX, AcY, AcZ);   //Calcolo angolo Roll
  Pitch = FunctionsPitchRoll(AcY, AcX, AcZ);  //Calcolo angolo Pitch

 
  Serial.print("Pitch: "); Serial.print(Pitch);
  Serial.print("\t");
  Serial.print("Roll: "); Serial.print(Roll);
  Serial.print("\n");
  
 


  if(Roll>5){
     driver.forward(abs(Roll)+100,0);
  }
   if(Roll<5){
     driver.backward(abs(Roll)+100,0);
  }

  if(Roll==5){
    driver.full_stop(0);
  }
 



  
 
}
 
void init_MPU(){
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
}
 
//Funzione per il calcolo degli angoli Pitch e Roll
double FunctionsPitchRoll(double A, double B, double C){
  double DatoA, DatoB, Value;
  DatoA = A;
  DatoB = (B*B) + (C*C);
  DatoB = sqrt(DatoB);
  
  Value = atan2(DatoA, DatoB);
  Value = Value * 180/3.14;
  
  return (int)Value;
}
 
//Funzione per l'acquisizione degli assi X,Y,Z del MPU6050
void FunctionsMPU(){
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
}

Con quel codice non vai da nessuna parte, manca tutto a partire dal pid per il pendolo inverso per finire con il kalman per ottenere i dati reali di assetto dal sensore inerziale.
Ti conviene dare un'occhiata al codice del Balboa 32u4 di Pololu, è compatibile con l'IDE di Arduino, anche se usa dei sensori diversi i concetti sono gli stessi e hai una buona base di partenza.