Trzykołowiec

Projekt prostego programowalnego robota kołowego z dwoma silnikami.

Do czego zmierzamy

Chodzi o to, żeby względnie małym nakładem pracy uzyskać robota, którego można łatwo zaprogramować. Dzięki temu bez zbędnych komplikacji będzie można rozpocząć realną zabawę w całkiem poważną robotykę. Robot będzie się składał z dwóch, oddzielnie sterowanych silników napędowych wyposażonych w koła, oraz trzeciego podpierającego. Możliwość sterowania obrotami każdego z kół z osobna pozwala zaprogramować dowolny kierunek jazdy i tworzyć dowolne trasy przejazdu. Podstawowe możliwości, uniwersalność i przez to przydatność takiego robota pokazuje poniższy film, gdzie projektanci z firmy Microsoft użyli dokładnie takiej konstrukcji w swoich projektach. Co więcej, tak samo jak my użyli Maliny jako "mózgu" robota.

Konstrukcja

Silnik

Silnik o rozmiarze NEMA 14.

Do napędu kół użyjemy silnika krokowego o następujących parametrach:

• rozmiar NEMA 14, czyli boki 35x35 mm, ponadto nasz silnik będzie miał 34 mm długości i oś wystającą na 20mm o średnicy 5mm
• waga 180g
• maksymalny moment obrotowy 120 mN/m (miliniutonów na metr)
• żeby wykonać pełny obrót silnik potrzebuje 200 kroków
• pobór prądu 750mA(miliamperów) przy napięciu zasilającym 4.35V

Tak wygląda w środku.

Zasadę działania silnika krokowego przedstawia animacja poniżej: włączamy prąd w kolejnych uzwojeniach, co wytwarza silne pole magnetyczne, które przyciąga metalowe wypustki wirnika i obraca go o pewien mały kąt. Zależnie od tego w jakiej kolejności włączamy cewki, silnik będzie się kręcił w lewo lub prawo. Dużo więcej magii w tym nie ma ;).

Jak widać na zdjęciu rozebranego realnego silnika, praktyczna realizacja jest trochę inna: w tym przypadku mamy 8 cewek elektromagnesów na obwodzie (miedziane zwoje drutu na dole zdjęcia), a nie 4 jak na schemacie. Jednak sama zasada pozostaje ta sama. Dodatkowe cewki i podwójny rotor jak widać na zdjęciu pozwalają poprawić siłę z jaką silnik się obraca oraz precyzję kontroli kąta obrotu. Szczegóły wyjaśnia film poniżej.

Co oznaczają podane parametry silnika?

Wymiary i masa

Z rozmiarów silnika zgodnego z normą NEMA 14 i jego masy widzimy, że choć silnik nie jest jakoś przerażająco wielki, to jest jednak całkiem solidnym kawałkiem metalu. Same dwa silniki napędowe będą ważyły 360g, a więc dodając jeszcze powerbank, możemy się spodziewać, że cały robot będzie ważył około 1kg. To oszacowanie przyda się przy projektowaniu podwozia. Dzięki niemu wiemy jaką mniej więcej powinno mieć wytrzymałość — w przybliżeniu powinno być sztywniejsze niż choćby karta bankomatowa.

Moment obrotowy 120 mNm

Zwiększanie się siły koniecznej do zatrzymania silnika na przykładzie użytego silnika Nema14 o znamionowym momencie siły 120 miliniutonometrów.

Gdyby przeczytać tą liczbę brzmiałaby "sto dwadzieścia miliniutonometrów". Pierwsza część, tj. 120mN (miliniutonów) to niewielka siła, okolice ciężaru odtwarzacza mp3. Ale o jaki metr chodzi?

O taki, jak gdyby do osi silnika przymocować metrowy patyk pod kątem prostym do osi (tak jak szprycha w kole roweru). Kiedy silnik pracowałby z całej siły wówczas koniec takiego metrowego patyka naciskałby na rękę z podaną siłą, lub jak kto woli wystarczyłoby delikatnie trzymać jego koniec, żeby silnik nie mógł się obracać.

Łatwo wyczuć intuicyjnie, że gdyby patyk był krótszy, wówczas trzeba by było użyć większej siły, żeby powstrzymać silnik przed obracaniem się. Kto nie wierzy niech spróbuje przytrzymać koło roweru za szprychę najpierw chwytając blisko opony, a potem bliżej środka. O razu widać, że o ile trzymanie przy oponie jakoś wychodzi, to bliżej środka palce są za słabe. Fizycy mówią, że jeśli skracamy ramię na które działa pewien moment obrotowy, to siła na końcu ramienia proporcjonalnie rośnie.

I to jest bardzo dobra wiadomość! Gdybyśmy mieli silnik pchający robota do przodu z siłą tylko 120mN moglibyśmy zapomnieć o jeździe po czymkolwiek mniej gładkim niż stół, a i to wątpliwe... Jeśli jednak skrócimy w wyobraźni metrowy patyk do połowy, siła na końcu wzrośnie proporcjonalnie do 240mN. Nadal szału nie ma, ale jest postęp. Gdybyśmy skrócili go do ${\frac {1}{10}}$ metra, wówczas otrzymamy 120mN * 10 = 1200mN. Przypomnę, że mili oznacza jedną tysięczną. Czyli 1200mN to już całkiem duża siła 1.2 niutona.

