@Override
public void fetchSample(float[] sample, int offset) {
getData(BASE_ACCEL, buf, 0, 6);
sample[offset + 0] = EndianTools.decodeShortLE(buf, OFF_X_ACCEL) * TO_SI;
sample[offset + 1] = EndianTools.decodeShortLE(buf, OFF_Y_ACCEL) * TO_SI;
sample[offset + 2] = -EndianTools.decodeShortLE(buf, OFF_Z_ACCEL) * TO_SI;
}
@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 void rotate(int channel, int value, int cmd) {
byte workingByte = 0;
motorParams[MOTPARAM_ROTATE][channel] = MOTPARAM_OP_TRUE;
if (cmd == CMD_ROTATE) {
// set the target based current + degrees passed
value = getEncoderValue(channel) + value * 4;
} else if (cmd == CMD_ROTATE_TO) {
value *= 4;
} else return;
this.limitangle[channel] = Math.round(value * .25f);
// set the encoder position
EndianTools.encodeIntBE(value, buf, 0);
sendData(REGISTER_MAP[REG_IDX_ENCODER_TARGET][channel], buf, 4);
motorState[channel] = STATE_ROTATE_TO;
// set the mode
setMode(channel, false);
// set the power to turn on the motor. Ensure it is positive (do not adjust for BACKWARDS)
workingByte = (byte) motorParams[MOTPARAM_POWER][channel];
sendData(REGISTER_MAP[REG_IDX_POWER][channel], workingByte);
// set up the RegulatedMotorListener notifier and waitToComplete notifier for the rotate
bUSYMonitors[channel] = new BUSYMonitor(channel);
bUSYMonitors[channel].start();
return;
}
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);
}
@Override
public void fetchSample(float[] sample, int offset) {
getData(REG_SPEED, buf, 2);
sample[offset] = -EndianTools.decodeShortBE(buf, 0) / 60;
}