Själv balansera upp och ned pendel (4 / 9 steg)

Steg 4: MPU6050 3 axel Gyro och Accelerometer

Vad vi behöver är en signal som är proportionell mot vinkeln på tilt - som Y-axeln i programvaran och denna Gyro när det sitter platt. Jag rekommenderar återigen inte bara hoppa till den slutliga lösningen jag har, testa Gyro på egen hand först och titta på den seriella porten vad vinkeln är som du luta den fram och tillbaka (med ingen motordrift). så du måste använda programmet från denna länk av Jeff Rowberg som skrev drivrutinen

https://github.com/jrowberg/i2cdevlib/BLOB/Master/...

Du måste också använda sitt bibliotek (samma länk). Om hans program inte kompilera har inte sedan du fått sitt bibliotek

i2cdevlib installerat på Arduino. Du kan hämta den från detta indestructible

När du kör testprogrammet kan du mycket väl se att när pendeln är på 90 grader vertikalt som vinkeln uppvägs av säga något upp till +/-10 grader!! Detta varierar från enhet till enhet (jag är benägen att tro) och det är varför jag satte en trim pott på den slutliga utformningen.

Ledningarna för MPU6050 Gyro är enkelt

Ansluta VCC och Gnd till 3.3V och slipat av Arduino (inte 5V!)

ansluta Int (avbrott) pin till Arduino Pin 2

ansluta SCL och XDA till motsvarande samma bokstäverna stiften på Arduino. Detta är för i2c bussen.

De andra stiften används inte.

Jag monterade gyrot på en liten plastbit via två små skruvar (hål redan borras på enheten naturligtvis). Jag använde sedan en limpistol för att limma församlingen att hjulet chassit. Här är testprogrammet för gyrot. Det inte driva motorerna, bara använda den för att testa. Använd den seriella bildskärmen på Arduino för att se Gyro utgångar.

------------------------------------------------------------------------------------------------------------------

I2C enheten klass (I2Cdev) demonstration Arduino skiss för MPU6050 klass med DMP (MotionApps v2.0)

6/21/2012 av Jeff Rowberg

Uppdateringar (förhoppningsvis) bör alltid finns på https://github.com/jrowberg/i2cdevlib

Ändringsloggen:

2013-05-08 - La sömlös Fastwire stöd

-Tillagd anmärkning om gyro kalibrering

2012-06-21 - lagt anteckning om Arduino 1.0.1 + Leonardo kompatibilitet fel

2012-06-20 - förbättrad FIFO overflow hantering och förenklade Läs process

2012-06-19 - helt ordnas DMP initieringskod och förenkling

2012-06-13 - dra gyro och accel data från FIFO paket istället för att läsa direkt

2012-06-09 - fixa trasiga FIFO Läs sekvens och ändra avbrott upptäckt till RESNING

2012-06-05 - Lägg till allvar-kompenseras första referens ram acceleration output

-lägga till 3D math helper fil till DMP6 exempel skiss

-lägga till Euler utgång och Yaw/Pitch/rulle utdataformat

2012-06-04 - ta bort accel offset clearing för bättre resultat (tack Sungon Lee)

2012-06-01 - fast gyro känslighet vara 2000 grader per sekund istället för 250

2012-05-30 - grundläggande DMP initiering arbetar

