commandCtrl.c

/*
  実際の通信処理
  Satofumi KAMIMURA
  $Id$

  C, C++ で共通に利用するために、RunCtrl のマクロが変な利用をされている
*/

#include <stdio.h>

#include "packetHandleHost.h"
#ifdef __cplusplus
#include "runCtrl.h"
#define RunCtrl RunCtrl::
#else
#include "run_ctrl.h"
#include "commandCtrl.h"
#define RunCtrl

static runCtrl_t *tbl;
static nodeInfo_t *node;
static runParams_t *params;
static int unique_id;
static int *con;

void run_initCommandCtrl(int *Con, runCtrl_t *Run, nodeInfo_t *Node,
                         runParams_t *Params, int *Unique_id) {
  con = Con;
  tbl = Run;
  node = Node;
  params = Params;
  unique_id = *Unique_id;
}
#endif


enum {
  SEND_PACKET_SIZE = 512,
  X = 0, Y = 1,
};


int RunCtrl recvVersion(long* version) {
  int ret_value;

  READ_PACKET_INIT;
  ADD_PACKET_DATA(&pa, tbl->version);
  send_size = terminatePacketInfo(&pa);
  ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
  *version = tbl->version;

  return ret_value;
}


int RunCtrl sendPositionInit(void) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;
  int i;

  WRITE_PACKET_INIT;
  for (i = 0; i < 2; ++i) {
    ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.cnt_integer[i], 0);
    ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.cnt_decimal[i], 0);
    ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.body_crd.km[i], 0);
    ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.body_crd.m[i], 0);
    ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.body_crd.mm[i], 0);
    ADD_WRITE_PACKET_DATA(&pa, tbl->gl_crd_offset.km[i], 0);
    ADD_WRITE_PACKET_DATA(&pa, tbl->gl_crd_offset.m[i], 0);
    ADD_WRITE_PACKET_DATA(&pa, tbl->gl_crd_offset.mm[i], 0);
  }
  ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.rotate_num, 0);
  ADD_WRITE_PACKET_DATA(&pa, tbl->bodyPos.div16_cnt, 0);
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


/* 推定自己位置の受信 */
int RunCtrl recvGetPosition(int *x, int *y, long* div16) {
  int ret_value;
  int i;

  READ_PACKET_INIT;
  ADD_PACKET_DATA(&pa, tbl->msec);
  ADD_PACKET_DATA(&pa, tbl->sec);
  for (i = 0; i < 2; ++i) {
    ADD_PACKET_DATA(&pa, tbl->gl_crd.km[i]);
    ADD_PACKET_DATA(&pa, tbl->gl_crd.m[i]);
    ADD_PACKET_DATA(&pa, tbl->gl_crd.mm[i]);
  }
  ADD_PACKET_DATA(&pa, tbl->gl_crd.div16);
  send_size = terminatePacketInfo(&pa);
  ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
  if (ret_value < 0) {
    return ret_value;
  }
  get_mmCoordinate(&tbl->gl_crd, x, y);
  *div16 = tbl->gl_crd.div16;

  return ret_value;
}


int RunCtrl sendStop(char *send_packet, int *send_packet_size) {
#ifdef __cplusplus
  runParams_t* params = &state_stack.front().params;
#endif

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl, MODE_STOP_FIRST);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, params->straight_ref_acc);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_NO_CTRL);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
  *send_packet_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl,
                            send_packet, *send_packet_size, unique_id);
}


/* 直線追従コマンドの送信 */
int RunCtrl sendFollowLine(int x, int y, int div16,
                           char *send_packet, int *send_packet_size) {
  int i;
#ifdef __cplusplus
  runParams_t* params = &state_stack.front().params;
#endif

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_HOLD_VELOCITY_FIRST);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.ref_vel, params->straight_ref_vel);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, params->straight_ref_acc);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_FOLLOW_LINE_FIRST);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, params->rotate_ref_vel);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.follow_r, params->follow_r);

  initCoordinateInfo(&tbl->run_crd_offset);
  tbl->run_crd_offset.mm[X] = -x;
  tbl->run_crd_offset.mm[Y] = -y;
  tbl->run_crd_offset.div16 = -div16;
  adjustCoordinateRange(&tbl->run_crd_offset);
  for (i = 0; i < 2; ++i) {
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.km[i]);
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.m[i]);
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.mm[i]);
  }
  ADD_PACKET_DATA(&pa, tbl->run_crd_offset.div16);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
  *send_packet_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl,
                            send_packet, *send_packet_size, unique_id);
}


