velocityInfo.c

Go to the documentation of this file.

#include "velocityInfo.h"
#include "movingAverage.h"

enum { SIZE_SHIFT = 4 };
static movingAverage_t Straight, Rotate;
static int Straight_buff[1 << SIZE_SHIFT];
static int Rotate_buff[1 << SIZE_SHIFT];


void initVelocityInfo(velocityInfo_t *vel) {
  vel->straight_mm_sec_vel = 0;
  vel->rotate_div16_sec_vel = 0;
  initMovingAverage(&Straight, Straight_buff, SIZE_SHIFT);
  initMovingAverage(&Rotate, Rotate_buff, SIZE_SHIFT);
}


static int change_bodyCnt2vel(int straight_cnt_vel, bodyPosition_t *bodyPos) {

  int straight_mm_sec_vel = (bodyPos->cnt2mm_const * straight_cnt_vel) >> 16;
  return straight_mm_sec_vel;
}


static int change_bodyCnt2div16(int rotate_cnt_vel,
                                bodyPosition_t *bodyPos) {

  int rotate_div16_sec_vel =
    (bodyPos->div16_cnt_mul * rotate_cnt_vel) >> (DIV16_MUL_SHIFT - 16);
  return rotate_div16_sec_vel;
}


void updateVelocityInfo(velocityInfo_t *velInfo,
                        bodyPosition_t *bodyPos) {

  setMovingAverage(&Straight,
                   change_bodyCnt2vel(1000 * bodyPos->straight_cnt_vel,
                                      bodyPos));
  setMovingAverage(&Rotate,
                   change_bodyCnt2div16(1000 * bodyPos->rotate_cnt_vel,
                                        bodyPos));

  velInfo->straight_mm_sec_vel = getAverageValue(&Straight) >> 1;
  velInfo->rotate_div16_sec_vel = getAverageValue(&Rotate);
}