آموزش آردوینو و ژیروسکوپ و شتاب سنج MPU6050

در این آموزش، یاد می گیرم که چگونه از سنسور شتاب سنج و ژیروسکوپ MPU6050 به همراه آردوینو استفاده کنیم. ابتدا، توضیح می دهیم که سنسور MPU6050 چگونه کار می کند و چگونه می توانیم از آن، داده ها را قرائت کنیم. و سپس دو مثال بعنوان تمرین خواهیم زد.

نکته: این مقاله فقط ترجمه شده است و صحت کارکرد آن توسط مترجم مورد تایید نیست و تست نشده است.

مقدمه

در مثال اول، از محیط نرم افزار Processing استفاده می کنیم. ما می خواهیم یک تجسم سه بعدی از جهت سنسور داشته باشیم و در مثال دوم، می خواهیم یک پلتفرم ساده ی خود تثبیت کننده(self-stabilizing) یا یک گیمبال(Gimbal) ایجاد کنیم. ما بر اساس راهنمای MPU6050 و داده های مخلوط شده ی شتاب سنج و ژیروسکوپ آن، سه سروو موتور را کنترل می کنیم. 

روش کار

قطعه ی MPU6050 IMU سه محور شتاب سنج و سه محور ژیروسکوپ، روی یک تراشه دارد. در این قطعه، ژیروسکوپ سرعت دورانی(rotational velocity) یا نرخ تغییر موقعیت زاویه ای در طول زمان را در امتداد محور X، Y و Z اندازه گیری می کند.

این قطعه از فناوری MEMS و اثر کوریولیس(Coriolis Effect) برای اندازه گیری، استفاده می کند اما برای اطلاعات بیشتر در مورد آن، می توانید به مقاله ی چگونه سنسور های MEMS کار می کنند، مراجعه کنید. خروجی ژیروسکوپ بر حسب درجه بر ثانیه(degrees per second) است، بنابراین برای اینکه موقعیت زاویه ای به دست بیاوریم، تنها نیاز داریم از سرعت زاویه ای انتگرال بگیریم.

از طرف دیگر، شتاب سنج MPU6050 شتاب را به همان روش که در ویدئوی اول مقاله(در سایت منبع) برای سنسور شتاب سنج ADXL345 توضیح دادیم، اندازه گیری می کند. به طور خلاصه، می توان گفت که این شتاب سنج، می تواند شتاب گرانشی در امتداد 3 محور را اندازه گیری کند و با استفاده از کمی مثلثات در ریاضی، می توانیم زاویه ی مورد نظر را در جایی که سنسور در آن قرار گرفته است، محاسبه کنیم.

بنابراین اگر داده های شتاب سنج و ژیروسکوپ را ترکیب کنیم، می توانیم اطلاعات بسیار دقیقی را درباره ی جهت(orientation) این سنسور، به دست بیاوریم. قطعه ی MPU6050 IMU را دستگاه ردیاب حرکت شش محور یا 6 DoF (مخفف six Degrees of Freedom) نیز می نامند؛ زیرا 6 خروجی دارد: یعنی سه خروجی برای شتاب سنج و سه خروجی برای ژیروسکوپ.

آردوینو و ماژول MPU6050

اجازه دهید ببینیم که چگونه می توانیم قطعه ی مذکور را متصل کرده و داده ها را با استفاده از آردوینو، از MPU6050 قرائت کنیم. ما از پروتکل I2C برای ارتباط با آردوینو استفاده می کنیم، بنابراین برای اتصال به آن، تنها به دو سیم نیاز داریم و به دو سیم دیگر نیز برای برق رسانی نیاز داریم.

کدنویسی قطعه ی MPU6050

آنچه در زیر می بینید، کدهای آردوینو برای قرائت داده ها از سنسور MPU6050 است. در زیر کدها، می توانید توضیحات آن را مشاهده کنید:

