bodyPosition.c

Go to the documentation of this file.
00001 
00010 #include "bodyPosition.h"
00011 #include "isincos.h"
00012 
00013 //#include <stdio.h>
00014 
00015 
00016 void initBodyPositionInfo(bodyPosition_t *bodyPos, bodyInfo_t *bodyInfo) {
00017   int i;
00018   long diameter_small = 0;
00019   long diameter_large = 0;
00020 
00021 
00022   bodyPos->body = bodyInfo;
00023 
00024   for (i = 0; i < 2; ++i) {
00025     bodyPos->cnt_integer[i] = 0;
00026     bodyPos->cnt_decimal[i] = 0;
00027   }
00028   bodyPos->rotate_num = 0;
00029 
00030   // 車輪の直径が大きい方の車輪の cnt_per_m を用いる
00031   bodyPos->cnt_per_m = (bodyInfo->whl[WHL_RIGHT]->cnt_per_m >
00032                         bodyInfo->whl[WHL_LEFT]->cnt_per_m) ?
00033     bodyInfo->whl[WHL_RIGHT]->cnt_per_m : bodyInfo->whl[WHL_LEFT]->cnt_per_m;
00034 
00035   // mm/sec 速度の計算用
00036   bodyPos->cnt2mm_const = ((1 << 16) * 1000) / bodyPos->cnt_per_m;
00037 
00038   // straight = right + left を 2 で割らないための調整
00039   bodyPos->cnt_per_m <<= 1;
00040 
00041   // 回転パラメータ周り
00042   bodyPos->div16_cnt = 0;
00043 
00044   // !!! コンパイラの最適化で消えるかなぁ?
00045   // !!! 要確認
00046   if (WHL_CNT_PER_M_RIGHT < WHL_CNT_PER_M_LEFT) {
00047     bodyPos->div16_cnt_max =
00048       (long)(BODY_TREAD_MM * M_PI * WHL_CNT_PER_MM_RIGHT);
00049     diameter_small = (long)WHL_CNT_PER_M_RIGHT;
00050     diameter_large = (long)WHL_CNT_PER_M_LEFT;
00051   } else {
00052     bodyPos->div16_cnt_max =
00053       (long)(BODY_TREAD_MM * M_PI * WHL_CNT_PER_MM_LEFT);
00054     diameter_small = (long)WHL_CNT_PER_M_LEFT;
00055     diameter_large = (long)WHL_CNT_PER_M_RIGHT;
00056   }
00057   bodyPos->div16_cnt_max <<= 1;
00058   bodyPos->div16_cnt_mul = (long)((1 << DIV16_MUL_SHIFT)
00059                                   / bodyPos->div16_cnt_max);
00060 
00061   // 車輪径の補正についての値を計算
00062   if (diameter_small != diameter_large) {
00063     bodyPos->adjust_base = diameter_small;
00064 
00065     bodyPos->adjust_current_cnt = 0;
00066     bodyPos->adjust_max = 65536;
00067 
00068     // !!! 計算する
00069     if (diameter_large == (long)WHL_CNT_PER_M_RIGHT) {
00070       bodyPos->adjust_cnt =
00071         (long)(1.0 * WHL_CNT_PER_M_RIGHT / WHL_CNT_PER_M_LEFT * 65536);
00072     } else {
00073       bodyPos->adjust_cnt =
00074         (long)(1.0 * WHL_CNT_PER_M_LEFT / WHL_CNT_PER_M_RIGHT * 65536);
00075     }
00076     bodyPos->adjust_cnt -= 65536;
00077 
00078     while (bodyPos->adjust_cnt && ((bodyPos->adjust_cnt & 0x01) == 0x00)) {
00079       bodyPos->adjust_cnt >>= 1;
00080       bodyPos->adjust_max >>= 1;
00081     }
00082 #if 0
00083     fprintf(stderr, "left: %d, right: %d\n", WHL_CNT_PER_M_RIGHT, WHL_CNT_PER_M_LEFT);
00084     fprintf(stderr, "base: %d\n", bodyPos->adjust_base);
00085     fprintf(stderr, "adjust_cnt: %d(0x%x), %d(0x%x)\n",
00086             bodyPos->adjust_cnt, bodyPos->adjust_cnt,
00087             bodyPos->adjust_max, bodyPos->adjust_max);
00088 #endif
00089 
00090   } else {
00091     bodyPos->adjust_cnt = 0;    // 無効値
00092     bodyPos->adjust_base = 0;
00093   }
00094   bodyPos->adjust_current_cnt = 0;
00095   bodyPos->adjust_max = 65536;
00096 
00097   // 座標系を初期化
00098   initCoordinateInfo(&bodyPos->body_crd);
00099 
00100   bodyPos->straight_cnt_vel = 0;
00101   bodyPos->rotate_cnt_vel = 0;
00102 }
00103 
00104 
00105 // !!! 未実装
00106 static int adjustEncCnt(int cnt, wheelInfo_t *whl, bodyPosition_t *bodyPos) {
00107 
00108   long adjust_current_cnt;
00109   long adjust_max;
00110 
00111   if ((bodyPos->adjust_cnt == 0) || (bodyPos->adjust_base == whl->cnt_per_m)) {
00112     // 補正の必要なし
00113     return cnt;
00114   }
00115 
00116   // 大きな車輪径の方のカウント値を水増しする
00117   adjust_current_cnt = bodyPos->adjust_current_cnt + cnt;
00118   //fprintf(stderr, "current_cnt: %d\n", adjust_current_cnt);
00119   adjust_max = bodyPos->adjust_max;
00120   while (adjust_current_cnt > adjust_max) {
00121     adjust_current_cnt -= adjust_max;
00122     cnt += bodyPos->adjust_cnt;
00123     //fprintf(stderr, "adjust +\n");
00124   }
00125   while (adjust_current_cnt < -adjust_max) {
00126     adjust_current_cnt += adjust_max;
00127     cnt -= bodyPos->adjust_cnt;
00128     //fprintf(stderr, "adjust -\n");
00129   }
00130 
00131   bodyPos->adjust_current_cnt = adjust_current_cnt;
00132   return cnt;
00133 }
00134 
00135 
00136 /* 推定自己位置の計算 */
00137 void updateBodyPosition(bodyPosition_t *bodyPos, int right_cnt, int left_cnt) {
00138   enum { X = COORD_X, Y = COORD_Y };
00139   int i;
00140   int adjust_right_cnt, adjust_left_cnt;
00141   long cnt[2];
00142 
00143   /* エンコーダ値の重みを揃える */
00144   adjust_right_cnt = adjustEncCnt(right_cnt,
00145                                   bodyPos->body->whl[WHL_RIGHT], bodyPos);
00146   adjust_left_cnt = adjustEncCnt(left_cnt,
00147                                  bodyPos->body->whl[WHL_LEFT], bodyPos);
00148 
00149   // 並進、回転速度の計算
00150   bodyPos->straight_cnt_vel = adjust_right_cnt + adjust_left_cnt;
00151   bodyPos->rotate_cnt_vel = adjust_right_cnt - adjust_left_cnt;
00152   bodyPos->div16_cnt += bodyPos->rotate_cnt_vel;
00153 
00154   // div16 を計算
00155   if (bodyPos->div16_cnt < 0) {
00156     bodyPos->div16_cnt += bodyPos->div16_cnt_max;
00157     --(bodyPos->rotate_num);
00158   } else if (bodyPos->div16_cnt >= bodyPos->div16_cnt_max) {
00159     bodyPos->div16_cnt -= bodyPos->div16_cnt_max;
00160     ++(bodyPos->rotate_num);
00161   }
00162   bodyPos->body_crd.div16
00163     = (unsigned short)((bodyPos->div16_cnt *
00164                         bodyPos->div16_cnt_mul) >> (DIV16_MUL_SHIFT - 16));
00165 
00166   // X, Y 成分の移動量を計算
00167   cnt[X] = (bodyPos->straight_cnt_vel * icos(bodyPos->body_crd.div16))
00168     + bodyPos->cnt_decimal[X];
00169   cnt[Y] = (bodyPos->straight_cnt_vel * isin(bodyPos->body_crd.div16))
00170     + bodyPos->cnt_decimal[Y];
00171 
00172   for (i = 0; i < 2; ++i) {
00173     int add = cnt[i] >> ISINCOS_VALUE_SHIFT;
00174     bodyPos->cnt_integer[i] += add;
00175     bodyPos->cnt_decimal[i] = cnt[i] - (add << ISINCOS_VALUE_SHIFT);
00176 
00177     // m の移動量を変位を加算
00178     while (bodyPos->cnt_integer[i] >= bodyPos->cnt_per_m) {
00179       bodyPos->cnt_integer[i] -= bodyPos->cnt_per_m;
00180       ++(bodyPos->body_crd.m[i]);
00181     }
00182     while (bodyPos->cnt_integer[i] < 0) {
00183       bodyPos->cnt_integer[i] += bodyPos->cnt_per_m;
00184       --(bodyPos->body_crd.m[i]);
00185     }
00186 
00187     // km の移動量の変位を加算
00188     while (bodyPos->body_crd.m[i] >= 1000) {
00189       bodyPos->body_crd.m[i] -= 1000;
00190       ++(bodyPos->body_crd.km[i]);
00191     }
00192     while (bodyPos->body_crd.m[i] < 0) {
00193       bodyPos->body_crd.m[i] += 1000;
00194       --(bodyPos->body_crd.km[i]);
00195     }
00196 
00197     // mm の位置を計算
00198     bodyPos->body_crd.mm[i] =
00199       (1000 * bodyPos->cnt_integer[i]) / bodyPos->cnt_per_m;
00200   }
00201 }
00202 
00203 
00204 void updateCoordinatePosition(coordinateInfo_t *crd,
00205                               bodyPosition_t *bodyPos) {
00206   int i;
00207 
00208   for (i = 0; i < 2; ++i) {
00209     crd->km[i] = bodyPos->body_crd.km[i];
00210     crd->m[i] = bodyPos->body_crd.m[i];
00211     crd->mm[i] = bodyPos->body_crd.mm[i];
00212     //crd->mm256[i] = bodyPos->body_crd.mm256[i];
00213   }
00214   crd->div16 = bodyPos->body_crd.div16;
00215 }
00216 

Generated on Mon Apr 13 22:52:04 2009 by  doxygen 1.5.7.1