No dobrze, ale przecież nie będziemy mocować patyków na osi silnika tylko koła... To nic nie zmienia! Wystarczy sobie wyobrazić, że nakładamy na oś jeden patyk za drugim dookoła osi (znów, tak jak szprychy w kole roweru). Jeśli nawkładalibyśmy ich odpowiednio dużo powstanie w końcu koło. Na tym polega geniusz wynalazku koła, którego ludzkość bardzo długo nie znała. Dzięki odsunięciu od podłoża osi, na której skupiony jest ciężar, możemy przesuwać masy ze znacznie mniejszym wysiłkiem, bo cały czas stosujemy dźwignię o takiej długości jak promień koła.

Teraz możemy wrócić do wymiarów w trójkołowcu. Jeśli użylibyśmy niewielkich kół o średnicy 2cm, wówczas promień miałby tylko 1cm, a więc 1/100 metra. Silnik pracujący z maksymalną mocą ciągnąłby pojazd na takich kołach z siłą 120 * 100 = 12000mN, czyli 12N. Z taką siłą naciska na rękę trzymane w niej 1,2 kg. Dla kół o średnicy dwa razy większej (4cm) siła spadłaby do 6N (~600g) i dla kółek o średnicy 6cm do około 4N (~400g).

Czy to dużo czy mało? Możemy łatwo oszacować co to oznacza wyliczając możliwe przyśpieszenia robota. Dla ułatwienia przyjmijmy, że cały będzie ważył około 1kg.

Przyśpieszenie podaje zupełnie prosty wzór, sprzed ponad 300 lat, pana Izaaka Newtona (czytaj niuton):

$a={\frac {F}{m}}$

gdzie:

• $F$ to siła wyrażona w, a jakże, w N (niutonach)
• $m$ to masa, u nas 1kg
• $a$ to przyśpieszenie jakiego należy się spodziewać w m/s²

Podstawiając dane dla kół o średnicy 6cm, które dają siłę pchającą 4N na jeden silnik, przy włączonym napędzie na oba koła mamy:

$a={\frac {2*4[N]}{1[kg]}}=8[m/s^{2}]$

Jeśli porównamy to z przyśpieszeniem ziemskim, wynoszącym $9,81[m/s^{2}]$ widzimy, że pojazd mógłby przyśpieszać prawie tak szybko jak szybko rzeczy spadają w polu grawitacji Ziemi. Inaczej mówiąc potrzebowałby niecałych 3 sekund "do setki" ! Oczywiście to tylko oszacowanie, które podaje maksymalne przyśpieszenie. W praktyce będzie ono mniejsze. Wystarczy choćby zauważyć, że nie uwzględniliśmy sił tarcia, która będzie go spowalniać. Nawet jednak gdybyśmy spadli w okolice połowy tego przyśpieszenia nadal taki napęd wydaje się sensowny.

Niestety oprócz zdolności silnika do przyśpieszania pojazdu, która jak widzimy jest dobra, musimy uwzględnić jeszcze jedno ograniczenie — maksymalną prędkość z jaką silnik potrafi się obracać. Jak widać na filmie w dalszej części strony, pojazd rusza natychmiast, bez wyraźnych opóźnień, a więc przyśpieszenie jest rzeczywiście dobre. Jednak jego maksymalna prędkość nie jest wielka właśnie dlatego, że układ sterowania silnika nie może podawać impulsów sterujących dowolnie szybko. Silnik wymaga 200 impulsów na każdy pełny obrót. Szacunkowo jeśli potrafilibyśmy generować 1000 impulsów na sekundę, koło obracałoby się 5 razy w ciągu sekundy, a więc pojazd z kołami o promieniu 3cm (jak na filmie) poruszałby się z prędkością

$5[obr/s]*2*\pi *r[m]=10*3.14*0.03=0.95[m/s]$

200 kroków na obrót

Silniki krokowe działają trochę inaczej niż zwykłe. W zwykłym wystarczy włączyć prąd i silnik się obraca. Silnikiem krokowym steruje się inaczej: podaje się odpowiednie impulsy, które powodują, że oś silnika obraca się o pewien kąt. Jak daleko obraca się oś w jednym kroku definiuje właśnie liczba kroków potrzebnych do pełnego obrotu. W przypadku naszego silnika będziemy musieli podać 200 impulsów żeby koło napędowe wykonało jeden obrót. Dzięki takiej dokładności możliwe będzie bardzo precyzyjne sterowanie odległością jaką przemierza każde koło, a przez to będzie można precyzyjnie skręcać.

Parametry elektryczne

Jeśli chodzi o napięcie pracy silnika, wynoszące 4.35V, jest ono nieco mniejsze niż 5V, którego używają porty USB, powerbank i którym zasilana jest Malina (przy czym część cyfrowa Maliny działa przy napięciach 0 i 3.3V).

