move_ctrl.c

00001 /*
00002   移動コマンド
00003   Satofumi KAMIMURA
00004   $Id$
00005 */
00006 #define C_RUNCTRL_SOURCE
00007 
00008 
00009 #include "move_ctrl.h"
00010 #include "coordinate_ctrl.h"
00011 #include "commandCtrl.h"
00012 #include "nodeAccess.h"
00013 #include <math.h>
00014 
00015 
00016 extern int run_getGLCoordinateOffset(int *offset_x, int *offset_y,
00017                                      int *offset_div16,
00018                                      int dest_id, int x, int y, int div16);
00019 
00020 
00021 enum { SEND_COMMAND_SIZE = 256 };
00022 static char send_command[SEND_COMMAND_SIZE];
00023 static int send_command_size = 0;
00024 static int cmd_crd_id = -1;
00025 static int cmd_x = 0;
00026 static int cmd_y = 0;
00027 static int cmd_div16 = 0;
00028 
00029 
00030 #define MOVE_CMD_FIRST \
00031   int gl_x, gl_y, gl_div16; \
00032   int ret_value; \
00033   if (crd_id == FS) { \
00034     int now_x, now_y, now_div16; \
00035     run_getBodyPos(GL, &now_x, &now_y, &now_div16); \
00036     run_setCoordinateParent(FS, GL, now_x, now_y, now_div16); \
00037   } \
00038   run_getGLCoordinateOffset(&gl_x, &gl_y, &gl_div16, crd_id, x, y, div16); \
00039 
00040 
00050 void run_getLastCommandPosition(int *crd_id, int *x, int *y, int *div16) {
00051   *crd_id = cmd_crd_id;
00052   *x = cmd_x;
00053   *y = cmd_y;
00054   *div16 = cmd_div16;
00055 }
00056 
00067 int run_followLine(int crd_id, int x, int y, int div16) {
00068 
00069   MOVE_CMD_FIRST;
00070   ret_value = sendFollowLine(gl_x, gl_y, gl_div16,
00071                              send_command, &send_command_size);
00072   if (ret_value < 0) {
00073     send_command_size = 0;
00074   }
00075   cmd_crd_id = crd_id;
00076   cmd_x = x;
00077   cmd_y = y;
00078   cmd_div16 = div16;
00079 
00080   return ret_value;
00081 }
00082 
00083 
00094 int run_stopLine(int crd_id, int x, int y, int div16) {
00095 
00096   MOVE_CMD_FIRST;
00097   ret_value = sendStopToLine(gl_x, gl_y, gl_div16,
00098                             send_command, &send_command_size);
00099   if (ret_value < 0) {
00100     send_command_size = 0;
00101   }
00102   cmd_crd_id = crd_id;
00103   cmd_x = x;
00104   cmd_y = y;
00105   cmd_div16 = div16;
00106 
00107   return ret_value;
00108 }
00109 
00110 
00123 int run_followCircle(int crd_id, int x, int y, int r) {
00124   int div16 = 0;
00125   MOVE_CMD_FIRST;
00126   ret_value = sendFollowCircle(gl_x, gl_y, r,
00127                              send_command, &send_command_size);
00128   if (ret_value < 0) {
00129     send_command_size = 0;
00130   }
00131   cmd_crd_id = crd_id;
00132   cmd_x = x;
00133   cmd_y = y;
00134   cmd_div16 = div16;
00135 
00136   return ret_value;
00137 }
00138 
00139 
00153 int run_followCircleOnLine(int crd_id, int x, int y, int div16, int r) {
00154 
00155   int t_add = (r > 0) ? +(0x10000 >> 2) : -(0x10000 >> 2);
00156   double radian = (2.0 * M_PI * (div16 + t_add) / 65536.0) + M_PI;
00157   int run_x = x + (int)(fabs(r) * cos(radian));
00158   int run_y = y + (int)(fabs(r) * sin(radian));
00159 
00160   return run_followCircle(crd_id, run_x, run_y, r);
00161 }
00162 
00163 
00171 int run_rotateAngle(int div16) {
00172   int ret_value = sendRotateAngle(div16, send_command, &send_command_size);
00173   if (ret_value < 0) {
00174     send_command_size = 0;
00175   }
00176 
00177   return ret_value;
00178 }
00179 
00180 
00189 int run_stopAngle(int crd_id, int div16) {
00190   int x = 0, y = 0;
00191   MOVE_CMD_FIRST;
00192   ret_value = sendTurnToDirection(gl_div16, send_command, &send_command_size);
00193   if (ret_value < 0) {
00194     send_command_size = 0;
00195   }
00196   cmd_crd_id = crd_id;
00197   cmd_div16 = div16;
00198 
00199   return  ret_value;
00200 }
00201 
00202 
00209 int run_spin(int div16) {
00210   int x = 0, y = 0;
00211   int crd_id = GL;
00212   MOVE_CMD_FIRST;
00213   ret_value = sendSpin(gl_div16, send_command, &send_command_size);
00214   if (ret_value < 0) {
00215     send_command_size = 0;
00216   }
00217   return  ret_value;
00218 }
00219 
00220 
00227 int run_stop(void) {
00228   return sendStop(send_command, &send_command_size);
00229 }
00230 
00231 
00238 int run_lastMovedCommand(void) {
00239   int last_command_unique_id;
00240   if (send_command_size <= 0) {
00241     return -1;
00242   }
00243   last_command_unique_id = getPacketUniqueId((unsigned char*)send_command);
00244   return sendLastMoveCommand(last_command_unique_id,
00245                              send_command, send_command_size);
00246 }
00247 
00248 
00255 int run_restartServoCtrl(void) {
00256   return sendServoCtrl(1);
00257 }
00258 
00259 
00266 int run_stopServoCtrl(void) {
00267   return sendServoCtrl(0);
00268 }
00269 

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