[C] [2WD] Robot nie jedzie prosto...

[cepelia]

Mam takie pytanie dotyczące sterowania silnikami w moim robocie...

mianowicie mam robota który ma napęd na 2 koła, w związku z tym potrzebowałem rozwiązać problem jazdy na wprost, w tym celu odczytuję prędkość koła prawego i lewego z enkoderów po czym odejmuje jedną od drugiej i w zależności od wyniku dodaję lub odejmuję od pwmu jednego i drugiego koła...

wynikiem takiego działania jest to że robot jedzie prosto ale nie za każdym razem...

gdzie mam szukać przyczyny ???

[marek1707]

Być może dlatego, że zrobiłeś regulator typu P czyli nie pamiętasz nawarstwiających się błędów. Jeżeli przez jakiś czas mierzysz liczbę impulsów a pod koniec tego okresu wyznaczasz poprawkę dla PWM silników to wcale nie znaczy, że "naprawiłeś" sytuację. W następnym okresie pomiarowym znów zaczynasz on nowa i zapomniałeś już, że poprzednio był jakiś niezerowy i niepoprawiony błąd. Spróbuj zrobić tak, by starą różnicę wskazań enkoderów pamiętać, w następnym okresie wyznaczyć nową na podstawie nowych pomiarów i do tej nowej dodawać tę starą ze znakiem, oczywiście. Teraz ta nowa staje się starą i tak w kółko. Wtedy nigdy żadnej różnicy nie przeoczysz i nawet jeśli będzie ona bardzo mała, to w końcu po którymś kroku zrobi się duża i odpowiednio zmieni PWM.

Czyli: zrób regulator typu PI

Wtedy, pod koniec dowolnie długiego czasu masz zawsze w tej kumulowanej różnicy błąd drogi obu kół a część odpowiedzialna na wyliczanie PWM wciąż będzie starać, by był on zerowy.

[cepelia]

rozumiem że powinienem zrobić coś takiego...

1. zadeklarować jakieś przerwanie w którym powiedzmy co 50 ms będę sprawdzał liczniki z enkoderów na silnikach

2. obliczyć w przerwaniu wartość z regulatora i dodać do wartości pwm który jest wysyłany na silniki

i powinno działać... a jeszcze nie działa dokładnie tak jak bym chciał...

int dt = 50;     //co ile pobiera się próbkę
int Kp=1;        //wzmocnienie
int Ti=1.2;      //stała całkowania
int Td=0.1;      //stała różniczkowania
int ep;          //uchyb poprzedni
int en;          //uchyb następny
int U;           //sygnał sterujący
int C;           //część całkująca

const byte encoder0pinA = 20;    //A pin -> przerwanie zewnętrzne na pin 20
const byte encoder0pinB = 21;    //B pin -> przerwanie zewnętrzne na pin 21
int durationL;                   //ilość impulsów w zmierzonuym czasie
int durationR;                   //koło prawe i lewe

//Standard PWM DC control
int E1 = 5;     //M1 PWM lewy
int E2 = 6;     //M2 PWM prawy
int M1 = 4;     //M1 kierunek
int M2 = 7;     //M2 prawe i lewe koło 

int speed_corect_L;
int speed_corect_R;
int low_speed = 100;
int med_speed = 140;
int fast_speed = 200;
int max_speed = 255;

int actual_speed;
char stan = 'w';

void setup(void) 
{ 
 int i;
 for(i=4;i<=7;i++)
   pinMode(i, OUTPUT);  
 Serial.begin(57600);      //Set Baud Rate
 EncoderInit();


                           //  advance(200,200);
                           // timer1  
 noInterrupts();           // disable all interrupts
 TCCR1A = 0;
 TCCR1B = 0;
 TCNT1  = 0;

 OCR1A = 3121;             // ustawienie 16MHz/256/20Hz
 TCCR1B |= (1 << WGM12);   // CTC mode
 TCCR1B |= (1 << CS12);    // 256 prescaler 
 TIMSK1 |= (1 << OCIE1A);  // enable timer compare interrupt
 interrupts();             // enable all interrupts


}  