/* ============================================

I2Cdev enhet bibliotek koden placeras under MIT license

Copyright (c) 2012 Jeff Rowberg

Tillstånd beviljas härmed, gratis, till någon person att erhålla en kopia

för denna programvara och tillhörande dokumentationsfiler ("programvaran"), att hantera

i programvaran utan begränsningar, inklusive utan begränsning rättigheter

för att använda, kopiera, ändra, sammanfoga, publicera, distribuera, underlicensiera eller sälja

kopior av programvaran, och att tillåta personer som programvaran är

inredda för att göra det, på följande villkor:

Ovanstående copyrightmeddelande och detta tillstånd tillkännagivande skall ingå i

alla kopior eller betydande delar av programvaran.

PROGRAMVARAN TILLHANDAHÅLLS "I BEFINTLIGT SKICK", UTAN GARANTIER AV NÅGOT SLAG, UTTRYCKLIGA ELLER

UNDERFÖRSTÅDDA, INKLUSIVE MEN INTE BEGRÄNSAT TILL GARANTIER OM SÄLJBARHET,

LÄMPLIGHET FÖR ETT VISST ÄNDAMÅL OCH ICKE-INTRÅNG. UNDER INGA OMSTÄNDIGHETER SKALL DEN

FÖRFATTARE ELLER UPPHOVSRÄTTSINNEHAVARE HÅLLAS ANSVARIGA FÖR EVENTUELLA ANSPRÅK, SKADOR ELLER ANDRA

ANSVAR, VARE SIG I EN ÅTGÄRD AV KONTRAKTET, KRÄNKNING ELLER ANNAT, SOM UPPSTÅR FRÅN,

AV ELLER I SAMBAND MED PROGRAMVARAN ELLER ANVÄNDNING ELLER ANDRA KONTAKTER I

PROGRAMVARAN.

===============================================

*

/ I2Cdev och MPU6050 måste installeras som bibliotek, eller annars .cpp/.h filer

för båda klasserna måste inkludera sökvägen till ditt projekt

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#include "MPU6050.h" / / inte nödvändigt om du använder MotionApps include-filen

Arduino tråd bibliotek krävs om I2Cdev I2CDEV_ARDUINO_WIRE genomförande

används i I2Cdev.h

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

#include "Wire.h"

#endif

klass standard I2C adressen är 0x68

specifika I2C adresser kan skickas som en parameter här

AD0 låg = 0x68 (standard för SparkFun breakout och InvenSense utvärderingskort)

AD0 hög = 0x69

MPU6050 mpu;

MPU6050 mpu(0x69); <--använda för AD0 hög

/* =========================================================================

Obs: förutom anslutning 3.3V, GND, SDA och SCL, denna skiss

beror på MPU-6050 INT stift att vara ansluten till Arduinoen

yttre mellanjobb #0 pin. På Arduino Uno och Mega 2560 är detta

Digital i/o pin 2.

* ========================================================================= *

* =========================================================================

Obs: Arduino v1.0.1 med Leonardo styrelsen genererar ett kompileringsfel

När du använder Serial.write (buf, len). Tekanna utdata använder denna metod.

Lösningen kräver en ändring till Arduino USBAPI.h filen, som

är lyckligtvis enkel, men irriterande. Detta kommer att fastställas i nästa IDE

släpp. För mer info, se dessa länkar:

http://Arduino.cc/forum/index.php/topic, 109987.0.h...

http://code.Google.com/p/Arduino/Issues/detail?ID=...

* ========================================================================= */

avkommentera "OUTPUT_READABLE_QUATERNION" om du vill se den faktiska

quaternion komponenter i en [w, x, y, z] format (inte bäst för tolkning

på en avlägsen värd som bearbetning eller något men)

#define OUTPUT_READABLE_QUATERNION

avkommentera "OUTPUT_READABLE_EULER" om du vill se Euler vinklar

(i grader) beräknat från quaternions kommer från FIFO.

