Raport. Manipulator -

Transkrypt

1 Projekt Sterowniki robotów Raport Manipulator - Skład grupy (): Aleksander Bojda, 8675 Piotr Stopyra, 855 Termin: wttn Prowadzący: mgr inż. Wojciech Domski 0 czerwca 07

2 Spis treści Opis projektu Konfiguracja mikrokontrolera Konfiguracja peryferiów 5. ADC IC RCC SYS TIM TIM TIM UART USB USB_DEVICE Opis funkcji pinów 7 5 Opis wykorzystanych układów zewnętrznych 8 6 Opis działania programu 9 6. Sterowanie joystickiem we współrzędnych wewnętrznych Sterowanie joystickiem we współrzędnych zewnętrznych Sterowanie poprzez Bluetooth (z wykorzystaniem protokołu UART) Sterowanie z aplikacji komputerowej (interfejs USB) Algorytm kinematyki odwrotnej 0 8 Mechanika 8. Konstrukcja Napęd Elektronika 9. Mikrokontroler Zasilanie Peryferia zewnętrzne Schemat ideowy Płytka PCB Realizacja zadań 5 0. Mechanika Elektronika Program Podsumowanie 5 Bibliografia 7

3 Opis projektu W ramach projektu powstał manipulator szeregowy o czterech stopniach swobody. Konstrukcja wykonana została z laminatu miedzianego, a jej napęd oparty jest o 9-gramowe serwa modelarskie. Moduł sterowania wyposażony jest w płytkę deweloperską STMFDiscovery mocowaną na płytce rozszerzeniowej zawierającej m. in. ekran LCD, joystick, sekcję zasilania oraz złącza przygotowane do zamontowania serw, modułu Bluetooth, enkoderów oraz efektora. Cały układ zasilany jest z akumulatora LiPo S. Serwomechanizmy oraz część logiczna układu zasilane są z napięć 6V i 5V uzyskiwanych przy użyciu stabilizatorów LM7806 oraz LM7. Urządzenie może pracować w jednym z czterech trybów: Sterowanie joystickiem we współrzędnych wewnętrznych Sterowanie joystickiem we współrzędnych zewnętrznych Sterowanie poprzez Bluetooth (z wykorzystaniem protokołu UART) z aplikacji mobilnej Sterowanie z aplikacji komputerowej (interfejs USB) Rysunek : Manipulator

4 Konfiguracja mikrokontrolera Konfiguracja portów MCU wygenerowana w programie STMCubeMX widoczna jest na rysunku. Konfiguracja zegarów przedstawiona jest na rysunku. Rysunek : Konfiguracja portów MCU

5 Rysunek : Konfiguracja zegarów

6 Konfiguracja peryferiów Poniższe tabele przedstawiają konfigurację wszystkich wykorzystywanych w projekcie peryferiów.. ADC Kanały i 9 wykorzystywane są do obsługi joysticka. wykorzystaniem DMA. Skonfigurowano je w trybie Single-ended z Tabela : ADC - uproszczona konfiguracja Mode Independent mode Clock Prescaler Synchronous clock mode divided by Resolution ADC -bit resolution Data Alignment Right alignment Scan Conversion Mode Enabled Contionous Conversion Mode Enabled DMA Continous Requests Enabled End of Conversion Selection End of single conversion Number of Conversions Sampling time - Channel 8.5 Cycles Sampling time - Channel Cycles. IC Wykorzystywany do komunikacji z ekranem LCD. Tabela : IC - uproszczona konfiguracja IC Speed Mode Standard mode IC Speed Frequency 00 khz. RCC High Speed Clock (HSE): Crystal/Ceramic Resonator. SYS Tabela : SYS - uproszczona konfiguracja Debug Serial Wire Timebase Source SysTick.5 TIM Wykorzystywany do sterowania serwomechanizmami - korzysta z DMA Channel: PWM Generation CH Channel: PWM Generation CH Channel: PWM Generation CH Channel: PWM Generation CH 5