/*
   Arduino and MPU6050 Accelerometer and Gyroscope Sensor Tutorial
   by Dejan, https://howtomechatronics.com
*/
#include <Wire.h>
const int MPU = 0x68; // MPU6050 I2C address
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float elapsedTime, currentTime, previousTime;
int c = 0;
void setup() {
  Serial.begin(19200);
  Wire.begin();                      // مقدار دهی اولیه ی ارتباط
  Wire.beginTransmission(MPU);       //MPU6050 شروع ارتباط با //MPU=0x68
  Wire.write(0x6B);                  //6B صحبت با رجیستر
  Wire.write(0x00);                  // 6B ریست کردن - قرار دادن 0 در رجیستر 
  Wire.endTransmission(true);        // پایان دادن به انتقال
  /*
  // +/- 2g تنظیم حساسیت شتاب سنج - محدوده کامل مقیاس -پیش فرض  

Wire.beginTransmission(MPU);
  Wire.write(0x1C);                  //(1C hex) ACCEL_CONFIG صحبت کردن با رجیستر
  Wire.write(0x10);                  // مقیاس کامل (1000deg/s) تنظیم بیت های رجیستر به صورت 00010000
  Wire.endTransmission(true);
  // Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);                   // Talk to the GYRO_CONFIG register (1B hex)
  Wire.write(0x10);                   // Set the register bits as 00010000 (1000deg/s full scale)
  Wire.endTransmission(true);
  delay(20);
  */
  // Call this function if you need to get the IMU error values for your module
  calculate_IMU_error();
  delay(20);
}
void loop() {
  // === Read acceleromter data === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
  //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value
  // Calculating Roll and Pitch from the accelerometer data
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)
  // === Read gyroscope data === //
  previousTime = currentTime;        // Previous time is stored before the actual time read
  currentTime = millis();            // Current time actual time read
  elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // Gyro data first register address 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
  // Correct the outputs with the calculated error values
  GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)
  GyroY = GyroY - 2; // GyroErrorY ~(2)
  GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)
  // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
  gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
  yaw =  yaw + GyroZ * elapsedTime;
  // Complementary filter - combine acceleromter and gyro angle values
  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
  
  // Print the values on the serial monitor
  Serial.print(roll);
  Serial.print("/");
  Serial.print(pitch);
  Serial.print("/");
  Serial.println(yaw);
}
void calculate_IMU_error() {
  // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
  // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
  // Read accelerometer values 200 times
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    // Sum all readings
    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
    c++;
  }
  //Divide the sum by 200 to get the error value
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;
  // Read gyro values 200 times
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
    // Sum all readings
    GyroErrorX = GyroErrorX + (GyroX / 131.0);
    GyroErrorY = GyroErrorY + (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
    c++;
  }
  //Divide the sum by 200 to get the error value
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;
  // Print the error values on the Serial Monitor
  Serial.print("AccErrorX: ");
  Serial.println(AccErrorX);
  Serial.print("AccErrorY: ");
  Serial.println(AccErrorY);
  Serial.print("GyroErrorX: ");
  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: ");
  Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ: ");
  Serial.println(GyroErrorZ);
}

توضیح کدهای بالا:

در ابتدا نیاز داریم کتابخانه ی Wire.h را که از آن برای ارتباطات I2C استفاده می کنیم را به کدها اضافه(include) کنیم و تعدادی متغیر برای ذخیره ی داده ها تعریف کنیم.

در بخش setup از کدها، نیاز داریم کتابخانه ی wire را مقدار دهی اولیه(initialize) کنیم و سنسور را از طریق رجیستر مدیریت برق(power management register) ریست کنیم. برای انجام این کار، نیاز داریم نگاهی به برگه ی داده ی(datasheet) سنسور بیندازیم؛ از آنجا می توانیم آدرس رجیستر را ببینیم.

 همچنین اگر بخواهیم می توانیم برای شتاب سنج و ژیروسکوپ، با استفاده از رجیسترهای پیکربندی آنها، Full-Scale Range را انتخاب کنیم. بعنوان مثال، ما می خواهیم محدوده یا برد 2g-+ را برای شتاب سنج و برد 250 درجه بر ثانیه(degrees/s) را برای ژیروسکوپ استفاده کنیم، بنابرای ما این بخش از کدها را به صورت کامنت در می آوریم.

// Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
  Wire.beginTransmission(MPU);
  Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)
  Wire.write(0x10);                  //Set the register bits as 00010000 (+/- 8g full scale range)
  Wire.endTransmission(true);
  // Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);                   // Talk to the GYRO_CONFIG register (1B hex)
  Wire.write(0x10);                   // Set the register bits as 00010000 (1000deg/s full scale)
  Wire.endTransmission(true);
  */

در بخش loop از کدها، در ابتدا شروع به قرائت داده های شتاب سنج می کنیم. داده ها برای هر محور، در دو بایت ذخیره یا رجیستر می شود و می توانیم آدرس های این رجیسترها را از برگه ی داده ی(datasheet) سنسور مشاهده کنیم.

 برای اینکه بتوانیم تمام این داده ها را قرائت کنیم، با رجیستر اولی شروع می کنیم و با استفاده از تابع requiestFrom() درخواست می کنیم تا تمام 6 رجیستر برای محورهای X و Y و Z قرائت شوند. سپس داده ها را از هر رجیستر می خوانیم و چون خروجی ها مکمل دو هستند، ما آنها را به طور مناسب ترکیب می کنیم تا مقادیر صحیح را به دست بیاوریم.

// === Read acceleromter data === //
  Wire.beginTransmission(MPU);
  Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers
  //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet
  AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value
  AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value
  AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value

برای اینکه مقادیر خروجی را از 1g- تا 1g+ دریافت کنیم، به طوری که برای محاسبه ی زوایا مناسب باشد، خروجی را بر حساسیت انتخاب شده قبلی تقسیم کنید:

 در نهایت، با استفاده از این دو فرمول زاویه های roll و pitch را از داد های شتاب سنج محاسبه می کنیم:

// Calculating Roll and Pitch from the accelerometer data
  accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
  accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)

سپس، با استفاده از روش مشابهی، داده های ژیروسکوپ را دریافت می کنیم.

ما شش رجیستر را برای ژیروسکوپ قرائت می کنیم و داده های آنها را به طور مناسب ترکیب می کنیم و آن را بر حساسیت انتخاب شده ی قبلی تقسیم می کنیم تا خروجی را به صورت درجه بر ثانیه دریافت کنیم.

// === Read gyroscope data === //
  previousTime = currentTime;        // Previous time is stored before the actual time read
  currentTime = millis();            // Current time actual time read
  elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
  Wire.beginTransmission(MPU);
  Wire.write(0x43); // Gyro data first register address 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
  GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
  GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
  GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

در اینجا متذکر می شویم که ما مقادیر خروجی را با مقدار کمی خطای محاسبه شده تصحیح می کنیم؛ که در ادامه به توضیح آنها خواهیم پرداخت. بنابراین، چونکه خروجی ها به صورت درجه بر ثانیه هستند، اکنون نیاز داریم تا آنها را در زمان ضرب کنیم تا فقط درجه ها را به دست بیاوریم. مقدار زمان، قبل از هربار قرائت، با استفاده از تابع millis() دریافت می شود.

// Correct the outputs with the calculated error values
  GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)
  GyroY = GyroY - 2; // GyroErrorY ~(2)
  GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)
  // Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
  gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg
  gyroAngleY = gyroAngleY + GyroY * elapsedTime;
  yaw =  yaw + GyroZ * elapsedTime;

در نهایت ما داده های شتاب سنج و ژیروسکوپ را با استفاده از یک فیلتر مکمل(complementary filter) مخلوط می کنیم. در اینجا، ما 96% از داده های ژیروسکوپ را می گیریم، زیرا دقت آن بسیار زیاد است و  نیروهای خارجی(external forces) را تحمل نمی کند.

در سمت پایین زیروسکوپ(The down side of the gyroscope)، دریفت(drift یا تغییر نامطلوب) ایجاد می شود یا اینکه با گذشت زمان، خطا در خروجی ایجاد می کند. بنابراین، در بلند مدت، که ما از داده های شتاب سنج استفاده می کنیم، در این مورد، 4% کافی است تا خطای دریفت ژیروسکوپ را از بین ببریم.

// Complementary filter - combine acceleromter and gyro angle values
  roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
  pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

