In het begin van Arduino includeer ik de bibliotheken die ik zal gebruiken, hierna declareer ik de nodige
variabelen. Dan begint de verbinding van de ESP8266 met het WiFi netwerk, hier achter stel ik de bibliotheek “Wire”
in.
In de eerste loop selecteer ik al mijn sensoren en stuur ik er naar welke registers we gaan gebruiken. Bij de 2de
lus zal ik 2000 keer de waarden van de sensoren halen om ze dan te kunnen kalibreren zodat enige offset er later kan
worden uitgehaald. In de 3de lus deel ik de waarden van alle sensoren door 2000 om de gemiddelde afwijking te
bekrijgen, dit steek ik dan in een aparte variabelen.
In de main program loop staat de code om van elke sensor de waarde te halen, hierna dan te verminderen door de
gemiddelde kalibratie. Hierna filter ik de uitkomst om de gyro drift te (proberen) verwijderen. Om de waarde te
krijgen van de yaw hoek moet de versnelling van deze hoek bij zichzelf optellen en delen door een vaste zelfgekozen
constante, dan wordt dit toegevoegd in een string die de waarden maal 1000 doet zodat er geen kommagetallen zijn met
tussen letters om te weten wanneer de nieuwe hoek begint
bv. 0F294F-234F3373S1F3644F43F3389S2F534F9474F9504S.
Wanneer dit 3 keer is gedaan (voor de 3 sensoren) wordt dit omgevormd naar een array van karakters. Dan wordt het
pakket opgesteld naar het vooraf ingestelde IP-adres met de vooraf ingestelde UDP-poort. Hierna wordt de array van
karakters doorgestuurd. Daarna is er een while loop die werkt als een delay, zodat de waarden niet te snel achter
elkaar worden gestuurd. De timer wordt terug op 0 gezet en de string wordt leeg gemaakt, en we starten opnieuw in de
loop om de waarden van de sensoren te lezen.
In de functie Get_Value (Waarde) selecteer de ik sensor volgens “Waarde” en verbind ik eraan. Hierna stuur ik de
registers die wij zullen gebruiken en aanpassen, de waarden worden dan gehaald van de sensor. De variabelen worden
dan aangepast volgens deze waarden, waardoor de pitch en roll berekent kunnen worden.
In de functie MPU_Callibratie wordt de functie Get_Value aangeroepen met de waarde 0, 1 en 2 (voor de 3 sensoren)
waarna de kalibratie wordt aangepast door de waarden.
In de functie MuxSelect (Waarde) kijk ik eerst of de gekozen “Waarde” niet groter is dan 7 (omdat er maar 8 I²C
poorten zijn op de multiplexer dus 0,1, 2, …,7) als dit niet groter dan 7 is dan start ik de transmissie met de
multiplexer waarna ik dit adres zal doorsturen.
Bij de functie Kalman_1d haal ik de waarden van de gyroscoop en van de accelerometer, met deze waarden wordt er
gebruik gemaakt van een algoritme die deze waarden combineert met elkaar, en ook met de waarden van de vorige keer.
In Unity includeer ik ook eerst de bibliotheken die ik zal gebruiken, hierna worden alle variabelen die zullen
worden gebruikt gedeclareerd. Ik zet dan de UDP-poort op dezelfde als die van de ESP8266.
Hierna wordt geprobeerd een nieuwe UDP-poort te maken met het gegeven poortnummer, hier wordt ook het eindpunt
gecreëerd van het bericht. De waarden worden vervolgens uitgelezen vanuit de seriële bus en worden in een byte array
geplaatst. De array wordt omgevormd naar string via ASCII bv. 0F294F-234F3373S1F3644F43F3389S2F534F9474F9504S.
Hierna wordt de string gesplitst door het de letter “S” (op einde van elke sensor string wordt er het karakter “S”
geplaatst). Deze array wordt terug gesplitst door het karakter “F” (op het einde van elke waarde wordt het karakter
“F” geplaatst in de string). Hiermee worden de Roll, Pitch en Yaw hoeken omgevormd naar floats, dit wordt dan in een
3D vector map geplaats en we delen dit terug door 1000. Wanneer dit is gebeurd, laat ik het 3D object bewegen door
de waarden. De laatste twee functies zijn voor het sluiten van Unity dat de poorten correct worden gesloten
(tenminste als ze aan staan) bij OnDisableUnity wilt zeggen op stop klikken en OnDestroyUnity is als je dit
geforceerd sluit.
