|
//Written by Ahmet Burkay KIRNIK
|
|
//TR_CapaFenLisesi
|
|
//Measure Angle with a MPU-6050(GY-521)
|
|
|
|
#include<Wire.h>
|
|
|
|
const int MPU_addr=0x68;
|
|
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
|
|
|
|
int minVal=265;
|
|
int maxVal=402;
|
|
int i,a,b;
|
|
double tab[50];
|
|
double nb[50];
|
|
double x;
|
|
double y;
|
|
double z;
|
|
|
|
|
|
|
|
void setup(){
|
|
Wire.begin();
|
|
Wire.beginTransmission(MPU_addr);
|
|
Wire.write(0x6B);
|
|
Wire.write(0);
|
|
Wire.endTransmission(true);
|
|
Serial.begin(9600);
|
|
}
|
|
void loop(){
|
|
Wire.beginTransmission(MPU_addr);
|
|
Wire.write(0x3B);
|
|
Wire.endTransmission(false);
|
|
Wire.requestFrom(MPU_addr,14,true);
|
|
GyX=Wire.read()<<8|Wire.read();
|
|
GyY=Wire.read()<<8|Wire.read();
|
|
GyZ=Wire.read()<<8|Wire.read();
|
|
int xAng = map(GyX,minVal,maxVal,-90,90);
|
|
int yAng = map(GyY,minVal,maxVal,-90,90);
|
|
int zAng = map(GyZ,minVal,maxVal,-90,90);
|
|
|
|
x= RAD_TO_DEG * (atan2(-yAng, -zAng)+PI)*10;
|
|
y= RAD_TO_DEG * (atan2(-xAng, -zAng)+PI)*10;
|
|
z= RAD_TO_DEG * (atan2(-yAng, -xAng)+PI)*10;
|
|
a=x;
|
|
nb[0]=1000;
|
|
nb[1]=100;
|
|
nb[2]=10;
|
|
nb[3]=1;
|
|
for (i=0;i<4;i++){
|
|
b=nb[i];
|
|
tab[i]=a/b;
|
|
a=a-tab[i]*b;
|
|
Serial.print(tab[i],0);
|
|
}
|
|
Serial.print(" ");
|
|
a=y;
|
|
for (i=0;i<4;i++){
|
|
b=nb[i];
|
|
tab[i]=a/b;
|
|
a=a-tab[i]*b;
|
|
Serial.print(tab[i],0);
|
|
}
|
|
Serial.println();
|
|
delay(400);
|
|
}
|