Możemy jednak przyjąć, że nieco wyższe napięcie zasilania jakiego użyjemy, nie będzie groźne dla uzwojeń silnika, a może poprawić prędkość z jaką silnik będzie wykonywał kolejne kroki. W normalnej sytuacji, kiedy silniki jest montowany w większej maszynie unikalibyśmy takiego "podkręcania", ale w naszym przypadku silnik będzie pracował w otwartej przestrzeni, i pewien nadmiar wydzielanego ciepła jest dopuszczalny.

Znaczenie poboru prądu wyjaśnimy przy okazji rozważań o baterii.

Bateria

Pobór prądu przez jeden silnik, który wynosi 750mA (miliamperów) oznacza, że gdybyśmy mieli baterię o pojemności 750mAh (miliamperogodzin) pojedynczy silnik mógłby działać przez godzinę. Ponieważ mamy dwa silniki i jeszcze Malinę, która również zużywa prąd, możemy oczekiwać zużycia na poziomie około 1800mAh.

Teoretycznie więc powerbank o takiej pojemności mógłby zasilać robota przez godzinę. W praktyce producenci obiecują gruszki na wierzbie, i lepiej przyjąć co najmniej 2500mA potrzebnej pojemności na godzinę pracy robota.

Powerbanki zwykle ładują się godzinami -- nie rzadko instrukcja podaje 8-10 godzin. Ponieważ byłoby niezbyt wygodnie po każdej godzinie testów czekać 8 godzin do kolejnego naładowania baterii, lepiej zastosować akumulator o większej pojemności niż tylko godzina użytkowania. Ostatecznie więc pobór prądu mówi nam tyle, że przydałby się akumulator o pojemności co najmniej 5000mAh.

Ponadto musimy uwzględnić maksymalne możliwe obciążenie prądowe. Cóż z tego, że mielibyśmy pełny basen wody, jeśli dałoby się z niego czerpać tylko po jednej kropli na godzinę? Musimy więc wybrać powerbank, który będzie miał też odpowiednią wydajność.

Dwa silniki zużywają 750mA * 2 = 1500mA. Do tego prądu trzeba dodać też Malinę, czyli co najmniej 500mA. Powerbank musi znieść obciążenie 2000mA, czyli 2A.

Projekt podwozia

Wykonanie podwozia jest zadaniem do wykonania samodzielnie. Wszystkie chwyty dozwolone! Jeśli masz natchnienie, żeby ulepić podwozie z taśmy klejącej i gumy do żucia proszę bardzo. Jest tylko jedno ograniczenie -- byłoby fajnie gdyby robot zbudowany w oparciu o dostarczone części i zaprojektowane podwozie jeździł! Żeby to ograniczenie dokładniej sprecyzować umówmy się, że:

Części robota (oprócz Maliny) przechodzą na własność konstruktora dopiero po udowodnieniu, że robot jeździ i to zgodnie z podanym programem, czyli inaczej mówiąc o własnych siłach zarówno fizycznych jak i "umysłowych".

Jeśli ktoś chciałby zaprojektować podwozie do wydruku na drukarce 3D (to jest możliwa opcja, nie wymaganie!) proponuję wykorzystanie programu FreeCAD. Dla ułatwienia poniżej plik wstępnie przygotowanego projektu robota. Zawiera podstawowe elementy zgodne z ich rozmiarami:

• płytkę Maliny
• silniki
• powerbank

Pozostaje tylko zająć się samym podwoziem i rozmieszczeniem wyżej wymienionych elementów na nim.

Plik:Trzykolowiec.fcstd

Przykładowe rozwiązanie mechanicznej części robota wykonanej na drukarce 3D przedstawiają zdjęcia poniżej.

Trzykołowiec wydrukowany na drukarce. Widok z góry.

I z dołu.

Elektronika

Opis wyprowadzeń łącza 40 pinowego

Piny złącza 40 pinowego liczy się zaczynając od kwadratowego (numer 1), a dalej "zygzakiem".

napięcie 3.3V	1	2	napięcie 5V
I²C: SDA (dane) Rezystor podciągający 1.8k do 3.3V	3	4	napięcie 5V
I²C: SCL (taktowanie) Rezystor podciągający 1.8k do 3.3V	5	6	MASA
GPCLK0 (zegar nr 0, ogólnego przeznaczenia)	7	8	UART0: TXD (wysyłka) (na tym łączu działa konsola systemu Raspbian)
MASA	9	10	UART0: RXD (odbiór) (na tym łączu działa konsola systemu Raspbian)
	11	12	PWM0/BCM18 (fala prostokątna o programowalnym wypełnieniu z generatora 0)
	13	14	MASA
	15	16
napięcie 3.3V	17	18
MOSI/BCM10 (urządzenie nadrzędne do podrzędnego na łączu SPI)	19	20	MASA
MISO (urządzenie podrzędne do nadrzędnego na łączu SPI)	21	22
SCLK/BCM11 (taktowanie łącza SPI)	23	24	CE0 (linia 0 wyboru chipu łącza SLI)
MASA	25	26	CE1 (linia 1 wyboru chipu łącza SLI)
ID_SC (dane łącza I²C do pamięci EEPROM)	27	28	ID_SC (taktowanie łącza I²C do pamięci EEPROM)
	29	30	MASA
	31	32	PWM0/BCM12
PWM1/BCM13	33	34	MASA
MISO (urządzenie podrzędne do nadrzędnego na łączu SPI)	35	36
	37	38	MOSI/BCM20 (urządzenie nadrzędne do podrzędnego na łączu SPI)
