Stap 2: Code
Hier is de code. Het gebruikt de I2C. Ik vond sommige delen van code van internet. (I2C deel)
Geschreven door Ahmet Burkay KIRNIK
TR_CapaFenLisesi //Measure hoek met een MPU-6050(GY-521)
#include
Const int MPU_addr = 0x68; int16_t AcX AcY, AcZ, Tmp, GyX, GyY, GyZ;
int minVal = 265; int maxVal = 402;
dubbele x; dubbele y; dubbele 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); 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 ("AngleX ="); Serial.println(x);
Serial.Print ("AngleY ="); Serial.println(y);
Serial.Print ("AngleZ ="); Serial.println(z); Serial.println("---"); delay(400); }
