/
Mpu6050Controller.java
121 lines (100 loc) · 4.41 KB
/
Mpu6050Controller.java
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
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
/*
* To change this license header, choose License Headers in Project Properties.
* To change this template file, choose Tools | Templates
* and open the template in the editor.
*/
package de.buschbaum.java.pathfinder.device.mpu6050;
import com.pi4j.io.i2c.I2CBus;
import com.pi4j.io.i2c.I2CDevice;
import com.pi4j.io.i2c.I2CFactory;
import de.buschbaum.java.pathfinder.Helper;
import java.io.IOException;
/**
*
* @author uli
*/
public class Mpu6050Controller {
private static I2CBus bus = null;
private static I2CDevice mpu6050 = null;
public static void initialize() throws IOException, InterruptedException {
initializeI2C();
configureMpu6050();
}
private static void initializeI2C() throws IOException {
System.out.println("Creating I2C bus");
bus = I2CFactory.getInstance(I2CBus.BUS_1);
System.out.println("Creating I2C device");
mpu6050 = bus.getDevice(0x68);
}
private static void configureMpu6050() throws IOException, InterruptedException {
//1 Waking the device up
writeConfigRegisterAndValidate(
"Waking up device",
"Wake-up config succcessfully written: ",
Mpu6050Registers.MPU6050_RA_PWR_MGMT_1,
Mpu6050RegisterValues.MPU6050_RA_PWR_MGMT_1);
//2 Configure sample rate
writeConfigRegisterAndValidate(
"Configuring sample rate",
"Sample rate succcessfully written: ",
Mpu6050Registers.MPU6050_RA_SMPLRT_DIV,
Mpu6050RegisterValues.MPU6050_RA_SMPLRT_DIV);
//3 Setting global config
writeConfigRegisterAndValidate(
"Setting global config (digital low pass filter)",
"Global config succcessfully written: ",
Mpu6050Registers.MPU6050_RA_CONFIG,
Mpu6050RegisterValues.MPU6050_RA_CONFIG);
//4 Configure Gyroscope
writeConfigRegisterAndValidate(
"Configuring gyroscope",
"Gyroscope config successfully written: ",
Mpu6050Registers.MPU6050_RA_GYRO_CONFIG,
Mpu6050RegisterValues.MPU6050_RA_GYRO_CONFIG);
//5 Configure Accelerometer
writeConfigRegisterAndValidate(
"Configuring accelerometer",
"Accelerometer config successfully written: ",
Mpu6050Registers.MPU6050_RA_ACCEL_CONFIG,
Mpu6050RegisterValues.MPU6050_RA_ACCEL_CONFIG);
//6 Configure interrupts
writeConfigRegisterAndValidate(
"Configuring interrupts",
"Interrupt config successfully written: ",
Mpu6050Registers.MPU6050_RA_INT_ENABLE,
Mpu6050RegisterValues.MPU6050_RA_INT_ENABLE);
//7 Configure low power operations
writeConfigRegisterAndValidate(
"Configuring low power operations",
"Low power operation config successfully written: ",
Mpu6050Registers.MPU6050_RA_PWR_MGMT_2,
Mpu6050RegisterValues.MPU6050_RA_PWR_MGMT_2);
for (byte i = 1; i <= 120; i++) {
byte registerData = Mpu6050Controller.readRegister(i);
System.out.println(i + "\t\tRegisterData:" + Helper.formatBinary(registerData));
}
System.exit(0);
}
private static void writeRegister(byte register, byte data) throws IOException {
mpu6050.write(register, data);
}
public static byte readRegister(byte register) throws IOException {
int data = mpu6050.read(register);
return (byte) data;
}
public static byte readRegister() throws IOException {
int data = mpu6050.read();
return (byte) data;
}
public static void writeConfigRegisterAndValidate(String initialText, String successText, byte register, byte registerData) throws IOException {
System.out.println(initialText);
writeRegister(register, registerData);
byte returnedRegisterData = Mpu6050Controller.readRegister(register);
if (returnedRegisterData == registerData) {
System.out.println(successText + Helper.formatBinary(returnedRegisterData));
} else {
throw new RuntimeException("Tried to write " + Helper.formatBinary(registerData) + " to "
+ register + ", but validiating value returned " + Helper.formatBinary(returnedRegisterData));
}
}
}