bodyPosition.c

Go to the documentation of this file.

#include "bodyPosition.h"
#include "isincos.h"

//#include <stdio.h>


void initBodyPositionInfo(bodyPosition_t *bodyPos, bodyInfo_t *bodyInfo) {
  int i;
  long diameter_small = 0;
  long diameter_large = 0;


  bodyPos->body = bodyInfo;

  for (i = 0; i < 2; ++i) {
    bodyPos->cnt_integer[i] = 0;
    bodyPos->cnt_decimal[i] = 0;
  }
  bodyPos->rotate_num = 0;

  // 車輪の直径が大きい方の車輪の cnt_per_m を用いる
  bodyPos->cnt_per_m = (bodyInfo->whl[WHL_RIGHT]->cnt_per_m >
                        bodyInfo->whl[WHL_LEFT]->cnt_per_m) ?
    bodyInfo->whl[WHL_RIGHT]->cnt_per_m : bodyInfo->whl[WHL_LEFT]->cnt_per_m;

  // mm/sec 速度の計算用
  bodyPos->cnt2mm_const = ((1 << 16) * 1000) / bodyPos->cnt_per_m;

  // straight = right + left を 2 で割らないための調整
  bodyPos->cnt_per_m <<= 1;

  // 回転パラメータ周り
  bodyPos->div16_cnt = 0;

  // !!! コンパイラの最適化で消えるかなぁ？
  // !!! 要確認
  if (WHL_CNT_PER_M_RIGHT < WHL_CNT_PER_M_LEFT) {
    bodyPos->div16_cnt_max =
      (long)(BODY_TREAD_MM * M_PI * WHL_CNT_PER_MM_RIGHT);
    diameter_small = (long)WHL_CNT_PER_M_RIGHT;
    diameter_large = (long)WHL_CNT_PER_M_LEFT;
  } else {
    bodyPos->div16_cnt_max =
      (long)(BODY_TREAD_MM * M_PI * WHL_CNT_PER_MM_LEFT);
    diameter_small = (long)WHL_CNT_PER_M_LEFT;
    diameter_large = (long)WHL_CNT_PER_M_RIGHT;
  }
  bodyPos->div16_cnt_max <<= 1;
  bodyPos->div16_cnt_mul = (long)((1 << DIV16_MUL_SHIFT)
                                  / bodyPos->div16_cnt_max);

  // 車輪径の補正についての値を計算
  if (diameter_small != diameter_large) {
    bodyPos->adjust_base = diameter_small;

    bodyPos->adjust_current_cnt = 0;
    bodyPos->adjust_max = 65536;

    // !!! 計算する
    if (diameter_large == (long)WHL_CNT_PER_M_RIGHT) {
      bodyPos->adjust_cnt =
        (long)(1.0 * WHL_CNT_PER_M_RIGHT / WHL_CNT_PER_M_LEFT * 65536);
    } else {
      bodyPos->adjust_cnt =
        (long)(1.0 * WHL_CNT_PER_M_LEFT / WHL_CNT_PER_M_RIGHT * 65536);
    }
    bodyPos->adjust_cnt -= 65536;

    while (bodyPos->adjust_cnt && ((bodyPos->adjust_cnt & 0x01) == 0x00)) {
      bodyPos->adjust_cnt >>= 1;
      bodyPos->adjust_max >>= 1;
    }
#if 0
    fprintf(stderr, "left: %d, right: %d\n", WHL_CNT_PER_M_RIGHT, WHL_CNT_PER_M_LEFT);
    fprintf(stderr, "base: %d\n", bodyPos->adjust_base);
    fprintf(stderr, "adjust_cnt: %d(0x%x), %d(0x%x)\n",
            bodyPos->adjust_cnt, bodyPos->adjust_cnt,
            bodyPos->adjust_max, bodyPos->adjust_max);
#endif

  } else {
    bodyPos->adjust_cnt = 0;    // 無効値
    bodyPos->adjust_base = 0;
  }
  bodyPos->adjust_current_cnt = 0;
  bodyPos->adjust_max = 65536;

  // 座標系を初期化
  initCoordinateInfo(&bodyPos->body_crd);

  bodyPos->straight_cnt_vel = 0;
  bodyPos->rotate_cnt_vel = 0;
}


