Kłopot z quadrocopterem a raczej z oprogramowaniem
: poniedziałek 20 sty 2014, 20:08
Witam. Jestem nowy na forum i chciał bym się przywitać. Jeśli źle trafiłem to proszę o przeniesienie. Z racji rozpoczęcia ferii zimowych postanowiłem zabawić się z moją platformą biedronki v929 (quadrocopter). Miała ona uszkodzony sterownik. Zabrałem się więc do pracy i wyposażyłem ją w mikrokontroler atmega8 i MPU6050. Kłopot jest taki że po obliczeniu mocy na każdą stronę quadro nie zachowuje się stabilnie tzn. chwieje się a po 1s ląduje na plecach. Raz po modyfikacji softu udało mi się utrzymać w miarę prosto ale i tak krążył wokół osi jednego z silników. Nie chcę uzyskać jakiegoś super dokładnego lotu ale żeby była to namiastka stabilizacji. Zawis w miejscu ogłupia go. Dane kat_x i kat_y to wartości podawane w stopniach od -90 do +90 ,moc to siła ciągu w zakresie od 0-255, PWM zrealizowany na timerze 2 programowo.
No i obsługa PWM
Moje pytanie wygląda tak. Jak zrealizować obliczanie przechyłu?? Może mię ktoś nakierować na poprawny kierunek?? Zaznaczam że, PWM i obsługa MPU6050 wraz z filtrami chodzi w miarę bezbłędnie.
Kod: Zaznacz cały
Pozycjonowanie:
'przetważanie przechyłu na współczynnik kierowany do obliczeń mocy silników
If Kat_x >= 0 Then Wsx = Kat_x * 3
If Kat_x < 0 Then Wsx = Kat_x * -3
If Kat_y >= 0 Then Wsy = Kat_y * 3
If Kat_y < 0 Then Wsy = Kat_y * -3
'wspłóczynnik x i y nieprzekracza 70
If Wsx > 70 Then Wsx = 70
If Wsy > 70 Then Wsy = 70
'jeśli prosto to błąd=0
If Kat_x = 0 Then
Blad_d = 0
Blad_b = 0
End If
If Kat_y = 0 Then
Blad_a = 0
Blad_c = 0
End If
'sterowanie ledami
If Kat_x = 0 Then Led2 = 1
If Kat_y = 0 Then Led3 = 1
If Kat_x <> 0 Then Led2 = 0
If Kat_y <> 0 Then Led3 = 0
'sortowanie wartości błędu w zależności od przechylenia
If Kat_x < 0 Then Blad_d = Wsx
If Kat_x > 0 Then Blad_b = Wsx
If Kat_y > 0 Then Blad_c = Wsy
If Kat_y < 0 Then Blad_a = Wsy
'wpisywanie wartości do PWM
Moc_a = Blad_a + Moc
Moc_b = Blad_b + Moc
Moc_c = Blad_c + Moc
Moc_d = Blad_d + Moc
Return
Kod: Zaznacz cały
Przerwanie_t2:
Incr Licznik
'PWM
If Licznik >= Moc_a Then Reset Ma Else Set Ma
If Licznik >= Moc_b Then Reset Mb Else Set Mb
If Licznik >= Moc_c Then Reset Mc Else Set Mc
If Licznik >= Moc_d Then Reset Md Else Set Md
Return