MASA	39	40	SCLK/BCM21 (taktowanie łącza SPI)

Powyżej zastosowane są następujące oznaczenia:

• pogrubiona czcionka — zwykłe piny elektryczne, nie działające w logice cyfrowej ponieważ nie łączą się z procesorem maliny (np. masa, napięcie 3.3V); napięcie 3.3V może pełnić rolę sygnału wejściowego oznaczającego 1 ponieważ piny cyfrowe właśnie takiego napięcia spodziewają się jako logicznej jedynki; możliwe jest więc połączenie kablem wyjścia 3.3V z którymś pinem logicznym żeby zasymulować podanie sygnału 1; można też zasilać z tego wyjścia jakieś niewielkie urządzenia np. podpiąć do niego diodę LED, której używamy jako sondy stanów logicznych i sprawdzić czy działa (wydajność zasilania 3.3V wynosi zaledwie kilkadziesiąt mA, nie należy go obciążać większymi układami)
• czerwona pogrubiona czcionka — napięcie zasilające 5V; może się przydać do zasilania drobnych urządzeń (takich jakie można podłączyć do wyjścia USB z którego zasilamy malinę); UWAGA: Napięcie 5V jest za wysokie dla wejść procesora i prawie na pewno go uszkodzi! Z tego powodu nie wolno łączyć kablem tych pinów z żadnymi wejściami cyfrowymi na łączu 40-pinowym! Właśnie dlatego oznaczone jest na czerwono! najlepiej na piny 2 i 4 nasunąć kawałek izolacji i o nich zapomnieć
• zwykła czcionka — piny cyfrowe gdzie ustawienie z programu wartości 0 daje napięcie 0V, a wysłanie wartości 1 daje napięcie około 3V
• kolory i opisy przy pinie — niektóre piny mają ustawioną jakąś domyślną funkcję w systemie Raspbian sterującym maliną; te funkcje można w większości przedefiniować, jednak w czasie włączania systemu pin działa w trybie domyślnym, dopóki go nie przedefiniujemy po swojemu - jest to bardzo ważne jeśli do pinu podpięte jest jakieś urządzenie ponieważ po włączeniu maliny do prądu będzie ono otrzymywać przypadkowe sygnały w trakcie wczytywania systemu co może prowadzić do uszkodzeń (np. jeśli jest to silnik ramienia robota); warto zauważyć, że zmiana działania pinu generuje ostrzeżenie i na przykład polecenie gpio.setup(8, gpio.OUT) (czyli ustawienie pinu 8 jako zwykłe wyjście cyfrowe) wyświetli komunikat, że pin był używany w innej roli; kolory oznaczają piny związane domyślnie z tym samym urządzeniem (np. pary dwupinowych łączy I²C, czy UART)
• brak opisu — piny nie wykonują żadnej specjalnej funkcji w domyślnej konfiguracji Raspbiana; przy włączeniu zasilania będą ustawione na 0; można ich dowolnie używać

Moduł sterownika silnika krokowego DRV8834 i jego połączenie z maliną

Sposób połączenia silnika, źródła zasilania, sterownika i Maliny przedstawia obrazek.

Jak widać w naszym prostym projekcie nie używamy wszystkich wyjść sterownika.

Na diagramie niektóre z używanych wyjść są podpisane kolorem czerwonym, a inne jasnozielonym. Chodzi o to, żeby wyraźnie pokazać, że choć tego nie widać sterownik rozdziela cały układ na dwa zupełnie różne światy:

• świat silnika — tutaj krążą duże prądy (takie, które mogą spokojnie odparować cieńsze przewody!), pojawiają się wyższe napięcia (rzędu kilkunastu woltów), układy pracują z dużą mocą (silnik potrafi być nawet gorący)
• świat cyfrowy — tutaj nie ma wielkiego znaczenia moc sygnału, a tylko czy ustawiony jest stan 1 (ok. 3 wolty) czy stan 0 (ok. zero woltów); wszystkie elementy z tego świata spodziewają się maksymalnie kilku woltów napięcia, nie zostały zaprojektowane do obsługiwania takich wielkich obciążeń jak silniki i cały ten podsystem mógłby być zasilany z małej zegarkowej bateryjki przez miesiące, na tyle jest "delikatny"

Ważne jest, żeby rozróżniać te dwa światy! Jeśli ktoś spróbowałby podłączyć silnik do tego samego pinu, który jest połączony z Maliną prawdopodobnie spaliłby i sterownik i komputer (choć Malina ma pewne zabezpieczenia przed takim brutalnym traktowaniem i czasem zdąży się wyłączyć w porę).

