Freestyle Autonomiczy robot balansujący

[Zbielu]

No mam trochę z tym problem ...

używam biblioteki z tej strony (to jest strona z bibliotekami dla Arduino) https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

Biblioteka MPU6050.h

Przerobiłem w niej tylko nazwy odnośników, aby łatwiej pisało mi się program. 

Odczytuję kąt z akcelerometru, który po zastosowaniu wzoru daje mi wyniki następujące: pion robota 0 stopni, poziom w lewo -89 stopni, poziom w prawo 88 stopni, a jak przekroczę 90 stopni (czyli zaczynam obracać robotem do góry nogami) to znowu kąt idzie od 90 do 0 stopni (zero stopni pokazuje mi, gdy robota jest w pionie i w pionie ale do góry nogami, czy to jest ok ?)

Widziałem gdzieś skalowanie gyro, nie wiem czy to poprawne, oraz czy w ogóle można odczytać kąt z samego gyro, ponieważ dość szybko odpływa ... ?

Na końcu jest filtr komplementarny.

Poniżej listing programu, oraz screen excela (pomarańczowy - acc, niebieski - filtr komp, szary - gyro)

void timerCallback() //główne przerwanie, które wykonuje się co 10ms
{
	currTime = millis();
	loopTime = currTime - prevTime;
	prevTime = currTime;
	  
  accelgyro.getMotion6(&accel, &gyro); //pobieramy czyste dane z MPU6050, acc i gyro
  
  gyroX = (float)gyro.x/131.072; //skaluje gyro
  gyroAngle = gyroX * (loopTime/1000.0); //próbuję odczytać kąt z samego gyro o ile to możliwe
  
  SgyroAngle = SgyroAngle + (float)gyroAngle; //scałkowany kąt w celu wrysowania na wykresie (wychodzi ujemny)

  angle_x = atan2(accel.x, sqrt(square(accel.y) + square(accel.z)))/(PI/180.0); //odczytuję kąt z acc w stopniach od -90 do +90, gdzie 0 to pion, tak samo jest gdy robot jest do góry nogami, czyli nigdy nie dostanę większej wartości niż +/-90 stopni
  
  angle_x_komp = 0.98*(angle_x_komp + gyroAngle) + (0.02 * angle_x); //próba filtra komplementarnego
}

[lubniewicz]

Kąt z akcelerometru dobrze pokazuje. To znaczy w przedziale od -90 do 90 stopni, tak ma być do regulatora. Nie jest potrzebny kąt do góry nogami.  Odnośnie skalowania to wynika to z rozdzielczości, wykonałem to tak:

void calcgRes()
{
    switch (gScale)
    {
    case G_SCALE_245DPS:
        gRes = ((int32_t)245*100000) / 32768;
        break;
    case G_SCALE_500DPS:
        gRes = ((int32_t)500*1000) / 32768;
        break;
    case G_SCALE_2000DPS:
        gRes = ((int32_t)2000*1000) / 32768;
        break;
    }
}



float calcGyro(int16_t gyro)
{
    // Return the gyro raw reading times our pre-calculated DPS / (ADC tick):
    return (((float)gyro * gRes)/(int32_t)100000.0);  //   /100
}

float gyro_() 
{
    readGyro();
    return calcGyro(gx);
}

Podsumowując to Twoje skalowanie: 

gyroX = (float)gyro.x/131.072; //skaluje gyro

to jest to samo co ja miałem ale powyżej, lecz już na sztywno ustalony współczynnik 131.072. Dobra szkoda czasu nie będziemy wnikać bo wydaje się, że działa. 

Odczyt kąta z gyro mam podobnie:

position.gyro_0 = gyro_()*(-0.01);

Wracając do Twojego pytania to tak można odczytać kąt z gyro. Jest to nawet niezbędne przy szybkich ruchach, ponieważ accel podaje wtedy błędne dane. Dryf jest w tym przypadku pomijalny. Całkowałeś go bez sensu. 

Filtr masz dobry, ja dałem współczynnik 0.03 zamiast twojego 0.02.

Proszę Ciebie abyś zrobił jeszcze raz test. Tylko, że zamiast na wykresie całkowane gyro to daj bieżąca wartość gyro i pomnóż to np razy 100 aby na wykresie było ładnie widać. Chcę zobaczyć, jak zmienia się odczyt z accel, gyro i komplementarny.  Odczyt z gyro jest niezbędny przy szybkich ruchach robotem. Możesz eksperymentalnie zmienić współczynnik na 0.03. Jak to zrobisz to polecimy dalej. Będziemy robić regulator kąta.  

[Zbielu]

Ogólnie zmieniłem znak mnożąc razy -1 wartość gyro .... nie wiem czy to tak mam być czy mam zostawić tak jak poprzednio było, zmienia się tylko jego odbicie lustrzane względem osi Y.

Przysyłam to o co prosiłeś .

