bodyPosition.c
Go to the documentation of this file.00001
00010 #include "bodyPosition.h"
00011 #include "isincos.h"
00012
00013
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
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
00036 bodyPos->cnt2mm_const = ((1 << 16) * 1000) / bodyPos->cnt_per_m;
00037
00038
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
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
00124 }
00125 while (adjust_current_cnt < -adjust_max) {
00126 adjust_current_cnt += adjust_max;
00127 cnt -= bodyPos->adjust_cnt;
00128
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
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
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
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
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
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
00213 }
00214 crd->div16 = bodyPos->body_crd.div16;
00215 }
00216