void loop(void)
{

// tutaj sterowanie z terminala  
 if(Serial.available())
 {
  char val = Serial.read();
   if(val != -1)
   {
     switch(val)
     {
     case 'w'://Move Forward
       stan = 'w';
       break;
     case 's'://Move Backward
       stan = 's';
       break;
     case 'a'://Turn Left
       stan = 'a';
       break;       
     case 'd'://Turn Right
       stan = 'd';
       break;
     case 'z':
       Serial.println("Hello");
       break;
     case 'x':
       stan = 'x';
       stop();
       break;
     }
   }

   else stop();
 }

Serial.print( speed_corect_L);
Serial.print(",");
Serial.println(speed_corect_R); 

delay(100);

}

void advance(char a,char b)          //Move forward
{
 analogWrite (E1,a);                //PWM Speed Control
 digitalWrite(M1,LOW);    
 analogWrite (E2,b);    
 digitalWrite(M2,LOW);
}
void back_off (char a,char b)          //Move backward
{
 analogWrite (E1,a);
 digitalWrite(M1,HIGH);   
 analogWrite (E2,b);    
 digitalWrite(M2,HIGH);
}
void turn_L (char a,char b)             //Turn Left
{
 analogWrite (E1,a);
 digitalWrite(M1,LOW);    
 analogWrite (E2,b);    
 digitalWrite(M2,HIGH);
}
void turn_R (char a,char b)             //Turn Right
{
 analogWrite (E1,a);
 digitalWrite(M1,HIGH);    
 analogWrite (E2,b);    
 digitalWrite(M2,LOW);
}
void stop(void)                    //Stop
{
 digitalWrite(E1,LOW);   
 digitalWrite(E2,LOW);
}   

void wheelSpeedL()
{
 durationL++;
}

void wheelSpeedR()
{
 durationR++;
}

void EncoderInit()
{
 attachInterrupt(0, wheelSpeedL, CHANGE);//int.0 
 attachInterrupt(1, wheelSpeedR, CHANGE);//int.1
}

int speed_control(int predkosc_zadana, int predkosc_odczytana)
{
en = (predkosc_zadana/1.48) - predkosc_odczytana;   
C = ((ep + en)/2)*dt;
U = Kp*(en + (1/Ti)*C/100 + Td*(en - ep)*100/dt);
ep = en;
U = 1.48 * U;
return U;
}



//////////////////////==========================///////////////////////////======================


ISR(TIMER1_COMPA_vect)                      // timer compare interrupt service routine
{
 switch(stan)
     {
     case 'w'://Move Forward
       actual_speed = fast_speed;
       advance(speed_corect_L,speed_corect_R);
       break;
     case 's'://Move Backward
       actual_speed = med_speed;
       back_off(speed_corect_L,speed_corect_R);
       break;
     case 'a'://Turn Left
       actual_speed = med_speed;
       turn_L(speed_corect_L,speed_corect_R);
       break;       
     case 'd'://Turn Right
       actual_speed = med_speed;
       turn_R(speed_corect_L,speed_corect_R);
       break;
     case 'z':
       Serial.println("Hello");
       break;
     case 'x':
       stan = 'x';
       stop();
       break;
     }

speed_corect_L = actual_speed + speed_control(actual_speed, durationL);
if(speed_corect_L >= 255) speed_corect_L = 255;
else speed_corect_L = speed_corect_L;

speed_corect_R = actual_speed + speed_control(actual_speed, durationR);
if(speed_corect_R >= 255) speed_corect_R = 255;
else speed_corect_R = speed_corect_R;

durationL = 0;
durationR = 0;
}

mam jeszcze prośbę o komentarz w temacie tego przerwania, czy nie jest tam za dużo wrzucone ...

[marek1707]

Nie napisałeś co dokładnie nie działa jak powinno a trudno tak wgapiać się w kawał kodu i wymyślać co może być nie tak. Ja tam nie jestem jakimś wielkim programistą, ale mam kilka uwag:

1. Robisz jakieś podejrzane operacje na int'ach, z natury rzeczy całkowitych, np:

int Ti=1.2;

lub:

U = 1.48 * U;

przecież obie te stałe zostaną zamienione na 1. Jeśli chcesz udawać ułamki na int'ach, musisz zacząć skalować liczby.

2. W funkcji wykonywanej co 20ms tyle operacji powinno się zmieścić, ale problem w tym, że jest to obsługa przerwania a te są w AVR jednopoziomowe co oznacza, że w tym czasie nie obsłużysz już żadnego innego. Jeżeli na przerwaniach zliczasz np. enkodery, to masz problem.

Ogólnie jeśli przewidujesz w systemie procesy wymagające dłuższego czasu to:

