الكود البرمجي (Arduino IDE)

 IDE عليك تنصيب مكتبة حساس الحركة والتسارع في ثلاث اتجاهات MPU6050.

بعد ذلك ارفع الكود الخاص بمحاكاة خط الأفق على لوحة الاردوينو حتى تتمكن من قراءة البيانات من الحساس.

#include<Wire.h> // wire library
const int MPU_addr=0x68; // MPU address
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; // 16 bit data array
int minVal=265;
int maxVal=402;
double x; double y; double z;
void setup() {
Serial.begin(9600);
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
}
void loop() {

Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true);
AcX=Wire.read()<<8|Wire.read();
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();


int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);

x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);
Serial.print(x);
Serial.print(" ");
Serial.print(y);
Serial.print(" ");
Serial.print(z);
Serial.print(" ");
Serial.print("\n");}

شرح الكود البرمجي

في البداية نقوم بإدراج مكتبة Wire.h، التي تحتوى على الدوال اللازمة للتواصل بين الاردوينو و حساس الحركة والتسارع في ثلاث اتجاهات.

#include<Wire.h> // wire library

بعد ذلك يتم تحديد بروتوكول الاتصال التسلسلي l2c الخاص بحساس التسارع.

const int MPU_addr=0x68; // MPU address

هنا يتم تعريف عدد من المتغيرات من النوع Array.

int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ; // 16 bit data array

يتم تحدد أقصى قيمة وأقل قيمة يقرأها الحساس بين (265-402) لقياس الزوايا من 0 إلى 360.

int minVal=265;
int maxVal=402;

المتغيرات x,y و z نوعها double, يتم تخزين قيمة المحور x في المتغير x وقيمة المحور y في المتغير y وقيمة المحور z في المتغير z.

double x; double y; double z;

في دالة ()void يبدأ الاتصال التسلسلي والنقل بين حساس التسارع والعنوان 0x68.

void setup() {
Serial.begin(9600);
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);

هنا يتم انهاء عملية النقل.

Wire.endTransmission(true);
}

في دالة ()loop,  يدخل حساس التسارع في وضع السكون بعد توصيله مع العنوان 0x3B.

Wire.write(0x3B);

هنا تبدأ عملية النقل من جديد.

Wire.endTransmission(false);

يتم طلب 14 مسجلًا من حساس التسارع والحركة في ثلاث اتجاهات.

ثم سيتم إرسال رسالة إيقاف بعد الطلب.

Wire.requestFrom(MPU_addr,14,true);

بعد قراءة البيانات لكل المحاور  (x و y و z) يتم تخزينها في AcX,AcY, AcZ.

AcX=Wire.read()<<8|Wire.read();
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();

بعد ذلك، يتم تعيين الحد الأدنى والحد الأقصى لقيمة بيانات المحاور من الحساس بنطاق معين من -90 إلى 90.

int xAng = map(AcX,minVal,maxVal,-90,90);
int yAng = map(AcY,minVal,maxVal,-90,90);
int zAng = map(AcZ,minVal,maxVal,-90,90);

هنا يتم حساب 360 درجة.

ثم يتم تحويل وحدة الراديان إلى وحدة درجة.

x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI);
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI);
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI);

هنا تتم طباعة قيم المحاور x,y و z على شاشة الاتصال التسلسلي.

Serial.print(x);
Serial.print(" ");
Serial.print(y);
Serial.print(" ");
Serial.print(z);
Serial.print(" ");
Serial.print("\n");}