اما، چونکه ما نمی توانیم Yaw را از داده های شتاب سنج محاسبه کنیم، نمی توانیم فیلتر مکمل(complementary filter) را روی آن پیاده سازی کنیم. قبل از اینکه نگاهی به نتایج بیاندازیم، اجازه دهید خیلی سریع توضیح دهیم که چگونه مقادیر تصحیح خطا(error correction values)  را دریافت کنیم. برای محاسبه ی این خطاها، می توانیم تابع سفارشی calculate_IMU_error() را هنگامی که سنسور در موقعیت ثابت و صاف قرار دارد، فراخوانی کنیم.

در اینجا، ما برای تمام خروجی ها، 200 قرائت را انجام داده ایم و آنها را جمع کرده و بر 200 تقسیم می کنیم. چونکه ما سنسور را در موقعیت ثابت و صاف نگه داشته ایم، مقادیر خروجی مورد انتظار باید 0 باشد. بنابراین، با این محاسبه، می توانیم خطای میانگینی که سنسور ایجاد می کند را دریافت کنیم.

void calculate_IMU_error() {
  // We can call this funtion in the setup section to calculate the accelerometer and gyro data error. From here we will get the error values used in the above equations printed on the Serial Monitor.
  // Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
  // Read accelerometer values 200 times
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    AccX = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccY = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0 ;
    // Sum all readings
    AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
    AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
    c++;
  }
  //Divide the sum by 200 to get the error value
  AccErrorX = AccErrorX / 200;
  AccErrorY = AccErrorY / 200;
  c = 0;
  // Read gyro values 200 times
  while (c < 200) {
    Wire.beginTransmission(MPU);
    Wire.write(0x43);
    Wire.endTransmission(false);
    Wire.requestFrom(MPU, 6, true);
    GyroX = Wire.read() << 8 | Wire.read();
    GyroY = Wire.read() << 8 | Wire.read();
    GyroZ = Wire.read() << 8 | Wire.read();
    // Sum all readings
    GyroErrorX = GyroErrorX + (GyroX / 131.0);
    GyroErrorY = GyroErrorY + (GyroY / 131.0);
    GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
    c++;
  }
  //Divide the sum by 200 to get the error value
  GyroErrorX = GyroErrorX / 200;
  GyroErrorY = GyroErrorY / 200;
  GyroErrorZ = GyroErrorZ / 200;
  // Print the error values on the Serial Monitor
  Serial.print("AccErrorX: ");
  Serial.println(AccErrorX);
  Serial.print("AccErrorY: ");
  Serial.println(AccErrorY);
  Serial.print("GyroErrorX: ");
  Serial.println(GyroErrorX);
  Serial.print("GyroErrorY: ");
  Serial.println(GyroErrorY);
  Serial.print("GyroErrorZ: ");
  Serial.println(GyroErrorZ);
}

ما به سادگی مقادیر مورد نظر را روی سریال مانیتور پرینت می کنیم و هنگامی که آنها را شناختیم، می توانیم آنها را در کدهایی که قبلا نشان دادیم، پیاده سازی کنیم؛ هم برای محاسبه ی rol و هم برای محاسبه ی pitch؛ و همچنین برای سه خروجی ژیروسکوپ.

 در نهایت، با استفاده از تابع Serial.print می توانیم مقادیر Roll و Pitch و Yaw را در سریال مانیتور چاپ کنیم و ببینیم که آیا سنسور به درستی کار می کند یا نه.

ردیابی جهت با MPU6050 - تجسم سه بعدی

در ادامه، برای اینکه از تجسم سه بعدی، یک مثال بزنیم، تنها نیاز داریم این داده ها را که آردوینو از طریق پورت سریال ارسال می کند را در محیط نرم افزار Processing بپذیریم. در زیر، کدهای کامل Processing را مشاهده می کنید:

