@Override
public void run() {
int retVal;
int failCount;
int[] tachoBegin = new int[CHANNELS];
long endTime, beginTime, timeDelta;
float[] degpersecAccum = new float[CHANNELS];
beginTime = System.currentTimeMillis();
Main:
while (!threadDie) {
Delay.msDelay(POLL_DELAY_MS);
getData(REG_ENCODERSREAD, buffer, 8);
// baseline the time after successful I2C transaction
endTime = System.currentTimeMillis();
timeDelta = endTime - beginTime;
beginTime = endTime;
// parse the buffer into the counts
synchronized (this) {
TachoCount[MOTOR_1] = EndianTools.decodeIntBE(buffer, 0);
TachoCount[MOTOR_2] = EndianTools.decodeIntBE(buffer, 4);
}
// Do velocity calcs (deg per sec)
for (int i = 0; i < CHANNELS; i++) {
synchronized (this) {
// rotate the index if needed
if (i == CHANNELS - 1) {
if (++sampleIndex >= samples[i].length) sampleIndex = 0;
}
// save a dps sample
samples[i][sampleIndex] =
(float) Math.abs(TachoCount[i] - tachoBegin[i]) / timeDelta * 250;
tachoBegin[i] = TachoCount[i];
}
// average the samples and the last result (degpersec)
for (int ii = 0; ii < samples[i].length; ii++) {
degpersecAccum[i] += samples[i][ii];
}
this.degpersec[i] = degpersecAccum[i] / (samples[i].length + 1);
degpersecAccum[i] = this.degpersec[i];
synchronized (this) {
ismoving[i] = (((int) degpersec[i]) != 0);
}
}
}
}
private int getEncoderValue(int channel) {
// use latest tachoMonitor value for the motor if available
if (tachoMonitorAlive()) {
return tachoMonitor.getTachoCount(channel);
}
// .. otherwise, query the controller to get it
getData(REGISTER_MAP[REG_IDX_ENCODER_CURRENT][channel], buf, 4);
return EndianTools.decodeIntBE(buf, 0);
}
@Override
public void fetchSample(float[] sample, int offset) {
getData(REG_ACCUMULATED_ANGLE, buf, 4);
sample[offset] = -EndianTools.decodeIntBE(buf, 0);
}