در این پروژه می خواهیم چگونگی راه اندازی سنسور MPU6050 با آردوینو را بررسی کنیم. سنسور ژیروسکوپ و شتاب سنج MPU6050 یک IMU یا Inertial Measurement Unit است. این سنسور دارای یک شتاب سنج سه محوره و یک ژیروسکوپ سه محوره است که در یک چیپ (تراشه) کوچک مجتمع شدهاند. خروجی ژیروسکوپ بر حسب درجه بر ثانیه است بنابراین با داشتن سرعت زاویه ای میتوان زاویه های سنسور نسبت به محورها را به دست آورد. از طرف دیگر، شتاب سنج MPU6050 ، شتاب گرانشی در امتداد هر سه محور را اندازه میگیرد و با کمی محاسبات مثلثاتی میتوان زاویه های مربوط به موقعیت سنسور را به دست آورد. بنابراین، با ترکیب دادههای خروجی ژیروسکوپ و شتاب سنج، موقعیت و جهت قرار گرفتن سنسور با دقت خیلی خوبی به دست خواهد آمد.
ژیروسکوپ سه محوره
همانطور که گفته شد، MPU6050 دارای یک ژیروسکوپ سه محوره است که میتواند سرعت چرخشی سنسور را در امتداد سه محور x، y و z با استفاده از فناوری MEMS اندازه بگیرد.
شتابسنج سه محوره