/*
    Arduino and MPU6050 IMU - 3D Visualization Example 
     by Dejan, https://howtomechatronics.com
*/
import processing.serial.*;
import java.awt.event.KeyEvent;
import java.io.IOException;
Serial myPort;
String data="";
float roll, pitch,yaw;
void setup() {
  size (2560, 1440, P3D);
  myPort = new Serial(this, "COM7", 19200); // starts the serial communication
  myPort.bufferUntil('\n');
}
void draw() {
  translate(width/2, height/2, 0);
  background(233);
  textSize(22);
  text("Roll: " + int(roll) + "     Pitch: " + int(pitch), -100, 265);
  // Rotate the object
  rotateX(radians(-pitch));
  rotateZ(radians(roll));
  rotateY(radians(yaw));
  
  // 3D 0bject
  textSize(30);  
  fill(0, 76, 153);
  box (386, 40, 200); // Draw box
  textSize(25);
  fill(255, 255, 255);
  text("www.HowToMechatronics.com", -183, 10, 101);
  //delay(10);
  //println("ypr:\t" + angleX + "\t" + angleY); // Print the values to check whether we are getting proper values
}
// Read data from the Serial Port
void serialEvent (Serial myPort) { 
  // reads the data from the Serial Port up to the character '.' and puts it into the String variable "data".
  data = myPort.readStringUntil('\n');
  // if you got any bytes other than the linefeed:
  if (data != null) {
    data = trim(data);
    // split the string at "/"
    String items[] = split(data, '/');
    if (items.length > 1) {
      //--- Roll,Pitch in degrees
      roll = float(items[0]);
      pitch = float(items[1]);
      yaw = float(items[2]);
    }
  }
}

در اینجا، ما داده های ورودی را از آردوینو قرائت می کنیم و آن را در مغیرهای مناسب Roll و Pitch و Yaw قرار می دهیم. در حلقه ی اصلی draw، ما از این مقادیر برای چرخاندن یک شیء سه بعدی استفاده کرده ایم؛ در این مورد، این شیء، یک جعبه ی ساده با یک رنگ خاص است و متنی روی ان قرار دارد. اگر ما اسکچ را اجرا کنیم، مشاهده می کنیم که سنسور MPU6050 به خوبی جهت را ردیابی می کند. شیء سه بعدی مورد نظر، جهت سنسور را کاملاً دقیق ردیابی می کند و بسیار واکنش گرا نیز می باشد.

همان طور که گفتیم، تنها جنبه منفی این است که Yaw در طول زمان دریفت(drift) خواهد شد؛ زیرا نمی توانیم از فیلتر مکمل(complementary filter) برای آن استفاده کنیم. برای بهبودی این مشکل، نیاز داریم از یک سنسور اضافی استفاده کنیم. این سنسور، یک سنسور مغناطیس سنج(magnetometer) است که می توانیم از آن بعنوان یک اصلاح کننده ی بلند مدت، برای دریفت Yaw ژیروسکوپ، استفاده کنیم.

اما، سنسور MPU6050 یک ویژگی دارد که پردازشگر دیجیتال حرکت(Digital Motion Processor) نامیده می شود که از آن برای برای محاسبات داخلی(onboard) داده ها استفاده می شود و می تواند دریفت Yaw را از بین ببرد.

 در اینجا، از همان مثال سه بعدی بالا استفاده شده است اما در آن از پردازشگر دیجیتال حرکت(Digital Motion Processor) استفاده شده است. اکنون می توانیم ببینیم که ردیابی جهت، بدون دریفت Yaw چقدر دقیق انجام می شود. پردازشگر داخلی مورد نظر، همچنین می تواند چهارگان(Quaternions) را محاسبه کرده و در خروجی قرار دهد. از چهارگان، برای ارائه ی جهت گیری و چرخش اجسام در سه بعد استفاده می شود.

در این مثال، ما در حقیقت از چهارگان، برای نشان دادن جهت گیری(orientation) استفاده می کنیم. این جهت گیری، همچنین از قفل گیمبال که هنگام استفاده از زوایای اویلر رخ می دهد، اثر نمی پذیرد.

 با این وجود، دریافت داده از سنسور نسبت به آنچه که قبلاً گفتیم، کمی پیچیده تر است. اول از همه، باید به یک پین دیجیتال آردوینو سیم اضافی وصل کنیم. این (پین) یک پین وقفه یا انترراپت(interrupt) است که از آن برای قرائت این نوع داده از MPU6050 استفاده می شود.

 نکته:  وقفه یا انترراپت(interrupt)، یک سیگنال به ریزپردازنده است که به توجه و پاسخ سریع CPU نیاز دارد. هنگامی که یک وقفه رخ می‌دهد، پردازنده عملیات جاری خود را متوقف می‌کند تا به درخواست وقفه رسیدگی کند.

