در وهله‌ی اول کتابخانه ماژول MPU6050 و سروو موتور را به برنامه اضافه می‌کنیم. کتابخانه "wire" مربوط به پروتکل SPI بین آردوینو و ماژول MPU6050 است.
#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>
سپس متغیری به نام sg90 ، از نوع Servo تعریف می‌کنیم. این متغیر به ما در استفاده از توابع کتابخانه سروو موتور کمک خواهد کرد. درنهایت پین 2 آردوینو را برای اتصال سروو موتور تنظیم می‌کنیم.
Servo sg90;
int servo_pin = 2;
در این قسمت متغیری از نام و نوع ماژول تعریف می‌کنیم ( .MPU6050). این متغیر به ما در استفاده از توابع کتابخانه ماژولIMU نیز کمک خواهد کرد.
MPU6050 sensor ;
int16_t ax, ay, az ;
int16_t gx, gy, gz ;
حال برخی متغیرها را که به منظور اندازه‌گیری خروجی ماژول MPU6050 استفاده خواهند شد، راه‌اندازی اولیه می‌کنیم. ورودی سروو موتور به پین 2 آردوینو متصل شده و سرعت انتقال داده نیز روی 9600 تنظیم می‌شود.
sg90.attach ( servo_pin );
Wire.begin ( );
Serial.begin (9600);
در این مرحله عملکرد ماژول MPU6050 بررسی می‌شود. اگر ماژول به درستی عمل کند پیغام"Taking Values from the sensor" و در غیر این صورت پیغام
“Connection failed" بر روی نمایشگر سریال آردوینو نشان داده خواهد شد.
Serial.println (sensor.testConnection ( ) ? "Successfully Connected" : "Connection failed");
delay (1000);
Serial.println ( "Taking Values from the sensor" );
delay (1000);
در حلقه برنامه خروجی‌های ماژول MPU6050 به مقداری که بتوانیم توسط آن سروو موتور را کنترل کنیم نگاشت می‌شود. سپس این مقدار به سروو موتور منتقل شده و آن را حرکت می‌دهد.
sensor.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
ax = map (ax, -17000, 17000, 0, 180) ;
Serial.println (ax);
sg90.write (ax);
delay (200);
کد کامل برنامه:
/*
Partineh
Arduino Tutorial Series
Author: Shokoufeh Davarzani
Website: www.Partineh.com
*/
#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>
Servo sg90;
int servo_pin = 2;
MPU6050 sensor ;
int16_t ax, ay, az ;
int16_t gx, gy, gz ;
void setup ( )
{
sg90.attach ( servo_pin );
Wire.begin ( );
Serial.begin (9600);
Serial.println ( "Initializing the sensor" );
sensor.initialize ( );
Serial.println (sensor.testConnection ( ) ? "Successfully Connected" : "Connection failed");
delay (1000);
Serial.println ( "Taking Values from the sensor" );
delay (1000);
}
void loop ( )
{
sensor.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
ax = map (ax, -17000, 17000, 0, 180) ;
Serial.println (ax);
sg90.write (ax);
delay (200);
}
