در این پروژه از ماژول MPU6050 جهت اندازه گیری زاویه ژایرو برای نمایش وضعیت افق و قطب نما در ربات های پرنده استفاده می شود . این پروژه می تواند در کنترل ربات های پروازی بسیار کار آمد باشد . ماژولMPU6050 که در این پروژه به کار رفته است یکی از مقرون به صرفه ترین ماژول های مبتنی بر MEMS جهت اندازی گیری شتاب و زاویه ژایرو می باشد . MPU6050 قابلیت اندازه گیری شتاب با دقت های 2 ، 4 ، 8 و 16 g را در سه جهت مجور های x,y و z را دارد . همچنین زاویه ژایرو نسبت به سه محور مختصات را به دست می دهد . رابط ارتباطی این ماژول I2C است که می توان آن را تنها از طریق پین های SDA و SCL کنترل نمود . MPU6050 دارای 1 کیلوبایت حافظه FIFO است که می توان از آن برای ذخیره داده در هنگامی که MCU نمی تواند مقادیر را از سنسور دریافت کند ، اسنتفاده کرد .
این پروژه شامل دو قسمت برنامه آردوینو و برنامه پروسسینگ است که در سمت کامپیوتر اجرا می شود . پراسسینگ یک محیط برنامه نویسی بصری بر پایه کتابخانه های منبع باز است که از آن برای ساخت محیط های گرافیکی می باشد . در IDE پراسسینگ از زبان برنامه نویسی جاوا استفاده می شود . در این پروژه قسمت های بصری که در سمت کامپیوتر اجرا می شوند با این زبان نوشته شده اند . برای اجرا برنامه نوشته شده پراسسینگ کافیست پس از دانلود پراسسینگ از اینجا آن را اجرا و سپس پروژه خود را باز کرده و آن را اجرا کنید . محیط پراسسینگ کاملا شبیه به محیط آردوینو IDE می باشد .
در شکل زیر نحوه شماتیک پروژه نشان داده شده است :
کد های آردوینو :
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#define LED_PIN 13
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
mpu.initialize();
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
//Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
//Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
Serial.print(devStatus);
Serial.println(F(")"));
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
//Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: 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
// display Euler angles in degrees
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
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
//Serial.print("Phi: ");
Serial.print(ypr[2] * 18/M_PI);
//Serial.print("\t theta: ");
Serial.print(" ");
Serial.print(ypr[1] * 180/M_PI);
//Serial.print("\t Psi: ");
Serial.print(" ");
Serial.println(ypr[0] * 180/M_PI);
//delay(100);
#endif
#ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
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
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
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
// display quaternion values in InvenSense Teapot 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, loops at 0xFF on purpose
#endif
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
برنامه پراسسینگ :
import org.firmata.*;
import cc.arduino.*;
//Thanks to Adrian Fernandez for the beta-version
//Modified and updated as per latest IDE by Aritro Mukherjee (April,2016)
//Check the detailed tutorial @ www.hackster.io/Aritro
// Sensor used while demonstration (MPU-6050,6DOF)
import processing.serial.*;
import cc.arduino.*;
int W=1400; //My Laptop's screen width
int H=700; //My Laptop's screen height
float Pitch;
float Bank;
float Azimuth;
float ArtificialHoizonMagnificationFactor=0.7;
float CompassMagnificationFactor=0.85;
float SpanAngle=120;
int NumberOfScaleMajorDivisions;
int NumberOfScaleMinorDivisions;
PVector v1, v2;
Serial port;
float Phi; //Dimensional axis
float Theta;
float Psi;
void setup()
{
size(W, H);
rectMode(CENTER);
smooth();
strokeCap(SQUARE);//Optional
println(Serial.list()); //Shows your connected serial ports
port = new Serial(this, "COMx", 115200); //در این قسمت پورتی که آردوینو به آن متصل است را وارد کنید
//Up there you should select port which arduino connected and same baud rate.
port.bufferUntil('\n');
}
void draw()
{
background(0);
translate(W/4, H/2.1);
MakeAnglesDependentOnMPU6050();
Horizon();
rotate(-Bank);
PitchScale();
Axis();
rotate(Bank);
Borders();
Plane();
ShowAngles();
Compass();
ShowAzimuth();
}
void serialEvent(Serial port) //Reading the datas by Processing.
{
String input = port.readStringUntil('\n');
if(input != null){
input = trim(input);
String[] values = split(input, " ");
if(values.length == 3){
float phi = float(values[0]);
float theta = float(values[1]);
float psi = float(values[2]);
print(phi);
print(theta);
println(psi);
Phi = phi;
Theta = theta;
Psi = psi;
}
}
}
void MakeAnglesDependentOnMPU6050()
{
Bank =-Phi/5;
Pitch=Theta*10;
Azimuth=Psi;
}
void Horizon()
{
scale(ArtificialHoizonMagnificationFactor);
noStroke();
fill(0, 180, 255);
rect(0, -100, 900, 1000);
fill(95, 55, 40);
rotate(-Bank);
rect(0, 400+Pitch, 900, 800);
rotate(Bank);
rotate(-PI-PI/6);
SpanAngle=120;
NumberOfScaleMajorDivisions=12;
NumberOfScaleMinorDivisions=24;
CircularScale();
rotate(PI+PI/6);
rotate(-PI/6);
CircularScale();
rotate(PI/6);
}
void ShowAzimuth()
{
fill(50);
noStroke();
rect(20, 470, 440, 50);
int Azimuth1=round(Azimuth);
textAlign(CORNER);
textSize(35);
fill(255);
text("Azimuth: "+Azimuth1+" Deg", 80, 477, 500, 60);
textSize(40);
fill(25,25,150);
text("FLIGHT SIMULATOR", -350, 477, 500, 60);
}
void Compass()
{
translate(2*W/3, 0);
scale(CompassMagnificationFactor);
noFill();
stroke(100);
strokeWeight(80);
ellipse(0, 0, 750, 750);
strokeWeight(50);
stroke(50);
fill(0, 0, 40);
ellipse(0, 0, 610, 610);
for (int k=255;k>0;k=k-5)
{
noStroke();
fill(0, 0, 255-k);
ellipse(0, 0, 2*k, 2*k);
}
strokeWeight(20);
NumberOfScaleMajorDivisions=18;
NumberOfScaleMinorDivisions=36;
SpanAngle=180;
CircularScale();
rotate(PI);
SpanAngle=180;
CircularScale();
rotate(-PI);
fill(255);
textSize(60);
textAlign(CENTER);
text("W", -375, 0, 100, 80);
text("E", 370, 0, 100, 80);
text("N", 0, -365, 100, 80);
text("S", 0, 375, 100, 80);
textSize(30);
text("COMPASS", 0, -130, 500, 80);
rotate(PI/4);
textSize(40);
text("NW", -370, 0, 100, 50);
text("SE", 365, 0, 100, 50);
text("NE", 0, -355, 100, 50);
text("SW", 0, 365, 100, 50);
rotate(-PI/4);
CompassPointer();
}
void CompassPointer()
{
rotate(PI+radians(Azimuth));
stroke(0);
strokeWeight(4);
fill(100, 255, 100);
triangle(-20, -210, 20, -210, 0, 270);
triangle(-15, 210, 15, 210, 0, 270);
ellipse(0, 0, 45, 45);
fill(0, 0, 50);
noStroke();
ellipse(0, 0, 10, 10);
triangle(-20, -213, 20, -213, 0, -190);
triangle(-15, -215, 15, -215, 0, -200);
rotate(-PI-radians(Azimuth));
}
void Plane()
{
fill(0);
strokeWeight(1);
stroke(0, 255, 0);
triangle(-20, 0, 20, 0, 0, 25);
rect(110, 0, 140, 20);
rect(-110, 0, 140, 20);
}
void CircularScale()
{
float GaugeWidth=800;
textSize(GaugeWidth/30);
float StrokeWidth=1;
float an;
float DivxPhasorCloser;
float DivxPhasorDistal;
float DivyPhasorCloser;
float DivyPhasorDistal;
strokeWeight(2*StrokeWidth);
stroke(255);
float DivCloserPhasorLenght=GaugeWidth/2-GaugeWidth/9-StrokeWidth;
float DivDistalPhasorLenght=GaugeWidth/2-GaugeWidth/7.5-StrokeWidth;
for (int Division=0;Division<NumberOfScaleMinorDivisions+1;Division++)
{
an=SpanAngle/2+Division*SpanAngle/NumberOfScaleMinorDivisions;
DivxPhasorCloser=DivCloserPhasorLenght*cos(radians(an));
DivxPhasorDistal=DivDistalPhasorLenght*cos(radians(an));
DivyPhasorCloser=DivCloserPhasorLenght*sin(radians(an));
DivyPhasorDistal=DivDistalPhasorLenght*sin(radians(an));
line(DivxPhasorCloser, DivyPhasorCloser, DivxPhasorDistal, DivyPhasorDistal);
}
DivCloserPhasorLenght=GaugeWidth/2-GaugeWidth/10-StrokeWidth;
DivDistalPhasorLenght=GaugeWidth/2-GaugeWidth/7.4-StrokeWidth;
for (int Division=0;Division<NumberOfScaleMajorDivisions+1;Division++)
{
an=SpanAngle/2+Division*SpanAngle/NumberOfScaleMajorDivisions;
DivxPhasorCloser=DivCloserPhasorLenght*cos(radians(an));
DivxPhasorDistal=DivDistalPhasorLenght*cos(radians(an));
DivyPhasorCloser=DivCloserPhasorLenght*sin(radians(an));
DivyPhasorDistal=DivDistalPhasorLenght*sin(radians(an));
if (Division==NumberOfScaleMajorDivisions/2|Division==0|Division==NumberOfScaleMajorDivisions)
{
strokeWeight(15);
stroke(0);
line(DivxPhasorCloser, DivyPhasorCloser, DivxPhasorDistal, DivyPhasorDistal);
strokeWeight(8);
stroke(100, 255, 100);
line(DivxPhasorCloser, DivyPhasorCloser, DivxPhasorDistal, DivyPhasorDistal);
}
else
{
strokeWeight(3);
stroke(255);
line(DivxPhasorCloser, DivyPhasorCloser, DivxPhasorDistal, DivyPhasorDistal);
}
}
}
void Axis()
{
stroke(255, 0, 0);
strokeWeight(3);
line(-115, 0, 115, 0);
line(0, 280, 0, -280);
fill(100, 255, 100);
stroke(0);
triangle(0, -285, -10, -255, 10, -255);
triangle(0, 285, -10, 255, 10, 255);
}
void ShowAngles()
{
textSize(30);
fill(50);
noStroke();
rect(-150, 400, 280, 40);
rect(150, 400, 280, 40);
fill(255);
Pitch=Pitch/5;
int Pitch1=round(Pitch);
text("Pitch: "+Pitch1+" Deg", -20, 411, 500, 60);
text("Bank: "+Bank*100+" Deg", 280, 411, 500, 60);
}
void Borders()
{
noFill();
stroke(0);
strokeWeight(400);
rect(0, 0, 1100, 1100);
strokeWeight(200);
ellipse(0, 0, 1000, 1000);
fill(0);
noStroke();
rect(4*W/5, 0, W, 2*H);
rect(-4*W/5, 0, W, 2*H);
}
void PitchScale()
{
stroke(255);
fill(255);
strokeWeight(3);
textSize(24);
textAlign(CENTER);
for (int i=-4;i<5;i++)
{
if ((i==0)==false)
{
line(110, 50*i, -110, 50*i);
}
text(""+i*10, 140, 50*i, 100, 30);
text(""+i*10, -140, 50*i, 100, 30);
}
textAlign(CORNER);
strokeWeight(2);
for (int i=-9;i<10;i++)
{
if ((i==0)==false)
{
line(25, 25*i, -25, 25*i);
}
}
}
