Enkla självbalanserande Robot med Galileo Gen 2 (3 / 6 steg)

Steg 3: Robotprogram

Jag väljer att genomföra robot skissen i en enda fil, vilket gör det lite lång, men sätter allt på ett ställe. Snarare än att gå genom den SW rad-för-rad, kommer jag bara kommentera hur den är organiserad och vad varje avsnitt är inriktad på.

Skissen börjar med rubriken standard kommentar och bibliotek innehålla filer. Jag dokument sköld pin användning i huvudet kommentaren för att hålla mig själv rakt på SW användning av stiften. Jag också definieras en datastruktur som används av funktionen PID, med tanken detta skulle göra det enklare att lägga till en andra överlappande hastigheten PID senare.

Nästa är #define uttalanden för konstanter och vanligen används värden som bekvämt kan ändras. Till exempel när du avgöra vad den optimala P, jag och D värden är för din robot, kan de permanent kodas här. Nästa kommer de varierande definitioner, som jag försökt att gruppera på ett logiskt sätt - du kan vara domaren av hur bra jag gjorde.

Nästa avsnitt innehåller funktioner som används senare i skissen. Den första funktionen implementerar balanserande PID. Detta är SW jag belånade tungt från Brett Beauregard tutorial. Ingår i funktionen PID är en sektion för att spela in PID information till SD-kort om data loggning är aktiverat. Medan tuning PID värden, hittade jag den instruktions- och underhållande, att se den enskilda PID sikt beteendet. Detta sparar data till en fil i CSV-format, som jag senare importeras till Excel och genererade grafer för visualisering. Denna alla uppenbarligen fungerar endast om du har ett SD-kort i Galileo styrelsen.

Detta följs av några motorisk kontrollfunktioner som kartan motorstyrning information till riktning, hastighet och broms kontroller för motor shield

Nästa är en funktion för att hämta positionsdata från modulen MPU-6050. Jag vann detta med stolthet från Jeff Rowberg referenskod. Jag använder en vanlig-intervall-data-drag-metoden för dataöverföring, snarare än med avbrott som Jeff gjorde, som jag hade vissa svårigheter att få avbrotten att arbeta. Tidsintervallet implementeras i huvudprogrammet slingan, och jag fann att en 3ms intervall var tillräcklig för att säkerställa det fanns inga data svämmar över.

Vi sedan gå vidare till en uppsättning funktioner att hantera Bluetooth-kommunikation. Dessa börjar med några funktioner för att skicka mer komplicerade kommandon - dessa bröts funktioner främst för koden läsbarhet. Sedan kommer det primära Bluetooth kommandot parser, som implementerar protokollet ta emot kommunikation med Android app. Denna funktion ackumuleras tecken från den seriella Bluetooth-porten tills slutet av kommandot tecken upptäcks, och sedan det avkodar kommandot. Som väntat, slutar avkoda att bara vara en stor case-sats.

Slutliga funktion är hög nivå robot kontroll tillståndsdatorn. Jag genomfört det i stil med en finite-state-maskin (FSM) - förmodligen overkill, men jag tänkt utöka detta funktioner läggs till roboten.

Nästa är setup-funktionen krävs av alla Arduino skisser. Kommentarerna i koden bör göra detta avsnitt ganska självklar. Det finns ett avsnitt som försök att initiera SD-filen för inloggning, och bör fel ut om ett SD-kort inte är närvarande, men jag inte spendera mycket tid verifiera detta.

Slutligen finns det huvudsakliga skiss slingan. Det första avsnittet implementerar en timer för att hämta data från MPU-6050. Om data hämtas, finns det några beräkningar att upptäcka om roboten är upprätt, eller ur balans.

Nästa den seriella porten är ansluten till Bluetooth-modulen är markerad för en mottagna tecken, och om man väntar, Bluetooth-funktionen kallas.

Detta följs av PID och motorisk kontroll timer. Uttrycket "om" säkerställer att kontrollen PID/motor bara händer när roboten aktivt försöker balansera. Det är där MPU_6050 data matas in PID, och PID utdata skickas till det motoriska kontrollerar. Detta är också där styrningen genomförs, genom att lägga till Bluetooth härrör riktning indata till en motorisk, och subtrahera det från den andra. Eftersom vi gör ytterligare matematik på PID-utgång, har vi igen begränsningen PID Max/Min begränsar för att hålla inom motor PWM värden.

Slutligen tillståndsdatorn robot kontroll kallas, och en timer används för att implementera funktionen robot hjärtat slå. Jag började med en separat hjärtat slå Bluetooth kommando och batteriets spänning kommando, men sedan samman dem till en enda funktion, med spänning som skickas som en indikation på heartbeat.

Och det är det - lite lång omständlig, men ingenting alltför komplex.