Realizujte pomocí desky KL25Z a jejích přídavných desek stabilizaci a ovládaní vozítka na dvou kolech. Pohyb bude ovládán vzdáleně pomocí RF modulu CC1101. Optimalizujte parametry PID tak, aby vozítko minimálně kmitalo.

Cílem projektu je vytvoření softwaru a hardwaru pro balančního robota. Zařízení bude využívat vyvojové desky pro získání stabilizačních parametrů, komunikaci a řízení motorů. Pro bezdrátovou komunikaci využijeme rádio CC1101 pracující v ISM pásmu, konkrétně 433MHz. Pro řízení DC motorů využijeme komerčně dostupné H-můstky. Vysílací část bude vytvořena z vyvojové desky Arduino Nano, CC1101, tlačítek a baterií. Čtyři tlačítka pro ovládání - dopředu, dozadu, doleva, doprava. Tato data z vysílače se přijmou na vozítku a provede se příslušná obsluha. Rychlost otáčení motorů bude řízena PWM signálem z vývojové desky na vozítku. Všechna logika bude napájena z bateríí. Pokud nenastanou problémy při vývoji a následném časovém skluzu, je v plánu vytisknout na 3D tiskárně celý model vozítka.

FRDM-KL25Z

• MKL25Z128VLK4 MCU – 48 MHz, 128 KB flash, 16 KB SRAM, USB OTG (FS), 80LQFP
• Capacitive touch „slider“, MMA8451Q accelerometer, tri-color LED
• Arm® Cortex®-M0+ processor

FRDM-FXS-MULT2-B

• The FRDM-FXS-MULT2-B is a sensor expansion board enabling NXP® Sensor Fusion using the MMA8652FC 3-axis accelerometer, FXLS8471Q 3-axis accelerometer, MAG3110 3-axis magnetometer, FXAS21002C 3-axis gyroscope, MPL3115A2 pressure sensor, FXOS8700CQ 3-axis accelerometer plus a 3 axis magnetometer and the MMA9553L sensing platform.

Arduino Nano

• Microcontroller ATmega328
• External Interrupts: 2 and 3
• PWM: 3, 5, 6, 9, 10, and 11. Provide 8-bit PWM output with the analogWrite() function
• SPI: 10 (SS), 11 (MOSI), 12 (MISO), 13 (SCK)

