Self Balancing ondersteboven slinger (4 / 9 stap)

Stap 4: De MPU6050 3 Axis Gyro en versnellingsmeter

Wat we nodig hebben is een signaal dat evenredig is aan de hoek van tilt - dat de Y-as in de software en deze Gyro is wanneer het plat zit. Nogmaals zou ik niet aanbevelen gewoon springen naar de uiteindelijke oplossing ik heb, de Gyro afkan eerst testen en kijken op de seriële poort wat de hoek is als je het heen en weer (met geen motor rijden kantelen). dus je moet het programma van deze link door Jeff Rowberg die schreef het stuurprogramma te gebruiken

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

U moet ook gebruik maken van zijn bibliotheek (dezelfde link). Als zijn programma doet niet gecompileerd nog niet dan heb je zijn bibliotheek

i2cdevlib geïnstalleerd op de Arduino. U kunt het downloaden van dit onverwoestbaar

Wanneer u de test uitvoert je se kan goed dat wanneer de slinger is bij 90 graden verticaal die de hoek wordt gecompenseerd door zeggen tot +/-10 graden!! Dit varieert van apparaat naar apparaat (ik ben geleid om te geloven) en dat is waarom ik een trim pot op het uiteindelijke ontwerp.

De bedrading van de MPU6050 Gyro is eenvoudig

VCC en Gnd verbinden met 3.3V en de grond van de Arduino (niet 5V!)

Int (de interrupt) pin verbinden met Arduino Pin 2

sluit SCL en XDA met de corre-dezelfde letters pinnen op de Arduino. Dit is voor de i2c-bus.

De andere pinnen niet worden gebruikt.

Ik de Gyro gemonteerd op een klein stukje plastic via twee kleine schroeven (gaten zijn al geboord op het apparaat natuurlijk). Ik gebruikte dan een lijmpistool te lijmen van de vergadering op het wiel-chassis. Hier is het testprogramma voor de Gyro. Het niet de macht van de motoren, gewoon gebruiken om te testen. Gebruik de seriële monitor op de Arduino te zien van Gyro uitgangen.

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

I2C apparaat klasse (I2Cdev) demonstratie Arduino sketch voor MPU6050 klasse met DMP (MotionApps v2.0)

6/21/2012 door Jeff Rowberg

Updates (hopelijk) moet altijd beschikbaar zijn op https://github.com/jrowberg/i2cdevlib

Changelog:

2013-05-08 - naadloze Fastwire ondersteuning toegevoegd

-toegevoegde opmerking over gyro kalibratie

2012-06-21 - opmerking over Arduino 1.0.1 + compatibiliteitsfout voor Leonardo toegevoegd

2012-06-20 - verbeterde FIFO overflow overslag en vereenvoudigde lees proces

2012-06-19 - volledig herschikt DMP-initialisatiecode en vereenvoudiging

2012-06-13 - gyro en accel gegevens ophalen uit FIFO packet in plaats van direct lezen

2012-06-09 - fix gebroken FIFO Lees volgorde en interrupt veranderingsdetectie te stijgen

2012-06-05 - toevoegen zwaarte-gecompenseerd eerste referentie kader versnelling uitvoer

-3D math helper bestand toevoegen aan DMP6 voorbeeld schets

-toevoegen van Euler output en Yaw/Pitch/Roll output formaten

2012-06-04 - verwijderen accel verschuiving schakelen voor betere resultaten (Bedankt Sungon Lee)

2012-06-01 - vaste gyro gevoeligheid voor 2000 deg/sec in plaats van 250

2012-05-30 - essentiële DMP initialisatie arbeidsvoorwaarden

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

I2Cdev bibliotheek apparaatcode is geplaatst onder de MIT-licentie

Copyright (c) 2012 Jeff Rowberg

Hierbij wordt toestemming verleend, kosteloos aan eenieder een exemplaar te verkrijgen

van deze software en de bijbehorende documentatie bestanden (de "Software"), om aan te pakken

in de Software zonder beperking, met inbegrip, zonder beperking, van de rechten

te gebruiken, kopiëren, wijzigen, samenvoegen, publiceren, distribueren, sublicentiëren en/of verkopen

kopieën van de Software, en toe te staan van de personen aan wie de Software

ingericht om dit te doen, onder de volgende voorwaarden:

De bovenstaande auteursrechtkennisgeving en deze machtigingsaanduiding worden opgenomen in

alle kopieën of aanzienlijke onderdelen van de Software.

