-
Notifications
You must be signed in to change notification settings - Fork 1
/
2 mpu config
90 lines (78 loc) · 2 KB
/
2 mpu config
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
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
#include <Wire.h>
const int MPU2 = 0x69, MPU1=0x68;
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ, gyroX, gyroY, gyroZ,rotX, rotY, rotZ;
long accelX2, accelY2, accelZ2;
float gForceX2, gForceY2, gForceZ2;
void setup(){
Wire.begin();
Wire.beginTransmission(MPU1);
Wire.write(0x6B);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.beginTransmission(MPU1);
Wire.write(0x1B);
Wire.write(0x00000000);
Wire.endTransmission();
Wire.beginTransmission(MPU1);
Wire.write(0x1C);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.begin();
Wire.beginTransmission(MPU2);
Wire.write(0x6B);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.beginTransmission(MPU2);
Wire.write(0x1B);
Wire.write(0x00000000);
Wire.endTransmission();
Wire.beginTransmission(MPU2);
Wire.write(0x1C);
Wire.write(0b00000000);
Wire.endTransmission();
Serial.begin(38400);
}
void loop(){
GetMpuValue(MPU1);
Serial.print("\t ||| \t");
GetMpuValue(MPU2);
Serial.println("");
}
void GetMpuValue(const int MPU){
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(MPU,6);
while(Wire.available() < 6);
accelX = Wire.read()<<8|Wire.read();
accelY = Wire.read()<<8|Wire.read();
accelZ = Wire.read()<<8|Wire.read();
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(MPU,6);
while(Wire.available() < 6);
gyroX = Wire.read()<<8|Wire.read();
gyroY = Wire.read()<<8|Wire.read();
gyroZ = Wire.read()<<8|Wire.read();
gForceX = accelX / 16384.0;
gForceY = accelY / 16384.0;
gForceZ = accelZ / 16384.0;
rotX = gyroX / 131.0;
rotY = gyroY / 131.0;
rotZ = gyroZ / 131.0;
Serial.print("gyro\t");
Serial.print(rotX);
Serial.print("\t");
Serial.print(rotY);
Serial.print("\t");
Serial.print(rotZ);
Serial.print("\tAcc\t");
Serial.print(gForceX);
Serial.print("\t");
Serial.print(gForceY);
Serial.print("\t");
Serial.print(gForceZ);
delay(100);
}