commandCtrl.c

00001 /*
00002   実際の通信処理
00003   Satofumi KAMIMURA
00004   $Id$
00005 
00006   C, C++ で共通に利用するために、RunCtrl のマクロが変な利用をされている
00007 */
00008 
00009 #include <stdio.h>
00010 
00011 #include "packetHandleHost.h"
00012 #ifdef __cplusplus
00013 #include "runCtrl.h"
00014 #define RunCtrl RunCtrl::
00015 #else
00016 #include "run_ctrl.h"
00017 #include "commandCtrl.h"
00018 #define RunCtrl
00019 
00020 static runCtrl_t *tbl;
00021 static nodeInfo_t *node;
00022 static runParams_t *params;
00023 static int unique_id;
00024 static int *con;
00025 
00026 void run_initCommandCtrl(int *Con, runCtrl_t *Run, nodeInfo_t *Node,
00027                          runParams_t *Params, int *Unique_id) {
00028   con = Con;
00029   tbl = Run;
00030   node = Node;
00031   params = Params;
00032   unique_id = *Unique_id;
00033 }
00034 #endif
00035 
00036 
00037 enum {
00038   SEND_PACKET_SIZE = 512,
00039   X = 0, Y = 1,
00040 };
00041 
00042 
00043 int RunCtrl recvVersion(long* version) {
00044   int ret_value;
00045 
00046   READ_PACKET_INIT;
00047   ADD_PACKET_DATA(&pa, tbl->version);
00048   send_size = terminatePacketInfo(&pa);
00049   ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00050   *version = tbl->version;
00051 
00052   return ret_value;
00053 }
00054 
00055 
00056 int RunCtrl sendPositionInit(void) {
00057   char send_packet[SEND_PACKET_SIZE];
00058   int send_size;
00059   int i;
00060 
00061   WRITE_PACKET_INIT;
00062   for (i = 0; i < 2; ++i) {
00063     ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.cnt_integer[i], 0);
00064     ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.cnt_decimal[i], 0);
00065     ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.body_crd.km[i], 0);
00066     ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.body_crd.m[i], 0);
00067     ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.body_crd.mm[i], 0);
00068     ADD_WRITE_PACKET_DATA(&pa, tbl->gl_crd_offset.km[i], 0);
00069     ADD_WRITE_PACKET_DATA(&pa, tbl->gl_crd_offset.m[i], 0);
00070     ADD_WRITE_PACKET_DATA(&pa, tbl->gl_crd_offset.mm[i], 0);
00071   }
00072   ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.rotate_num, 0);
00073   ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.div16_cnt, 0);
00074   send_size = terminatePacketInfo(&pa);
00075 
00076   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00077 }
00078 
00079 
00080 /* 推定自己位置の受信 */
00081 int RunCtrl recvGetPosition(int *x, int *y, long* div16) {
00082   int ret_value;
00083   int i;
00084 
00085   READ_PACKET_INIT;
00086   ADD_PACKET_DATA(&pa, tbl->msec);
00087   ADD_PACKET_DATA(&pa, tbl->sec);
00088   for (i = 0; i < 2; ++i) {
00089     ADD_PACKET_DATA(&pa, tbl->gl_crd.km[i]);
00090     ADD_PACKET_DATA(&pa, tbl->gl_crd.m[i]);
00091     ADD_PACKET_DATA(&pa, tbl->gl_crd.mm[i]);
00092   }
00093   ADD_PACKET_DATA(&pa, tbl->gl_crd.div16);
00094   send_size = terminatePacketInfo(&pa);
00095   ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00096   if (ret_value < 0) {
00097     return ret_value;
00098   }
00099   get_mmCoordinate(&tbl->gl_crd, x, y);
00100   *div16 = tbl->gl_crd.div16;
00101 
00102   return ret_value;
00103 }
00104 
00105 
00106 int RunCtrl sendStop(char *send_packet, int *send_packet_size) {
00107 #ifdef __cplusplus
00108   runParams_t* params = &state_stack.front().params;
00109 #endif
00110 
00111   WRITE_PACKET_INIT;
00112   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
00113   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl, MODE_STOP_FIRST);
00114   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, params->straight_ref_acc);
00115   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_NO_CTRL);
00116   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
00117   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
00118   *send_packet_size = terminatePacketInfo(&pa);
00119 
00120   return waitPacketResponse(con, tbl,
00121                             send_packet, *send_packet_size, unique_id);
00122 }
00123 
00124 
00125 /* 直線追従コマンドの送信 */
00126 int RunCtrl sendFollowLine(int x, int y, int div16,
00127                            char *send_packet, int *send_packet_size) {
00128   int i;
00129 #ifdef __cplusplus
00130   runParams_t* params = &state_stack.front().params;
00131 #endif
00132 
00133   WRITE_PACKET_INIT;
00134   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
00135   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_HOLD_VELOCITY_FIRST);
00136   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.ref_vel, params->straight_ref_vel);
00137   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, params->straight_ref_acc);
00138   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_FOLLOW_LINE_FIRST);
00139   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, params->rotate_ref_vel);
00140   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
00141   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.follow_r, params->follow_r);
00142 
00143   initCoordinateInfo(&tbl->run_crd_offset);
00144   tbl->run_crd_offset.mm[X] = -x;
00145   tbl->run_crd_offset.mm[Y] = -y;
00146   tbl->run_crd_offset.div16 = -div16;
00147   adjustCoordinateRange(&tbl->run_crd_offset);
00148   for (i = 0; i < 2; ++i) {
00149     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.km[i]);
00150     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.m[i]);
00151     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.mm[i]);
00152   }
00153   ADD_PACKET_DATA(&pa, tbl->run_crd_offset.div16);
00154   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
00155   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
00156   *send_packet_size = terminatePacketInfo(&pa);
00157 
00158   return waitPacketResponse(con, tbl,
00159                             send_packet, *send_packet_size, unique_id);
00160 }
00161 
00162 
00163 int RunCtrl sendFollowCircle(int x, int y, int radius,
00164                              char *send_packet, int *send_packet_size) {
00165   int i;
00166 #ifdef __cplusplus
00167   runParams_t* params = &state_stack.front().params;
00168 #endif
00169 
00170   WRITE_PACKET_INIT;
00171   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
00172   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_HOLD_VELOCITY_FIRST);
00173   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.ref_vel, params->straight_ref_vel);
00174   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, params->straight_ref_acc);
00175   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_FOLLOW_CIRCLE_FIRST);
00176   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, params->rotate_ref_vel);
00177   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
00178   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.circle_r, radius);
00179   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.follow_r, params->follow_r);
00180   initCoordinateInfo(&tbl->run_crd_offset);
00181   tbl->run_crd_offset.mm[X] = -x;
00182   tbl->run_crd_offset.mm[Y] = -y;
00183   adjustCoordinateRange(&tbl->run_crd_offset);
00184   for (i = 0; i < 2; ++i) {
00185     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.km[i]);
00186     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.m[i]);
00187     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.mm[i]);
00188   }
00189   ADD_WRITE_PACKET_DATA(&pa, tbl->run_crd_offset.div16, 0);
00190   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
00191   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
00192   *send_packet_size = terminatePacketInfo(&pa);
00193 
00194   return waitPacketResponse(con, tbl,
00195                             send_packet, *send_packet_size, unique_id);
00196 }
00197 
00198 
00199 int RunCtrl sendStopToLine(int x, int y, int div16,
00200                            char *send_packet, int *send_packet_size) {
00201   int i;
00202 #ifdef __cplusplus
00203   runParams_t* params = &state_stack.front().params;
00204 #endif
00205 
00206   WRITE_PACKET_INIT;
00207   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
00208   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_HOLD_POSITION_FIRST);
00209   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.ref_vel,params->straight_ref_vel);
00210   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, params->straight_ref_acc);
00211   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_HOLD_ANGLE_FIRST);
00212   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, params->rotate_ref_vel);
00213   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
00214   initCoordinateInfo(&tbl->run_crd_offset);
00215   tbl->run_crd_offset.mm[X] = -x;
00216   tbl->run_crd_offset.mm[Y] = -y;
00217   tbl->run_crd_offset.div16 = -div16;
00218   adjustCoordinateRange(&tbl->run_crd_offset);
00219   for (i = 0; i < 2; ++i) {
00220     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.km[i]);
00221     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.m[i]);
00222     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.mm[i]);
00223   }
00224   ADD_PACKET_DATA(&pa, tbl->run_crd_offset.div16);
00225   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
00226   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
00227   *send_packet_size = terminatePacketInfo(&pa);
00228 
00229   return waitPacketResponse(con, tbl,
00230                             send_packet, *send_packet_size, unique_id);
00231 }
00232 
00233 
00234 int RunCtrl sendTurnToDirection(int rotate_div16,
00235                                 char *send_packet, int *send_packet_size) {
00236 #ifdef __cplusplus
00237   runParams_t* params = &state_stack.front().params;
00238 #endif
00239 
00240   WRITE_PACKET_INIT;
00241   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
00242   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_NO_CTRL);
00243   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_HOLD_ANGLE_FIRST);
00244   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, params->rotate_ref_vel);
00245   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
00246   ADD_WRITE_PACKET_DATA(&pa, tbl->run_crd_offset.div16, -rotate_div16);
00247   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
00248   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
00249   *send_packet_size = terminatePacketInfo(&pa);
00250 
00251   return waitPacketResponse(con, tbl,
00252                             send_packet, *send_packet_size, unique_id);
00253 }
00254 
00255 
00256 int RunCtrl sendRotateAngle(int rotate_div16,
00257                             char *send_packet, int *send_packet_size) {
00258 #ifdef __cplusplus
00259   runParams_t* params = &state_stack.front().params;
00260 #endif
00261 
00262   WRITE_PACKET_INIT;
00263   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
00264   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_NO_CTRL);
00265   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_ROTATE_ANGLE_FIRST);
00266   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, params->rotate_ref_vel);
00267   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
00268   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.circle_r, rotate_div16);
00269   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
00270   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
00271   *send_packet_size = terminatePacketInfo(&pa);
00272 
00273   return waitPacketResponse(con, tbl,
00274                             send_packet, *send_packet_size, unique_id);
00275 }
00276 
00277 
00278 int RunCtrl sendSpin(int rotate_div16,
00279                      char *send_packet, int *send_packet_size) {
00280 #ifdef __cplusplus
00281   runParams_t* params = &state_stack.front().params;
00282 #endif
00283 
00284   WRITE_PACKET_INIT;
00285   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
00286   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_NO_CTRL);
00287   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl,
00288                         MODE_HOLD_ROTATE_VELOCITY_FIRST);
00289   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, rotate_div16);
00290   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
00291   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
00292   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
00293   *send_packet_size = terminatePacketInfo(&pa);
00294 
00295   return waitPacketResponse(con, tbl,
00296                             send_packet, *send_packet_size, unique_id);
00297 }
00298 
00299 
00300 int RunCtrl sendStraightRef(int mm_vel, int straight_acc) {
00301   char send_packet[SEND_PACKET_SIZE];
00302   int send_size;
00303 
00304   WRITE_PACKET_INIT;
00305   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.ref_vel, mm_vel);
00306   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, straight_acc);
00307   send_size = terminatePacketInfo(&pa);
00308 
00309   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00310 }
00311 
00312 
00313 int RunCtrl sendRotateRef(int rotate_div16_vel, int rotate_acc) {
00314   char send_packet[SEND_PACKET_SIZE];
00315   int send_size;
00316 
00317   WRITE_PACKET_INIT;
00318   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, rotate_div16_vel);
00319   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, rotate_acc);
00320   send_size = terminatePacketInfo(&pa);
00321 
00322   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00323 }
00324 
00325 
00326 int RunCtrl sendCurveRadius(int radius_mm) {
00327   char send_packet[SEND_PACKET_SIZE];
00328   int send_size;
00329 
00330   WRITE_PACKET_INIT;
00331   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.follow_r, radius_mm);
00332   send_size = terminatePacketInfo(&pa);
00333 
00334   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00335 }
00336 
00337 
00338 int RunCtrl recvCurveRadius(int *radius_mm) {
00339   int ret_value;
00340 
00341   READ_PACKET_INIT;
00342   ADD_PACKET_DATA(&pa, tbl->mode.follow_r);
00343   send_size = terminatePacketInfo(&pa);
00344 
00345   ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00346   *radius_mm = tbl->mode.follow_r;
00347 
00348   return ret_value;
00349 }
00350 
00351 
00352 int RunCtrl sendStraightRefVel(int straight_ref_vel) {
00353   char send_packet[SEND_PACKET_SIZE];
00354   int send_size;
00355 
00356   WRITE_PACKET_INIT;
00357   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.ref_vel, straight_ref_vel);
00358   send_size = terminatePacketInfo(&pa);
00359 
00360   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00361 }
00362 
00363 
00364 int RunCtrl sendStraightRefAcc(int straight_ref_acc) {
00365   char send_packet[SEND_PACKET_SIZE];
00366   int send_size;
00367 
00368   WRITE_PACKET_INIT;
00369   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, straight_ref_acc);
00370   send_size = terminatePacketInfo(&pa);
00371 
00372   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00373 }
00374 
00375 
00376 int RunCtrl recvStraightVel(int *straight_vel) {
00377   int ret_value;
00378 
00379   READ_PACKET_INIT;
00380   ADD_PACKET_DATA(&pa, tbl->velInfo.straight_mm_sec_vel);
00381   send_size = terminatePacketInfo(&pa);
00382   ret_value = waitPacketResponse(con, tbl,
00383                                      send_packet, send_size, unique_id);
00384   *straight_vel = tbl->velInfo.straight_mm_sec_vel;
00385 
00386   return ret_value;
00387 }
00388 
00389 
00390 int RunCtrl sendRotateRefVel(int rotate_ref_vel) {
00391   char send_packet[SEND_PACKET_SIZE];
00392   int send_size;
00393 
00394   WRITE_PACKET_INIT;
00395   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, rotate_ref_vel);
00396   send_size = terminatePacketInfo(&pa);
00397 
00398   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00399 }
00400 
00401 
00402 int RunCtrl sendRotateRefAcc(int rotate_ref_acc) {
00403   char send_packet[SEND_PACKET_SIZE];
00404   int send_size;
00405 
00406   WRITE_PACKET_INIT;
00407   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, rotate_ref_acc);
00408   send_size = terminatePacketInfo(&pa);
00409 
00410   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00411 }
00412 
00413 
00414 int RunCtrl recvRotateVel(int *rotate_vel) {
00415   int ret_value;
00416 
00417   READ_PACKET_INIT;
00418   ADD_PACKET_DATA(&pa, tbl->velInfo.rotate_div16_sec_vel);
00419   send_size = terminatePacketInfo(&pa);
00420   ret_value = waitPacketResponse(con, tbl,
00421                                      send_packet, send_size, unique_id);
00422   *rotate_vel = tbl->velInfo.rotate_div16_sec_vel;
00423 
00424   return ret_value;
00425 }
00426 
00427 
00428 int RunCtrl recvStableValue(int *isStable) {
00429   int ret_value;
00430 
00431   READ_PACKET_INIT;
00432   ADD_PACKET_DATA(&pa, tbl->straight.stable);
00433   ADD_PACKET_DATA(&pa, tbl->rotate.stable);
00434   send_size = terminatePacketInfo(&pa);
00435   ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00436   *isStable = (tbl->straight.stable * tbl->rotate.stable != 0) ? 1 : 0;
00437 
00438   return ret_value;
00439 }
00440 
00441 
00442 int RunCtrl sendServoCtrl(int servo) {
00443   char send_packet[SEND_PACKET_SIZE];
00444   int send_size;
00445 
00446   WRITE_PACKET_INIT;
00447   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl,
00448                         servo ? MODE_SERVO_CTRL : MODE_SERVO_FREE);
00449   send_size = terminatePacketInfo(&pa);
00450 
00451   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00452 }
00453 
00454 
00455 int RunCtrl sendMotorDuty(int id, unsigned char duty_array[], int num) {
00456   char send_packet[SEND_PACKET_SIZE];
00457   int send_size;
00458   int i;
00459 
00460   WRITE_PACKET_INIT;
00461   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_DIRECT_CTRL);
00462   for (i = 0; i < num; ++i) {
00463     ADD_WRITE_PACKET_DATA(&pa, tbl->direct[id + i].pwm, duty_array[i]);
00464   }
00465   send_size = terminatePacketInfo(&pa);
00466 
00467   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00468 }
00469 
00470 
00471 int RunCtrl sendMotorMode(int id, unsigned char mode_array[], int num) {
00472   char send_packet[SEND_PACKET_SIZE];
00473   int send_size;
00474   int i;
00475 
00476   WRITE_PACKET_INIT;
00477   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_DIRECT_CTRL);
00478   for (i = 0; i < num; ++i) {
00479     ADD_WRITE_PACKET_DATA(&pa, tbl->direct[id + i].mode, mode_array[i]);
00480   }
00481   send_size = terminatePacketInfo(&pa);
00482 
00483   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00484 }
00485 
00486 
00487 int RunCtrl recvEncoderDiff(int id, int diff_array[], int num) {
00488   int ret_value;
00489   int i;
00490 
00491   READ_PACKET_INIT;
00492   if ((id < 0) || (id + num > 2)) {
00493     return -1;
00494   }
00495   for (i = 0; i < num; ++i) {
00496     ADD_PACKET_DATA(&pa, tbl->enc[id + i].diff);
00497   }
00498   send_size = terminatePacketInfo(&pa);
00499   ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00500   for (i = 0; i < num; ++i) {
00501     diff_array[i] = tbl->enc[id + i].diff;
00502   }
00503   return ret_value;
00504 }
00505 
00506 
00507 int RunCtrl recvEncoderValue(int id, unsigned short cnt_array[], int num) {
00508   int ret_value;
00509   int i;
00510 
00511   READ_PACKET_INIT;
00512   if ((id < 0) || (id + num > 2)) {
00513     return -1;
00514   }
00515   for (i = 0; i < num; ++i) {
00516     ADD_PACKET_DATA(&pa, tbl->enc[id + i].prev);
00517   }
00518   send_size = terminatePacketInfo(&pa);
00519   ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00520   for (i = 0; i < num; ++i) {
00521     cnt_array[i] = tbl->enc[id + i].prev;
00522   }
00523   return ret_value;
00524 }
00525 
00526 
00527 int RunCtrl sendWheelVel(int id, int mm_vel_array[], int num) {
00528   char send_packet[SEND_PACKET_SIZE];
00529   int send_size;
00530   int i;
00531 
00532   WRITE_PACKET_INIT;
00533   ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_DIRECT_WHEEL_CTRL);
00534   for (i = 0; i < num; ++i) {
00535     ADD_WRITE_PACKET_DATA(&pa, tbl->directWhl[id + i].ref_mm_sec_vel,
00536                           -mm_vel_array[i]);
00537   }
00538   send_size = terminatePacketInfo(&pa);
00539 
00540   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00541 }
00542 
00543 
00544 int RunCtrl sendLastMoveCommand(int last_command_unique_id,
00545                                 char *send_packet,
00546                                 int send_packet_size) {
00547   return waitPacketResponse(con, tbl, send_packet, send_packet_size,
00548                             last_command_unique_id);
00549 }
00550 
00551 
00552 int RunCtrl sendChangeCoordinateOffset(int x, int y, int div16) {
00553   char send_packet[SEND_PACKET_SIZE];
00554   int send_size;
00555   int i;
00556 
00557   WRITE_PACKET_INIT;
00558   initCoordinateInfo(&tbl->run_crd_offset);
00559   tbl->run_crd_offset.mm[X] = x;
00560   tbl->run_crd_offset.mm[Y] = y;
00561   adjustCoordinateRange(&tbl->run_crd_offset);
00562   for (i = 0; i < 2; ++i) {
00563     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.km[i]);
00564     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.m[i]);
00565     ADD_PACKET_DATA(&pa, tbl->run_crd_offset.mm[i]);
00566   }
00567   ADD_WRITE_PACKET_DATA(&pa, tbl->run_crd_offset.div16, div16);
00568   ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
00569   ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
00570   send_size = terminatePacketInfo(&pa);
00571 
00572   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00573 }
00574 
00575 
00576 int RunCtrl recvModuleMsec(unsigned long* module_msec) {
00577   int ret_value;
00578 
00579   READ_PACKET_INIT;
00580   ADD_PACKET_DATA(&pa, tbl->msec);
00581   ADD_PACKET_DATA(&pa, tbl->sec);
00582   send_size = terminatePacketInfo(&pa);
00583   ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00584   *module_msec = tbl->sec * 1000 + tbl->msec;
00585 
00586   return ret_value;
00587 }
00588 
00589 
00590 int RunCtrl sendWatchDogMsec(unsigned long msec) {
00591   char send_packet[SEND_PACKET_SIZE];
00592   int send_size;
00593 
00594   WRITE_PACKET_INIT;
00595   ADD_WRITE_PACKET_DATA(&pa, tbl->watch_dog_msec, msec);
00596   ADD_WRITE_PACKET_DATA(&pa, tbl->watch_dog_msec_max, msec);
00597   send_size = terminatePacketInfo(&pa);
00598   return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00599 }
00600 
00601 
00602 int RunCtrl recvNoResetPosition(int* x, int* y, int* div16) {
00603   int ret_value;
00604   int i;
00605 
00606   READ_PACKET_INIT;
00607   for (i = 0; i < 2; ++i) {
00608     ADD_PACKET_DATA(&pa, tbl->bodyPos.body_crd.km[i]);
00609     ADD_PACKET_DATA(&pa, tbl->bodyPos.body_crd.m[i]);
00610     ADD_PACKET_DATA(&pa, tbl->bodyPos.body_crd.mm[i]);
00611   }
00612   ADD_PACKET_DATA(&pa, tbl->bodyPos.body_crd.div16);
00613   send_size = terminatePacketInfo(&pa);
00614   ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00615   if (ret_value < 0) {
00616     return ret_value;
00617   }
00618   get_mmCoordinate(&tbl->bodyPos.body_crd, x, y);
00619   *div16 = tbl->bodyPos.body_crd.div16;
00620 
00621   return ret_value;
00622 }
00623 
00624 
00625 int RunCtrl recvBodyRotateCount(int* count) {
00626   int ret_value;
00627 
00628   READ_PACKET_INIT;
00629   ADD_PACKET_DATA(&pa, tbl->bodyPos.rotate_num);
00630   send_size = terminatePacketInfo(&pa);
00631   ret_value = waitPacketResponse(con, tbl,
00632                                      send_packet, send_size, unique_id);
00633   *count = tbl->bodyPos.rotate_num;
00634 
00635   return ret_value;
00636 }
00637 
00638 
00639 int RunCtrl recvEncoderRotateCount(int id, int count[], int num) {
00640   int ret_value;
00641   int i;
00642 
00643   READ_PACKET_INIT;
00644   if ((id < 0) || (id + num > 2)) {
00645     return -1;
00646   }
00647   for (i = 0; i < num; ++i) {
00648     ADD_PACKET_DATA(&pa, tbl->enc[id + i].total);
00649   }
00650   send_size = terminatePacketInfo(&pa);
00651   ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
00652   for (i = 0; i < num; ++i) {
00653     count[i] = tbl->enc[id + i].total;
00654   }
00655   return ret_value;
00656 }
00657 

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