DE SOFTWARE WORDT GELEVERD "AS IS", ZONDER GARANTIE VAN ENIGE SOORT, UITDRUKKELIJK OF

IMPLICIET, INCLUSIEF MAAR NIET BEPERKT TOT DE GARANTIES VAN VERKOOPBAARHEID,

GESCHIKTHEID VOOR EEN BEPAALD DOEL EN NIET-INBREUKMAKENDHEID. IN GEEN GEVAL ZAL DE

AUTEURS OF AUTEURSRECHTHOUDERS AANSPRAKELIJK VOOR ENIGE CLAIM, SCHADELOOSSTELLING OF ANDERE

AANSPRAKELIJKHEID, HETZIJ DOOR EEN HANDELING UIT CONTRACT, ONRECHTMATIGE DAAD OF ANDERSZINS, DIE VOORTVLOEIEN UIT,

UIT OF IN VERBAND MET DE SOFTWARE OF HET GEBRUIK OF ANDERE BEWERKING MET BETREKKING TOT

DE SOFTWARE.

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

*

/ I2Cdev en MPU6050 moeten worden geïnstalleerd als bibliotheken, of anders de .cpp/.h-bestanden

voor beide klassen moeten worden in de relatief pad van uw project

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#include "MPU6050.h" / / niet noodzakelijk als gebruikt MotionApps-bestand opnemen

Arduino draad bibliotheek is vereist als I2Cdev I2CDEV_ARDUINO_WIRE uitvoering

wordt gebruikt in I2Cdev.h

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

#include "Wire.h"

#endif

klasse standaard I2C adres is 0x68

specifieke I2C-adressen kunnen worden doorgegeven als een parameter hier

AD0 laag = 0x68 (standaard voor SparkFun breakout en InvenSense evaluatie board)

AD0 hoge = 0x69

MPU6050 mpu;

MPU6050 mpu(0x69); <--gebruiken voor AD0 hoge

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

Opmerking: naast verbinding 3.3V, GND, SDA en SCL, deze schets

afhankelijk van de MPU-6050 INT pin wordt aangesloten op de Arduino

externe interrupt #0 pin. Op de Arduino Uno en Mega 2560 is dit

digitale I/O pin 2.

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

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

Opmerking: Arduino v1.0.1 met het Leonardo-bord genereert treedt er een compileerfout

bij het gebruik van Serial.write (buf, len). De theepot output wordt deze methode gebruikt.

De oplossing vereist een wijziging aan het bestand van de Arduino USBAPI.h, die

is gelukkig eenvoudig, maar vervelend. Dit zal worden vastgesteld in de volgende IDE

release. Voor meer info, zie deze links:

http://Arduino.CC/Forum/index.php/topic, 109987.0.h...

http://code.Google.com/p/Arduino/issues/detail?id=...

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

uncomment "OUTPUT_READABLE_QUATERNION" als u wilt zien de werkelijke

Quaternion componenten in een [w, x, y, z] formaat (niet best voor het parsen

op een externe host zoals Processing of iets wel)

#define OUTPUT_READABLE_QUATERNION

uncomment "OUTPUT_READABLE_EULER" als u wilt zien van de hoeken van Euler

berekend op basis van de quaternionen vanuit de FIFO (in graden).

