czech english

Marina

projekt Roboloď

Projekt „Roboloď” pro mne začal na vánočním večírku 2015, tj. skoro před třemi lety. Bývalý kolega z práce říkal, že jeho kamarád shání někoho, kdo by dokázal zrobotizovat lodičku zavážející krmení do sádek na rybníce. Projekt byl (a stále je) takový „tajuplný”, ale nakonec jsme se se dvěma dalšími kamarády rozhodli do toho jít. Když nic, tak ověříme, jak se nám spolupracuje a na vodě jsme s robotama ještě nebyli …

Historické pozadí

Po několika iteracích se ukázalo, že to je vlastně kamarád kamaráda, který to poptává pro svého dobrého zákazníka a ten to možná dělá ještě pro někoho dalšího. První domluva, o co přesně jde, nějaký čas trvala.
Asi první zklamání (setkání s realitou) bylo, když se ukázalo, že „zavážková lodička” je fakticky malý RC model. Do té doby jsem měl představu pramice nebo nějakého motorového člunu, který uveze člověka, který by zavážel návnadu na uživatelem zadanou GPS souřadnici. Zatímco u velké lodě bychom se s elektronikou a i nějakým vývojem do základní ceny možná nějak schovali, u plastové čínské hračky to už tehdy (kdybychom si to přiznali) nepřipadalo v úvahu.
Komerčně jsme to v tento okamžik měli vzdát, což jsme neudělali. „Vždyť pro to všechno máme a v Číně se dají součástky pořídit za pár drobných!” Jo, jo, kolikrát v životě člověk ještě podobnou chybu udělá
Na druhou stranu výhodou malé lodičky bylo, že nám ji jednoho dne mohla kamarádka kamaráda v krabici přivézt, a následně nebyl velký problém jít testovat „k lachtanovi” na Petřín nebo na Vltavu.

Technologie

PPM řízení serv

Ona to vlastně byla celkem pěkná hračka a holt jsme (obávám se, že nemůžu použít minulého času) jak děti a tak jsme ji vyzkoušeli a obratem rozebrali. Řízení lodi je diferenciální, tj. dva motory a dva vodní šrouby. Dálkově je možné ještě zapínat LEDky (hodí se v noci nebo za šera) a vysypávat náklad. Celkově čtyřkanálová vysílačka s přijímačem. Komunikace je jednosměrná, i když je možné dokoupit např. sonarový modul pro detekci ryb, a ten by pravděpodobně obsahoval i posílání dat zpět uživateli.
Připojení přijímače do řídící jednotky (časem pracovně nazývaná „čínská hlava”) je pomocí trojžilového kabelu. Zem, napájení a „nějaký” signál. Pulzy pro řízení serv již léta znám, ale jak je až 8 kanálů kombinovaných do jednoho signálu byla pro mne novinka. Jedná se o takzvané PPM kódování, co používá Futaba: jednotlivé 1.5ms dlouhé pulzy definují náběžnou hranou jednotlivé kanály a hlavní cyklus zachovává 20ms periodu (tj. 50Hz).
Před pár měsíci si Šimi pořídil digitální sondu a z ní lze vyčíst a nakreslit obrázek výše (kéž bychom toto měli na začátku).
A další plán? Napíchneme si na tento kabel „štěnici”, která bude signál odposlouchávat a podle typu navigace buď přijímaný signál přepošle dál nebo vygeneruje vlastní pro autonomní řízení. Snadné, není-liž pravda?

Zavařené čínské hlavy