Ale jest też odwrotnie. W sprzyjających warunkach "delikatna" część cyfrowa potrafi "zamordować" część dużych mocy sterującą silnikami. Jak może do tego dojść? W dokładnie taki sam sposób, jak delikatne, ale nieprzemyślane ruchy kierownicą mogą zniszczyć cały samochód. Chodzi o to, że część cyfrowa steruje systemem, który ma już jakąś niemałą energię (silnik, koła). W przypadku projektu z diodą, jeśli kod zawierał jakąś nie do końca przemyślaną instrukcję, najgorszym co mogło się stać był brak światła lub świecenie diody w nieoczekiwany sposób. Nic ponad to. Tymczasem tutaj robot może niespodziewanie ruszyć, skręcić, cofnąć się, i tylko aktualne otoczenie decyduje o tym jak to się skończy (coś do picia, klawiatura i szalony robot to całkiem skuteczny przepis na większe wydatki). Z tego powodu najlepiej na początku ustawić robota z kołami napędowymi w powietrzu (albo bez kół), żeby odebrać mu możliwość niekontrolowanego ruszania.

Trzeba też uwzględnić specyficzną cechę samego sterownika. Jego instrukcja mówi o tym, że nie wolno podłączać/odłączać przewodów silnika kiedy zasilanie silnika jest włączone (podany sygnał 1 na wyjściu SLP). Ponieważ sygnał na wyjściu SLP podaje Malina najprościej zapamiętać, że jeśli potrzebujemy odłączyć sterownik to zawsze najpierw odłączamy Malinę (czyli 8 punktową wtyczkę, która będzie po lewej stronie na diagramie), a potem silniki (czyli wtyczka po prawej, pin "masy" Maliny znajdujący się na dole tej wtyczki nie jest problemem). I w odwrotnej kolejności podłączamy sterownik, czyli najpierw silniki, potem Malinę. W ten sposób mamy zawsze pewność, że przewody silników będą wyłączane/włączane przy wyłączonym zasilaniu.

Wyprowadzenie zasilania z wtyczki USB. Dodatkowe szczegóły zawiera opis obrazka dostępny po jego kliknięciu.

I jeszcze jedno wyjaśnienie dlaczego wyjście M0 jest podłączone do masy. Chodzi o to, że zastosowany sterownik obsługuje funkcję bardzo przydatną w bardziej precyzyjnych urządzeniach. Potrafi mianowicie wykonywać tylko ułamek kroku zamiast od razu cały. Typowy silnik krokowy (taki jak nasz) wykonuje 200 kroków na pełny obrót. Ten sterownik potrafi podzielić jeden krok nawet na 32 drobniejsze, co daje aż 6400 kroków na jeden pełny obrót! W bardziej precyzyjnych urządzeniach (np. drukarce 3D) ta opcja może być przydatna, ale nam nie jest potrzebna. O tym jak dzielony jest krok decydują piny M0 i M1 sterownika. Jeśli M1 pozostawimy niepodłączony, a na M0 podamy logiczne 0 (czyli zero woltów, a więc właśnie masę) wówczas sterownik nie będzie dzielił kroku i jeden impuls na wejściu STEP będzie powodował wykonanie 1/200 obrotu przez silnik. Dzięki temu wystarczy posłanie tylko 200 impulsów żeby obrócić koło jeden raz.

Oprogramowanie

Pomocniczy moduł do obsługi silników

Żeby móc się skupić na programowaniu konkretnych tras do obsługi silników użyjemy gotowego, prostego modułu pythona. Jego kod można pobrać stąd:

Plik:Silniki.zip

Moduł zawiera kilka prostych funkcji, dzięki którym można wykonać automatycznie inicjowanie portów maliny, włączać/wyłączać silniki, wykonywać pojedynczy krok w zadanym kierunku, a nawet całe sekwencje kroków. Z jego użyciem zaprogramowanie trasy przejazdu sprowadza się do napisania po prostu instrukcji dokąd jechać. Zwróć uwagę, że choć sam plik modułu jest długi, większość to komentarze i opisy działania funkcji.

