-
Notifications
You must be signed in to change notification settings - Fork 0
/
MPU6050Sensor.h
68 lines (60 loc) · 1.53 KB
/
MPU6050Sensor.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
#ifndef MPU6050SENSOR_H
#define MPU6050SENSOR_H
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;
#define MPU_addr 0x68
int xAng, yAng;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
double z = 0;
#define minVal 265
#define maxVal 402
int MPU_Angle()
{
int value = 0;
if (!mpu.begin())
{
Serial.println("MPU6050 Module Not Found!!");
// display.println(F("Module Error")); // set info
// display.display();
}
else
{
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
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();
xAng = map(AcX, minVal, maxVal, -90, 90);
yAng = map(AcY, minVal, maxVal, -90, 90);
z = RAD_TO_DEG * (atan2(-yAng, -xAng) + PI);
z = z + 90;
if (z > 360) z = z - 360;
// Serial.print(F("Current angle: "));
// Serial.println(z);
value = round(z);
value = abs(value);
//thia section is for correcting the angle
float corrected_angle = value - 180;
// Normalize the angle to be within 0 to 360 degrees
if (corrected_angle < 0)
{
corrected_angle += 360;
}
else if (corrected_angle >= 360)
{
corrected_angle -= 360;
}
value = corrected_angle;
Serial.print("MPU angle: ");
Serial.println(value);
}
return value;
}
#endif //MPU6050SENSOR_H