int RunCtrl sendFollowCircle(int x, int y, int radius,
                             char *send_packet, int *send_packet_size) {
  int i;
#ifdef __cplusplus
  runParams_t* params = &state_stack.front().params;
#endif

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_HOLD_VELOCITY_FIRST);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.ref_vel, params->straight_ref_vel);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, params->straight_ref_acc);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_FOLLOW_CIRCLE_FIRST);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, params->rotate_ref_vel);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.circle_r, radius);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.follow_r, params->follow_r);
  initCoordinateInfo(&tbl->run_crd_offset);
  tbl->run_crd_offset.mm[X] = -x;
  tbl->run_crd_offset.mm[Y] = -y;
  adjustCoordinateRange(&tbl->run_crd_offset);
  for (i = 0; i < 2; ++i) {
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.km[i]);
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.m[i]);
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.mm[i]);
  }
  ADD_WRITE_PACKET_DATA(&pa, tbl->run_crd_offset.div16, 0);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
  *send_packet_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl,
                            send_packet, *send_packet_size, unique_id);
}


int RunCtrl sendStopToLine(int x, int y, int div16,
                           char *send_packet, int *send_packet_size) {
  int i;
#ifdef __cplusplus
  runParams_t* params = &state_stack.front().params;
#endif

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_HOLD_POSITION_FIRST);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.ref_vel,params->straight_ref_vel);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, params->straight_ref_acc);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_HOLD_ANGLE_FIRST);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, params->rotate_ref_vel);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
  initCoordinateInfo(&tbl->run_crd_offset);
  tbl->run_crd_offset.mm[X] = -x;
  tbl->run_crd_offset.mm[Y] = -y;
  tbl->run_crd_offset.div16 = -div16;
  adjustCoordinateRange(&tbl->run_crd_offset);
  for (i = 0; i < 2; ++i) {
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.km[i]);
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.m[i]);
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.mm[i]);
  }
  ADD_PACKET_DATA(&pa, tbl->run_crd_offset.div16);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
  *send_packet_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl,
                            send_packet, *send_packet_size, unique_id);
}


int RunCtrl sendTurnToDirection(int rotate_div16,
                                char *send_packet, int *send_packet_size) {
#ifdef __cplusplus
  runParams_t* params = &state_stack.front().params;
#endif

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_NO_CTRL);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_HOLD_ANGLE_FIRST);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, params->rotate_ref_vel);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
  ADD_WRITE_PACKET_DATA(&pa, tbl->run_crd_offset.div16, -rotate_div16);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
  *send_packet_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl,
                            send_packet, *send_packet_size, unique_id);
}


int RunCtrl sendRotateAngle(int rotate_div16,
                            char *send_packet, int *send_packet_size) {
#ifdef __cplusplus
  runParams_t* params = &state_stack.front().params;
#endif

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_NO_CTRL);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl, MODE_ROTATE_ANGLE_FIRST);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, params->rotate_ref_vel);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.circle_r, rotate_div16);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
  *send_packet_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl,
                            send_packet, *send_packet_size, unique_id);
}


int RunCtrl sendSpin(int rotate_div16,
                     char *send_packet, int *send_packet_size) {
#ifdef __cplusplus
  runParams_t* params = &state_stack.front().params;
#endif

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_SERVO_CTRL);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.straight_ctrl,MODE_NO_CTRL);
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.rotate_ctrl,
                        MODE_HOLD_ROTATE_VELOCITY_FIRST);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, rotate_div16);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, params->rotate_ref_acc);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
  *send_packet_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl,
                            send_packet, *send_packet_size, unique_id);
}


