1#include <Wire.h>
2#include <MPU6050.h>
3
4MPU6050 mpu;
5
6
7unsigned long timer = 0;
8float timeStep = 0.01;
9
10
11float pitch = 0;
12float roll = 0;
13float yaw = 0;
14
15
16int led = 13;
17int ena = 5;
18int enb = 6;
19int ina = 7;
20int inb = 4;
21int inc = 9;
22int ind = 8;
23
24
25
26
27void setup()
28{
29 Serial.begin(115200);
30
31
32 while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
33 {
34 Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
35 delay(500);
36 }
37
38
39
40 mpu.calibrateGyro();
41
42
43
44 mpu.setThreshold(3);
45pinMode(led, OUTPUT);
46pinMode(ena, OUTPUT);
47pinMode(enb, OUTPUT);
48pinMode(ina, OUTPUT);
49pinMode(inb, OUTPUT);
50pinMode(inc, OUTPUT);
51pinMode(ind, OUTPUT);
52
53delay(5000);
54}
55
56void loop()
57{
58 timer = millis();
59
60
61 Vector norm = mpu.readNormalizeGyro();
62
63
64 pitch = pitch + norm.YAxis * timeStep;
65 roll = roll + norm.XAxis * timeStep;
66 yaw = yaw + norm.ZAxis * timeStep;
67
68
69 Serial.print(" Pitch = ");
70 Serial.print(pitch);
71 Serial.print(" Roll = ");
72 Serial.print(roll);
73 Serial.print(" Yaw = ");
74 Serial.println(yaw);
75
76
77 delay((timeStep*1000) - (millis() - timer));
78
79 if ( yaw == 0 ) {
80 digitalWrite(ina, HIGH);
81 digitalWrite(inb,HIGH);
82 analogWrite(ena, 0);
83 digitalWrite(inc,HIGH);
84 digitalWrite(ind,HIGH);
85 analogWrite(enb, 0);
86 digitalWrite(led, HIGH);
87 }
88 else {
89 if ( yaw < 0 ) {
90 digitalWrite(ina, HIGH);
91 digitalWrite(inb,LOW);
92 analogWrite(ena, 80);
93 digitalWrite(inc,LOW);
94 digitalWrite(ind,HIGH);
95 analogWrite(enb, 80);
96 digitalWrite(led, LOW);
97 }
98
99 else {
100 digitalWrite(ina, LOW);
101 digitalWrite(inb,HIGH);
102 analogWrite(ena, 80);
103 digitalWrite(inc,HIGH);
104 digitalWrite(ind,LOW);
105 analogWrite(enb, 80);
106 digitalWrite(led, LOW);
107 }
108
109 }
110}
111