Simple MPU6050 IMU + Arduino bot UPDATE

1#include <Wire.h>
2#include <MPU6050.h>
3
4MPU6050 mpu;
5
6//  Timers
7unsigned long timer = 0;
8float timeStep = 0.01;
9
10// Pitch, Roll  and Yaw values
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  // Initialize MPU6050
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  // Calibrate gyroscope. The calibration must be  at rest.
39  // If you don't want calibration, comment this line.
40  mpu.calibrateGyro();
41
42  // Set threshold sensivty. Default 3.
43  // If you don't want use threshold,  comment this line or set 0.
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); // use the delay to place  barney on the floor
54}
55
56void loop()
57{
58  timer = millis();
59
60  // Read normalized values
61  Vector norm = mpu.readNormalizeGyro();
62
63  // Calculate Pitch, Roll and Yaw
64  pitch = pitch + norm.YAxis * timeStep;
65  roll = roll + norm.XAxis * timeStep;
66  yaw = yaw + norm.ZAxis * timeStep;
67
68  // Output raw
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  // Wait to full timeStep period
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