1. #include "Wire.h"
2. #include "Math.h"
3. #include "I2Cdev.h"
4. #include "MPU6050.h"
5. int ledPinL = 11;
6. int ledPinR = 10;
7. int ledPin0 = 9;
8.  
9. MPU6050 accelerometer;
10.  
11. const float pi = 3.141592;
12. const int sample_no = 100; // no of samples for aproximation
13. int16_t ax, ay, az;
14. float x, y, z;
15. int sample;
16. float _angle_x, angle_x, _angle_y, angle_y;
17. long ax_sum, ay_sum, az_sum;
18.  
19. void setup() {
20.  
21. Wire.begin();
22. Serial.begin(9600);
23. accelerometer.initialize();
24. if (accelerometer.testConnection());
25. Serial.println("MPU 6050 connection OK...");
26. delay(1000);
27.  
28. }
29.  
30. void loop() {
31. accelerometer.getAcceleration(&ax, &ay, &az);
32. ax_sum = ax_sum + ax;
33. ay_sum = ay_sum + ay;
34. az_sum = az_sum + az;
35. sample++;
36.  
37. if (sample == sample_no)
38. {
39. // mean values
40. x = ax_sum/sample_no;
41. y = ay_sum/sample_no;
42. z = az_sum/sample_no;
43.  
44. // Calculate of roll and pitch in deg
45. angle_x = atan2(x, sqrt(square(y) + square(z)))/(pi/180);
46. angle_y = atan2(y, sqrt(square(x) + square(z)))/(pi/180);
47.  
48. digitalWrite(ledPinL, (angle_x < -0.51) ? HIGH:LOW);
49. digitalWrite(ledPin0, (angle_x >-0.50 && angle_x <0.50) ? HIGH:LOW);
50. digitalWrite(ledPinR, (angle_x > 0.51) ? HIGH:LOW);
51.  
52. }
53.  
54.  
55. Serial.print(angle_x);
56. Serial.print("\t"); // \t = tablator
57. Serial.println(angle_y);
58. }