Merk op dat de hoeken van Euler gimbal lock lijden (voor meer info, zie

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

#define OUTPUT_READABLE_EULER

uncomment "OUTPUT_READABLE_YAWPITCHROLL" als u wilt zien de yaw

/ worp/roll hoeken (in graden) berekend op basis van de quaternionen komen

van de FIFO. Opmerking hiervoor ook zwaartekracht vector berekeningen.

Merk ook op dat yaw/pitch/roll hoeken gimbal lock (lijden

meer info, zie: http://en.wikipedia.org/wiki/Gimbal_lock)

#define OUTPUT_READABLE_YAWPITCHROLL

uncomment "OUTPUT_READABLE_REALACCEL" als u wilt zien van de versnelling

componenten met zwaartekracht verwijderd. Dit referentiekader versnelling is

niet gecompenseerd voor oriëntatie, dus + X is altijd + X volgens de

sensor, maar wel zonder de effecten van de zwaartekracht. Als u versnelling wilt

gecompenseerd voor oriëntatie, ons OUTPUT_READABLE_WORLDACCEL in plaats daarvan.

#define OUTPUT_READABLE_REALACCEL

uncomment "OUTPUT_READABLE_WORLDACCEL" als u wilt zien van de versnelling

componenten met zwaartekracht verwijderd en aangepast aan het frame van de wereld van

Referentie (yaw is ten opzichte van de oorspronkelijke afdrukstand, sinds geen magnetometer

aanwezig is in dit geval). Zou heel handig zijn in sommige gevallen.

#define OUTPUT_READABLE_WORLDACCEL

uncomment "OUTPUT_TEAPOT" als u wilt dat uitvoer die overeenkomt met de

indeling die wordt gebruikt voor de InvenSense theepot demo

#define OUTPUT_TEAPOT

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

BOOL blinkState = false;

MPU controle/status vars

BOOL dmpReady = false; Deze eigenschap is true als DMP init succesvol was ingesteld

uint8_t mpuIntStatus; werkelijke interrupt status byte van MPU houdt

uint8_t devStatus; status na operatie van elk apparaat retourneren (0 = succes! 0 = fout)

uint16_t packetomvang; verwacht DMP pakketgrootte (standaard is 42 bytes)

uint16_t fifoCount; telling van alle bytes in FIFO

uint8_t fifoBuffer [64]; FIFO-opslag buffer

oriëntatie/beweging vars

Quaternion q; [w, x, y, z] quaternion container

VectorInt16 aa; [x, y, z] accel sensor metingen

VectorInt16 aaReal; [x, y, z] zwaartekracht-vrije accel sensor metingen

VectorInt16 aaWorld; [x, y, z] wereld-frame accel sensor metingen

VectorFloat Ernst; [x, y, z] zwaartekracht vector

zweven van euler [3]; [psi, theta, phi] Euler hoek container

float ypr, [3]; [yaw, pitch, roll] yaw/pitch/roll container en zwaartekracht vector

Packet structuur voor InvenSense theepot demo

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

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

=== INTERRUPT DETECTIE ROUTINE ===

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

vluchtige bool mpuInterrupt = false; geeft aan of MPU interrupt pin hoge is gegaan

ongeldig dmpDataReady() {}

mpuInterrupt = true;

}

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

=== BASISINSTELLINGEN ===

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

VOID Setup {}

deelnemen aan I2C bus (I2Cdev bibliotheek niet dit automatisch doet)

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

Wire.begin();

TWBR = 24; 400kHz I2C klok (200kHz als CPU 8MHz is)

#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

Fastwire::Setup (400, true);

#endif

initialiseren van seriële communicatie

(115200 gekozen omdat het is vereist voor de uitvoer van de theepot Demo, maar het heeft

echt aan u, afhankelijk van uw project)

Serial.begin(115200);

terwijl (!. Serieel); wachten voor Leonardo opsomming, anderen blijven onmiddellijk

Opmerking: 8MHz of tragere processors van de gastheer, zoals de Teensy @ 3,3 v of Ardunio

Pro Mini draait op 3,3 v, kan niet overweg met deze baud-rate betrouwbaar te wijten aan

de baud timing wordt ook uitgelijnd met processor teken. U moet gebruiken

38400 of langzamer in deze gevallen, of gebruiken een soort van externe afzonderlijke

Crystal oplossing voor de UART-timer.

initialiseren van apparaat

Serial.println (F ("initialiseren I2C apparaten..."));

MPU.initialize();

verbinding controleren

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

Serial.println(MPU.testConnection()? F("MPU6050 Connection successful"): F ("MPU6050 connection failed"));

wachten op klaar

Serial.println (F ("\nSend een teken om te beginnen DMP programmering en demo:"));

terwijl (Serial.available() & & Serial.read()); lege buffer

terwijl (!. Serial.available()); wachten op gegevens

terwijl (Serial.available() & & Serial.read()); lege buffer weer

laden en configureer de DMP

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

devStatus = mpu.dmpInitialize();

levering eigen gyro compenseert hier, geschaald voor min gevoeligheid

mpu.setXGyroOffset(220);

mpu.setYGyroOffset(76);

mpu.setZGyroOffset(-85);

mpu.setZAccelOffset(1788); 1688-fabrieksinstellingen voor mijn test-chip

Zorg ervoor dat het werkte (geeft als resultaat 0 als dat zo is)

Als (devStatus == 0) {}

inschakelen van de DMP, nu dat het klaar is

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

mpu.setDMPEnabled(true);

Arduino interrupt-detectie inschakelen

Serial.println (F ("inschakelen interrupt detectie (Arduino externe interrupt 0)..."));

attachInterrupt (0, dmpDataReady, stijgt);

mpuIntStatus = mpu.getIntStatus();

onze DMP klaar vlag instellen zodat de belangrijkste loop functie weet dat het is o.k. om het te gebruiken

Serial.println (F ("DMP klaar! Waiting for eerste interrupt..."));

dmpReady = true;

krijgen van de verwachte DMP pakketgrootte voor later vergelijking

packetomvang = mpu.dmpGetFIFOPacketSize();

} else {}