1x H-můstek modul L9110S 56 Kč (https://www.gme.cz/h-mustek-modul-l9110s)

2x CC1101 ~3€/ks (Alliexpess)

2x Arduino Nano ~4€/ks (Ebay)

2x DC motory 100 Kč (https://www.gme.cz/elektromotorek-12v-mot3n)

1x Univerzální pájivé pole 75 Kč (https://www.gme.cz/univerzalni-spoj-100x50-rm-2-54-pasove-spoje-tri-body)

Plán:

1. Vývoj a výroba vysílače
2. Vývoj a výroba přijímače
3. Tvorba prototypu vozítka
4. MCU
5. Testování

Nejprve byl vytvořen vysílač s RF modulem CC1101 řízený z desky Arduino Nano. Tento modul komunikuje pomocí protokolu SPI a podle specifikace pracuje v rozmezí napětí 2.6V až 3.6V. Arduino Nano obsahuje přednastavené piny pro SPI komunikaci, ale jelikož je veškerá logika na 5V jedná se o problém s kompatibilitou. Relativně jednoduchým řešením by bylo přidání napěťových děličů mezi komponentu a periferii, popřípadě výměnou stabilizátoru na desce za jiný a tím řídit celé Arduino na 3.3V.

Abychom vyslali jakákoli data musíme RF modul nejprve nakonfigurovat – nastavit příslušné registry. Naštěstí existuje mnoho knihoven na internetu, které nám umožňují nastavení registrů za nás. Byla využita knihovna (https://github.com/veonik/arduino-cc1101), která provádí nejen nastavení registrů, ale i veškerou SPI komunikaci potřebnou pro přenos dat.

Spojení periferie CC1101 →Arduino Nano:

• MISO→ D12
• MOSI→ D11
• SCK→ D13
• CSN→ D10
• GDO0→D2
• VCC→3v3
• GND→GND

Modul byl připojen na piny Arduina a byl spuštěn kód. Kupodivu rozdíl SPI napětí modulu nevadí a funguje, takže zajišťování kompatibility nebylo potřeba. Napájení z baterií je připojeno na Vin pin. Kód pro vysílač i příjmač je přiložen na konci projektu. Celý vysílač je poté zkompletován do standalone prototypu na univerzálním pájivém poli.

Vysílač obsahuje:

• 4 tlačítka pro ovládání
• 1 tlačítko pro připojení baterií
• CC1101
• Arduino Nano
• 3x AAA baterie

Příjmač vycházel ze stejného základu jako vysílač. Jelikož byl projekt tvořen ve vývojovém prostředí Codewarrior v jazyce C, a knihovny pro nastavení a vysílání modulu CC1101 v jazyce C++, nastal konflikt. Byla snaha přepsat knihovnu do jazyka C, aby ji bylo možno jednoduše naimportovat, ale zabralo to příliš mnoho času s nefunkčním výsledkem. Ve snaze rozjet prvotní prototyp byl vytvořen příjmač z Arduina + CC1101. Ten fungoval bez problémů s dosahem více jak 100m. Tyto 2 moduly byly připevněny na konstrukci. Výstupy z Arduina byly připojeny na piny desky KL25Z (pro řízení směru).

Nejprve byl vytvořen prototyp vozítka z jednduchých materiálů (dřevo + lepidlo). Byly připevněny motory, 9V baterie, H-můstek a deska KL25Z s přidaným shieldem pro získávaní stabilizačních parametrů. Plán byl vytvořit celý model pomocí 3D tisku, ale na to nezbyl čas, kvůli událostem které nastaly při tvorbě projektu.

Projekt je vytvořen ve vývojovém prostředí Codewarrior v jazyce C s využitím Processor expert. Celý kód je napsán v úloze „UserHighFrequencyTaskRun“, který běží s frekvencí 200Hz. V projektu jsme využili sensorovou přídavnou desku FRDM-FXS-MULT2B komunikující po sběrnici I2C.

Jako stabilizační parametr jsme zvolili eulerův úhel natočení s kalmanovým filtrem: Euler Angles [deg], 9DOF accelerometer, magnetometer and gyro (Kalman) structure.

Funkce slouží k nastavení PWM výstupu dle stavu parametru Sum, který je součtem proporcionální a integrační složky. Kód obsahuje ochranu proti nekonečnému růstu integrátoru a také limitní hodnoty proměnné Sum. Velikost PWM nastavujeme 16bitovou celočíselnou hodnotou. Periodu PWM jsme nastavili na 5ms. Můžeme vidět zakomentované příkazy PWM1_ConnectPin, které slouží pro přepínání pinů na desce KL25 a tím způsobené změny směrů. Nejsou zatím použité, protože už ve fázi testování nastal problém o kterém bude více napsáno v závěru.

void UserHighFrequencyTaskRun(void)
{
	// The default frequency at which this code runs is 200Hz.
	// This code runs after sensors are sampled.
	// In general, try to keep "high intensity" code out of UserHighFrequencyTaskRun.
	// The high frequency task also has highest priority.
	//
	// PUT YOUR CODE HERE
	
	int8 direction;
	lay = thisSV_9DOF_GBY_KALMAN.faSePl[Y];
	//9DOF accelerometer, magnetometer and gyro (Kalman) structure
	temp=abs((int)(lay*100));
	if(lay>0){
		direction=1;
	}else{
		direction=-1;
	}
	P=-temp*Pk;
	
	I=I+(int)lay*-100*Ik;
	if(I>100){
		I=100;
	}
	if(I<-100){
		I=-100;
	}
	Sum=(P+I);
	if(Sum>=100){
		Sum=100;
	}
	if(Sum<=-100){
		Sum=-100;
	}
	uint16 Set =  abs(65535-abs(Sum)*655);
	PWM1_SetRatio16(PointerData1,Set);
	//0-65535 655*100=65535

	if(direction==-1){
		//PWM1_ConnectPin(Pointer, Maska na piny);
		//
		Bit1_SetVal();
		Bit2_ClrVal();
		//reverse
	}else{
		//PWM1_ConnectPin(Pointer, Maska na piny);
		//
		Bit2_SetVal();
		Bit1_ClrVal();
	}
	return;
}

Seznam důležitých komponentů v Processor expertu:

• LED - pro debugovací účely
• UART - pro komunikaci mezi PC a deskou
• Piny UP, DOWN, LEFT, RIGHT - vstupy z Arduina
• Bit1 a Bit2 - směr H-můstku

H-můstek

Vybrali jsme H-můstek s rozsahem napětí 2,5 - 12 VDC (max 800mA). Tento H-můstek byl řízen PWM signálem. Pro prvotní testy jsme použili pouze jeden PWM signál, který řídil oba motory.

DC motory

Byly vybrány motory s rozsahem napětí 6 až 14V DC, které mají 8768ot./min při zátěži. Hrubým odhadem velikosti kol bylo vypočítáno, že je potřeba pouze přibližne 5ot./sec. Tento typ motorů nemá velký kroutící moment a mohli bychom jej zvětšit převodní jednotkou s poměrem 1:30.

PID

Pro PID regulátor jsme zvolili jako cílovou regulační hodnotu eulerův úhel náklonu, který jsme se snažili stabilizovat na hodnotě 0°. V kódu jsme využili integrační a proporcionální složky.

Schéma zapojení:

Komunikace mezi deskou a shieldem: I2C

Komunikace mezi arduinem a CC1101: SPI

Komunikace mezi Arduiem a deskou: jednoduchý přenos bitů přes piny.

PWM1 a PWM2 jsou 2 PWM signály, které ovládají pohyb každého kola separátně (pokud bychom chtěli zatáčet). Arduino Nano je napájeno z vyvojové desky KL25Z (5V pin → Vin Pin).

I když byl H-můstek napájen z 9V baterie (která je ve specifikaci), nastal problém a přestal po chvilce fungovat. Pravděpodobně se spálil i když nic nezapáchalo či bylo horké. Naštěstí byly k dispozici tyto H-můstky 2. Tento můstek byl připojen na nižší napětí a řízení fungovalo (všechny směry a oba motory). Poté byly připojeny všechny desky mezi sebou, pouze KL25Z byla napájena z USB. Motory se spínaly v závislosti na natočení desky (bohužel nebylo ještě pořízno video) - větší úhel než 0° zapnut jeden směr, menší úhel než 0° zapnut druhý směr (zatím není implementovaná PWM). Napájení bylo ponecháno a byly odpojeny řídící piny můstku. Bylo provedeno nahrátí nového kódu do desky. Po připojení pinů můstku do vývojové desky přestal můstek fungovat. Pouze LED diody na KL25Z změnily jas a LED dioda na H můstku zhasla. Poslední můstek byl zničen.

Poté jsme přikročili k další části práce - vlastní napájecí zdroj pro vozítko. Kde nastal další problém, který zabral spoustu času. Po připojení 9V baterie na pin Vin deska nijak nereagovala. Na videu lze vidět průběh napájecího napětí při připojené desce a při odpojené. Když byla připojená, napětí pokleslo z 9V na 8.72V a zobrazil se průběh jaký lze vidět na osciloskopu ve videu. Bohužel tento problém způsobil takové zpoždění, že projekt zůstal ve fázi testování.

Testování:

V projektu jsme získávali stabilizační parametry (z accelerometru, magnetometru a gyroskopu) z přídavné desky pomocí komunikace I2C. Mezi deskami Arduina a CC1101 se používala komunikace SPI. Projekt byl tvořen ve vývojovém prostředí Codewarrior, kde byl využit nástroj Processor Expert. Demonstrace funkčnosti úhlu natočení na šířce pulzu PWM lze pozorovat na přiloženém videu. Šířka pulzu byla řízena PI regulátorem, jehož vstupem byly stabilizační parametry.

Při tvorbě projektu nastaly chyby, které znemožnily vytvoření finální verze. První problém nastal s H-můstkem, který se pravděpodobně spálil a druhý problém nastal s napájením desky KL25Z. I když jsme připojili 9V baterii na desku, která podporuje maximálně 9V, deska nefungovala. Fungovala, pouze pokud bylo přivedeno napájení z USB. Byla snaha vytvořit vlastní H-můstek nejprve z diskrétnich tranzistorů BC337 a BC336, poté i s pomocí IGBT tranzistorů, ale nedosáhlo se funkčních vysledků.

Plán byl vytvořit celý model pomocí 3D tisku, ale na to nezbyl čas, kvůli událostem které nastaly při tvorbě projektu.

Zobrazení vlivu natočení úhlu na velikosti PWM (levé video). Čím větší chyba úhlu, tím větší šířka pulzu. Zobrazení napájecího napětí na pinu Vin (pravé video).

Kód pro vysílač: https://github.com/DanielHubik/MPOA/blob/master/arduino-cc1101-master/MPOA/Tx_strana/Tx_strana.ino/Tx_strana.ino.ino

Kód pro příjmač: https://github.com/DanielHubik/MPOA/blob/master/arduino-cc1101-master/MPOA/Rx_strana/Rx_strana.ino

Kód pro MCU (Codewarrior): https://github.com/DanielHubik/MPOA