7 Tabela : TIM - uproszczona konfiguracja Prescaler 959 Counter Period (ARR) 999 Channel - mode PWM mode Channel - mode PWM mode Channel - mode PWM mode Channel - mode PWM mode.6 TIM5 Wykorzystywany jako podstawa czasowa do wyboru trybu pracy Tabela 5: TIM5 - uproszczona konfiguracja Prescaler 79 Counter Period (ARR) TIM6 Wykorzystywany jako podstawa czasowa do sterowania joystickiem Tabela 6: TIM6 - uproszczona konfiguracja Prescaler 79 Counter Period (ARR) UART5 Wykorzystywany do komunikacji z aplikacją mobilną poprzez moduł Bluetooth Tabela 7: UART5 - uproszczona konfiguracja Baud Rate 9600 Word Length 8 Bits (including parity) Parity None Stop Bits Data Direction Receive and Transmit.9 USB Wykorzystywany do komunikacji z aplikacją komputerową Tabela 8: USB - uproszczona konfiguracja Mode Device (FS) Speed Full Speed MBit/s.0 USB_DEVICE Wykorzystywany do komunikacji z aplikacją komputerową Tabela 9: USB_DEVICE - uproszczona konfiguracja Class Communication Device Class (Virtual Port Com) 6

8 Opis funkcji pinów Tabela 0: Tryby działania pinów oraz ich funkcje Numer pinu: PIN: Tryb pracy: Funkcja: 6 PC GPIO EXTI Przycisk Joysticka 7 PC ADC IN8 Pomiar napięcia na baterii 8 PC ADC IN9 Oś Y Joysticka PA ADC IN Oś X Joysticka 9 PA TIM CH Sterowanie drugim serwem 5 PB0 TIM CH Sterowanie trzecim serwem 6 PB TIM CH Sterowanie czwartym serwem 8 PE7 GPIO EXTI7 Obsługa efektora 8 PB GPIO Output Obsługa efektora 5 PB GPIO Output Obsługa efektora 56 PD9 GPIO EXTI9 Wybór trybu 58 PD GPIO EXTI Wybór trybu 60 PD GPIO EXTI Wybór trybu 6 PD5 GPIO EXTI5 Wybór trybu 68 PA9 IC SCL Linia SCL wyświetlacza LCD 69 PA0 IC SDA Linia SDA wyświetlacza LCD 80 PC UART5 TX Linia TX UART5 8 PD UART5 RX Linia RX UART5 90 PB TIM CH Sterowanie pierwszym serwem 7

9 5 Opis wykorzystanych układów zewnętrznych W projekcie wykorzystane zostały następujące układy zewnętrzne: Ekran LCD - Zgodny z HD780 [], wyposażony w ekspander PCF857AT [], pozwalający na obsługę wyświetlacza przy pomocy interfejsu IC. Poniżej widoczny jest fragment kodu wraz z komentarzami, przedstawiający inicjalizację wyświetlacza: #d e f i n e LCD_ADRESS 0x8 // Adres wyswietlacza LCD #d e f i n e EN_HIGH 0x0 // Wlaczenie pinu EN na wyswietlaczu #d e f i n e LIGHT_ON 0 x08 // Wlaczenie p o d s w i e t l e n i a na w y s w i e t l a c z u 5 void LCD_Init ( ) 6 { 7 / T a b l i c a przechowuje komendy w y s w i e t l a c z a LCD / 8 / / 9 uint8_t tab [ ] = {0 x0, 0x0, 0x0, 0x0, 0x0, 0x80, 0x00, 0x80, 0x00, 0x60, 0x00, 0 xc0 } ; 0 uint8_t tmp [ ] [ ] ; f o r ( i n t i = 0 ; i < ; i ++) { tmp [ i ] [ 0 ] = tab [ i ] EN_HIGH LIGHT_ON; 5 tmp [ i ] [ ] = tab [ i ] LIGHT_ON; 6 } 7 8 HAL_Delay ( 0 0 ) ; 9 0 / I n i c j a l i z a c j a wyś w i e t l a c z a / f o r ( i n t i = 0 ; i < ; i ++) { HAL_IC_Master_Transmit(& hic, LCD_ADRESS <<, tmp [ i ],, 00) ; HAL_Delay ( 5 ) ; 5 } 6 7 /,, w e j s c i e w tryb programowania (D i D5 wlaczone ) 8 tryb bitowy (D5 wlaczone ) 9 5 l i n i e (D5 wlaczone ) 0 6 rozmiar 5x7 (D7 wlaczone ) 7 ( 0 ) 8 kursor, wyswietlacz i miganie kursora wylaczone (D7 wlaczone ) 9 ( 0 ) 0 kursor inkrementuje z l e w e j do prawej, wyswietlacz n i e przesuwa s i e (D5 i D6 wlaczone ) 5 ( 0 ) 6 w l a c z e n i e w y s w i e t l a c z a (D7 i D6 wlaczone ) 7 8 D x80 9 D x0 0 D x0 D x0 BL x08 EN x0 RW x0 5 RS x0 6 / 7 } Joystick z dwoma wyjściami analogowymi (obsługiwanymi przez przetwornik ADC) oraz przyciskiem Moduł Bluetooth (komunikacja przez protokół UART) - wykorzystywany do komunikacji z aplikacją mobilną Ultradźwiękowy czujnik odległości HC-SR0 wykorzystywany w roli przykładowego efektora 8

10 6 Opis działania programu Dostępne są cztery tryby działania, pozwalające na obsługę manipulatora. Aby wybrać dany tryb należy włożyć zworkę w odpowiednie, podpisane miejsce na płytce PCB. Sprawdzanie aktualnego trybu działania odbywa się z częstotliwością 0Hz w przerwaniu czasowym od timera Sterowanie joystickiem we współrzędnych wewnętrznych Za pomocą przesuwania joysticka w pionie dokonywany jest wybór przegubu, przy przesuwaniu w poziomie w zależności od kierunku wychylenia wartość kąta danego przegubu zwiększa się lub zmniejsza. Aktualny przegub oraz wartość jego kąta widoczne są na wyświetlaczu LCD. 6. Sterowanie joystickiem we współrzędnych zewnętrznych Za pomocą przesuwania joysticka w pionie dokonywany jest wybór osi według której dokonywana jest zmiana położenia, zaś przesuwając joystick w poziomie zmienia się wartość przesunięcia w danej osi. Aktualna oś oraz wartość przesunięcia widoczne są na wyświetlaczu LCD. 6. Sterowanie poprzez Bluetooth (z wykorzystaniem protokołu UART) Pozwala na sterowanie we współrzędnych wewnętrznych i zewnętrznych korzystając z aplikacji na telefonie. 6. Sterowanie z aplikacji komputerowej (interfejs USB) Pozwala na sterowanie we współrzędnych wewnętrznych i zewnętrznych za pomocą aplikacji na komputerze. 9

11 7 Algorytm kinematyki odwrotnej W celu uzyskania możliwości operowania efektorem we współrzędnych zewnętrznych został zaimplementowany iteracyjny algorytm FABRIK [] (Forward And Backward Reaching Inverse Kinematic). Został on wybrany ze względu na dużą szybkość działania w porównaniu do innych algorytmów, co jest szczególnie istotne przy stosunkowo małej mocy obliczeniowej mikrokontrolera. Porównanie działania popularnych algorytmów kinematyki odwrotnej oraz algorytmu FABRIK widoczne jest na rysunku. Rysunek : Porównanie algorytmów kinematyki odwrotnej [] Algorytm FABRIK zwraca współrzędne położenia każdego z przegubów w ten sposób, aby między przegubami zachowane były odpowiednie odległości odpowiadające długościom ogniw, a także położenie efektora zgadzało się (z pewną zadaną tolerancją) z położeniem docelowym. Po uzyskaniu współrzędnych każdego z przegubów przeliczane są wartości kątów, w ten sposób, żeby przeguby przemieściły się z aktualnej pozycji do docelowej. 0

