Borstlösa Gimbal balansera Robot (5 / 8 steg)
Steg 5: Arduino kod
-Koden körs på en mega328p och sammanställdes med hjälp av Arduino 1.6.6
-Koden är cykeltiden en konstant ~ 250Hz
I hjärtat av koden är en räknare som är avancerad varje gång TIMER1 svämmar över och utlöser en avbrott rutin. Den här räknaren utlöser andra evenemang med jämna mellanrum så att koden för att köra i en känd takt och förenkla math i den kontrollerar kretsar.
Tiltvinkel av roboten beräknas med ett kompletterande filter med gyrometer och acceleration värden från IC-MPU6050. Detta matas in i en kontrollerar kretsar med tre andra värden: tid derivatan av tilt vinkel, robot velocity (acceleration integrerad) och robot deplacement (hastighet integrerad). Alla fyra värden läggs med förinställda vinst att få en utgång term proportionell mot robot acceleration. En bra förklaring av detta kontrollerar kretsar av David P. Anderson och mer information om hans balanserande robot.
Om du vill flytta roboten är en förskjutning till förskjutningsvärdet i den kontrollerar kretsar. Om du vill aktivera är vänster eller höger en offset lagt/subtraheras från höger och vänster hjul hastigheter. Hjulet hastigheter matas in den motorstyrning del av koden.
Borstlösa motorer kräver 3-fas strömförsörjning. Titta på denna förklaring av hur vinkel kontroll av borstlösa gimbal motorer uppnås. Motorisk kontroll portion av min kod är baserade bort av den BruGi borstlös gimbal kontrollkod. Alla tre timer inställd på 31.25KHZ fas rätt PWM-utgång (6 PWM outs). En användbar artikel om hur du justerar Arduino timers.
En uppslagstabell som innehåller utrymme vektor PWM värden används för att dra tre värden med 120 graders fasseparation. De värden som hämtas från uppslagstabellen är avancerade med en hastighet som är proportionell mot behövs vänster eller höger robot hjulet hastighet.
Koden behandlar motoreffekt justering att korrigera för vridmoment falloff vid högre hastigheter och större behövs vridmoment under högre acceleration. Batterispänningen övervakas för att förhindra skador på LiPo batterierna. Koden behandlar också Bluetooth-kommunikation mellan en smartphone.