Arduino Leonardo – jak napisać program do robota balansującego z AltIMU-10 v4?

Witam,

przychodzę o kilka porad w pisaniu programu oraz ogólnych porad w budowie robota balansującego.

Opisze jego budowę:
- silniki Pololu 25D 34:1 165obr/min z enkoderami
- sterownik silnika Pololu Dual MC33926
- arduino Leonardo
- żyroskop + akceerometr - AltIMU-10 v4
- pakiet 7,4V, 2200mA
- koła o średnicy

Program dla arduino:



Co do konstrukcji - ma ona 20cm wysokości, środek ciężkości położony jest w 2/3 wys.

Postanowiłem przefiltrować wstępnie dane z każdej osi, lecz ten pomysł w zamyśle był chybiony.
Generalnie mam problem z doborem parametrów konstruktora filtra Kalmana. Generalnie nie jestem pewien czy też właściwie obliczam jego prędkość kątową. Czy działanie pętli z częstotliwością 50Hz jest optymalne ?

Miałem też problem z doborem parametrów PID, lecz na razie nie ma raczej sensu tego rozgrzebywać jeśli nie mam prawidłowo innych rzeczy. Zwykle robot oscylował zbyt szybko, nie zdołał utrzymać równowagi dłużej niż 1s.

Z góry dziękuję za wskazówki

Chcesz filtrować filtrem Kalmana każdą oś osobno? (jak tak to kompletnie bez sensu). Napisz coś więcej o koncepcji użycia filtru Kalmana bo nie znam biblioteki "Kalman.h" (ale sam temat filtracji Kalmana jest mi znany bardzo dobrze)

@Meh05

Już zrozumiałem, że to bez sensu. Korzystam z tej biblioteki : https://github.com/TKJElectronics/KalmanFilter

Chcę dobrać parametry do jej działania, domyślnie w bibliotece są takie jak poniżej: