Zdalnie sterowany samochód - RPI2, Camera, LabVIEW, Simulink, Dualshock3

[borcio]

Celem projektu było stworzenie zdalnie sterowanego samochodu, którym użytkownik będzie mógł sterować na podstawie otrzymywanego obrazu video (FPV), który będzie wyświetlany na komputerze PC lub telefonie z androidem, a cały program zostanie stworzony w środowisku MATLAB/Simulink. W tym celu wykorzystałem starą zabawkę, z której wyrzuciłem całą elektronikę i zastąpiłem swoją. Na rysunku poniżej widać schemat blokowy całego układu.

Elektronika

W projekcie wykorzystałem:

1. Raspberry Pi 2 z kamerą.

2. Przetwornicę 5V, 5A

3. Akumulator 7,4V 5,1Ah

4. Jednokanałowy sterownik silników DC VNH5019

5. Dwukanałowy sterownik serwomechanizmów zrealizowany na mikrokontrolerze dsPIC33FJ128MC202 - zaprogramowany także poprzez MATLAB/Simulink

6. Napęd tylnej osi - silnik DC klasy 600 (jakiś noname)

7. Przednia oś - standardowe serwo

8. Obrót kamery w dwóch płaszczyznach - dwa micro serwa

9. Przetwornik ADC MCP3302 - pomiar napięcia na akumulatorze

Raspberry Pi 2 + Simulink

Program obsługujący cały samochód stworzyłem Simulinku, dzięki dostępnemu narzędziu do współpracy Simulinka z Raspberry Pi (http://www.mathworks.com/hardware-support/raspberry-pi-simulink.html). Pozwala ono w bardzo łatwy sposób tworzyć aplikacje i uruchamiać je na docelowej platformie sprzętowej. Po instalacji dodatku, pojawia się nowa biblioteka w simulinku, która zawiera podstawowe bloczki takie, jak: GPIO Read, GPIO Write, UDP Receive, UDP Send czy Video Display. Niestety nie ma dostępu do innych funkcji RPI. W tym momencie przychodzą z pomocą tak zwane s-funkcje, czyli import kodu C do symulacji. Tym samym potencjał do rozwijania bardziej skomplikowanych aplikacji drastycznie wzrasta.

Na powyższym rysunku widać cały diagram simulinkowy, na podstawie którego wygenerowany został kod C i skompilowany na Raspberry Pi. Idąc lewej strony widać pierwszy blok zawierający s-funkcję, która miała za zadanie inicjalizację programu, czyli utworzenie socketów UDP, otwarcie UARTu i skonfigurowanie SPI. Dalej jest kolejna s-funkcja która odbiera pakiety UDP i od razu je analizuje. Wśród nich znajduje się sterowanie przesyłane z joysticka, a także odczyt z sensorów telefonu z androidem. Na samej górze znajduje się część odpowiedzialna za sterowanie silnikiem i serwomechanizmem przedniej osi samochodu. S-funkcja MCP3302_sfn realizuje odczyt z przetwornika ADC, który wysyłany jest do komputera PC. Ostatnia część programu to przetworzenie odczytu z sensorów telefonu na dane podawane do sterownika serwomechanimów (dsPIC) by móc sterować obrotem kamery. Cały program może działać w dwóch trybach: external oraz standalone. Pierwszy z nich uruchamia aplikację na RPI z jednoczesną komunikacją z komputerem, co pozwala na zmianę i podgląd niektórych sygnałów (bardzo dobra opcja do debugowania). Tryb standalone to uruchomienie aplikacji tak by była niezależna od komputera PC.

Sterowanie ręczne

Samochód może być sterowany na dwa sposoby: z pada Dualshock 3 lub klawiatury. W przypadku klawiatury sterowanie jest zerojedynkowe, co niewątpliwie jest wadą (po prostu stworzyłem tą opcję gdy nie miałem dostępu do pada, a chciałem pojeździć). Zarządcą komunikacji dualshock - raspberry jest prosty program napisany w LabVIEW (obrazek poniżej).

Sterowanie samochodem do przodu i do tyłu, w lewo i prawo to po prostu aktualny odczyt z gałek analogowych. W lewym górnym rogu wpisywany jest adres IP raspberry, a po prawej stronie wyświetlany jest aktualny stan akumulatora. Poniżej widać realizację tego zadania w LabVIEW.

Sterownik serwomechanizmów - dsPIC

Powodem zastosowania dodatkowego sterownika serwomechanizmów jest to, że raspberry ma tylko dwa wyjścia PWM, które już wcześniej zostały wykorzystane. Softwareowe generowanie PWM także nie wchodzi w grę, ponieważ wtedy drastycznie wzrasta zużycie procesora, a jitter sygnału nie jest akceptowalny. Zadanie tego sterownika jest proste: odebrać po UART 4 bajty, stworzyć z nich dwie liczby uint16, które jednocześnie będą współczynnikiem wypełnienia dwóch kanałów PWM. Całość została także zaprogramowana w simulinku (obrazek). Biblioteka obsługująca mikrokontrolery z rodziny dsPIC jest dostępna za darmo na stronie Microchip.

Odbiór obrazu z kamery

Za obsługę kamery odpowiedzialny jest "raspivid", a za wysyłanie streamu "gstreamer". Gstreamer zapewnia bardzo małe opóźnienia w przesyle, dzięki czemu w dość komfortowy sposób można sterować samochodem patrząc tylko na obraz z kamery. Poniżej zamieściłem przykładowe wywołanie raspivida z gstreamerem.

Po stronie raspberry:

raspivid -t 0 -w 640 -h 480 -fps 25 -rot 180 -b 1500000 -o - | gst-launch-1.0 -e -vvv fdsrc ! h264parse ! rtph264pay pt=96 config-interval=5 ! udpsink host=192.168.10.143 port=47277

Po stronie PC lub telefonu

gst-launch-1.0 udpsrc port=47277 ! application/x-rtp, payload=96 ! rtpjitterbuffer ! rtph264depay ! avdec_h264 ! fpsdisplaysink sync=false

Aby móc odebrać stream na telefonie trzeba mieć zainstalowaną aplikację "RaspberryPi Camera Viewer" i w ustawieniach streamu wpisać powyższą komendę (bez "gst-launch-1.0").

Filmy

Za jakość przepraszam (toster miał słabszy dzień). Sterowanie obrotem kamery nie wyszło najlepiej, ponieważ aplikacja była w trybie external, a wtedy ilość pakietów w powietrzu jest duża, przez co mogą pojawić się przycinki.

Podsumowanie

Dzięki dobrej znajomości środowiska MATLAB/Simulink udało się zrealizować zadane cele, a czas poświęcony na całość projektu był stosunkowo niewielki (samo stworzenie aplikacji to kwestia kilku dni). Celem na najbliższą przyszłość jest stworzenie (a raczej dokończenie) aplikacji miksującej obraz tak, aby był wyświetlany drugi tak sam (lub zmodyfikowany odpowiednio) obok. Dzięki temu możliwe będzie zamontowanie telefonu w jakimś urządzeniu do rozszerzonej rzeczywistości (np. google cardboard) co zwiastuje super zabawę.