int RunCtrl sendStraightRef(int mm_vel, int straight_acc) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.ref_vel, mm_vel);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, straight_acc);
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl sendRotateRef(int rotate_div16_vel, int rotate_acc) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, rotate_div16_vel);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, rotate_acc);
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl sendCurveRadius(int radius_mm) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.follow_r, radius_mm);
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl recvCurveRadius(int *radius_mm) {
  int ret_value;

  READ_PACKET_INIT;
  ADD_PACKET_DATA(&pa, tbl->mode.follow_r);
  send_size = terminatePacketInfo(&pa);

  ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
  *radius_mm = tbl->mode.follow_r;

  return ret_value;
}


int RunCtrl sendStraightRefVel(int straight_ref_vel) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.ref_vel, straight_ref_vel);
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl sendStraightRefAcc(int straight_ref_acc) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.acc, straight_ref_acc);
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl recvStraightVel(int *straight_vel) {
  int ret_value;

  READ_PACKET_INIT;
  ADD_PACKET_DATA(&pa, tbl->velInfo.straight_mm_sec_vel);
  send_size = terminatePacketInfo(&pa);
  ret_value = waitPacketResponse(con, tbl,
                                     send_packet, send_size, unique_id);
  *straight_vel = tbl->velInfo.straight_mm_sec_vel;

  return ret_value;
}


int RunCtrl sendRotateRefVel(int rotate_ref_vel) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.ref_vel, rotate_ref_vel);
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl sendRotateRefAcc(int rotate_ref_acc) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.acc, rotate_ref_acc);
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl recvRotateVel(int *rotate_vel) {
  int ret_value;

  READ_PACKET_INIT;
  ADD_PACKET_DATA(&pa, tbl->velInfo.rotate_div16_sec_vel);
  send_size = terminatePacketInfo(&pa);
  ret_value = waitPacketResponse(con, tbl,
                                     send_packet, send_size, unique_id);
  *rotate_vel = tbl->velInfo.rotate_div16_sec_vel;

  return ret_value;
}


int RunCtrl recvStableValue(int *isStable) {
  int ret_value;

  READ_PACKET_INIT;
  ADD_PACKET_DATA(&pa, tbl->straight.stable);
  ADD_PACKET_DATA(&pa, tbl->rotate.stable);
  send_size = terminatePacketInfo(&pa);
  ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
  *isStable = (tbl->straight.stable * tbl->rotate.stable != 0) ? 1 : 0;

  return ret_value;
}


int RunCtrl sendServoCtrl(int servo) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl,
                        servo ? MODE_SERVO_CTRL : MODE_SERVO_FREE);
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl sendMotorDuty(int id, unsigned char duty_array[], int num) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;
  int i;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_DIRECT_CTRL);
  for (i = 0; i < num; ++i) {
    ADD_WRITE_PACKET_DATA(&pa, tbl->direct[id + i].pwm, duty_array[i]);
  }
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl sendMotorMode(int id, unsigned char mode_array[], int num) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;
  int i;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_DIRECT_CTRL);
  for (i = 0; i < num; ++i) {
    ADD_WRITE_PACKET_DATA(&pa, tbl->direct[id + i].mode, mode_array[i]);
  }
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl recvEncoderDiff(int id, int diff_array[], int num) {
  int ret_value;
  int i;

  READ_PACKET_INIT;
  if ((id < 0) || (id + num > 2)) {
    return -1;
  }
  for (i = 0; i < num; ++i) {
    ADD_PACKET_DATA(&pa, tbl->enc[id + i].diff);
  }
  send_size = terminatePacketInfo(&pa);
  ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
  for (i = 0; i < num; ++i) {
    diff_array[i] = tbl->enc[id + i].diff;
  }
  return ret_value;
}


int RunCtrl recvEncoderValue(int id, unsigned short cnt_array[], int num) {
  int ret_value;
  int i;

  READ_PACKET_INIT;
  if ((id < 0) || (id + num > 2)) {
    return -1;
  }
  for (i = 0; i < num; ++i) {
    ADD_PACKET_DATA(&pa, tbl->enc[id + i].prev);
  }
  send_size = terminatePacketInfo(&pa);
  ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
  for (i = 0; i < num; ++i) {
    cnt_array[i] = tbl->enc[id + i].prev;
  }
  return ret_value;
}