MPU6050 دارای یک شتابسنج سه محوره است که میتواند اندازه زاویه انحراف جسم نسبت به محورهای مختصات را با استفاده از فناوری MEMS تعیین کند. شتاب در راستای محورهای مختصات که موجب انحراف جسم میشود، باعث تغییر اندازه ظرفیت خازنها در ساختار سنسور MEMS میشود که با اندازه گیری این تغییر میتوان مقدار شتاب را به دست آورد. دیجیتال شدن مقادیر توسط یک ADC 16 بیتی انجام میشود. محدوده اندازه گیری خروجی ±۲g، ±۴g، ±۸g و ±۱۶g است. در حالتی که جسم روی یک سطح صاف و در وضعیت عادی خود قرار دارد، مقدار شتاب در راستای محورهای x، y و z به ترتیب ۰g، ۰g و +۱g است.
سنسور دما روی چیپ
خروجی سنسور دما توسط ADC به مقدار دیجیتال تبدیل میشود و در خروجی ماژول قابل خواندن است.
پردازشگر حرکت دیجیتال (Digital motion Processor (DMP))
این بخش یک پردازنده تعبیه شده در ماژول برای انجام محاسبات روی دادههای سنسور است. با خواندن رجیسترهای DMP میتوان به مقادیر حاصل شده از بخش DMP دست پیدا کرد. وجود این بخش در MPU6050 از حجم محاسبات پردازنده میزبان مانند آردوینو میکاهد.
پایههای MPU6050
این ماژول دارای ۸ پایه است:
INT: خروجی دیجیتال وقفه است. این پایه نشان میدهد که دادهای برای خواندن توسط میکروکنترلر در دسترس است.
AD0: LSB آدرس slave در ارتباط I2C را مشخص میکند. این پایه کم ارزشترین بیت در آدرس ۷ بیتی slave است. در واقع زمانی که از یک میکروکنترلر برای ارتباط با چندین MPU6050 استفاده میشود، این پایه برای تفکیک آدرسها به کار گرفته میشود.
XCL: خط کلاک کمکی ارتباط سریال I2C است. این پایه برای اتصال پایه SCL دیگر واسطهای I2C به MPU6050 استفاده میشود.
XDA: خط داده کمکی ارتباط سریال I2C است. این پایه برای اتصال پایه SDA دیگر واسطهای I2C به MPU6050 استفاده میشود.
SCL: پایه کلاک ارتباط سریال I2C است. این پایه به پایه SCL میکروکنترلر متصل میشود.
SDA: پایه داده ارتباط سریال I2C است. این پایه به پایه SDA میکروکنترلر متصل میشود.
GND: پایه زمین است.
VCC: پایه تغذیه است که ولتاژ آن میتواند از ۳ ولت تا ۵ ولت باشد.
راه اندازی سنسور MPU6050 با آردوینو
معمولا میکروکنترلرها دارای ارتباط I2C هستند و برخی از پایههای آن ها نیز به این ارتباط سریال اختصاص داده شده است. برای مثال در آردوینو Nano که در این مقاله برای راهاندازی سنسور MPU6050 استفاده شده، پایه های A4 و A5 به ترتیب برای SDA و SCL ارتباط I2C در نظر گرفته شدهاند. بنابراین، در اتصال آردوینو و ماژول، پایه A4 آردوینو باید به SDA ماژول و پایه A5 به SCL ماژول متصل شود. یکی از ماژولهایی که دارای سنسور MPU6050 هستند، ماژول GY-521 است که در اینجا از آن برای راهاندازی سنسور 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 ; voidsetup () { Serial.begin (19200 ); Wire.begin ();// Initialize comunication Wire.beginTransmission (MPU );// Start communication with MPU6050 // MPU=0x68 Wire.write (0x6B );// Talk to the register 6B Wire.write (0x00 );// Make reset - place a 0 into the 6B register Wire.endTransmission (true );//end the transmission /* // 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); delay(20); */ // Call this function if you need to get the IMU error values for your module calculate_IMU_error ();delay (20 ); } voidloop () {// === 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.equestFrom (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.println (yaw); } voidcalculate_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."AccErrorX: " ); Serial.println (AccErrorX ); Serial."AccErrorY: " ); Serial.println (AccErrorY ); Serial."GyroErrorX: " ); Serial.println (GyroErrorX ); Serial."GyroErrorY: " ); Serial.println (GyroErrorY ); Serial."GyroErrorZ: " ); Serial.println (GyroErrorZ ); }
توضیحات کد
در ابتدای کد کتابخانه Wire.h به منظور به کارگیری ارتباط I2C افزوده شده است. علاوه بر آن چند متغیر نیز برای ذخیره کردن دادهها تعریف شده است.
/* Arduino Brushless Motor Control by 118ELEC.com */ #include <Servo.h >Servo ESC;// create servo object to control the ESC int potValue;// value from the analog pin void setup() {// Attach the ESC on pin 9 ESC.attach (9 ,1000 ,2000 );// (pin, min pulse width, max pulse width in microseconds) }void loop() { potValue =analogRead (A0 );// reads the value of the potentiometer (value between 0 and 1023) potValue =map (potValue,0 ,1023 ,0 ,180 );// scale it to use it with the servo library (value between 0 and 180) ESC.write (potValue);// Send the signal to the ESC }
در تابع setup ابتدا تنظیمات اولیه کتابخانه Wire انجام و سپس سنسور از طریق رجیستر مدیریت تغذیهی آن ریست میشود. برای شروع ارسال دستور به ماژول از دستور Wire.beginTransmission(Module) استفاده میشود. در این دستور، به جای Module آدرس ماژول مورد نظر قرار میگیرد. به طور کلی برقراری ارتباط با سنسور MPU6050 از طریق رجیسترهای آن انجام میگیرد که جزئیات آنها در دیتاشیت سنسور آمده است. با مطالعه دیتاشیت سنسور مشاهده میشود که آدرس ماژول در رجیستر ۱۱۷ به نام “Who Am I” ذخیره شده است که به صورت پیش فرض برابر با ۰x68 است. دستورات ()Wire.write و ()Wire.read به یک آدرس مشخص، داده میفرستد یا از آن داده میخواند. در نهایت، دستور ()Wire.endTransmission انتهای پیام ارسالی را به ماژول اطلاع میدهد.
رجیستر ۰x6B آدرس مربوط به رجیستر مدیریت توان ماژول است که با نوشتن صفر در آن ماژول ریست میشود.
voidsetup () { Serial.begin (19200 ); Wire.begin ();// Initialize comunication Wire.beginTransmission (MPU );// Start communication with MPU6050 // MPU=0x68 Wire.write (0x6B );// Talk to the register 6B Wire.write (0x00 );// Make reset - place a 0 into the 6B register Wire.endTransmission (true );
با کمک رجیسترهای پیکربندی سنسور MPU6050 میتوان محدوده اندازهگیری ژیروسکوپ و شتابسنج را تغییر داد. در این مثال، مقادیر پیش فرض ±۲g برای شتاب سنج و ±۲۵۰ degrees/s برای ژیروسکوپ در نظر گرفته شده است. بنابراین قسمت تنظیمات این رجیسترها در کد، کامنت شده است.
تابع loop با خواندن خروجی شتابسنج شروع شده است. دادههای مربوط به هر محور در دو بایت یا دو رجیستر ذخیره میشود. آدرس این رجیسترها در دیتاشیت سنسور آمده است.


برای خواندن هر شش رجیستر با هم، از دستور ()Wire.requestFrom استفاده میشود. با این دستور تمام رجیسترهای مربوط به سه محور X، Y و Z خوانده میشود و سپس دادههای مربوط به هر محور از آن استخراج میشود. با توجه به جدول مربوط به حساسیت سنسور که در دیتاشیت آمده و محدوده اندازهگیری خروجی که از –g تا +g است، خروجی باید بر ۱۶۳۸۴ تقسیم شود.
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
در نهایت، با استفاده از روابط موجود در این فایل، میتوان میزان چرخش Roll (چرخش حول محور X) و Pitch (چرخش حول محور Y) را محاسبه کرد. سنسور شتاب سنج نسبت به چرخش حول محور Z حساسیت ندارد. بنابراین، محاسبه زاویه Yaw به این روش انجام نمیشود.
// 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)
برای خواندن خروجی سنسور ژیروسکوپ از روشی مشابه استفاده میشود. آدرس رجیسترهای مربوط به خروجی ژیروسکوپ برای هر سه محور در دیتاشیت آمده است.


شش رجیستر ژیروسکوپ با هم خوانده میشود و سپس با توجه به مقدار حساسیت برای هر LSB، خروجی ژیروسکوپ بر حسب درجه بر ثانیه محاسبه میشود. در این مثال، از آنجا که محدوده اندازه گیری ژیروسکوپ ±۲۵۰ درجه بر ثانیه انتخاب شده است، مقادیر رجیسترها باید بر ۱۳۱ تقسیم شوند تا خروجی بر حسب درجه بر ثانیه به دست آید.


// === 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.equestFrom (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;
در خط های بعدی مشاهده میشود که مقادیر خروجی ژیروسکوپ با در نظر گرفتن خطا اصلاح شده است. روش به دست آمدن مقادیر خطا در انتهای مقاله توضیح داده شده است. خروجی ژیروسکوپ باید در زمان ضرب شود تا مقادیر بر حسب درجه به دست آید. مقدار زمان قبل از هر تکرار حلقه با استفاده از تابع ()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;
از آنجا که تمامی ابزارهای اندازهگیری دارای خطا هستند، باید با روشهایی خطا را تا جایی که امکان دارد کاهش داد. یکی از این روشها ترکیب خروجی سنسورهای مختلف است. در مورد سنسور MPU6050 ، خروجی شتابسنج دارای نویز زیاد است. از طرف دیگر، خروجی ژیروسکوپ با اینکه نویز کمتری دارد اما با گذشت زمان خطای آن افزایش می یابد و از مقدار درست فاصله میگیرد. برای برطرف کردن این مشکل باید خروجی دو سنسور شتابسنج و ژیروسکوپ را با یکدیگر ترکیب کرد. برای ترکیب خروجیهای دو سنسور از میانگین وزندار استفاده میشود و با توجه به اینکه خروجی سنسور شتابسنج نویز زیادی دارد نباید وزن زیادی به آن تعلق بگیرد. وزن ۴ درصد برای جبران انحراف خروجی ژیروسکوپ در طولانی مدت کافی است.
// Complementary filter - combine acceleromter and gyro angle values roll = 0.96 * gyroAngleX + 0.04 * accAngleX; pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
حال به چگونگی محاسبه مقدار خطا برای اصلاح خروجی ژیروسکوپ پرداخته میشود. برای به دست آوردن مقادیر خطا میتوان از تابع ()calculate_IMU_error در زمانی که سنسور در حالت ثابت روی سطح صاف قرار دارد، استفاده کرد. در اینجا، خروجی ژیروسکوپ ۲۰۰ بار خوانده و میانگین آن ها محاسبه شده است. زمانی که سنسور بدون زاویه روی سطح صاف است، انتظار میرود که خروجی سنسور صفر باشد. اما مشاهده میشود که خروجی سنسور در این حالت صفر نیست و مقادیر به دست آمده میانگین خطای سنسور را نشان میدهد.
voidcalculate_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."AccErrorX: " ); Serial.println (AccErrorX ); Serial."AccErrorY: " ); Serial.println (AccErrorY ); Serial."GyroErrorX: " ); Serial.println (GyroErrorX ); Serial."GyroErrorY: " ); Serial.println (GyroErrorY ); Serial."GyroErrorZ: " ); Serial.println (GyroErrorZ ); }
در نهایت مقادیر Roll، Pitch و Yaw در قسمت مانیتور سریال نرم افزار آردوینو قابل مشاهده است.