کد ما از این هم کمی پیچیده تر است؛ علت این پیچیدگی، این است که می خواهیم از کتابخانه ی i2cdevlib نوشته ی Jeff Rowberg استفاده کنیم. این کتابخانه را می توانیم در گیتهاب دانلود کنیم و ما در این مقاله، لینک دانلود آن را نیز قرار خواهیم داد. هنگامی که ما این کتابخانه را نصب کنیم، می توانیم مثال MPU6050_DMP6 را در آن باز کنیم. این مثال، با استفاده از کامنت ها در هر خط توضیح داده شده است.

در اینجا، می توانیم انتخاب کنیم که از چه نوع خروجی قصد داریم استفاده کنیم: چهارگان ها(quaternions)، زوایای اویلر(Euler angles)، pitch یا roll یا مقدار شتاب سنج، یا چهارگان برای تجسم سه بعدی. این کتابخانه، همچنین حاوی یک اسکچ Processing برای مثال تجسم سه بعدی(3D visualization) است. ما آن را اصلاح کرده ایم تا شکل جعبه را مانند مثال قبلی به دست آوریم.

در زیر، کد Processing تجسم سه بعدی، قرار دارد که با مثال MPU6050_DPM6 برای خروجی انتخاب شده ی “OUTPUT_TEAPOT” کار می کند.

/*
    Arduino and MPU6050 IMU - 3D Visualization Example 
     by Dejan, https://howtomechatronics.com
*/
import processing.serial.*;
import java.awt.event.KeyEvent;
import java.io.IOException;
Serial myPort;
String data="";
float roll, pitch,yaw;
void setup() {
  size (2560, 1440, P3D);
  myPort = new Serial(this, "COM7", 19200); // starts the serial communication
  myPort.bufferUntil('\n');
}
void draw() {
  translate(width/2, height/2, 0);
  background(233);
  textSize(22);
  text("Roll: " + int(roll) + "     Pitch: " + int(pitch), -100, 265);
  // Rotate the object
  rotateX(radians(-pitch));
  rotateZ(radians(roll));
  rotateY(radians(yaw));
  
  // 3D 0bject
  textSize(30);  
  fill(0, 76, 153);
  box (386, 40, 200); // Draw box
  textSize(25);
  fill(255, 255, 255);
  text("www.HowToMechatronics.com", -183, 10, 101);
  //delay(10);
  //println("ypr:\t" + angleX + "\t" + angleY); // Print the values to check whether we are getting proper values
}
// Read data from the Serial Port
void serialEvent (Serial myPort) { 
  // reads the data from the Serial Port up to the character '.' and puts it into the String variable "data".
  data = myPort.readStringUntil('\n');
  // if you got any bytes other than the linefeed:
  if (data != null) {
    data = trim(data);
    // split the string at "/"
    String items[] = split(data, '/');
    if (items.length > 1) {
      //--- Roll,Pitch in degrees
      roll = float(items[0]);
      pitch = float(items[1]);
      yaw = float(items[2]);
    }
  }
}

 بنابراین در اینجا، با استفاده از تابع serialEvent() ما چهارگان(quaternion) را که از آردوینو می آید، دریافت می کنیم و در حلقه ی(loop) اصلی draw از آنها برای چرخاندن شیء سه بعدی استفاده می کنیم.

اگر اسکچ مورد نظر را اجرا کنیم، مشاهده می کنیم که چهارگان ها، چقدر برای چرخاندن اشیاء در سه بعد خوب و مناسب هستند. برای اینکه این آموزش بیش از حد سنگین نشود، ما مثال دوم را در یک مقاله ی جداگانه، به نام : خودتان امتحان کنید گیمبال آردوینو یا پلتفرم خود تثبیت کننده، قرار داده ایم.

منابع:

https://howtomechatronics.com