FOUT!

1 = eerste geheugen laden mislukt

2 = DMP configuratie updates mislukt

(als het gaat om te breken, meestal de code zullen 1)

Serial.Print (F ("DMP-initialisatie is mislukt (code"));

Serial.Print(devStatus);

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

}

LED voor uitvoer configureren

pinMode (LED_PIN, uitvoer);

}

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

=== MAIN PROGRAMMA LUS ===

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

void loop {}

Als programmeren mislukt, probeer niet om iets te doen

Als (! dmpReady) terugkeer;

wachten op MPU interrupt of extra pakket(ten) beschikbaar

terwijl (! mpuInterrupt & & fifoCount < packetomvang) {}

andere programma gedrag stuff hier

// .

// .

// .

Als u echt paranoïde bent, kunt u vaak tussen andere testen

dingen te zien als mpuInterrupt waar is, en zo ja, "break;" van de

while() lus onmiddellijk de MPU om gegevens te verwerken

// .

// .

// .

}

resetten van de interrupt-vlag en krijgen INT_STATUS byte

mpuInterrupt = false;

mpuIntStatus = mpu.getIntStatus();

huidige FIFO tellen

fifoCount = mpu.getFIFOCount();

selectievakje voor overloop (dit mag nooit gebeuren tenzij onze code te inefficiënt is)

Als ((mpuIntStatus & 0x10) || fifoCount == 1024) {}

resetten zodat we netjes blijven kunnen

mpu.resetFIFO();

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

anders, Controleer voor DMP gegevens klaar interrupt (dit zou moeten gebeuren vaak)

} else if (mpuIntStatus & 0x02) {}

wachten op de lengte van de juiste beschikbare gegevens, moet een zeer korte wachttijd

terwijl (fifoCount < packetomvang) fifoCount = mpu.getFIFOCount();

een pakje van de FIFO leest

mpu.getFIFOBytes (fifoBuffer, packetomvang);

bijhouden van FIFO graaf hier in het geval er > 1 pakket beschikbaar

(dit laat ons onmiddellijk Lees meer zonder te wachten tot een interrupt)

fifoCount-= packetomvang;

#ifdef OUTPUT_READABLE_QUATERNION

quaternion weergavewaarden in gemakkelijk matrixvorm: 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

Euler-hoeken in graden weergegeven

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

Euler-hoeken in graden weergegeven

mpu.dmpGetQuaternion (& q, fifoBuffer);

mpu.dmpGetGravity (zwaartekracht, & q);

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

Serial.Print("ypr\t")

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

Serial.Print("\t");

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

/ Dit is de Gyro hoek ypr [1], wij gebruiken deze keer voor de kar van de slinger en converteren naar graden in de regel eronder.

Zorg ervoor dat je dicht bij nul wanneer de kar vertical.b n is

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

Serial.Print("\t");

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

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

#endif

#ifdef OUTPUT_READABLE_REALACCEL

echte versnelling, aangepast om te verwijderen van de zwaartekracht weergeven

mpu.dmpGetQuaternion (& q, fifoBuffer);

mpu.dmpGetAccel (& aa, fifoBuffer);

mpu.dmpGetGravity (zwaartekracht, & q);

mpu.dmpGetLinearAccel (& aaReal, & aa, & zwaartekracht);

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

eerste versnelling van de wereld-frame, aangepast om te verwijderen van de zwaartekracht weergeven

en geroteerd op basis van bekende oriëntatie van de quaternion

mpu.dmpGetQuaternion (& q, fifoBuffer);

mpu.dmpGetAccel (& aa, fifoBuffer);

mpu.dmpGetGravity (zwaartekracht, & q);

mpu.dmpGetLinearAccel (& aaReal, & aa, & zwaartekracht);

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

quaternion weergavewaarden in InvenSense theepot demo-indeling:

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, loops bij 0xFF moedwil

#endif

knipperen van LED om aan te geven van de activiteit

blinkState =! blinkState;

digitalWrite (LED_PIN, blinkState);

}

}