@Override
public void update(Object param) {
int reading;
try {
reading = mGyroInterface.getReading();
} catch (GyroInterface.GyroException e) {
System.out.println("Gyro read failed: " + e.getMessage());
return;
}
GyroInterface.StatusFlag status = GyroInterface.extractStatus(reading);
List<GyroInterface.ErrorFlag> errors = GyroInterface.extractErrors(reading);
if (GyroInterface.StatusFlag.VALID_DATA != status || !errors.isEmpty()) {
System.out.println(
"Gyro read failed. Status: " + status + ". Errors: " + Util.joinStrings(", ", errors));
return;
}
if (mRemainingStartupCycles > 0) {
mRemainingStartupCycles--;
return;
}
if (mVolatileShouldReZero) {
mVolatileShouldReZero = false;
mVolatileHasData = false;
mIsZerod = false;
}
double unbiasedAngleRate = GyroInterface.extractAngleRate(reading);
mZeroRateSamples[mZeroRateSampleIndex] = unbiasedAngleRate;
mZeroRateSampleIndex++;
if (mZeroRateSampleIndex >= K_ZEROING_SAMPLES) {
mZeroRateSampleIndex = 0;
mHasEnoughZeroingSamples = true;
}
if (!mIsZerod) {
if (!mHasEnoughZeroingSamples) {
return;
}
mZeroBias = 0;
for (Double sample : mZeroRateSamples) {
mZeroBias += sample / K_ZEROING_SAMPLES;
}
mAngle = 0;
mVolatileAngle = 0;
GyroThread.this.reset();
mIsZerod = true;
return;
}
double now = edu.wpi.first.wpilibj.Timer.getFPGATimestamp();
double timeElapsed = mLastTime == 0 ? 1.0 / K_READING_RATE : now - mLastTime;
mLastTime = now;
mVolatileRate = unbiasedAngleRate - mZeroBias;
mAngle += mVolatileRate * timeElapsed;
mVolatileAngle = mAngle;
mVolatileHasData = true;
}
@Override
public void run() {
boolean initialized = false;
while (!initialized) {
try {
mGyroInterface.initializeGyro();
initialized = true;
} catch (GyroInterface.GyroException e) {
System.out.println("Gyro failed to initialize: " + e.getMessage());
}
if (!initialized) {
try {
Thread.sleep(1000);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
}
System.out.println(
"gyo initialized, part ID: 0x" + Integer.toHexString(mGyroInterface.readPartId()));
synchronized (mNotifier) {
mNotifier.startPeriodic(1.0 / K_READING_RATE);
}
}
