AMiRo-OS

   * @brief   Wheel diameter in m.

  float wheel_diameter;

#define QEI_POLL_INTERVAL_MS          100

  apalQEICount_t qei_range[2];

  apalQEICount_t qei_increments[2][2];

  apalQEICount_t qei_increments_diff[2];

  _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result);

  chprintf(stream, "both wheels full speed forward...\n");

  status = APAL_STATUS_SUCCESS;

  status |= a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_forward, APAL_PWM_WIDTH_MAX);

  status |= a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_forward, APAL_PWM_WIDTH_MAX);

  aosThdMSleep(100);

  qei_count[WHEEL_LEFT][0] = 0;

  qei_count[WHEEL_LEFT][1] = 0;

  qei_count[WHEEL_RIGHT][0] = 0;

  qei_count[WHEEL_RIGHT][1] = 0;

  status |= apalQEIGetRange(((ut_a3906data_t*)ut->data)->qei.left, &qei_range[WHEEL_LEFT]);

  status |= apalQEIGetRange(((ut_a3906data_t*)ut->data)->qei.right, &qei_range[WHEEL_RIGHT]);

  timeout_counter = 0;

  qei_increments[WHEEL_LEFT][0] = 0;

  qei_increments[WHEEL_LEFT][1] = 0;

  qei_increments[WHEEL_RIGHT][0] = 0;

  qei_increments[WHEEL_RIGHT][1] = 0;

  stable_counter = 0;

  do {

    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][0]);

    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][0]);

    aosThdMSleep(QEI_POLL_INTERVAL_MS);

    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][1]);

    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][1]);

    timeout_counter += QEI_POLL_INTERVAL_MS;

    qei_increments[WHEEL_LEFT][0] = qei_increments[WHEEL_LEFT][1];

    qei_increments[WHEEL_LEFT][1] = (qei_count[WHEEL_LEFT][1] > qei_count[WHEEL_LEFT][0]) ? (qei_count[WHEEL_LEFT][1] - qei_count[WHEEL_LEFT][0]) : (qei_count[WHEEL_LEFT][1] + (qei_range[WHEEL_LEFT] - qei_count[WHEEL_LEFT][0]));

    qei_increments[WHEEL_RIGHT][0] = qei_increments[WHEEL_RIGHT][1];

    qei_increments[WHEEL_RIGHT][1] = (qei_count[WHEEL_RIGHT][1] > qei_count[WHEEL_RIGHT][0]) ? (qei_count[WHEEL_RIGHT][1] - qei_count[WHEEL_RIGHT][0]) : (qei_count[WHEEL_RIGHT][1] + (qei_range[WHEEL_RIGHT] - qei_count[WHEEL_RIGHT][0]));

    qei_increments_diff[WHEEL_LEFT] = abs((int32_t)(qei_increments[WHEEL_LEFT][0]) - (int32_t)qei_increments[WHEEL_LEFT][1]);

    qei_increments_diff[WHEEL_RIGHT] = abs((int32_t)(qei_increments[WHEEL_RIGHT][0]) - (int32_t)qei_increments[WHEEL_RIGHT][1]);

    stable_counter = (qei_increments_diff[WHEEL_LEFT] <= QEI_DIFF_THRESHOLD && qei_increments_diff[WHEEL_RIGHT] < QEI_DIFF_THRESHOLD) ? stable_counter+1 : 0;

  } while((stable_counter * QEI_POLL_INTERVAL_MS < MILLISECONDS_PER_SECOND) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= ((ut_a3906data_t*)ut->data)->timeout));

  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > ((ut_a3906data_t*)ut->data)->timeout) ? APAL_STATUS_TIMEOUT : 0x00;

  if (status == APAL_STATUS_SUCCESS) {

    aosUtPassed(stream, &result);

    const float tps[2] = {qei_increments[WHEEL_LEFT][1] * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS),

                          qei_increments[WHEEL_RIGHT][1] * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS)};

    const float rpm[2] = {tps[WHEEL_LEFT] * SECONDS_PER_MINUTE / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution),

                          tps[WHEEL_RIGHT] * SECONDS_PER_MINUTE / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution)};

    const float velocity[2] = {tps[WHEEL_LEFT] / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution) * ((ut_a3906data_t*)ut->data)->wheel_diameter * acos(-1),

                               tps[WHEEL_RIGHT] / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution) * ((ut_a3906data_t*)ut->data)->wheel_diameter * acos(-1)};

    chprintf(stream, "left wheel:\n");

    chprintf(stream, "\t%f tps\n", tps[WHEEL_LEFT]);

    chprintf(stream, "\t%f RPM\n", rpm[WHEEL_LEFT]);

    chprintf(stream, "\t%f m/s\n", velocity[WHEEL_LEFT]);

    chprintf(stream, "right wheel:\n");

    chprintf(stream, "\t%f tps\n", tps[WHEEL_RIGHT]);

    chprintf(stream, "\t%f RPM\n", rpm[WHEEL_RIGHT]);

    chprintf(stream, "\t%f m/s\n", velocity[WHEEL_RIGHT]);

    chprintf(stream, "\n");

  } else {

    aosUtFailedMsg(stream, &result, "0x%08X\n", status);

  }