- albo podziel duże i długie na mniejsze zadania i wykonuj je szeregowo w przerwaniu wykonywanym częściej, np. obiczenia tylko dla jednego silnika albo wręcz obliczenia po kawałku itp

- albo wykonuj je w tle tzn. w pętli głównej a tylko ich start synchronizuj jakąś flagą ustawianą w przerwaniu i kasowaną po wykonaniu zdania,
- albo odblokuj przerwania w czasie obsługi tego długiego (ale: uwaga wszystkie funkcje wykonywane przez procesy muszą być wtedy reentrant czyli.. chyba "wielobieżne") przez zastosowanie specjalnego atrybutu ISR_NOBLOCK dla funkcji z nagłówkiem ISR().

Generalnie powinieneś przemyśleć strukturę wszystkich zadań i tak je poupychać, by najczęściej wykonywane przerwanie miało szansę zostać obsłużone w swoim czasie a jednocześnie wszystkie obliczenia miały szansę się wykonać. Inaczej system przestanie nadążać za czasem rzeczywistym i.. kaszana gotowa. Taki projekt nie jest trywialny szczególnie gdy zadań robi się dużo i każde walczy o czas procesora, ale konieczne i ważne.

Jeśli masz przerwania od enkoderów to oszacuj ich minimalny okres i to będzie warunek wykonywania każdego innego najdłuższego przerwania. Wszystko musisz temu podporządkować. Być może część zadań mniej krytycznych czasowo trzeba przenieść do pętli głównej.

Pamiętaj też o synchronizacji. Jeżeli używasz int'ów dłuższych niż 8-bitów to taka zmienna jest pobierana/ładowana z/do pamięci w dwóch (lub 4 dla int32_t itd) osobnych cyklach rozkazowych. Z natury rzeczy mogą one zostać przerwane i jeśli tak się stanie a obsługa przerwania zmieni wartość tej zmiennej, odczytywana liczba będzie kompletnie bez sensu. Warto o tym myśleć i wbudować w swoją wielozadaniowość pewne mechanizmy synchronizacji dostępu do zasobów wspólnych. No wiesz, semafory, mutexy itp albo jakieś komunikaty, kolejki itp. Żaden proces (oprócz nieprzerywalnego przerwania lub sekcji krytycznej pracującej z wyłączonymi przerwaniami) nie powinien korzystać ze zmiennych statycznych ot tak, wprost. Najpierw powinien "poprosić" o ich udostępnienie, poczekać na potwierdzenie itd.. Z resztą co ja tu piszę, to dla programistów są chyba oczywiste sprawy.

A właściwie czy mógłbyś napisać co nie działa?

[cepelia]

Dzięki za zainteresowanie tematem...

oczywiście zauważyłem już wcześniej że ułamków nie przechowuje się w int... choć dopiero wtedy gdy wprowadzane zmiany nie skutkowały zmianami w zachowaniu się regulatora...

co do operacji o jakie pytasz to mnożenie *1,48 to stosunek liczby impulsów policzonych w 20ms do wartości PWM którą chcę uzyskać...

jeśli chodzi o przerwania to wykorzystuję 3, dwa w których liczę impulsy z enkoderów i jedno czasowe w którym wprowadzam korekty wartości PWM i kasuję liczniki enkoderów...

jeśli chodzi o to "grubsze przerwanie" to nie ma tam nic specjalnego... z bardziej czasożernych operacji jest jeden case, 3* digital read reszta to skoki do procedur gdzie zasadniczo następuje tylko przypisanie... więc wydaje się że wyrabia się biorąc pod uwagę że to "grubsze przerwanie" wykonuje się co 20ms a zegar ustawiony jest na 16MHz...

jeśli chodzi o przerwania od enkoderów to chyba nic krótszego nie uda mi się napisać...

durationL ++;

jeśli chodzi o mutexy to raczej nie w mc... zresztą projekt raczej nie zakłada żeby wszystko znalazło się w kontrolerze...

plan jest taki aby napisać podstawową obsługę silników i czujników zamontowanych w robocie, które to miały by wystawiać flagi, bądź podawać odpowiednie wartości a komputer (zwykły pc) w oparciu o MS Robotic Studio (bądź sam napiszę jakąś aplikację) będzie wykonywał polecone zadania.

