@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;
 }