Na wykresie połączeń elektrycznych powyżej napisane jest, że należy wybrać dowolne z wolnych portów Maliny. Jednak kod modułu obsługi silników musi znać już konkretne numery portów, bo inaczej nie byłoby jasne na które wyjścia posyłać sygnały sterujące. Dlatego w liniach 22-24 dla lewego silnika, i 26-28 dla prawego, wpisane zostały numery portów Maliny, do których podpięte są kable łączące do wyjść SLP, STEP, DIR sterowników. Połączenia pomiędzy Maliną ze sterownikami najprościej wykonać patrząc na tabelę zaczynającą się w linii 8. Jakieś drobne pomyłki w połączeniach nie powinny być szkodliwe, ale to jest właśnie taki moment przez który robot rusza w nieoczekiwanych momentach, dlatego lepiej zachować uwagę. Najlepiej też zrobić połączenia przed włączeniem zasilania.

  1 # coding: utf-8
  2 
  3 import RPi.GPIO as gpio
  4 from time import sleep
  5 
  6 # Poniższa numeracja pinów zakłada następujące połączenia kablowe:
  7 #
  8 # |   PIN MALINY  |  LEWY STEROWNIK SILNIKA | PRAWY STEROWNIK SILNIKA |
  9 #  --------------- ------------------------- -------------------------
 10 # |        9      |                         |          GND            |
 11 # |       11      |                         |          SLP            |
 12 # |       13      |                         |          STEP           | 
 13 # |       15      |                         |          DIR            | 
 14 # |               |                         |                         | 
 15 # |       16      |            DIR          |                         | 
 16 # |       18      |            STEP         |                         | 
 17 # |       20      |            GND          |                         | 
 18 # |       22      |            SLP          |                         | 
 19 # 
 20 # Oczywiście jeśli wybrało się inne wyjścia numery poniżej można 
 21 # (i trzeba) pozmieniać
 22 PIN_SILNIK_LEWY_KIERUNEK = 16    
 23 PIN_SILNIK_LEWY_KROK = 18       
 24 PIN_SILNIK_LEWY_WLACZ = 22
 25 
 26 PIN_SILNIK_PRAWY_WLACZ = 11
 27 PIN_SILNIK_PRAWY_KROK = 13
 28 PIN_SILNIK_PRAWY_KIERUNEK = 15
 29 
 30 
 31 # Stałe pozwalające czytelniej pisać kod. 
 32 # Zamiast krok(-1, 1) można napisać krok(TYL, PRZOD).
 33 # Obie instrukcje zrobią to samo, ale druga jest czytelniejsza.
 34 PRZOD = 1
 35 STOP = 0
 36 TYL = -1
 37 
 38 # Domyślny czas wykonywania kroku (5 milisekund). 
 39 #
 40 # Przy 5 milisekundach można wykonać maksymalnie 200 kroków/s, 
 41 # co przy 60mm kole daje prędkość ~200mm/s. 
 42 #
 43 # Ustawienie większej wartości daje silnikowi więcej czasu 
 44 # na wykonanie ruchu i jest on pewniejszy (silnik zawsze zdąży).
 45 #
 46 # Krótszy czas zwiększa maksymalną prędkość obrotową, ale może 
 47 # powodować, że pierwsza lepsza przeszkoda obciąży silnik na tyle, 
 48 # że nie zdąży on wykonać kroku i zablokuje się w miejscu.
 49 #
 50 # Poniżej 1ms silnik może się blokować nawet bez obciążenia.
 51 DOMYSLNY_CZAS_KROKU = 0.005
 52 
 53 
 54 def przygotuj():
 55     '''
 56     Ta funkcja przygotowuje piny Maliny do komunikacji ze sterownikami 
 57     silników. Należy ją wywołać na początku programu, przed wszystkimi 
 58     innymi. 
 59     '''
 60     gpio.setmode(gpio.BOARD)
 61     piny_sterowania_silnikow = [PIN_SILNIK_LEWY_KROK, 
 62         PIN_SILNIK_LEWY_KIERUNEK, PIN_SILNIK_LEWY_WLACZ, 
 63         PIN_SILNIK_PRAWY_KROK, PIN_SILNIK_PRAWY_KIERUNEK, 
 64         PIN_SILNIK_PRAWY_WLACZ]
 65 
 66     for pin in piny_sterowania_silnikow:    
 67         gpio.setup(pin, gpio.OUT)
 68 
 69 
 70 def podlacz(lewy = 0, prawy = 0):
 71     '''
 72     Włącza lub wyłącza zasilanie silników. Na przykład:
 73     podlacz(1,0) -- włączy zasilanie w silniku lewym i wyłączy w prawym
 74     podlacz(0,0) -- wyłączy zasilanie obu silników
 75     podlacz(1, None) -- włącza zasilanie lewego silnika nie zmieniając 
 76                         stanu zasilania prawego
 77     podlacz() -- brak parametrów spowoduje przyjęcie domyślnych 0,0
 78                  a więc ta instrukcja wyłącza zasilanie obu silników
 79     '''
 80     # Przełączamy zasilanie silnika o ile wartość nie jest 
 81     # nieokreślona (None)
 82     if lewy is not None:
 83         gpio.output(PIN_SILNIK_LEWY_WLACZ, lewy)
 84     if prawy is not None:
 85         gpio.output(PIN_SILNIK_PRAWY_WLACZ, prawy)
 86 
 87 
 88 def krok(lewy, prawy, czas = None):
 89     '''
 90     Wywołanie tej funkcji wysyła impusly sterujące do sterowników silnika, 
 91     które powodują wykonanie jednego kroku. 
 92     
 93     Na przykład:
 94        krok(PRZOD, PRZOD) -- wykonanie kroku w przód przez oba silniki
 95        krok(PRZOD, TYL)   -- lewy silnik wykona krok w przód, a prawy w tyl, 
 96                              pojazd skręci lekko w prawo w miejscu 
 97        krok(PRZOD, STOP)  -- lewy silnik wykona krok w przód, a prawy 
 98                              nie poruszy się
 99 
100     Jeśli nie chcemy używać domyślnego czasu wykonywania kroku (patrz opis 
101     zmiennej DOMYSLNY_CZAS_KROKU powyżej) możemy podać inną wartość czasu
102     dla aktualnego kroku jako trzeci paramter. 
103     
104     Na przykład:
105        krok(PRZOD, PRZOD, 0.01) -- krok do przodu na obu silnikach z względnie
106                                    długim czasem -- w tym trybie powinno się 
107                                    dać pokonać trudne przeszkody, które blokują
108                                    silnik kiedy czas jest krótszy
109     '''
110 
111     # Zamieniamy podane polecenie na odpowiednie wartości dla pinów STEP i DIR.
112 
113     if lewy == PRZOD:      # PRZOD: generujemy impuls kroku z pinem kierunku na 1
114         lewy_krok = 1
115         lewy_kierunek = 1    
116     elif lewy == STOP:     # STOP: bez impulsu kroku, kierunek bez znaczenia
117         lewy_krok = 0
118         lewy_kierunek = None
119     elif lewy == TYL:      # TYL: impuls kroku z pinem kierunku na 0  
120         lewy_krok = 1
121         lewy_kierunek = 0
122     else:
123         raise "Nieznane polecenie!"  # Nie PRZOD, nie TYL, nie STOP??? To co?   
124 
125     # Jak wyżej, tylko dla prawego silnika. Zwróć uwagę, że pin kierunku włączany
126     # jest odwrotnie (dla PRZOD ma wartosc 1, dla TYL 0). Wynika to z budowy 
127     # robota gdzie silniki są obrócone do siebie o 180 stopni i trzeba je obracać 
128     # w przeciwną stronę, żeby napędzały robota w tym samym kierunku.
129     if prawy == PRZOD:       
130         prawy_krok = 1
131         prawy_kierunek = 0
132     elif prawy == STOP:  
133         prawy_krok = 0
134         prawy_kierunek = None
135     elif prawy == TYL:   
136         prawy_krok = 1
137         prawy_kierunek = 1
138     else:
139         raise "Nieznane polecenie!"  
140 
141     # Jeśli czas nie jest podany ustawiamy domyślną wartość zdefiniowaną
142     # w DOMYSLNY_CZAS_KROKU
143     if czas is None:
144         czas = DOMYSLNY_CZAS_KROKU
145 
146     # I wykonanie kroku:
147  
148     # Najpierw ustawiamy sygnały na pinach kierunków o ile jest to potrzebne 
149     # (np. polecenie STOP nie wymaga ustawiania kierunku)
150     if lewy_kierunek is not None:
151         gpio.output(PIN_SILNIK_LEWY_KIERUNEK, lewy_kierunek)
152 
153     if prawy_kierunek is not None:
154         gpio.output(PIN_SILNIK_PRAWY_KIERUNEK, prawy_kierunek)
155 
156     # Teraz wysyłamy impuls wykonania kroku, czyli przez połowę podanego czasu 
157     # ustawiamy pin na wyznaczoną powyżej wartość, a po chwili dajemy 0. 
158     gpio.output(PIN_SILNIK_LEWY_KROK, lewy_krok)
159     gpio.output(PIN_SILNIK_PRAWY_KROK, prawy_krok)
160     sleep(czas*0.5)
161 
162     gpio.output(PIN_SILNIK_LEWY_KROK, 0)
163     gpio.output(PIN_SILNIK_PRAWY_KROK, 0)
164     sleep(czas*0.5)
165 
166     # Dokładniejszy opis: 
167     #
168     # Jeśli wyznaczona wartość ..._krok wynosiła 1, sygnał na pinie STEP skoczy 
169     # do 1 (czyli ok. 3 wolty) i potem spadnie do 0. Sterownik zinterpretuje 
170     # to jako impuls na wejściu STEP i wygeneruje odpowiednie sygnały 
171     # na wyjściach silnika, które przekręcą go o krok. 
172     #
173     # Jeśli wartość ..._step wynosi 0, wówczas sygnał na pinie będzie cały czas 
174     # ustawiony na 0, sterownik nie otrzyma impulsu na wejściu STEP i nie 
175     # wygeneruje nowych sygnałów na wyjściu silnika, który wobec tego pozostanie 
176     # w niezmienionej pozycji.
177     #
178     # Wykresy napięcia jakie "widzi" wejście STEP lewego sterownika zależnie 
179     # od wartości lewy_krok:
180     # 
181     #
182     #                   |         1        |                  |
183     # 3V                |   ...............|                  |
184     #                   |  .               |                  | 
185     #  lewy_krok = 1    | .                |.                 |
186     #                   |.                 | .      0         |
187     # 0V ...............|                  |  ................|.............
188     #                   |                  |                  |
189     #                   |                  |                  |
190     #                   |                  |                  |
191     # 3V                |                  |                  |
192     #                   |                  |                  | 
193     #  lewy_krok = 0    |                  |                  |
194     #                   |         0        |        0         |
195     # 0V ...............|..................|..................|.............
196     #                   |                  |                  |
197     #                     <-- czas*0.5 -->   <-- czas*0.5 -->     
198     #
199     #                     <------------  czas  ------------->
200 
201 
202 def zestaw(lewo, prawo, powtorek = 1):
203     '''
204     Funkcja bierze polecenie i powtarza jego wykonanie podaną ilość razy: 
205     lewo - polecenie dla lewego silnika
206     prawo - polecenie dla prawego silnika
207     powtorek - oznacza ile razy nalezy ten zestaw kroków powtórzyć
208 
209     Na przykład:
210     zestaw(PRZOD, STOP, 10)   -- wykona 10 kroków do przodu lewym silnikiem,
211     zestaw(PRZOD, PRZOD, 200) -- wykona 200 kroków do przodu oboma silnikami,
212                                  a więc koła obrócą się o pełny obrót  
213     zestaw(PRZOD, TYL, 100)   -- obróci lewe koło 100 kroków do przodu, 
214                                  a prawe 100 do tyłu, a więc robot powinien 
215                                  wykonać skręt w prawo w miejscu.
216     ''' 
217     for i in xrange(1, 1 + powtorek):
218         krok(lewo, prawo)
219     
220 
221 def sekwencja(lista_zestawow):
222     '''
223     Czyta podaną listę i wykonuje kolejno podane w niej zestawy kroków.
224 
225     lista_zestawów -- lista zawierająca kolejne zestawy; w pythonie 
226                       listę podaje się w nawiasach kwadratowych [] 
227                       oddzielając elementy przecinkami 
228 
229     Na przykład:
230 
231     sekwencja([ (PRZOD, PRZOD, 400), (PRZOD, TYL, 100), (TYL, PRZOD, 100) ]) 
232 
233          -- wykona kolejno trzy zestawy: 
234             1. (PRZOD, PRZOD, 400)
235             2. (PRZOD, TYL, 100)
236             3. (TYL, PRZOD, 100)
237             Pierwszy spowoduje podjechanie modelu o dwa pełne obroty kół 
238             do przodu. Drugi obrócenie lewego koła do przodu a prawego 
239             do tyłu o pół obrotu, a wiec robot skręci w miejscu w prawo. 
240             Ostatni zestaw spowoduje skręt w miejscu w przeciwną stronę 
241             - robot powróci do pierwotnego kierunku. Cała sekwencja wyglądać 
242             będzie tak jakby robot podjechał do przodu i rozejrzał się 
243             w prawo. 
244     '''
245     for kolejny_zestaw in lista_zestawow:
246         zestaw(*kolejny_zestaw)

