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.