U čínských výrobků jde hlavně o cenu, takže je očekávané (teď už to vím), že součástky nebudou nijak předimenzované, spíše naopak. Pro řízení motoru nebyl použit žádný H-můstek, jak je známe z našich robotů, ale kombinace relátek a tranzistorů, které na některé signální sekvence reagovaly vyhořením :-(. Trošku nás uklidnila (možná nás měla naopak více znervóznit!) zpráva od dodavatele, že se to občas děje i při normálním používání (?!), ale stavte nad tím autonomii!
Nakonec jsme používali pouze tři příkazy vlevo, rovně a vpravo a dávali si pozor ať z příkazu vlevo nepřecházíme do příkazu vpravo. Stejně tak jsme eliminovali couvání a tedy změnu směru otáčení motorů.

Senzory pro navigaci

Je až neuvěřitelné, jak za ta léta klesla cena GPS a IMU jednotek. Tento trend je jistě spojen s integrací senzorů do telefonů, ale možná i nejrůznějších hraček. Šimi poměrně dost brblal, když jsem chtěl nahradit 2D kompas z první dávky 3D kompasem s gyrem … přeci jenom to zdvojnásobilo cenu součástek (!). Poučení? U prvních prototypů neřešte cenu součástek, stejně nakonec budou v produkci jiné!

Komunikace

Automonmí řízení mělo být realizováno pomocí WiFi připojení. Šimi si již delší dobu hrál s oblíbeným ESP8266, zase v ceně pár dolarů v jednokusech a stačilo „jen to zapojit”.

Arduino

Pro odposlech přijímaného signálu a generování nového jsme chtěli použít Arduino. Zase je to o ceně - to ani obyčejnou destičku vám nevyrobí za cenu již připravené a osazené varianty s Arduinem. Arduino jsem již jednou programoval (viz FireAnt), ale stejně mne to párkrát dostalo (detaily viz dále).

Android aplikace a celkové schema

Aby byl obrázek kompletní, je třeba ještě zmínit hlavní řídící aplikaci, která běžela na Android telefonu (telefon nebyl součástí dodávky). Uživatel se k lodičce připojil pomocí WiFi, realizované ESP8266 čipem. Ten se dále postaral o sběr a přeposílání dat z GPS přes seriový port a IMU a Arduino přes I2C.

Komunikační protokol

Vše to začalo přeposíláním dat ze seriáku přes WiFi (je na to možná i nějaký vzorový příklad, ze kterého Šimi vycházel?). A jelikož na seriáku byla GPS, která mluvila NMEA protokolem, tak i výsledný komunikační protokol tomu zůstal hodně podobný, tj. textový, '$' na začátku a '*' s kontrolním součtem na konci.

TCP vs. UDP

Co je lepší používat na řízení lodičky, TCP nebo UDP protokol? Na první pohled by se zdálo, že samozřejmě TCP, tam je přece zaručený přenos, neztrácí se pakety a o vše se stará systém. Myslím, že i důvod pro první použití TCP byl právě příklad sériová linka-WiFi, který TCP používal (přeci jenom nechcete, aby se vám ztrácely bajty).
A jak to fungovalo? No zvláštně. S ultrastarým notebookem to šlo celkem dobře, ale s mým (teď už asi jenom 10 let starým) notebookem nikoliv. Detaily si teď už moc nepamatuji, ale pokud jsem po každém přijatém paketu něco poslal, tak to začalo fungovat lépe. Důvod? Ukázalo se, že TCP na straně ESP8266 úplně dobře nepodporuje okénko pro potvrzení přijetí, nebo to bylo špatně nakonfigurované, a novější OS jako default počítal s potvrzením až po několika zprávách. ESP8266 však udrželo jenom jeden TCP paket.
UDP s upraveným protokolem byla lepší volba. Co dělat při ztrátě paketu? Poslat ho znova nebo raději již poslat další s novějšími daty? Je to podobné jako u streamování videa - pokud něco vypadne, tak je to hloupé, ale pokud toho není moc, tak to skoro nepoznáte. Do UDP paketů tedy přibyl čítač (index paketu), aby bylo možné výpadky snadno detekovat. Přibyla i časová známka pro měření latence. Konečně lodička posílala i poslední akceptovaný příkaz, podle kterého momentálně naviguje.

Navigační smyčka

Klasicky verze 0 byla nejtriviálnější varianta, která by mohla fungovat:
  • zjisti GPS pozici
  • zjisti úhel z kompasu
  • spočítej úhel k cílovému bodu
  • je-li absolutní rozdíl úhlu (vezme-li se v úvahu 360 stupňů perioda) větší než daný limit, proveď korekci vlevo nebo vpravo, jinak jeď rovně
  • je-li vzdálenost menší než 2m, tak na chvíli zastav a pak naviguj na další bod
Prostě takový „vodní” RoboOrienteering . A kupodivu to nebylo až tak zlé! Resp. začaly vylézat úplně jiné problémy, než, které jsme rešili na začátku. Např. se ukázalo, že nutný požadavek je, aby se lodička autonomně navigovala do cílů vzdálených až 300m. Jako on tam občas nějaký WiFi packet doletí, ale na nějakou spolehlivou navigaci zapomeňte.
S tím byl spojen i problém ultra dlouhé zpětné vazby (navíc s vypadáváním paketů). Pošlete-li příkaz: „zatáčej doprava” a následných 10 příkazových paketů se ztratí, co myslíte, že se stane? HW byl poddimenzovaný, tak jsme to dál flikovali rozšířením příkazů o dobu jejich trvání (de-facto počet 20ms period) Po jejím vypršení se přešlo na defaultní příkaz, který byl „jeď rovně”.
Dobré, resp. lepší než předtím, ale … s novým přesnějším nástrojem se ukázalo, že „čínská hlava” na korekce občas vůbec nereaguje!? Pokud zatočíte volantem doprava a jen někdy na to stroj zareaguje opravdovým zatočením, tak to je skoro … těžké. Bylo by třeba posílat daný přikaz a rovnou nějak ověřovat, že je realizovaný. Asi vás nemusím 2x přesvědčovat, že to už je pak jednodušší si tam dát vlastní H-můstek a ty motory si prostě řídit sám.
Druhá limitace HW, tentokrát našeho, byl výkon. Pokud chcete nějak rozumně zpracovávat gyra, kompas a nedej bože akcelerometry, tak potřebujete vyšší vzorkovací frekvenci, ať můžete průměrovat/filtrovat. My se horkotěžko dostali na 8Hz.

Gyro vs. kompas

Jak jsem psal výše, verze 0 používala pouze kompas pro určení aktuálního směru lodi. Co je ale dobré mít na pozoru, že kompas je typicky hodně filtrovaný a má tedy relativně pomalou odezvu. Pokud se lodička točí jak blázen, tak získané měření z kompasu moc relevantní nebude … to jsem chytrý, co? … to víte, generál po boji.
Ve zkratce to byl jeden ze zdrojů kmitání … prostě lodička si myslela, že kouká špatným směrem, ale fakticky už měla jet rovně. Pro rychlé korekce úhlu je rozhodně lepší použít čistě integraci úhlové rychlosti z gyra.

Android aplikace

Android verzi uživatelské aplikace nám naprogramoval PetrS. Ano, klasicky se ukázalo, že i ta nejtriviálnější verze není jen tak a hned. A pak „drobnosti” typu: „vy určitě chcete být na internetu, ale ten přes lodičku nějak nefunguje, tak více to, já vás raději přepojím na jinou wifinu ...”
Výsledné řešení se mi svoji jednoduchostí libilo . Jelikož manualní řízení přes RC vysílačku stále fungovalo (v libovolný okamžik tak uživatel může převzít řízení automatu), tak na jednotlivé cílove body člověk najel ručně. V Android aplikaci pak pouze zmáčnul plus jako přidej bod. První a automaticky i poslední bod byl HOME, kam se lodička po skončení úkolu vrací.
Autonomní jízda byla pak podobně jako u přehrávání muziky: tlačítko Play, Pause, Stop.
Drobnou vadou na kráse byla nutnost kalibrace kompasu, ale tato akce byla schovaná v nastavení společně s před-výběrem WiFi spotu.

Závěr a „Matrix reloaded”?

Jednou z mých (skoro tajných) motivací v tomto projektu bylo vytvoření „černé krabičky”, kterou dáte do libovolné RC hračky a získáte tím autonomního robota se znalosti GPS pozice a WiFi komunikací. Ono se ale ukazuje, že zmíněný Futaba PPM protokol zas tak běžný v levných RC hračkách není, což je asi ale teprve první problém.
Teď bych šel Šimiho cestou, který si to celé vedle postavil znova, jen místo Arduina použil Raspberry Pi Zero W a to řízení si naprogramoval v Pythonu. Vše to jelo rovnou na lodi, tj. žádné problémy s výpadky paketů. Ano, je to o trošku dražší, ale jestli jsme to definitivně nevzdali (já myslím, že jo), tak toto je cesta (možná).

Appendix — Arduino digitalWrite()

Asi první zrada s Arduino programováním bylo debugování pomocí LEDky. Co jsem si z programování AVRka pamatoval, že dané makra, jako např. sei(), se přímo překládají na 1:1 asemblerovské instrukce. A zápis na port by měl proběhnout v jednom taktu. Toto ale rozhodně neplatí pro funkci digitalWrite() a volat tuto funkci v interruptu rozhodně není dobrý nápad.
Co se vám v podobném případě může hodit je znalost, jak generovat assembler výstup? Odpověď naleznete na Arduino fóru:
m:\arduino-nightly\hardware\tools\avr\bin\avr-objdump.exe 
Files > Preferences > verbose compilation
Zde je pro představu dekompilace celé funkce digitalWrite() (no vlastně není celá, protože v některých případech volá ještě další pod-funkce):
void digitalWrite(uint8_t pin, uint8_t val)
{
     99e:	0f 93       	push	r16
     9a0:	1f 93       	push	r17
     9a2:	cf 93       	push	r28
     9a4:	df 93       	push	r29
     9a6:	1f 92       	push	r1
     9a8:	cd b7       	in	r28, 0x3d	; 61
     9aa:	de b7       	in	r29, 0x3e	; 62
	uint8_t timer = digitalPinToTimer(pin);
     9ac:	28 2f       	mov	r18, r24
     9ae:	30 e0       	ldi	r19, 0x00	; 0
     9b0:	f9 01       	movw	r30, r18
     9b2:	e8 59       	subi	r30, 0x98	; 152
     9b4:	ff 4f       	sbci	r31, 0xFF	; 255
     9b6:	84 91       	lpm	r24, Z
	uint8_t bit = digitalPinToBitMask(pin);
     9b8:	f9 01       	movw	r30, r18
     9ba:	e4 58       	subi	r30, 0x84	; 132
     9bc:	ff 4f       	sbci	r31, 0xFF	; 255
     9be:	14 91       	lpm	r17, Z
	uint8_t port = digitalPinToPort(pin);
     9c0:	f9 01       	movw	r30, r18
     9c2:	e0 57       	subi	r30, 0x70	; 112
     9c4:	ff 4f       	sbci	r31, 0xFF	; 255
     9c6:	04 91       	lpm	r16, Z
	volatile uint8_t *out;

	if (port == NOT_A_PIN) return;
     9c8:	00 23       	and	r16, r16
     9ca:	c9 f0       	breq	.+50     	; 0x9fe 

	// If the pin that support PWM output, we need to turn it off
	// before doing a digital write.
	if (timer != NOT_ON_TIMER) turnOffPWM(timer);
     9cc:	88 23       	and	r24, r24
     9ce:	21 f0       	breq	.+8      	; 0x9d8 
     9d0:	69 83       	std	Y+1, r22	; 0x01
     9d2:	0e 94 6d 04 	call	0x8da	; 0x8da 
     9d6:	69 81       	ldd	r22, Y+1	; 0x01

	out = portOutputRegister(port);
     9d8:	e0 2f       	mov	r30, r16
     9da:	f0 e0       	ldi	r31, 0x00	; 0
     9dc:	ee 0f       	add	r30, r30
     9de:	ff 1f       	adc	r31, r31
     9e0:	ec 55       	subi	r30, 0x5C	; 92
     9e2:	ff 4f       	sbci	r31, 0xFF	; 255
     9e4:	a5 91       	lpm	r26, Z+
     9e6:	b4 91       	lpm	r27, Z

	uint8_t oldSREG = SREG;
     9e8:	9f b7       	in	r25, 0x3f	; 63
	cli();
     9ea:	f8 94       	cli

	if (val == LOW) {
		*out &= ~bit;
     9ec:	8c 91       	ld	r24, X
	out = portOutputRegister(port);

	uint8_t oldSREG = SREG;
	cli();

	if (val == LOW) {
     9ee:	61 11       	cpse	r22, r1
     9f0:	03 c0       	rjmp	.+6      	; 0x9f8 
		*out &= ~bit;
     9f2:	10 95       	com	r17
     9f4:	81 23       	and	r24, r17
     9f6:	01 c0       	rjmp	.+2      	; 0x9fa 
	} else {
		*out |= bit;
     9f8:	81 2b       	or	r24, r17
     9fa:	8c 93       	st	X, r24
	}

	SREG = oldSREG;
     9fc:	9f bf       	out	0x3f, r25	; 63
}
     9fe:	0f 90       	pop	r0
     a00:	df 91       	pop	r29
     a02:	cf 91       	pop	r28
     a04:	1f 91       	pop	r17
     a06:	0f 91       	pop	r16
     a08:	08 95       	ret

00000a0a <_ZN6StringD1Ev>:
	*this = dtostrf(value, (decimalPlaces + 2), decimalPlaces, buf);
}
Pokud ale potřebujete rychlou verzi (tak, jak jsem to naivně předpokládal), tak je třeba použít přímo bitovou konstrukci:
ARDUINO_LED_PORT |= ARDUINO_LED_BIT; // faster digitalWrite(ARDUINO_LED, HIGH);
ARDUINO_LED_PORT &= ~ARDUINO_LED_BIT; // faster digitalWrite(ARDUINO_LED, LOW);