Przykłady użycia

Dzięki modułowi silniki sterowanie ruchem sprowadza się do prostych instrukcji.

Na początek spróbujmy zobaczyć instrukcję zestaw(lewy, prawy, powtorek) w akcji.

# coding: utf-8
import silniki as s

# Przygotowanie portów Maliny do pracy
s.przygotuj()

# Włączenie zasilania silników -- po tej instrukcji silniki powinny zablokować się w ustalonej pozycji
s.podlacz(1,1)

# 10 obrotów kół (czyli 10*200 kroków) w przód
s.zestaw(s.PRZOD, s.PRZOD, 2000) 

# Jeden obrót lewego koła w przód, prawe stoi -- robot skręca w prawo
s.zestaw(s.PRZOD, s.STOP, 200)

# Koniec programu, wyłączamy zasilanie silników -- po tej instrukcji koła znów obracają się lekko
s.podlacz(0,0)

Po zapisaniu tego kodu w pliku silniki_p1.py i wydaniu polecenia

pi@raspberrypi:~ $ python silniki_p1.py 

robot powinien przejechać 10 obrotów kół do przodu, a potem skręcić w prawo. Oczywiście nie będzie to możliwe jeśli będziemy się łączyć z Maliną poprzez kabel USB dlatego w tej chwili najlepiej zdjąć koła, albo unieść je w górę i obserwować czy wykonują oczekiwane ruchy. Poniżej pokażemy gdzie wstawić powyższą instrukcję, żeby robot wykonywał podany kod zaraz po włączeniu. Wówczas będzie można podłączyć Malinę do powerbanku i oglądać wykonanie instrukcji w ruchu.