int RunCtrl sendWheelVel(int id, int mm_vel_array[], int num) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;
  int i;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->mode.ctrl, MODE_DIRECT_WHEEL_CTRL);
  for (i = 0; i < num; ++i) {
    ADD_WRITE_PACKET_DATA(&pa, tbl->directWhl[id + i].ref_mm_sec_vel,
                          -mm_vel_array[i]);
  }
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl sendLastMoveCommand(int last_command_unique_id,
                                char *send_packet,
                                int send_packet_size) {
  return waitPacketResponse(con, tbl, send_packet, send_packet_size,
                            last_command_unique_id);
}


int RunCtrl sendChangeCoordinateOffset(int x, int y, int div16) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;
  int i;

  WRITE_PACKET_INIT;
  initCoordinateInfo(&tbl->run_crd_offset);
  tbl->run_crd_offset.mm[X] = x;
  tbl->run_crd_offset.mm[Y] = y;
  adjustCoordinateRange(&tbl->run_crd_offset);
  for (i = 0; i < 2; ++i) {
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.km[i]);
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.m[i]);
    ADD_PACKET_DATA(&pa, tbl->run_crd_offset.mm[i]);
  }
  ADD_WRITE_PACKET_DATA(&pa, tbl->run_crd_offset.div16, div16);
  ADD_WRITE_PACKET_DATA(&pa, tbl->straight.stable, 0);
  ADD_WRITE_PACKET_DATA(&pa, tbl->rotate.stable, 0);
  send_size = terminatePacketInfo(&pa);

  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl recvModuleMsec(unsigned long* module_msec) {
  int ret_value;

  READ_PACKET_INIT;
  ADD_PACKET_DATA(&pa, tbl->msec);
  ADD_PACKET_DATA(&pa, tbl->sec);
  send_size = terminatePacketInfo(&pa);
  ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
  *module_msec = tbl->sec * 1000 + tbl->msec;

  return ret_value;
}


int RunCtrl sendWatchDogMsec(unsigned long msec) {
  char send_packet[SEND_PACKET_SIZE];
  int send_size;

  WRITE_PACKET_INIT;
  ADD_WRITE_PACKET_DATA(&pa, tbl->watch_dog_msec, msec);
  ADD_WRITE_PACKET_DATA(&pa, tbl->watch_dog_msec_max, msec);
  send_size = terminatePacketInfo(&pa);
  return waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
}


int RunCtrl recvNoResetPosition(int* x, int* y, int* div16) {
  int ret_value;
  int i;

  READ_PACKET_INIT;
  for (i = 0; i < 2; ++i) {
    ADD_PACKET_DATA(&pa, tbl->bodyPos.body_crd.km[i]);
    ADD_PACKET_DATA(&pa, tbl->bodyPos.body_crd.m[i]);
    ADD_PACKET_DATA(&pa, tbl->bodyPos.body_crd.mm[i]);
  }
  ADD_PACKET_DATA(&pa, tbl->bodyPos.body_crd.div16);
  send_size = terminatePacketInfo(&pa);
  ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
  if (ret_value < 0) {
    return ret_value;
  }
  get_mmCoordinate(&tbl->bodyPos.body_crd, x, y);
  *div16 = tbl->bodyPos.body_crd.div16;

  return ret_value;
}


int RunCtrl recvBodyRotateCount(int* count) {
  int ret_value;

  READ_PACKET_INIT;
  ADD_PACKET_DATA(&pa, tbl->bodyPos.rotate_num);
  send_size = terminatePacketInfo(&pa);
  ret_value = waitPacketResponse(con, tbl,
                                     send_packet, send_size, unique_id);
  *count = tbl->bodyPos.rotate_num;

  return ret_value;
}


int RunCtrl recvEncoderRotateCount(int id, int count[], int num) {
  int ret_value;
  int i;

  READ_PACKET_INIT;
  if ((id < 0) || (id + num > 2)) {
    return -1;
  }
  for (i = 0; i < num; ++i) {
    ADD_PACKET_DATA(&pa, tbl->enc[id + i].total);
  }
  send_size = terminatePacketInfo(&pa);
  ret_value = waitPacketResponse(con, tbl, send_packet, send_size, unique_id);
  for (i = 0; i < num; ++i) {
    count[i] = tbl->enc[id + i].total;
  }
  return ret_value;
}