Czy fakt że kąt po filtrze jest przesunięty względem acc, jest ok ?

I szybkie obracanie robota w zakresie -90 do +90 powoduje że filtr nie ogarnia i mocno uśrednia to, ale to były bardzo szybkie ruchy, których robot nie wiem czy jest w stanie wykonać, a po drugie to nie są jego zakresy normalnej pracy.

Edytowano Sierpień 17, 2019 przez Zbielu

[lubniewicz]

Analizuje Twoje wykresy i wydaje mi się, że niepotrzebnie zmieniłeś wartość gyro mnożąc -1. Wiesz to zależy jak masz ustawiony gyro. Powinno to wyglądać tak jak na wykresie poniżej.

PS. Według mnie poprzednie ustawienie było lepsze ponieważ obecnie kąt z filtra ma mniejszą amplitudę. Dlatego bez wykresów ciężko się zorientować czy wszystko dobrze zrobiliśmy. Zmień na 1 i może przesunięcie będzie mniejsze. Jak będziesz gotowy polecimy dalej.

A na żyroskopie jakie masz ustawione "Full scale range (dps)" zakres w stopniach na sekundę, im mniej tym większa dokładność, ale po przekroczeniu zakresu może się nasycić. 

Edytowano Sierpień 17, 2019 przez lubniewicz

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[Zbielu]

Od gyro nigdy nie będę miał takiego gładkiego wykresu jak ten zielony ... chyba że jakieś nastawy w MPU6050 trzeba zrobić ...

To nie będę mnożył razy -1 .... a w tym wątku coś było pisane o mnożeniu razy -1 w celu zmiany znaku ... o co chodziło ?

Ale ogólnie jest ok odczyt czy jeszcze coś muszę zrobić ?

Edytowano Sierpień 17, 2019 przez Zbielu

[lubniewicz]

Dobrze, że przesłałeś mi fotkę. Według mnie prędkość kątową powinieneś mierzyć po osi Y, a nie X. 

Twoje poprzednie pomiary wskazują jakby gyro łapał coś przypadkowego.

Edytowano Sierpień 17, 2019 przez lubniewicz

[Zbielu]

Faktycznie masz rację powinienem mierzyć po Y

...

Strasznie jest duże opóźnienie filtra komplementarnego ... jak to ogarnąć ? jak wykonuję pętle co 10ms to opóźnienie filtra jest takie, że gdy acc i gyro przedstawiają kąt 0 stopni to filtr pokazuje to 0,8sekundy później, ja pętle wykonam co 5ms to opóźnienie filtra jest o 0,4 sekundy.

niebieski to filtr, żółty to gyro, pomarańcz to acc

 

[Zbielu]

Wskazówki super ! ogarnięty odczyt kąta myślę 😉

Proszę zobacz. (niebieski to - kompl, pomarańcz - acc, siwy - gyro)

Lecimy dalej ?

[lubniewicz]

Dobra powoli. Zaraz Tobie to wytłumaczę. Wysyłasz dane po uart i to ma wpływ na obliczenia. Masz przez to laga. Wydaje mi się, że ten proces nie odpala się 100 razy na sekundę. Przez to masz laga. Pamiętaj, że tam masz dt czyli sampling rate. Jak proces się nie wyrabia to obliczenia idą w łeb. Proszę zrób test. Zobaczy czy przez 10 sekund proces odpalił się 1000 razy. Wszystko zostaw, tylko licznik dodaj. 

[Zbielu]

Zobacz post wyżej 

[lubniewicz]

Dla potomnych napisz co zmieniłeś. Co powodowało problem. 

[Zbielu]

Bezpośrednia zmiana danych z gyro poprzez pomnożenie przez -1 plus fakt, że odczytujemy z Y a nie z X, to wszystko. 

Poprzednio było tak że odczytywałem gyro po X, co było mega błędem. Po drugie gdy robiłem sobie wizualizację odbijałem sobie w samym excelu odczyt z gyro (*-1), a przecież to co trafiało do filtra nie było pomnożone przez -1, co powodowało, że sygnały nie były z sobą w fazie i przez to występowało takie opóźnienie w komplementarnym filtrze.

Edytowano Sierpień 17, 2019 przez Zbielu

[lubniewicz]

To dobrze, bo dałeś mi niezłą łamigłówkę. Lecimy dalej. Robimy zgodnie z umową regulator kąta. Czyli na wejście regulatora podajemy kąt obliczony i kąt zadany. Kąt zadany to w chwili obecnej będzie 0 stopni. Później zrobimy kolejny regulator który będzie obliczał ten kąt. Podsumowując tworzymy regulator PID, wejście aktualny kąt, oraz kąt zadany na sztywno 0 stopni. Na wyjściu musimy otrzymać PWM. Może później damy jakiś filtr dolnoprzepustowy, zobaczymy. Tam też dodamy korektę po napięciu, bo wiadomo, że im mniejsze napięcie tym musimy dać więcej PWM, ponieważ regulator się rozjedzie. 