12 8 Mechanika 8. Konstrukcja Konstrukcja robota wykonana została z laminatu miedzianego o grubości.6mm. Materiał ten został wybrany ze względu na łatwość obróbki, wytrzymałość oraz mała wagę. Z laminatu zostały wykonane wszystkie główne elementy konstrukcyjne robota takie jak obrotowa podstawa, ogniwa oraz uchwyty montażowe. Dodatkowo jako łączniki wykorzystane zostały metalowe elementy dystansowe. Całość konstrukcji została połączona za pomocą śrub M i M oraz za pomocą cyny lutowniczej. Projekt konstrukcji mechanicznej został wykonany w programie Autodesk Inventor Professional 05 i jest widoczny na rysunku Napęd Manipulator jest napędzany za pomocą czterech serwomechanizmów. Wybrano micro serwomechanizmy Redox S90 MG posiadające metalowe tryby oraz stosunkowo duży moment siły (,55 kg*cm dla zasilania,8v) w porównaniu do ich niewielkiej masy (0g). Rysunek 5: Model manipulatora

13 9 Elektronika 9. Mikrokontroler Jednostką sterującą jest mikrokontroler STMF0VC [] znajdujący się na płytce deweloperskiej Discovery F. Najważniejsze parametry mikrokontrolera widoczne są w tabeli. Tabela : Parametry mikrokontrolera STMF0VC Napięcie zasilania:.0 -.6V Pamięć Flash: 56kB Maksymalna częstotliwość taktowania: 7MHz Ilość wejść/wyjść: 87 Ilość timerów: Interfejsy komunikacyjne: x CAN, x IC, 5x UART, x SPI, x USB 9. Zasilanie Manipulator zasilany jest za pomocą akumulatora Li-Po o napięciu znamionowym 7.V. Napięcie zasilania obniżane jest do napięć 5V (LM7) oraz 6V (LM7806), każde napięcie dostarczane jest przez połączone ze sobą równolegle dwa stabilizatory w celu zwiększenia wydajności prądowej. Płytka deweloperska zasilana jest napięciem 5V. W celu zabezpieczenia układu zasilania zastosowano tranzystor N-MOSFET chroniący układ przed podłączeniem odwrotnej polaryzacji. W celu ochrony baterii oraz układu przed zwarciem zastosowano bezpiecznik polimerowy o prądzie znamionowym równym A. 9. Peryferia zewnętrzne Peryferia zewnętrzne podłączane są za pomocą złącz z kluczem, zapobiegającym odwrotnemu podłączeniu lub wypdanięciu złącza z gniazda oraz zwykłych gniazd goldpin. 9. Schemat ideowy Schemat ideowy układu widoczny jest na rysunku Płytka PCB Projekt płytki PCB widoczny jest na rysunku 7.

