Вся работа идёт в прерывании. В основном цикле только ждём прерывание.
Timer2 настраиваем в режим CTC. Частота CK/256. В OCR2 записываем 103 (десятичное).
Получаем прерывание с частотой 300гц.
Теперь каждые 3.33мс работаем:
1  считываем гироскоп
2  "фильтруем" его данные (отнимаем постоянную составляющую, убираем "шумящие" младшие разряды).
3  считываем акселерометр (ось X)
4  считываем акселерометр (ось Y)
5  вычитаем Y из X. Это для увеличения "чувствительности" акселерометра
5  "прогоняем" данные гиро и аселя через . Получаем текущее положение в пространстве.
6  высчитываем отклонение от вертикали
7  это отклонение "загоняем" в . Он выдаёт величину, подаваемую на моторы.
У меня в браузере куча закладок про ПИД, про комплиментарный фильтр, которые помогли мне въехать в тему. Могу поделиться, если надо.
А вообще то если вы пишете на СИ, то можно поискать "балансирующий робот на Ардуино". Миллион ссылок 

.
Есть с исходниками на СИ. Правда всё забугорное, на инглише.