Przeszukaj forum

Witam! Chciałbym przedstawić wam moją konstrukcję, która stała się przedmiotem mojej pracy inżynierskiej. Mechanika: Konstrukcja mechaniczna w zasadzie oparta jest na 2 płytkach PCB. Do płytki głównej zamontowane są dwie kulki podporowe, a także silniki pololu o przekładni 1:10 co stanowi napęd robota. Z przodu zamontowana jest płytka z podstawą zawierająca uchwyty do czujników ultradźwiękowych. Taki sam uchwyt pod czujnik znajduje się również na 'ogonie' robota - jest on nie używany i został zamontowany eksperymentalnie. Na przedłużonej osi silników zostały zamontowane magnesy enkoderów magnetycznych co ilustruje poniższa fotografia. Elektronika: Sercem robota jest mikrokontroler AVR ATmega162, którego zadaniem jest sterowanie robotem w tym: obsługa enkoderów, sterowanie silnikami, odbiór informacji z mikrokontrolera slave nt. odległości mierzonych przez czujniki, realizację algorytmów sterowania, realizacja połączenia bluetooth, obsługa wyświetlacza LCD hd44780. Drugi mikrokontroler również AVR, ATmega328 realizuje tylko i wyłącznie obsługę czujników odległości HC-SR04. Pomiary normalizuje i wysyła interfejsem SPI do jednostki głównej ATmega162. Na pokładzie robota znajdują się również dwa enkodery magnetyczne AS5040 pracujące w pętli sterowania regulatora PI silników. Enkodery te pracują w trybie defaultowym czyli wyjście kwadraturowe, rozdzielczość 2x256. Silniki sterowane są poprzez mostek H L293. Komunikacją z robotem zajmuje się moduł bluetooth bt222 - czyli komunikacja jest przez UART mikrokontrolera. Główny mikrokontroler jest taktowany zewnętrznym kwarcem 16Mhz co przy takiej ilości obsługiwanych urządzeń i wykorzystywanych interfejsów było po prostu konieczne, z kolei mikrokontroler atmega328 jest taktowany zewnętrznym kwarcem jednak z powodu lepszej podstawy czasowej, która jest konieczna do dokładnych odczytów z czujników odległości. Na wyświetlaczu LCD są wyświetlane aktualnie mierzone wartości odległości czujników. Automatyka: Zostało wspomniane, że silnikami steruje algorytm regulatora PI. Aby obliczyć jego nastawy została przeprowadzona identyfikacja transmitancji. W tym celu wykorzystałem UART do przesyłania aktualnej prędkości silników oraz środowisko Matlab do wizualizacji danych na wykresie. Z wyliczeń wyszło, że silniki z zastosowanymi kołami mają transmitancję: G(s)=107/(0.19s+1) Po obliczeniu nastaw regulatora i zastosowaniu w praktyce okazało się, że można jeszcze troszkę dopieścić nastawy i ostatecznie wykresy odpowiedzi skokowej bez i z regulatorem wyglądają następująco: Software: Napisane w C w środowisku Eclipse. Umożliwia robotowi 3 tryby. Tryb avoid. W trybie tym robot ma za zadanie omijać przeszkody wykorzystując regulator rozmyty Takagi-Sugeno. Wykorzystuje w tym celu odczyty z czujników odległości. Tryb goal seeking. Robot podąża do celu czyli do odebranych przez bluetooth współrzędnych kartezjańskich podanych w [mm], zakładając, że miejsce od którego zaczyna lub dostał nowy zestaw współrzędnych, ma współrzędne 0,0. W trybie tym regulator Takagi-Sugeno ma za zadanie na podstawie zaimplementowanej odometrii oraz współrzędnych końcowych sterować robotem w celu osiągnięcia zadanego położenia. Problem algorytmu przedstawia ilustracja: Tryb avoid + goal seeking. Jest to połączenie dwóch wcześniej opisanych trybów. W trybie tym działają równocześnie obydwa wcześniej opisane algorytmy, jednak na wejście regulatorów PI sterujących silnikami jest podawany zbalansowany sygnał z obu algorytmów. Tzn w zależności od najmniejszej odległości od przeszkody mierzonej przez któryś z czujników jest obliczany procentowy udział wartości sterowania od dwóch algorytmów. Brzmi skomplikowanie ale takie nie jest. Najlepiej zobrazuje to wzór: V=K*Vg+(1-K)*Va gdzie V jest prędkością zadaną na regulator PI silnika, Vg jest prędkością wynikającą z algorytmu goal seeking, Va jest prędkością wynikającą z algorytmu avoid, K jest parametrem, który przyjmuje wartość z przedziału 0-1 w zależności od mierzonej minimalnej odległości do przeszkody. Robota wprowadza się w odpowiedni tryb za pomocą dwóch przycisków lub poprzez interfejs bluetooth. Odpowiednie tryby sygnalizują diody LED. W celu przesłania komendy zawierającej współrzędne celu najpierw należy wprowadzić robota w odpowiedni tryb, a następnie wysłać do niego wg stworzonego prze zemnie protokołu dane. Protokół wysyłania danych jest następujący: X±⌴⌴⌴Y±⌴⌴⌴& gdzie +- – znak współrzędnej, _ – wartość współrzędnej. Testy. Konstrukcja została przetestowana pod względem dokładności odometrii. Robot podczas licznych testów na bieżąco wysyłał swoje współrzędne, które w dalszej kolejności były obrabiane w środowisku Octave do przyjemnych w analizie wykresów. Przykładowy wykres drogi, którą pokonał robot na 'placu manewrowym' w trybie avoid: ... oraz rzut z góry na 'plac manewrowy': U góry pozycja początkowa, na dole końcowa robota. Cały ten obszar był dookoła ogrodzony ścianką. Słowa samokrytyki Co bym zmienił ? Czujniki. Ze względu na problematyczność wykrywania małych przeszkód lub gdy płaszczyzna przedmiotu uniemożliwia odbicie się fali ultradźwiękowej w stronę odbiornika. Zdarza się, że z powodu gubienia przeszkód robot po prostu w nie wpada. Najciekawsze fragmenty programu: Regulatory PI: //######################################## REGULATOR PRĘDKOŚCI ############################################ erra = vl-Va;//liczenie błedu prędkości errb = vp-Vb; if(ua<1023&&ua>-1023)Ca = Ca+(erra*dt);//całka błędu ua = kp*(erra + Ca/Ti);//regulator PI //P I if(ub<1023&&ub>-1023)Cb = Cb+(errb*dt);//całka błędu ub = kp*(errb + Cb/Ti);//regulator PI //P I if((ua<10)&&(ua>-10))ua=0; if((ub<10)&&(ub>-10))ub=0; motor_set(ua,ub);//ustawia sterowanie silników Regulator rozmyty Takagi-Sugeno trybu goal seeking: inline void PsiZ2Vlr(float *Psi, float *z, int16_t *vl,int16_t *vp) { float PSI[3]; float XL[6],XR[6]; float NR,FR,L,S,P,VL=0,VP=0,m=0; uint8_t i,k; ///////////////////////////////LICZENIE PRZYNALEŻNOŚCI////////////////////////////////// if(*z<=100)NR=-0.01*(*z)+1;else NR=0;//jak bardzo blisko jest cel if(*z<=100)FR=0.01*(*z);else FR=1;//jak bardzo daleko jest cel if(*Psi<=0)L=-0.3183*(*Psi);else if(*Psi<-M_PI)L=1;else L=0; if(*Psi<=0)S=0.3183*(*Psi)+1;else if(*Psi<-M_PI)S=0; else if(*Psi>M_PI)S=0;else S=-0.3183*(*Psi)+1; if(*Psi>=0)P=0.3183*(*Psi);else if(*Psi>M_PI)P=1;else P=0; /////////////////////////////////////TABLICA REGUŁ////////////////////////////////////// PSI[0]=L; PSI[1]=S; PSI[2]=P; //NR; XL[0]=-4000;XR[0]=4000; XL[1]=0;XR[1]=0; XL[2]=4000;XR[2]=-4000; //FR; XL[3]=-4000;XR[3]=4000; XL[4]=4000;XR[4]=4000; XL[5]=4000;XR[5]=-4000; /////////////////////////////obliczanie prędkości silników////////////////////////////// for(k=0,i=0;k<3;k++,i++)//pierwszy wiersz tabeli { VL+=NR*PSI[k]*XL[i]; VP+=NR*PSI[k]*XR[i]; m+=NR*PSI[k]; } for(k=0;i<6;k++,i++)//drugi wiersz tabeli { VL+=FR*PSI[k]*XL[i]; VP+=FR*PSI[k]*XR[i]; m+=FR*PSI[k]; } *vl=(int16_t)VL/m; *vp=(int16_t)VP/m; } Obsługa czujników odległości: //PRZERWANIE OD CAPTURE PIN ISR(TIMER1_CAPT_vect) { if( (TCCR1B & (1<<ICES1)) ) { LastCapture = ICR1;//jesli zbocze narastajace, zlap ICR1 } else { PulseWidth = ICR1 - LastCapture;//Jeśli zbocze opadajace oblicz PW pomiar=0;//wyzeruj flagę pomiaru } TCCR1B ^= (1<<ICES1);//Zmiana zbocza wyw. przerwanie } Mam nadzieję że robocik się podoba. W razie pytań proszę śmiało. Całości kodu nie udostępniam ze względu na prawa autorskie niektórych funkcji bibliotecznych. Co mogłem udostępniłem. Pozdrawiam was wszystkich !