jeśli chodzi o "napisz co nie działa" to cały czas (choć teraz już dużo słabiej) robot skręca w bok jadąc do przodu... stąd pomysł żeby utrzymywać zadaną prędkość na podstawie odczytu z enkoderów (na początku popełniłem błąd który polegał na tym że nie utrzymywałem odpowiedniego reżimu czasowego i nie kasowałem liczników w odpowiednim momencie).

do dostrojenia regulatora PID napisałem sobie prosty terminal z wykresem i jak zobaczysz na wykresie są tam wartości PWM wysyłane na mostek żeby utrzymać zadaną ilość impulsów enkodera czyli tą samą prędkość kół...

na wykresie widać start, zmniejszenie prędkości po zadziałaniu czujnika i stop...

jeszcze raz dzięki za pomoc, jak byś miał jakieś uwagi albo pomysły lub pytania to pisz bo myślałem że będzie to jeździło jednak prosto a nie bardzo chce...

[marek1707]

Mnożenie samo w sobie podejrzane nie jest. Mi chodziło o stałą 1.48, która na skali dziwności w kontekście używania wyłącznie liczb całkowitych osiąga moim zdaniem co najmniej 8. Nie analizowałem kodu na tyle dokładnie, by zastanawiać się dlaczego jest tam akurat taka wartość.

Nie jest ważne, co jaki czas wywołujesz to "długie" przerwanie tylko jak długo ono się wykonuje. Jeżeli dłużej niż okres zgłaszania przerwań od enkoderów, to z ich obsługą masz kłopoty (i to je możesz gubić) a nie z tym wolnym a długim.

Tamte enkoderowe są OK, rzeczywiście nic prostszego niż ++ chyba być nie może

Jeżeli wykluczysz poślizgi kół, obie przekładnie mają takie same przełożenia a koła napędowe takie same obwody to zostaje tylko algorytm sterowania silnikami. Skręcanie może być spowodowane tym, że o czymś zapominasz. Nie rozkminiałem Twojego kodu choć jeśli będę zmuszony to pewnie się do tego zabiorę. Z resztą dzisiaj to pewnie działa u Ciebie już coś innego.

Powiedz tylko w skrócie: czy masz takie zmienne w których przechowujesz całkowite liczby kroków zliczonych przez oba enkodery osobno? Zrób coś takiego, niech to będzie nawet int32 i sprawdź, czy na końcu jazdy po prostej masz tam jakieś istotne różnice w liczbie impulsów. Jeżeli nie, to problem leży w mechanice (średnice kół?) lub w dynamice ruchu (mikropoślizgi przy nagłych zmianach wysterowania?). Jeżeli różnice są duże i mniej więcej zgodnie z odchyłką trajektorii, to zmodyfikuj algorytm tak, by nigdy nie tracił tych różnic.

Jeżeli obecny działa na krótkich odcinkach czasu i porównuje liczbę kroków w tym czasie wykonanych, to de facto kontroluje prędkość a to chyba słabe rozwiązanie. Bezstratne naliczanie drogi przynajmniej gwarantuje równoległość toru do prostej startowej.

Jak długie są w rzeczywistości Twoje odcinki pomiarowe, jakiego rzędu odchyłki rejestrujesz i jakie masz różnice w liczbie impulsów?

[cepelia]

myślałem że rozwiązanie z kontrolą prędkości będzie dobre...

jutro zmodyfikuję algorytm i sprawdzę drogę tak jak piszesz... postaram się też sprawdzić jak długo wykonuje się przerwanie modyfikujące prędkość, ewentualnie przeniosę je do pętli głównej a w przerwaniu zostawię tylko kasowane liczników do enkoderów albo w ogóle zburzę wszystko i pomyślę o tym z jakiejś innej strony...

[mactro]

Kontrola prędkości jest zazwyczaj dobrym rozwiązaniem. Jeśli odchyłki są niewielkie (czyli tak konkretnie jakie?) to zazwyczaj winne są niedokładności mechaniczne. Przykładowo, sam miałem przypadki, kiedy z kilku identycznych robotów wszystkie jeździły prosto, a jedne skręcał, po czym okazywało się, że np. opona była nierówno ułożona.

[cepelia]

no tak... ale zrobiłem liczniki tak jak radził mi kolega i się okazuje że różnica w drodze to jakiś 1 obrót koła na 15 sekund jazdy...

czyli sporo... kombinuję dalej... tyle że urwałem usb znowu...