00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <math.h>
00022 #include <stdio.h>
00023 #include <string.h>
00024 #include <unistd.h>
00025
00026 #include <fcntl.h>
00027 #include <sys/stat.h>
00028 #include <sys/time.h>
00029 #include <sys/types.h>
00030 #include <time.h>
00031
00032 #ifdef HAVE_CONFIG_H
00033 #include <config.h>
00034 #endif // HAVE_CONFIG_H
00035
00036
00037 #include <shvel-param.h>
00038
00039
00040 #include <param.h>
00041 #include <control.h>
00042 #include <adinput.h>
00043 #include <utility.h>
00044 #include <yprintf.h>
00045 #include <communication.h>
00046 #include <serial.h>
00047 #include <ssm_spur_handler.h>
00048
00049
00050 #include <ypspur.h>
00051 #include <cartesian2d.h>
00052
00053 double g_interval;
00054 double g_offset;
00055 int g_offset_point;
00056 double g_estimated_delay = 0;
00057
00058 CSptr g_GL;
00059 CSptr g_SP;
00060 CSptr g_LC;
00061 CSptr g_BS;
00062 CSptr g_FS;
00063 CSptr g_BL;
00064
00065 Odometry g_odometry;
00066 ErrorState g_error_state;
00067
00068
00069 void init_coordinate_systems(void)
00070 {
00071
00072 g_BS = CS_add(0, 0, 0, 0);
00073 g_GL = CS_add(g_BS, 0, 0, 0);
00074 g_LC = CS_add(g_GL, 0, 0, 0);
00075 g_FS = CS_add(g_BS, 0, 0, 0);
00076 g_BL = CS_add(g_BS, 0, 0, 0);
00077 g_SP = CS_add(g_BS, 0, 0, 0);
00078 }
00079
00080 void init_odometry(void)
00081 {
00082 int i;
00083 g_odometry.x = 0;
00084 g_odometry.y = 0;
00085 g_odometry.theta = 0;
00086 for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
00087 {
00088 g_odometry.enc[i] = 0;
00089 g_odometry.enc_init[i] = 0;
00090 g_odometry.wang[i] = 0;
00091 g_odometry.wtorque[i] = 0;
00092 g_odometry.wvel[i] = 0;
00093 g_error_state.state[i] = 0;
00094 g_error_state.time[i] = 0;
00095 }
00096 g_odometry.v = 0;
00097 g_odometry.w = 0;
00098 g_odometry.time = 0;
00099 g_offset_point = 0;
00100 }
00101
00102 CSptr get_cs_pointer(YPSpur_cs cs)
00103 {
00104 switch (cs)
00105 {
00106 case CS_FS:
00107 return g_FS;
00108 break;
00109 case CS_LC:
00110 return g_LC;
00111 break;
00112 case CS_GL:
00113 return g_GL;
00114 break;
00115 case CS_SP:
00116 return g_SP;
00117 break;
00118 case CS_BS:
00119 return g_BS;
00120 break;
00121 case CS_BL:
00122 return g_BL;
00123 break;
00124 default:
00125 return NULL;
00126 break;
00127 }
00128 return NULL;
00129 }
00130
00131 void cstrans_xy(YPSpur_cs src, YPSpur_cs dest, double *x, double *y, double *theta)
00132 {
00133 if (src == dest)
00134 return;
00135 CS_recursive_trans(get_cs_pointer(dest), get_cs_pointer(src), x, y, theta);
00136 }
00137
00138 void set_cs(YPSpur_cs cs, double x, double y, double theta)
00139 {
00140 CS_set(get_cs_pointer(cs), x, y, theta);
00141 }
00142
00143
00144 void odometry(OdometryPtr xp, short *cnt, short *pwm, double dt, double time)
00145 {
00146 double v, w;
00147 double wvel[YP_PARAM_MAX_MOTOR_NUM], mvel[YP_PARAM_MAX_MOTOR_NUM];
00148 double mtorque[YP_PARAM_MAX_MOTOR_NUM], wtorque[YP_PARAM_MAX_MOTOR_NUM];
00149 double volt[YP_PARAM_MAX_MOTOR_NUM], vc[YP_PARAM_MAX_MOTOR_NUM];
00150 double torque_trans, torque_angular;
00151 Parameters *param;
00152 param = get_param_ptr();
00153
00154 int i;
00155 for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
00156 {
00157 if (!param->motor_enable[i])
00158 continue;
00159
00160 short cnt_diff;
00161 if (param->device_version > 8)
00162 {
00163 cnt_diff = (short)cnt[i] - (short)xp->enc[i];
00164 xp->enc[i] = cnt[i];
00165 if (!xp->enc_init[i])
00166 {
00167 cnt_diff = 0;
00168 if (cnt[i] != 0)
00169 {
00170 xp->enc_init[i] = 1;
00171 }
00172 }
00173 }
00174 else
00175 {
00176 cnt_diff = cnt[i];
00177 xp->enc[i] += cnt_diff;
00178 }
00179
00180
00181 mvel[i] = 2.0 * M_PI *
00182 ((double)cnt_diff) * pow(2, p(YP_PARAM_ENCODER_DIV, i)) /
00183 (p(YP_PARAM_COUNT_REV, i) * dt);
00184 wvel[i] = mvel[i] / p(YP_PARAM_GEAR, i);
00185
00186
00187 volt[i] = (double)pwm[i] * p(YP_PARAM_VOLT, i) / (p(YP_PARAM_PWM_MAX, i) * (dt / p(YP_PARAM_CYCLE, i)));
00188 vc[i] = (p(YP_PARAM_MOTOR_VC, i) / 60) * 2 * M_PI;
00189
00190 mtorque[i] = (p(YP_PARAM_MOTOR_TC, i) * (volt[i] - mvel[i] / vc[i])) /
00191 (p(YP_PARAM_MOTOR_R, i) * p(YP_PARAM_ENCODER_DENOMINATOR, i));
00192
00193 if (wvel[i] > 0)
00194 {
00195 mtorque[i] -= p(YP_PARAM_TORQUE_NEWTON, i) + p(YP_PARAM_TORQUE_VISCOS, i) * fabs(mvel[i]);
00196 }
00197 else if (wvel[i] < 0)
00198 {
00199 mtorque[i] += p(YP_PARAM_TORQUE_NEWTON_NEG, i) + p(YP_PARAM_TORQUE_VISCOS_NEG, i) * fabs(mvel[i]);
00200 }
00201 wtorque[i] = mtorque[i] * p(YP_PARAM_GEAR, i);
00202 }
00203
00204
00205 v = p(YP_PARAM_RADIUS, MOTOR_RIGHT) * wvel[MOTOR_RIGHT] / 2.0 + p(YP_PARAM_RADIUS, MOTOR_LEFT) * wvel[MOTOR_LEFT] / 2.0;
00206 w = +p(YP_PARAM_RADIUS, MOTOR_RIGHT) * wvel[MOTOR_RIGHT] / p(YP_PARAM_TREAD, 0) - p(YP_PARAM_RADIUS, MOTOR_LEFT) * wvel[MOTOR_LEFT] / p(YP_PARAM_TREAD, 0);
00207
00208 torque_trans = wtorque[MOTOR_RIGHT] / p(YP_PARAM_RADIUS, MOTOR_RIGHT) + wtorque[MOTOR_LEFT] / p(YP_PARAM_RADIUS, MOTOR_LEFT);
00209 torque_angular = (+wtorque[MOTOR_RIGHT] / p(YP_PARAM_RADIUS, MOTOR_RIGHT) - wtorque[MOTOR_LEFT] / p(YP_PARAM_RADIUS, MOTOR_LEFT)) * p(YP_PARAM_TREAD, 0) / 2;
00210
00211
00212 xp->x = xp->x + v * cos(xp->theta) * dt;
00213 xp->y = xp->y + v * sin(xp->theta) * dt;
00214 xp->theta = xp->theta + w * dt;
00215 xp->time = time;
00216 if (option(OPTION_SHOW_TIMESTAMP))
00217 {
00218 static int cnt = 0;
00219 if (++cnt % 100 == 0)
00220 printf("now - stamp: %0.3f ms\n", (get_time() - time) * 1000.0);
00221 }
00222 xp->v = v;
00223 xp->w = w;
00224 for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
00225 {
00226 if (!param->motor_enable[i])
00227 continue;
00228 xp->wvel[i] = wvel[i];
00229 xp->wang[i] = xp->wang[i] + xp->wvel[i] * dt;
00230 xp->wtorque[i] = wtorque[i];
00231 }
00232 xp->torque_trans = torque_trans;
00233 xp->torque_angular = torque_angular;
00234
00235
00236
00237
00238
00239
00240 CS_set(g_FS, xp->x, xp->y, xp->theta);
00241
00242
00243 param_calc();
00244 }
00245
00246
00247 void process_int(
00248 OdometryPtr xp, ErrorStatePtr err, int param_id, int id, int value, double receive_time)
00249 {
00250 Parameters *param;
00251 param = get_param_ptr();
00252
00253 if (!param->motor_enable[id])
00254 return;
00255
00256 switch (param_id)
00257 {
00258 case INT_enc_index_rise:
00259 case INT_enc_index_fall:
00260 {
00261
00262 const unsigned short enc_div =
00263 ((unsigned int)xp->enc[id] << ((int)p(YP_PARAM_ENCODER_DIV, id))) & 0xFFFF;
00264 const short enc_diff = (short)enc_div - (short)value;
00265 const double ang_diff =
00266 enc_diff * 2.0 * M_PI / (p(YP_PARAM_COUNT_REV, id) * p(YP_PARAM_GEAR, id));
00267
00268 const double index_ratio = p(YP_PARAM_INDEX_GEAR, id) / p(YP_PARAM_GEAR, id);
00269 double ref_ang;
00270 if (param_id == INT_enc_index_rise)
00271 {
00272 if (!isset_p(YP_PARAM_INDEX_RISE_ANGLE, id))
00273 break;
00274 ref_ang = p(YP_PARAM_INDEX_RISE_ANGLE, id);
00275 }
00276 else
00277 {
00278 if (!isset_p(YP_PARAM_INDEX_FALL_ANGLE, id))
00279 break;
00280 ref_ang = p(YP_PARAM_INDEX_FALL_ANGLE, id);
00281 }
00282
00283 ref_ang *= index_ratio;
00284
00285 xp->wang[id] = round((xp->wang[id] - ref_ang - ang_diff) / (2.0 * M_PI * index_ratio)) *
00286 2.0 * M_PI * index_ratio +
00287 ref_ang + ang_diff;
00288 break;
00289 }
00290 case INT_error_state:
00291 {
00292 err->state[id] = value;
00293 err->time[id] = receive_time;
00294 if (value != ERROR_NONE)
00295 yprintf(OUTPUT_LV_ERROR, "Error: The driver of motor_id %d returned ", id);
00296 if (value & ERROR_LOW_VOLTAGE)
00297 yprintf(OUTPUT_LV_ERROR, "ERROR_LOW_VOLTAGE ");
00298 if (value & ERROR_HALL_SEQ)
00299 yprintf(OUTPUT_LV_ERROR, "ERROR_HALL_SEQ ");
00300 if (value & ERROR_HALL_ENC)
00301 yprintf(OUTPUT_LV_ERROR, "ERROR_HALL_ENC ");
00302 if (value & ERROR_WATCHDOG)
00303 yprintf(OUTPUT_LV_ERROR, "ERROR_WATCHDOG ");
00304
00305 if (value != ERROR_NONE)
00306 yprintf(OUTPUT_LV_ERROR, "\n");
00307 break;
00308 }
00309 default:
00310 yprintf(OUTPUT_LV_ERROR, "Error: Unknown interrput data (%d, %d, %d)\n", param_id, id, value);
00311 break;
00312 }
00313 }
00314
00315
00316 void cstrans_odometry(YPSpur_cs cs, OdometryPtr dst_odm)
00317 {
00318 double x, y, theta;
00319 x = g_odometry.x;
00320 y = g_odometry.y;
00321 theta = g_odometry.theta;
00322
00323 CS_recursive_trans(get_cs_pointer(cs), g_BS, &x, &y, &theta);
00324
00325 dst_odm->x = x;
00326 dst_odm->y = y;
00327 dst_odm->theta = theta;
00328 dst_odm->time = g_odometry.time;
00329 }
00330
00331
00332 OdometryPtr get_odometry_ptr()
00333 {
00334 return &g_odometry;
00335 }
00336
00337 ErrorStatePtr get_error_state_ptr()
00338 {
00339 return &g_error_state;
00340 }
00341
00346 double time_estimate(int readnum)
00347 {
00348 return g_offset + g_interval * (double)(readnum - g_offset_point) + g_estimated_delay;
00349 }
00350
00357 double time_synchronize(double receive_time, int readnum, int wp)
00358 {
00359 static double prev_time = 0.0;
00360 static int prev_readnum = 0;
00361 double measured_time;
00362
00363
00364 if (SER_BAUDRATE != 0)
00365 {
00366 measured_time = receive_time - (wp / (SER_BAUDRATE / 8.0));
00367 }
00368 else
00369 {
00370 measured_time = receive_time;
00371 }
00372
00373
00374 measured_time -= 0.0005;
00375
00376 if (g_offset_point <= 0 || fabs(g_offset - measured_time) > 0.5)
00377 {
00378
00379 g_offset = measured_time;
00380 g_interval = SER_INTERVAL;
00381 g_offset_point = readnum;
00382 prev_time = measured_time;
00383 }
00384
00385
00386 g_offset += g_interval * (readnum - g_offset_point);
00387 g_offset_point = readnum;
00388
00389 const double dt = measured_time - prev_time;
00390 double error = measured_time - g_offset;
00391
00392 const int lost = lround(error / g_interval);
00393 if (lost != 0)
00394 {
00395 if (option(OPTION_SHOW_TIMESTAMP))
00396 printf("%d packet(s) might be lost!\n", lost);
00397 error -= lost * g_interval;
00398 g_offset_point -= lost;
00399 }
00400
00401 static double error_integ = 0;
00402 error_integ += error * dt;
00403
00404 if (error < g_estimated_delay && lost == 0)
00405 {
00406 if (option(OPTION_SHOW_TIMESTAMP))
00407 printf("[update min_delay] delay: %0.3f\n",
00408 error * 1000.0);
00409 g_estimated_delay = g_estimated_delay * 0.5 + error * 0.5;
00410 }
00411 g_estimated_delay *= (1.0 - dt / 120.0);
00412
00413 g_offset += error * 0.2 + error_integ * 0.1;
00414
00415
00416 g_interval =
00417 0.99 * g_interval +
00418 0.01 * ((measured_time - prev_time) / (double)(readnum - prev_readnum));
00419
00420 static int cnt_show_timestamp = 0;
00421 if (option(OPTION_SHOW_TIMESTAMP) && ++cnt_show_timestamp % 100 == 0)
00422 printf("epoch: %0.8f, interval: %0.3f, delay: %0.3f, min_delay: %0.3f\n",
00423 g_offset, g_interval * 1000.0, error * 1000.0, g_estimated_delay * 1000.0);
00424
00425 prev_time = measured_time;
00426 prev_readnum = readnum;
00427
00428 return g_offset;
00429 }
00430
00431
00432 int odometry_receive(char *buf, int len, double receive_time, void *data)
00433 {
00434 static int com_wp = 0;
00435 static int receive_count = 0;
00436 static char com_buf[128];
00437 static enum
00438 {
00439 ISOCHRONOUS = 0,
00440 INTERRUPT
00441 } mode = 0;
00442
00443 int i;
00444 int odometry_updated;
00445 int readdata_num;
00446 int decoded_len = 0;
00447 int decoded_len_req;
00448 int readdata_len = 0;
00449
00450
00451 Odometry odm_log[100];
00452 Short_2Char cnt1_log[100];
00453 Short_2Char cnt2_log[100];
00454 Short_2Char pwm1_log[100];
00455 Short_2Char pwm2_log[100];
00456 int ad_log[100][8];
00457 Parameters *param;
00458
00459 param = get_param_ptr();
00460
00461 decoded_len_req =
00462 (+get_ad_num()
00463 + get_dio_num()
00464 + param->num_motor_enable * 2
00465 ) *
00466 2 ;
00467 readdata_num = 0;
00468 odometry_updated = 0;
00469
00470 for (i = 0; i < len; i++)
00471 {
00472 if (buf[i] == COMMUNICATION_START_BYTE)
00473 {
00474 com_wp = 0;
00475 mode = ISOCHRONOUS;
00476 }
00477 else if (buf[i] == COMMUNICATION_INT_BYTE)
00478 {
00479 com_wp = 0;
00480 mode = INTERRUPT;
00481 }
00482 else if (buf[i] == COMMUNICATION_END_BYTE)
00483 {
00484 unsigned char data[48];
00485
00486
00487 decoded_len = decode((unsigned char *)com_buf, com_wp, (unsigned char *)data, 48);
00488 if ((mode == ISOCHRONOUS && decoded_len != decoded_len_req) ||
00489 (mode == INTERRUPT && decoded_len != 6))
00490 {
00491 com_buf[com_wp] = '\0';
00492 yprintf(OUTPUT_LV_WARNING, "Warn: Broken packet '%s' (%d bytes) received. (%d bytes expected)\n", com_buf, decoded_len, decoded_len_req);
00493 com_wp = 0;
00494 continue;
00495 }
00496
00497 switch (mode)
00498 {
00499 case ISOCHRONOUS:
00500 {
00501 short cnt[YP_PARAM_MAX_MOTOR_NUM], pwm[YP_PARAM_MAX_MOTOR_NUM];
00502 int i, p = 0;
00503 for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
00504 {
00505 if (!param->motor_enable[i])
00506 continue;
00507 Short_2Char val;
00508 val.byte[1] = data[p++];
00509 val.byte[0] = data[p++];
00510 cnt[i] = val.integer;
00511 }
00512 for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
00513 {
00514 if (!param->motor_enable[i])
00515 continue;
00516 Short_2Char val;
00517 val.byte[1] = data[p++];
00518 val.byte[0] = data[p++];
00519 pwm[i] = val.integer;
00520 }
00521
00522 process_addata(&data[p], decoded_len - p);
00523
00524 cnt1_log[readdata_num].integer = cnt[0];
00525 cnt2_log[readdata_num].integer = cnt[1];
00526 pwm1_log[readdata_num].integer = pwm[0];
00527 pwm2_log[readdata_num].integer = pwm[1];
00528 memcpy(ad_log[readdata_num], get_addataptr(), sizeof(int) * 8);
00529 readdata_num++;
00530
00531 if (state(YP_STATE_MOTOR) && state(YP_STATE_VELOCITY) && state(YP_STATE_BODY))
00532 {
00533 odometry(&g_odometry, cnt, pwm, g_interval, time_estimate(receive_count + readdata_num));
00534 odm_log[odometry_updated] = g_odometry;
00535 odometry_updated++;
00536 }
00537
00538 if (option(OPTION_SHOW_ODOMETRY))
00539 printf("%f %f %f\n", g_odometry.x, g_odometry.y, g_odometry.theta);
00540 }
00541 break;
00542 case INTERRUPT:
00543 {
00544 char param, id;
00545 Int_4Char value;
00546
00547 param = data[0];
00548 id = data[1];
00549 value.byte[3] = data[2];
00550 value.byte[2] = data[3];
00551 value.byte[1] = data[4];
00552 value.byte[0] = data[5];
00553
00554 process_int(&g_odometry, &g_error_state, param, id, value.integer, receive_time);
00555 }
00556 break;
00557 }
00558 readdata_len = com_wp;
00559 com_wp = 0;
00560 }
00561 else
00562 {
00563 com_buf[com_wp] = buf[i];
00564 com_wp++;
00565 if (com_wp >= 128)
00566 {
00567 com_wp = 128 - 1;
00568 yprintf(OUTPUT_LV_WARNING, "Warn: Read buffer overrun.\n");
00569 }
00570 }
00571 }
00572
00573 if (readdata_num > 0)
00574 {
00575 receive_count += readdata_num;
00576 if (com_wp == 0)
00577 time_synchronize(receive_time, receive_count, com_wp);
00578 }
00579
00580 write_ypspurSSM(odometry_updated, receive_count, odm_log, readdata_num, cnt1_log, cnt2_log, pwm1_log, pwm2_log,
00581 ad_log);
00582 return 1;
00583 }
00584
00585 int odometry_receive_loop(void)
00586 {
00587 int ret;
00588 Parameters *param;
00589 param = get_param_ptr();
00590
00591 g_interval = SER_INTERVAL;
00592 while (1)
00593 {
00594 ret = serial_recieve(odometry_receive, NULL);
00595 if (param->parameter_applying)
00596 {
00597 yprintf(OUTPUT_LV_INFO, "Restarting odometry receive loop.\n");
00598 continue;
00599 }
00600 break;
00601 }
00602
00603 return ret;
00604 }