Observera att Euler vinklar lider gimbal lock (för mer information, se

http://en.wikipedia.org/wiki/Gimbal_lock)

#define OUTPUT_READABLE_EULER

avkommentera "OUTPUT_READABLE_YAWPITCHROLL" om du vill se yaw

/ pitch/rulle vinklar (i grader) beräknas från quaternions kommer

från FIFO. Observera detta också krävs gravitationen vector beräkningar.

Observera också att gir/pitch/rulle vinklar lider gimbal lock (för

Mer info, se: http://en.wikipedia.org/wiki/Gimbal_lock)

#define OUTPUT_READABLE_YAWPITCHROLL

avkommentera "OUTPUT_READABLE_REALACCEL" om du vill se acceleration

komponenter med gravitation bort. Denna acceleration referensram är

inte kompenseras för orientering, så + X är alltid + X enligt till den

sensor, bara utan effekterna av allvar. Om du vill att acceleration

ersättning för orientering, oss OUTPUT_READABLE_WORLDACCEL i stället.

#define OUTPUT_READABLE_REALACCEL

avkommentera "OUTPUT_READABLE_WORLDACCEL" om du vill se acceleration

komponenter med gravitation bort och justerat för världen ram

Referens (gir är i förhållande till inledande orientering, sedan ingen magnetometer

Det finns i detta fall). Kan vara ganska händig i vissa fall.

#define OUTPUT_READABLE_WORLDACCEL

avkommentera "OUTPUT_TEAPOT" om du vill att utmatningen som matchar den

format som används för InvenSense tekanna demo

#define OUTPUT_TEAPOT

#define LED_PIN 13 / / (Arduino är 13, Teensy är 11, Teensy ++ är 6)

bool blinkState = false;

MPU kontroll/status vars

bool dmpReady = false; Ange sant om DMP init var framgångsrik

uint8_t mpuIntStatus; innehar faktiska interrupt status byte från MPU

uint8_t devStatus; returstatus efter varje enhet operation (0 = framgång,! 0 = fel)

uint16_t packetSize; förväntat DMP paketstorlek (standard är 42 bytes)

uint16_t fifoCount; räkning av alla byte i FIFO

uint8_t fifoBuffer [64]; FIFO lagring buffert

läggning/rörelse vars

Quaternion q; [w, x, y, z] quaternion behållare

VectorInt16 aa; [x, y, z] accel sensor mätningar

VectorInt16 aaReal; [x, y, z] gravity-fri accel sensor mätningar

VectorInt16 aaWorld; [x, y, z] världen-frame accel sensor mätningar

VectorFloat allvar; [x, y, z] gravity vektor

flyta euler [3]. [psi, theta, phi] Euler vinkel behållare

float ypr [3]. [yaw, pitch, rulla] gir/pitch/rulle behållare och allvar vektor

paketstruktur för InvenSense tekanna demo

uint8_t teapotPacket [14] = {'$', 0x02, 0,0 0,0, 0,0, 0,0, 0x00, 0x00, "\r", "\n"};

// ================================================================

=== AVBROTT UPPTÄCKT RUTIN ===

// ================================================================

flyktiga bool mpuInterrupt = false; Anger om MPU avbryta pin har gått hög

void dmpDataReady() {

mpuInterrupt = sant;

}

// ================================================================

=== INITIAL SETUP ===

// ================================================================

void setup() {

gå med I2C bussen (I2Cdev bibliotek inte gör detta automatiskt)

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

Wire.BEGIN();

TWBR = 24. 400kHz I2C klocka (200kHz om CPU är 8MHz)

#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

Fastwire::Setup (400, true);

#endif

initiera seriell kommunikation

(115200 valt eftersom det krävs för tekanna Demo utgång, men det har

verkligen upp till dig beroende på ditt projekt)

Serial.BEGIN(115200);

tag (!. Seriell); vänta på Leonardo uppräkning, andra fortsätta omedelbart

Obs: 8MHz eller långsammare värd processorer, som Teensy @ 3,3 v eller Ardunio

Pro Mini kör på 3.3V, kan inte hantera detta överföringshastighet på ett tillförlitligt sätt tack vare

baud tidpunkten att vara alltför dåligt med processor fästingar. Du måste använda

38400 eller långsammare i dessa fall, eller använda någon form av extern separat

Crystal lösning för UART timern.

initiera enheten

Serial.println F ("initierar I2C enheter...").

MPU.Initialize();

Kontrollera anslutning

Serial.println F ("Testing enhetsanslutningar...").

Serial.println(MPU.testConnection()? F("MPU6050 Connection successful"): F ("MPU6050 anslutning misslyckades"));

vänta för klar

Serial.println (F ("\nSend någon karaktär att börja DMP programmering och demo:"));

medan (Serial.available() & & Serial.read()); Tom buffert

tag (!. Serial.available()); vänta på data

medan (Serial.available() & & Serial.read()); Tom buffert igen

Ladda och konfigurera DMP

Serial.println F ("initierar DMP...").

devStatus = mpu.dmpInitialize();

lämna din egen gyro förskjutningar här, skalas för min känslighet

mpu.setXGyroOffset(220);

mpu.setYGyroOffset(76);

mpu.setZGyroOffset(-85);

mpu.setZAccelOffset(1788); 1688 fabriksinställningarna för mitt test chip

se till att det fungerade (returnerar 0 om så är fallet)

om (devStatus == 0) {

slå på DMP, nu när den är klar

Serial.println F ("Aktivera DMP...").

mpu.setDMPEnabled(true);

Aktivera Arduino avbrott upptäckt

Serial.println (F ("Aktivera avbryta upptäckt (Arduino externa avbrott 0)..."));

attachInterrupt (0, dmpDataReady, RISING);

mpuIntStatus = mpu.getIntStatus();

ställa vår DMP redo flagga så funktionen huvudsakliga loop() vet att det är okej att använda det

Serial.println (F "(DMP redo! Väntar på första avbrott..."));

dmpReady = sant;

få förväntade DMP paketstorlek för senare jämförelse

packetSize = mpu.dmpGetFIFOPacketSize();

} annat {

FEL!

1 = första minne ladda misslyckades

2 = DMP konfiguration uppdateringar misslyckades

(om det kommer att bryta, oftast koden kommer att vara 1)

Serial.Print (F ("DMP initialisering misslyckades (kod"));

Serial.Print(devStatus);

Serial.println(F(")"));

}

Konfigurera LED för utdata

pinMode (LED_PIN, OUTPUT);

}

// ================================================================

=== MAIN PROGRAM LOOP ===

// ================================================================

void loop() {

om programmering misslyckades, försök inte att göra något

om (! dmpReady) återvända;

vänta på MPU avbrott eller extra paket togs tillgängliga

medan (! mpuInterrupt & & fifoCount < packetSize) {

andra program beteende grejer här

// .

// .

// .

Om du är riktigt paranoid kan du testa ofta mellan andra

saker för att se om mpuInterrupt är sant, och i så fall "break;" från den

While() slinga att omedelbart behandla MPU uppgifterna

// .

// .

// .

}

återställa avbrott flagga och få INT_STATUS byte

mpuInterrupt = false;

mpuIntStatus = mpu.getIntStatus();

få aktuella FIFO räkna

fifoCount = mpu.getFIFOCount();

check för overflow (detta skulle aldrig hända om inte vår kod är alltför ineffektiva)

om ((mpuIntStatus & 0x10) || fifoCount == 1024) {

Återställ så vi kan fortsätta renlig

mpu.resetFIFO();

Serial.println F ("FIFO overflow!").

annars kolla för DMP data redo avbrott (detta bör ske ofta)

} else om (mpuIntStatus & 0x02) {

vänta på rätt tillgänglig datalängd, bör vara en mycket kort vänta

medan (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

läsa ett paket från FIFO

mpu.getFIFOBytes (fifoBuffer, packetSize);

spåra FIFO räkningen här om det finns > 1 packet

(detta låter oss omedelbart Läs mer utan att invänta ett avbrott)

fifoCount-= packetSize;

#ifdef OUTPUT_READABLE_QUATERNION

Visa quaternion värden i lätt matrisform: w x y z

mpu.dmpGetQuaternion (& q, fifoBuffer);

Serial.Print("quat\t");

Serial.Print(q.w);

Serial.Print("\t");

Serial.Print(q.x);

Serial.Print("\t");

Serial.Print(q.y);

Serial.Print("\t");

Serial.println(q.z);

#endif

#ifdef OUTPUT_READABLE_EULER

Visa Euler vinklar i grader

mpu.dmpGetQuaternion (& q, fifoBuffer);

mpu.dmpGetEuler (euler, & q);

Serial.Print("euler\t");

Serial.Print (euler [0] * 180/M_PI);

Serial.Print("\t");

Serial.Print (euler [1] * 180/M_PI);

Serial.Print("\t");

Serial.println (euler [2] * 180/M_PI);

#endif

#ifdef OUTPUT_READABLE_YAWPITCHROLL

Visa Euler vinklar i grader

mpu.dmpGetQuaternion (& q, fifoBuffer);

mpu.dmpGetGravity (& gravitation, & q);

mpu.dmpGetYawPitchRoll (ypr, & q & gravitation);

Serial.Print("ypr\t")

Serial.Print (ypr [0] * 180/M_PI);

Serial.Print("\t");

/////////////////////////////////////////////////////////////////////////////////////////

/ Detta är Gyro vinkel ypr [1], vi använder här en för pendeln vagnen och konvertera till grader i raden nedan.

se till att du får nära noll när vagnen är vertical.b n

Serial.Print (ypr [1] * 180/M_PI);

Serial.Print("\t");

//////////////////////////////////////////////////////////////////////////////////////////////////////////

Serial.println (ypr [2] * 180/M_PI);

#endif

#ifdef OUTPUT_READABLE_REALACCEL

Visa verkliga acceleration, justeras för att ta bort allvar

mpu.dmpGetQuaternion (& q, fifoBuffer);

mpu.dmpGetAccel (& aa, fifoBuffer);

mpu.dmpGetGravity (& gravitation, & q);

mpu.dmpGetLinearAccel (& aaReal, & Axelsson, & gravitation);

Serial.Print("areal\t");

Serial.Print(aaReal.x);

Serial.Print("\t");

Serial.Print(aaReal.y);

Serial.Print("\t");

Serial.println(aaReal.z);

#endif

#ifdef OUTPUT_READABLE_WORLDACCEL

Visa första värld-frame acceleration, justeras för att ta bort allvar

och roterade baserat på kända orientering från quaternion

mpu.dmpGetQuaternion (& q, fifoBuffer);

mpu.dmpGetAccel (& aa, fifoBuffer);

mpu.dmpGetGravity (& gravitation, & q);

mpu.dmpGetLinearAccel (& aaReal, & Axelsson, & gravitation);

mpu.dmpGetLinearAccelInWorld (& aaWorld, & aaReal, & q);

Serial.Print("aworld\t");

Serial.Print(aaWorld.x);

Serial.Print("\t");

Serial.Print(aaWorld.y);

Serial.Print("\t");

Serial.println(aaWorld.z);

#endif

#ifdef OUTPUT_TEAPOT

Visa quaternion värden i InvenSense tekanna demo format:

teapotPacket [2] = fifoBuffer [0];

teapotPacket [3] = fifoBuffer [1].

teapotPacket [4] = fifoBuffer [4].

teapotPacket [5] = fifoBuffer [5].

teapotPacket [6] = fifoBuffer [8].

teapotPacket [7] = fifoBuffer [9].

teapotPacket [8] = fifoBuffer [12].

teapotPacket [9] = fifoBuffer [13].

Serial.write (teapotPacket, 14);

teapotPacket [11] ++; packetCount, loopar på 0xFF med flit

#endif

blinka LED att ange aktivitet

blinkState =! blinkState;

digitalWrite (LED_PIN, blinkState);

}

}