14 A B C D P A Listwa Lewa V PC PC PA PA PF PA5 PA7 PC5 PB PE7 PE9 PE PE PE5 PB PB PB5 PD9 PD PD PD5 PC6 Zcza goldpinów Sekcja zasilania V NCST PC0 PC PF PA0 PA PA PA6 PC PB0 PB PE8 PE0 PE PE PB0 PB PB PD8 PD0 PD PD PC P Listwa Prawa 5V PF9 PF0 PC PE6 PE PE PE0 PB8 BOOT0 PB6 PB PD7 PD5 PD PD PC PC0 PA PF6 PA PA0 PA8 PC8 5V PF0 PF PC5 PC PE5 PE PE PB9 VDD PB7 PB5 PB PD6 PD PD PD0 PC PA5 PA PA PA9 PC9 NC VCC LM7 VI VO B P Serwo_ PB PWM PWM Servo_Power P Serwo_ PB0 PWM PWM Servo_Power +V P5 Bluetooth P9 VCC +8V -8V 00R R +8V P Wcznik -8V VCC C 00uF LM7 U VI VO +5V C 00uF P8 Wcznik F A VCC C 00uF U +5V C R 0K Zcze_zasilania VCC 00uF VCC 70R R LED D PD PC UART_RX UART_TX UART_RX UART_TX C Sheet: / File: Manipulator_shield.sch Title: Size: A Date: KiCad E.D.A. kicad.0.0-rc-stable 5 6 Rev: Id: / D R6 0K R5 0K PC POMIAR_BAT POMIAR_BAT Filtracja przy serwach +5V 80R R LED C5 000uF VCC VCC D U LM7806ACT Power_Integrations:TO-0 IN OUT Przeczniki Serwa Enkodery Bluetooth Efektor +V EF_PWM EF_GPIO EF_GPIO +5V 5 6 P Efektor 00nF C9 PD5 PD9 EF_PWM EF_GPIO EF_GPIO C 0nF U LM7806ACT Power_Integrations:TO-0 IN OUT +6V C 00nF C0 0nF P Zasilanie serw +6V +5V +6V C 00nF Servo_Power Servo_Power_Selection Servo_Power Servo_Power V 5V +V +6V +5V 70R R7 LED D PA PWM PWM PB PWM PWM P7 P8 Serwo_ Serwo_ Servo_Power Servo_Power +V P0 LCD PA Joy_X PC Joy_Y PC Joy_SW Joystick 5 P6 P7 LCD 5 P0 PA9 PA0 LCD_SCL LCD_SDA Wybór_trybu Joystick Enkoder PB7 PB6 ENC_SDA ENC_SCL ENC_SDA ENC_SCL +V P Enkoder +V P ENC_SDA ENC_SCL Enkoder ENC_SDA ENC_SCL +V P5 Enkoder ENC_SDA ENC_SCL +V Joy_X Joy_Y Joy_SW PD PD TRYB TRYB TRYB TRYB +V TRYB TRYB TRYB TRYB PB PB PE7 +5V LCD_SCL LCD_SDA Q Q_NMOS_GSD Rysunek 6: Schemat ideowy

15 Rysunek 7: Schemat płytki PCB

16 0 Realizacja zadań 0. Mechanika Część mechaniczna została ukończona w 00%. Zadania zrealizowane: Projekt mechaniki w programie Autodesk Inventor Wycięcie elementów konstrukcyjnych z laminatu Montaż części mechanicznej 0. Elektronika Część elektroniczna została ukończona w 00%. Zadania zrealizowane: Zaprojektowanie schematu ideowego Zaprojektowanie płytki PCB Wytrawienie płytki PCB Lutowanie elementów elementów elektronicznych 0. Program Proces programowania został ukończony w 00%. Zadania zrealizowane: Konfiguracja peryferiów w STMCubeMX Powstanie podstawowej pętli sterowania i wyboru trybu Sterowanie serwomechanizmami Obsługa joysticka Obsługa ekranu LCD Komunikacja Bluetooth z aplikacją mobilną Implementacja algorytmu odwrotnej kinematyki Podsumowanie Modułowość całej konstrukcji zapewnia łatwość wymiany elementów w razie ich zużycia lub zepsucia oraz umożliwia montaż różnych efektorów wykorzystywanych do realizacji różnych zadań. 5

17 Po ukończeniu zajęć projektowych planowany jest dalszy rozwój konstrukcji uwzględniający m. in.: Montaż enkoderów i wykorzystanie ich do dokładniejszego sterowania pozycją Stworzenie kilku wymiennych efektorów (np. chwytak) Wykorzystanie zewnętrznego układu inercyjnego do sterowania pozycją efektora we współrzędnych zewnętrznych 6

18 Literatura [] HD7780 Datasheet. [] PCF857 Datasheet. [] STMF0VC Datasheet. document/datasheet/f/f/e//ef/59/d/50/dm pdf/files/dm pdf/jcr: content/translations/en.dm pdf. [] Andreas Aristidou and Joan Lasenby. FABRIK: a fast, iterative solver for the Inverse Kinematics problem. Graphical Models, 0. 7