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
|
package frc.robot.util;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.Timer;
public class IMUGyro
{
public I2C mpu;
public double pitch;
public double roll;
public double yaw;
private double gyroOffsets[] = {
0.312449302886265,
0.04883417185589417,
-0.12945104079301352
};
// for deltatime
private double prev_time = Timer.getFPGATimestamp();
private final int PWR_MGMT_1 = 0x6B;
private final int CONFIG_ADDR = 0x1A;
private final int SMPLRT_DIV = 0x19;
private final int GYRO_ADDR = 0x43;
private final int GYRO_CONFIG = 0x1B;
private final double SCALE_GYRO = (Math.PI/(180*16.384) * 2 /*??*/);
public IMUGyro(I2C gyro)
{
mpu = gyro;
reset();
mpu.write(CONFIG_ADDR, 0b00000101);
// mpu.write(SMPLRT_DIV, 0b00010011);
mpu.write(GYRO_CONFIG, 0b00011000);
// mpu.write(ACCL_CONFIG, 0b00000000);
}
public void reset()
{
mpu.write(PWR_MGMT_1, 0b00000000);
pitch = 0;
roll = 0;
yaw = 0;
}
public void update()
{
double cur_time = Timer.getFPGATimestamp();
double dt = cur_time - prev_time;
prev_time = cur_time;
double[] gRates = gyroRates();
roll += gRates[0] * dt;
pitch += gRates[1] * dt;
yaw += gRates[2] * dt;
}
public Rotation2d toRotation2d()
{
return new Rotation2d(-yaw);
}
public void calibrateGyro(int iter)
{
gyroOffsets = new double[3];
double x = 0, y = 0, z = 0;
for(int i = 0; i < iter; i++)
{
double[] rates = gyroRates();
x += rates[0];
y += rates[1];
z += rates[2];
}
gyroOffsets[0] = x/iter;
gyroOffsets[1] = y/iter;
gyroOffsets[2] = z/iter;
}
private double[] gyroRates()
{
return scaledRates(GYRO_ADDR, SCALE_GYRO);
}
private double[] scaledRates(int addr, double scale)
{
byte[] buffer = new byte[6];
mpu.read(addr, 6, buffer);
double dx = decode(buffer[0], buffer[1]) * scale - gyroOffsets[0];
double dy = decode(buffer[2], buffer[3]) * scale - gyroOffsets[1];
double dz = decode(buffer[4], buffer[5]) * scale - gyroOffsets[2];
return new double[] {dx, dy, dz};
}
private double decode(byte first, byte second)
{
return ((first << 8) | second);
}
}
|