// !!! 未実装
static int adjustEncCnt(int cnt, wheelInfo_t *whl, bodyPosition_t *bodyPos) {

  long adjust_current_cnt;
  long adjust_max;

  if ((bodyPos->adjust_cnt == 0) || (bodyPos->adjust_base == whl->cnt_per_m)) {
    // 補正の必要なし
    return cnt;
  }

  // 大きな車輪径の方のカウント値を水増しする
  adjust_current_cnt = bodyPos->adjust_current_cnt + cnt;
  //fprintf(stderr, "current_cnt: %d\n", adjust_current_cnt);
  adjust_max = bodyPos->adjust_max;
  while (adjust_current_cnt > adjust_max) {
    adjust_current_cnt -= adjust_max;
    cnt += bodyPos->adjust_cnt;
    //fprintf(stderr, "adjust +\n");
  }
  while (adjust_current_cnt < -adjust_max) {
    adjust_current_cnt += adjust_max;
    cnt -= bodyPos->adjust_cnt;
    //fprintf(stderr, "adjust -\n");
  }

  bodyPos->adjust_current_cnt = adjust_current_cnt;
  return cnt;
}


/* 推定自己位置の計算 */
void updateBodyPosition(bodyPosition_t *bodyPos, int right_cnt, int left_cnt) {
  enum { X = COORD_X, Y = COORD_Y };
  int i;
  int adjust_right_cnt, adjust_left_cnt;
  long cnt[2];

  /* エンコーダ値の重みを揃える */
  adjust_right_cnt = adjustEncCnt(right_cnt,
                                  bodyPos->body->whl[WHL_RIGHT], bodyPos);
  adjust_left_cnt = adjustEncCnt(left_cnt,
                                 bodyPos->body->whl[WHL_LEFT], bodyPos);

  // 並進、回転速度の計算
  bodyPos->straight_cnt_vel = adjust_right_cnt + adjust_left_cnt;
  bodyPos->rotate_cnt_vel = adjust_right_cnt - adjust_left_cnt;
  bodyPos->div16_cnt += bodyPos->rotate_cnt_vel;

  // div16 を計算
  if (bodyPos->div16_cnt < 0) {
    bodyPos->div16_cnt += bodyPos->div16_cnt_max;
    --(bodyPos->rotate_num);
  } else if (bodyPos->div16_cnt >= bodyPos->div16_cnt_max) {
    bodyPos->div16_cnt -= bodyPos->div16_cnt_max;
    ++(bodyPos->rotate_num);
  }
  bodyPos->body_crd.div16
    = (unsigned short)((bodyPos->div16_cnt *
                        bodyPos->div16_cnt_mul) >> (DIV16_MUL_SHIFT - 16));

  // X, Y 成分の移動量を計算
  cnt[X] = (bodyPos->straight_cnt_vel * icos(bodyPos->body_crd.div16))
    + bodyPos->cnt_decimal[X];
  cnt[Y] = (bodyPos->straight_cnt_vel * isin(bodyPos->body_crd.div16))
    + bodyPos->cnt_decimal[Y];

  for (i = 0; i < 2; ++i) {
    int add = cnt[i] >> ISINCOS_VALUE_SHIFT;
    bodyPos->cnt_integer[i] += add;
    bodyPos->cnt_decimal[i] = cnt[i] - (add << ISINCOS_VALUE_SHIFT);

    // m の移動量を変位を加算
    while (bodyPos->cnt_integer[i] >= bodyPos->cnt_per_m) {
      bodyPos->cnt_integer[i] -= bodyPos->cnt_per_m;
      ++(bodyPos->body_crd.m[i]);
    }
    while (bodyPos->cnt_integer[i] < 0) {
      bodyPos->cnt_integer[i] += bodyPos->cnt_per_m;
      --(bodyPos->body_crd.m[i]);
    }

    // km の移動量の変位を加算
    while (bodyPos->body_crd.m[i] >= 1000) {
      bodyPos->body_crd.m[i] -= 1000;
      ++(bodyPos->body_crd.km[i]);
    }
    while (bodyPos->body_crd.m[i] < 0) {
      bodyPos->body_crd.m[i] += 1000;
      --(bodyPos->body_crd.km[i]);
    }

    // mm の位置を計算
    bodyPos->body_crd.mm[i] =
      (1000 * bodyPos->cnt_integer[i]) / bodyPos->cnt_per_m;
  }
}


void updateCoordinatePosition(coordinateInfo_t *crd,
                              bodyPosition_t *bodyPos) {
  int i;

  for (i = 0; i < 2; ++i) {
    crd->km[i] = bodyPos->body_crd.km[i];
    crd->m[i] = bodyPos->body_crd.m[i];
    crd->mm[i] = bodyPos->body_crd.mm[i];
    //crd->mm256[i] = bodyPos->body_crd.mm256[i];
  }
  crd->div16 = bodyPos->body_crd.div16;
}