W akcji

Przykładowy kod sterujący i zachowaniem trzykołowca

# coding: utf-8
import silniki as s

s.przygotuj()                      # Przygotowanie pinów Maliny do wysyłania sygnałów.
s.podlacz(1,1)                     # Włączamy zasilanie silników, koła trudno obracać. 

s.zestaw(s.TYL, s.PRZOD, 60)       # lewe koło to tyłu, prawe do przodu o 60 kroków -- skręt w lewo
s.zestaw(s.PRZOD, s.PRZOD, 400)    # 400 kroków w przód, bok kwadratu ok. 50cm
s.zestaw(s.TYL, s.PRZOD, 60)       # skręt i drugi bok
s.zestaw(s.PRZOD, s.PRZOD, 400)     
s.zestaw(s.TYL, s.PRZOD, 60)       # skręt i trzeci bok
s.zestaw(s.PRZOD, s.PRZOD, 400)     
s.zestaw(s.TYL, s.PRZOD, 60)       # skręt i czwarty bok
s.zestaw(s.PRZOD, s.PRZOD, 400)     

s.zestaw(s.PRZOD, s.TYL, 60)       # lewe koło w przód, prawe w tył -- skręt w prawo
s.zestaw(s.PRZOD, s.PRZOD, 400)    # robot jedzie w kierunku stopy na filmie
s.zestaw(s.PRZOD, s.TYL, 60)       # kolej skręt i bok
s.zestaw(s.PRZOD, s.PRZOD, 400)     
s.zestaw(s.PRZOD, s.TYL, 60)       # następny
s.zestaw(s.PRZOD, s.PRZOD, 400)     
s.zestaw(s.PRZOD, s.TYL, 60)       # ostatni skręt i bok
s.zestaw(s.PRZOD, s.PRZOD, 400)     

# ... i tak dalej, kolejny kwadrat w lewo

s.podlacz(0,0)           # Wyłączamy zasilanie silników, koła znów obracają się lekko