Вся работа идёт в прерывании. В основном цикле только ждём прерывание.
Timer2 настраиваем в режим CTC. Частота CK/256. В OCR2 записываем 103 (десятичное).
Получаем прерывание с частотой 300гц.
Теперь каждые 3.33мс работаем:
1 считываем гироскоп
2 "фильтруем" его данные (отнимаем постоянную составляющую, убираем "шумящие" младшие разряды).
3 считываем акселерометр (ось X)
4 считываем акселерометр (ось Y)
5 вычитаем Y из X. Это для увеличения "чувствительности" акселерометра
5 "прогоняем" данные гиро и аселя через . Получаем текущее положение в пространстве.
6 высчитываем отклонение от вертикали
7 это отклонение "загоняем" в . Он выдаёт величину, подаваемую на моторы.
У меня в браузере куча закладок про ПИД, про комплиментарный фильтр, которые помогли мне въехать в тему. Могу поделиться, если надо.
А вообще то если вы пишете на СИ, то можно поискать "балансирующий робот на Ардуино". Миллион ссылок
.
Есть с исходниками на СИ. Правда всё забугорное, на инглише.
Вся работа идёт в прерывании. В основном цикле только ждём прерывание.
Timer2 настраиваем в режим CTC. Частота CK/256. В OCR2 записываем 103 (десятичное).
Получаем прерывание с частотой 300гц.
Теперь каждые 3.33мс работаем:
1 считываем гироскоп
2 "фильтруем" его данные (отнимаем постоянную составляющую, убираем "шумящие" младшие разряды).
3 считываем акселерометр (ось X)
4 считываем акселерометр (ось Y)
5 вычитаем Y из X. Это для увеличения "чувствительности" акселерометра
5 "прогоняем" данные гиро и аселя через [url=http://we.easyelectronics.ru/Theory/chestno-prostoy-cifrovoy-filtr.html]комплиментарный фильтр[/url]. Получаем текущее положение в пространстве.
6 высчитываем отклонение от вертикали
7 это отклонение "загоняем" в [url=http://we.easyelectronics.ru/Theory/pid-regulyatory--dlya-chaynikov-praktikov.html]ПИД[/url]. Он выдаёт величину, подаваемую на моторы.
У меня в браузере куча закладок про ПИД, про комплиментарный фильтр, которые помогли мне въехать в тему. Могу поделиться, если надо.
А вообще то если вы пишете на СИ, то можно поискать "балансирующий робот на Ардуино". Миллион ссылок :).
Есть с исходниками на СИ. Правда всё забугорное, на инглише.