/
GyroscopeHeading.java
53 lines (40 loc) · 1.55 KB
/
GyroscopeHeading.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
package com.tbdftc.sensorhandlers;
import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
import com.qualcomm.robotcore.hardware.GyroSensor;
import com.tbdftc.controlhandlers.RollingAverageCalculator;
public class GyroscopeHeading {
private static final int CLOCKWISE = -1;
private static final int COUNTERCLOCKWISE = 1;
private static final int VALUES_TO_AVERAGE = 8;
private ModernRoboticsI2cGyro gyroscope;
private RollingAverageCalculator rollingAverage;
private int virtualHeadingZero;
public GyroscopeHeading(GyroSensor aGyroscope) {
gyroscope = (ModernRoboticsI2cGyro) aGyroscope;
rollingAverage = new RollingAverageCalculator(VALUES_TO_AVERAGE);
virtualHeadingZero = 0;
calibrate();
}
private void calibrate() {
gyroscope.calibrate();
}
public double getAverageZAxisHeading() {
rollingAverage.addNumber(CLOCKWISE * (gyroscope.getIntegratedZValue() - virtualHeadingZero));
return rollingAverage.getAverage();
}
public int getZAxisHeading() {
return CLOCKWISE * (gyroscope.getIntegratedZValue() - virtualHeadingZero);
}
public int getZAxisRate() {
return gyroscope.rawZ();
}
private boolean isCalibrated() {
return !gyroscope.isCalibrating();
}
public void resetZAxisHeading() {
virtualHeadingZero = gyroscope.getIntegratedZValue();
}
public void resetZAxisHeadingPhysically() {
gyroscope.resetZAxisIntegrator();
}
}