• Obliczmy błąd:

          errors = (targetPosition - currentPosition);

• Obliczony błąd sumujemy:

          integrated_errors += errors*1.00;

• Wprowadzamy Windup błędu (czyli granicę do której sumujemy błąd. Jest to ważne aby człon całkujący nie nasycił się za mocno)

          integrated_errors = constrain(integrated_errors,-ram_rejestr.i_lim, ram_rejestr.i_lim);

• Człon proporcjonalny 

          pTerm = ram_rejestr.p_kp *errors*1.00;

• Człon całkujący

         iTerm = (ram_rejestr.p_ki/10.00) * integrated_errors*1.0;          

• Człon różniczkujący (ten parametr -10 to tylko dlatego aby zmienna "ram_rejestr.p_kd" była mniejsza)

            dTerm = ram_rejestr.p_kd * parametry.angular_velocity * (-10.00);

• Wprowadzamy ograniczenie - zmienna 8 bit PWM

         temp = -constrain((pTerm + iTerm + dTerm)*multipler, -power, power)

• Korekta po napięciu

           multipler = (144.0000 - parametry.u12/100.0000)*0.0086+1.0000;

• Wynik

           return temp;

 

Dobra możesz działać. Myślę, że się domyślisz. Zrób dodatkowo aby:  ram_rejestr.p_kp, ram_rejestr.p_ki, ram_rejestr.p_kd czyli nastawy regulatora, oraz ram_rejestr.i_lim czyli windup można było ustawiać zdalnie. Jak to zrobisz to powiem Tobie jak to wyregulować, a potem zrobimy kolejne dwa regulatory. 

PS. Ten PID ustaw w tym samym procesie, poniżej tego filtru komplementarnego. Oczywiście te mnożniki w członach "x10" i "x-10" możesz wywalić. Po prostu nastawy będą dziesięć razy mniejsze, nie chciałem się bawić w float . 

 

   

Edytowano Sierpień 17, 2019 przez lubniewicz

[Zbielu]

1. Proszę wyjaśnij dlaczego ten parametr -10 ??

"

Człon różniczkujący (ten parametr -10 to tylko dlatego aby zmienna "ram_rejestr.p_kd" była mniejsza)

            dTerm = ram_rejestr.p_kd * parametry.angular_velocity * (-10.00);

"

2. Proszę wyjaśnij szczegółowo korektę napięcia

"

Korekta po napięciu

           multipler = (144.0000 - parametry.u12/100.0000)*0.0086+1.0000

"

3. Rozumiem, że return temp to "wyplucie" wypełnienia do driverów silników ?

 

[lubniewicz]

1. Nie dajesz -10. To było dla mnie takie ułatwienie aby nie wpisywać dużej wartości ram_rejestr.p_kd. Mam tutaj na myśli, że zamiast wpisać 500, ja wpisałem tylko 50. Minus był dlatego, że wcześniej miałem: position.gyro_0 = gyro_()*(-0.01). W Twoim przypadku nie patrz na to.
2. Powiem Tobie o co chodzi. Odejmuję od napięcia jakie mogę otrzymać maksymalnie (i jakie miałem w trakcie regulacji) czyli 14.4V wartość aktualnego napięcia  czyli np 12.1V mnożę to przez współczynnik i dodaję +1. Czyli to jest zwykła funkcja liniowa. Tą funkcję linową i współczynnik wyznaczyłem doświadczalnie. Wyglądało to tak że zebrałem parametry prąd, napięcie, moc dla danych wartości PWM. Teraz niezależnie od napięcia na akumulatorze silniki pobierają stałą moc. Jest podbijane napięcie poprzez zwiększenie PWM. Tak w skrócie aby nie zaciemniać podam przykład: PWM 128 przy napięciu 14.4V =  7.2V, natomiast już dla napięcia dla aku 11.2V =  5.6V. Rozumiesz już. My musimy niezależnie od naładowania aku utrzymać stałe napięcie przy danym PWM. Oczywiście najlepiej zmierzyć prąd i napięcie. Nie zawracaj sobie na razie tym głowy. To było takie moje usprawnienie, bo widziałem, że robot zaczął się inaczej zachowywać przy pełnym i pustym akumulatorze. Na razie odpuść temat później sam sobie zrobisz kompensację. Nie sugeruj się moimi współczynnikami sam sobie to kiedyś zrobisz. Po prostu z:    temp = -constrain((pTerm + iTerm + dTerm)*multipler, -power, power) usuń  multipler. Chcę Tobie tylko wskazać na problem ze regulator może inaczej działać przy innym zasilaniu niż jak miałeś w czasie regulacji. 
3. Tak, wyplucie. Pisałem skrótami bo wiem, że to zrozumiesz.