command.c
Go to the documentation of this file.
1 // Copyright (c) 2010-2016 The YP-Spur Authors, except where otherwise indicated.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16 // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19 // SOFTWARE.
20 
21 #include <math.h>
22 #include <stdio.h>
23 #include <strings.h>
24 #include <unistd.h>
25 
26 #include <fcntl.h>
27 #include <sys/time.h>
28 #include <time.h>
29 
30 #ifdef HAVE_CONFIG_H
31 #include <config.h>
32 #endif // HAVE_CONFIG_H
33 
34 /* ボディパラメータ */
35 #include <shvel-param.h>
36 
37 /* yp-spur用 */
38 #include <serial.h>
39 #include <param.h>
40 #include <control.h>
41 #include <command.h>
42 #include <ipcommunication.h>
43 #include <yprintf.h>
44 
45 /* ライブラリ用 */
46 #include <ypspur.h>
47 #include <cartesian2d.h>
48 
49 #include <pthread.h>
50 
51 SpurUserParams g_spur;
52 
53 SpurUserParamsPtr get_spur_user_param_ptr()
54 {
55  return &g_spur;
56 }
57 
58 void command_loop_cleanup(void *data)
59 {
60  struct ipcmd_t *ipcmd;
61 
62  ipcmd = data;
63  yprintf(OUTPUT_LV_INFO, "Command analyzer stopped.\n");
64 
65  ipcmd_close(ipcmd);
66 }
67 
68 void init_spur_command(void)
69 {
70  g_spur.x = 0;
71  g_spur.y = 0;
72  g_spur.theta = 0;
73  g_spur.v = 0;
74  g_spur.w = 0;
75  g_spur.radius = 0;
76  g_spur.tilt = 0;
77  g_spur.dir = 0;
78  g_spur.run_mode = RUN_STOP;
79  g_spur.before_run_mode = -10000;
80  g_spur.freeze = 0;
81  g_spur.before_freeze = 0;
82  g_spur.run_mode_cnt = 0;
83  pthread_mutex_init(&g_spur.mutex, NULL);
84 }
85 
86 /* コマンド受信 */
87 void command(void)
88 {
89  YPSpur_msg msg, res_msg;
90  int len;
91  struct ipcmd_t ipcmd;
92  char param_name[YP_PARAM_NUM][30] = YP_PARAM_NAME;
93  ParametersPtr param;
94 
95  param = get_param_ptr();
96 
97  /* initialize message queue */
98  if (option(OPTION_SOCKET))
99  {
100  len = ipcmd_open_tcp(&ipcmd, NULL, get_param_ptr()->port);
101  }
102  else
103  {
104  len = ipcmd_open_msq(&ipcmd, get_param_ptr()->msq_key, 1);
105  }
106  if (len < 0)
107  {
108  /* queue_error */
109  yprintf(OUTPUT_LV_ERROR, "Error: Can't initialize message queue.\n");
110  return;
111  }
112 
113  // メッセージキューを空にする
114  ipcmd.flush(&ipcmd);
115 
116  yprintf(OUTPUT_LV_INFO, "Command analyzer started.\n");
117  pthread_cleanup_push(command_loop_cleanup, &ipcmd);
118 
119  while (1)
120  {
121  /* 1コマンド取得 */
122  len = ipcmd.recv(&ipcmd, &msg);
123  if (len < YPSPUR_MSG_SIZE)
124  {
125  yprintf(OUTPUT_LV_ERROR, "Error: Invalid command received\n");
126  continue;
127  }
128 
129  pthread_mutex_lock(&g_spur.mutex);
130  /* コマンド解析・返信 */
131  switch (msg.type)
132  {
133  /*--------command_set.c--------*/
134  case YPSPUR_SET_POS:
135  set_pos_com(msg.cs, msg.data, &g_spur);
136  yprintf(OUTPUT_LV_DEBUG, "Command: set pos %f %f %f\n", msg.data[0], msg.data[1], msg.data[2]);
137  break;
138  case YPSPUR_SET_GL_GL:
139  set_GL_on_GL_com(msg.data, &g_spur);
140  yprintf(OUTPUT_LV_DEBUG, "Command: GL on GL %f %f %f\n", msg.data[0], msg.data[1], msg.data[2]);
141  break;
142  case YPSPUR_ADJUST:
143  set_adjust_com(msg.cs, msg.data, &g_spur);
144  yprintf(OUTPUT_LV_DEBUG, "Command: adjust %f %f %f\n", msg.data[0], msg.data[1], msg.data[2]);
145  break;
146  case YPSPUR_SET_VEL:
147  set_vel_com(msg.data, &g_spur);
148  yprintf(OUTPUT_LV_DEBUG, "Command: vel %f\n", g_spur.v);
149  break;
150  case YPSPUR_SET_ANGVEL:
151  set_ang_vel_com(msg.data, &g_spur);
152  yprintf(OUTPUT_LV_DEBUG, "Command: w %f\n", g_spur.w);
153  break;
154  case YPSPUR_SET_ACCEL:
155  set_accel_com(msg.data, &g_spur);
156  yprintf(OUTPUT_LV_DEBUG, "Command: dvel %f\n", g_spur.dv);
157  break;
158  case YPSPUR_SET_ANGACCEL:
159  set_ang_accel_com(msg.data, &g_spur);
160  yprintf(OUTPUT_LV_DEBUG, "Command: dw %f\n", g_spur.dw);
161  break;
162  case YPSPUR_SET_TILT:
163  set_tilt_com(msg.cs, msg.data, &g_spur);
164  yprintf(OUTPUT_LV_DEBUG, "Command: tilt %f %f\n", g_spur.dir, g_spur.tilt);
165  break;
166 
167  /*--------command_run.c----------*/
168  case YPSPUR_LINE:
169  line_com(msg.cs, msg.data, &g_spur);
170  yprintf(OUTPUT_LV_DEBUG, "Command: line %f %f %f\n", g_spur.x, g_spur.y, g_spur.theta);
171  break;
172  case YPSPUR_STOP_LINE:
173  stop_line_com(msg.cs, msg.data, &g_spur);
174  yprintf(OUTPUT_LV_DEBUG, "Command: stop_line %f %f %f\n", g_spur.x, g_spur.y, g_spur.theta);
175  break;
176  case YPSPUR_CIRCLE:
177  circle_com(msg.cs, msg.data, &g_spur);
178  yprintf(OUTPUT_LV_DEBUG, "Command: circle %f %f %f\n", g_spur.x, g_spur.y, g_spur.radius);
179  break;
180  case YPSPUR_SPIN:
181  spin_com(msg.cs, msg.data, &g_spur);
182  yprintf(OUTPUT_LV_DEBUG, "Command: spin %f\n", g_spur.theta);
183  break;
184  case YPSPUR_ORIENT:
185  orient_com(msg.cs, msg.data, &g_spur);
186  yprintf(OUTPUT_LV_DEBUG, "Command: orient %f\n", g_spur.theta);
187  break;
188  case YPSPUR_STOP:
189  stop_com(msg.data, &g_spur);
190  yprintf(OUTPUT_LV_DEBUG, "Command: stop\n");
191  break;
192  case YPSPUR_OPENFREE:
193  openfree_com(msg.data, &g_spur);
194  yprintf(OUTPUT_LV_DEBUG, "Command: openfree\n");
195  break;
196  case YPSPUR_FREE:
197  free_com(msg.data, &g_spur);
198  yprintf(OUTPUT_LV_DEBUG, "Command: free\n");
199  break;
200  case YPSPUR_FREEZE:
201  g_spur.freeze = 1;
202  yprintf(OUTPUT_LV_DEBUG, "Command: freeze\n");
203  break;
204  case YPSPUR_UNFREEZE:
205  g_spur.freeze = 0;
206  yprintf(OUTPUT_LV_DEBUG, "Command: unfreeze\n");
207  break;
208  case YPSPUR_ISFREEZE:
209  res_msg.data[0] = g_spur.freeze;
210  message_return(&ipcmd, msg.pid, &res_msg);
211  yprintf(OUTPUT_LV_DEBUG, "Command: isfreeze %d\n", g_spur.freeze);
212  break;
213  case YPSPUR_VEL:
214  vel_com(msg.data, &g_spur);
215  yprintf(OUTPUT_LV_DEBUG, "Command: vel %f %f\n", g_spur.vref, g_spur.wref);
216  break;
217  case YPSPUR_WHEEL_VEL:
218  wheel_vel_com(msg.data, &g_spur);
219  yprintf(OUTPUT_LV_DEBUG, "Command: wheel_vel %f %f\n", g_spur.wvelref[0], g_spur.wvelref[1]);
220  break;
221  case YPSPUR_WHEEL_TORQUE:
222  set_torque_com(msg.data, &g_spur);
223  yprintf(OUTPUT_LV_DEBUG, "Command: set_torque %f %f\n", msg.data[0], msg.data[1]);
224  break;
225  case YPSPUR_SET_WHEEL_VEL:
226  set_wheel_vel_com(msg.data, &g_spur);
227  yprintf(OUTPUT_LV_DEBUG, "Command: set_wheel_vel %f %f\n", g_spur.wheel_vel[0], g_spur.wheel_vel[1]);
228  break;
229  case YPSPUR_SET_WHEEL_ACCEL:
230  set_wheel_accel_com(msg.data, &g_spur);
231  yprintf(OUTPUT_LV_DEBUG, "Command: set_wheel_accel %f %f\n", g_spur.wheel_accel[0], g_spur.wheel_accel[1]);
232  break;
233  case YPSPUR_WHEEL_ANGLE:
234  wheel_angle_com(msg.data, &g_spur);
235  yprintf(OUTPUT_LV_DEBUG, "Command: wheel_angle %f %f\n", g_spur.wheel_angle[0], g_spur.wheel_angle[1]);
236  break;
237 
238  /*----------command_get.c------------------*/
239  case YPSPUR_GET_POS:
240  get_pos_com(msg.cs, msg.data, res_msg.data, &g_spur);
241  message_return(&ipcmd, msg.pid, &res_msg);
242  yprintf(OUTPUT_LV_DEBUG, "Command: get %f %f %f\n", res_msg.data[0], res_msg.data[1], res_msg.data[2]);
243  break;
244  case YPSPUR_GET_VEL:
245  get_vel_com(msg.cs, msg.data, res_msg.data, &g_spur);
246  message_return(&ipcmd, msg.pid, &res_msg);
247  yprintf(OUTPUT_LV_DEBUG, "Command: getvel %f %f\n", res_msg.data[0], res_msg.data[1]);
248  break;
249  case YPSPUR_GET_VREF:
250  get_vref_com(msg.cs, msg.data, res_msg.data, &g_spur);
251  message_return(&ipcmd, msg.pid, &res_msg);
252  yprintf(OUTPUT_LV_DEBUG, "Command: getvref %f %f\n", res_msg.data[0], res_msg.data[1]);
253  break;
254  case YPSPUR_GET_WHEEL_VREF:
255  get_wheel_vref_com(msg.cs, msg.data, res_msg.data, &g_spur);
256  message_return(&ipcmd, msg.pid, &res_msg);
257  yprintf(OUTPUT_LV_DEBUG, "Command: getwheelvref %f %f\n", res_msg.data[0], res_msg.data[1]);
258  break;
259  case YPSPUR_GET_FORCE:
260  get_force_com(msg.cs, msg.data, res_msg.data, &g_spur);
261  message_return(&ipcmd, msg.pid, &res_msg);
262  yprintf(OUTPUT_LV_DEBUG, "Command: getforce %f %f\n", res_msg.data[0], res_msg.data[1]);
263  break;
264  case YPSPUR_GET_WHEEL_TORQUE:
265  get_wheel_torque_com(msg.cs, msg.data, res_msg.data, &g_spur);
266  message_return(&ipcmd, msg.pid, &res_msg);
267  yprintf(OUTPUT_LV_DEBUG, "Command: getwheeltorque %f %f\n", res_msg.data[0], res_msg.data[1]);
268  break;
269  case YPSPUR_GET_WHEEL_VEL:
270  get_wheel_vel_com(msg.data, res_msg.data, &g_spur);
271  message_return(&ipcmd, msg.pid, &res_msg);
272  yprintf(OUTPUT_LV_DEBUG, "Command: get wheelvel %f %f\n", res_msg.data[0], res_msg.data[1]);
273  break;
274  case YPSPUR_GET_WHEEL_ANG:
275  get_wheel_ang_com(msg.data, res_msg.data, &g_spur);
276  message_return(&ipcmd, msg.pid, &res_msg);
277  yprintf(OUTPUT_LV_DEBUG, "Command: get wheelang %f %f\n", res_msg.data[0], res_msg.data[1]);
278  break;
279  case YPSPUR_NEAR_POS:
280  res_msg.cs = near_pos_com(msg.cs, msg.data, res_msg.data, &g_spur);
281  message_return(&ipcmd, msg.pid, &res_msg);
282  yprintf(OUTPUT_LV_DEBUG, "Command: near pos ( dist = %f )\n", res_msg.data[0]);
283  break;
284  case YPSPUR_NEAR_ANG:
285  res_msg.cs = near_ang_com(msg.cs, msg.data, res_msg.data, &g_spur);
286  message_return(&ipcmd, msg.pid, &res_msg);
287  yprintf(OUTPUT_LV_DEBUG, "Command: near ang ( dist = %f )\n", res_msg.data[0]);
288  break;
289  case YPSPUR_OVER_LINE:
290  res_msg.cs = over_line_com(msg.cs, msg.data, res_msg.data, &g_spur);
291  message_return(&ipcmd, msg.pid, &res_msg);
292  yprintf(OUTPUT_LV_DEBUG, "Command: over line ( dist = %f )\n", res_msg.data[0]);
293  break;
294 
295  /*-------------command_param.c---------------*/
296  case YPSPUR_PARAM_SET:
297  param_set_com(msg.cs, msg.data, &g_spur);
298  yprintf(OUTPUT_LV_DEBUG, "Command: param_set %s %f\n", param_name[msg.cs], msg.data[0]);
299  break;
300 
301  case YPSPUR_PARAM_GET:
302  res_msg.cs = param_get_com(msg.cs, res_msg.data, &g_spur);
303  message_return(&ipcmd, msg.pid, &res_msg);
304  yprintf(OUTPUT_LV_DEBUG, "Command: param_get %s %f\n", param_name[msg.cs], msg.data[0]);
305  break;
306 
307  case YPSPUR_PARAM_STATE:
308  param_state_com(msg.cs, msg.data, &g_spur);
309  break;
310 
311  /*-------------command_aux.c---------------*/
312  case YPSPUR_GETAD:
313  get_ad_com(msg.data, res_msg.data);
314  message_return(&ipcmd, msg.pid, &res_msg);
315  yprintf(OUTPUT_LV_DEBUG, "Command: get A/D%d value %d\n", (int)msg.data[0], (int)res_msg.data[0]);
316  break;
317 
318  case YPSPUR_SETIODIR:
319  set_io_dir_com(msg.data, res_msg.data);
320  yprintf(OUTPUT_LV_DEBUG, "Command: set IO dir %d\n", (int)msg.data[0]);
321  break;
322 
323  case YPSPUR_SETIODATA:
324  set_io_data_com(msg.data, res_msg.data);
325  yprintf(OUTPUT_LV_DEBUG, "Command: set IO data %d\n", (int)msg.data[0]);
326  break;
327 
328  case YPSPUR_GET_ERROR_STATE:
329  get_error_state_com(msg.data, res_msg.data);
330  message_return(&ipcmd, msg.pid, &res_msg);
331  yprintf(OUTPUT_LV_DEBUG, "Command: get error state %d (id: %d)\n", (int)msg.data[0], (int)res_msg.data[0]);
332  break;
333 
334  /*-------------command_joint.c---------------*/
335  case YPSPUR_JOINT_TORQUE:
336  joint_torque_com(msg.cs, msg.data, &g_spur);
337  yprintf(OUTPUT_LV_DEBUG, "Command: joint %d torque %f\n", msg.cs, msg.data[0]);
338  break;
339 
340  case YPSPUR_JOINT_VEL:
341  joint_vel_com(msg.cs, msg.data, &g_spur);
342  yprintf(OUTPUT_LV_DEBUG, "Command: joint %d vel %f\n", msg.cs, msg.data[0]);
343  break;
344 
345  case YPSPUR_JOINT_ANG:
346  joint_ang_com(msg.cs, msg.data, &g_spur);
347  yprintf(OUTPUT_LV_DEBUG, "Command: joint %d ang %f\n", msg.cs, msg.data[0]);
348  break;
349 
350  case YPSPUR_JOINT_ANG_VEL:
351  joint_ang_vel_com(msg.cs, msg.data, &g_spur);
352  yprintf(OUTPUT_LV_DEBUG, "Command: joint %d ang %f vel %f\n", msg.cs, msg.data[0], msg.data[1]);
353  break;
354 
355  case YPSPUR_SET_JOINT_ACCEL:
356  set_joint_accel_com(msg.cs, msg.data, &g_spur);
357  yprintf(OUTPUT_LV_DEBUG, "Command: set joint %d accel %f\n", msg.cs, msg.data[0]);
358  break;
359 
360  case YPSPUR_SET_JOINT_VEL:
361  set_joint_vel_com(msg.cs, msg.data, &g_spur);
362  yprintf(OUTPUT_LV_DEBUG, "Command: set joint %d vel %f\n", msg.cs, msg.data[0]);
363  break;
364 
365  case YPSPUR_GET_JOINT_VEL:
366  get_joint_vel_com(msg.cs, res_msg.data, &g_spur);
367  message_return(&ipcmd, msg.pid, &res_msg);
368  yprintf(OUTPUT_LV_DEBUG, "Command: get joint %d vel %f\n", msg.cs, res_msg.data[0]);
369  break;
370 
371  case YPSPUR_GET_JOINT_VREF:
372  get_joint_vref_com(msg.cs, res_msg.data, &g_spur);
373  message_return(&ipcmd, msg.pid, &res_msg);
374  yprintf(OUTPUT_LV_DEBUG, "Command: get joint %d vref %f\n", msg.cs, res_msg.data[0]);
375  break;
376 
377  case YPSPUR_GET_JOINT_ANG:
378  get_joint_ang_com(msg.cs, res_msg.data, &g_spur);
379  message_return(&ipcmd, msg.pid, &res_msg);
380  yprintf(OUTPUT_LV_DEBUG, "Command: get joint %d ang %f\n", msg.cs, res_msg.data[0]);
381  break;
382 
383  case YPSPUR_GET_JOINT_TORQUE:
384  get_joint_torque_com(msg.cs, res_msg.data, &g_spur);
385  message_return(&ipcmd, msg.pid, &res_msg);
386  yprintf(OUTPUT_LV_DEBUG, "Command: get joint %d torque %f\n", msg.cs, res_msg.data[0]);
387  break;
388 
389  default:
390  yprintf(OUTPUT_LV_WARNING, "Command: unknown\n");
391  break;
392  }
393 
394  /* 走行モードに変化があったか */
395  if (g_spur.run_mode != g_spur.before_run_mode || g_spur.before_freeze != g_spur.freeze)
396  {
397  if (!g_spur.freeze)
398  {
399  if (g_spur.run_mode == RUN_FREE || g_spur.run_mode == RUN_WHEEL_TORQUE)
400  { /* フリーになった */
401  int i;
402  for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
403  {
404  if (!param->motor_enable[i])
405  continue;
406  if (p(YP_PARAM_VEHICLE_CONTROL, i) > 0)
407  g_spur.wheel_mode[i] = MOTOR_CONTROL_FREE;
408  }
409  yprintf(OUTPUT_LV_DEBUG, "Mode: free\n");
410  }
411  else if (g_spur.run_mode == RUN_OPENFREE)
412  {
413  int i;
414  for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
415  {
416  if (!param->motor_enable[i])
417  continue;
418  if (p(YP_PARAM_VEHICLE_CONTROL, i) > 0)
419  g_spur.wheel_mode[i] = MOTOR_CONTROL_OPENFREE;
420  }
421  yprintf(OUTPUT_LV_DEBUG, "Mode: openfree\n");
422  }
423  else
424  {
425  int i;
426  for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
427  {
428  if (!param->motor_enable[i])
429  continue;
430  if (p(YP_PARAM_VEHICLE_CONTROL, i) > 0)
431  g_spur.wheel_mode[i] = MOTOR_CONTROL_VEHICLE;
432  }
433  yprintf(OUTPUT_LV_DEBUG, "Mode: servo %d\n", g_spur.run_mode);
434  }
435  }
436  else
437  {
438  yprintf(OUTPUT_LV_DEBUG, "Mode: freeze\n");
439  }
440  }
441  g_spur.before_run_mode = g_spur.run_mode;
442  g_spur.before_freeze = g_spur.freeze;
443 
444  pthread_mutex_unlock(&g_spur.mutex);
445  // スレッドの停止要求チェック
446  pthread_testcancel();
447  }
448 
449  pthread_cleanup_pop(1);
450 }
451 
452 /* メッセージを返す */
453 void message_return(struct ipcmd_t *ipcmd, long retpid, YPSpur_msg *res_msg)
454 {
455  res_msg->type = 0;
456  res_msg->msg_type = retpid;
457  res_msg->pid = 0;
458  ipcmd->send(ipcmd, res_msg);
459 }
460 
461 /* すれっどの初期化 */
462 void init_command_thread(pthread_t *thread)
463 {
464  if (pthread_create(thread, NULL, (void *)command, NULL) != 0)
465  {
466  yprintf(OUTPUT_LV_ERROR, "Can't create command thread\n");
467  }
468 }
free_com
void free_com(double *data, SpurUserParamsPtr spur)
Definition: command_run.c:129
g_spur
SpurUserParams g_spur
Definition: command.c:51
set_io_data_com
void set_io_data_com(double *data, double *resdata)
Definition: command_aux.c:64
command
void command(void)
Definition: command.c:87
_spur_user_params::run_mode
SpurRunMode run_mode
Definition: command.h:85
set_ang_accel_com
void set_ang_accel_com(double *data, SpurUserParamsPtr spur)
Definition: command_set.c:161
set_wheel_accel_com
void set_wheel_accel_com(double *data, SpurUserParamsPtr spur)
Definition: command_set.c:184
param_state_com
void param_state_com(int cs, double *data, SpurUserParamsPtr spur)
Definition: command_param.c:93
get_joint_torque_com
void get_joint_torque_com(int id, double *data, SpurUserParamsPtr spur)
Definition: command_joint.c:157
init_spur_command
void init_spur_command(void)
Definition: command.c:68
_spur_user_params::w
double w
Definition: command.h:60
_spur_user_params::dir
double dir
Definition: command.h:81
message_return
void message_return(struct ipcmd_t *ipcmd, long retpid, YPSpur_msg *res_msg)
Definition: command.c:453
_spur_user_params::wvelref
double wvelref[YP_PARAM_MAX_MOTOR_NUM]
Definition: command.h:65
set_joint_accel_com
void set_joint_accel_com(int id, double *data, SpurUserParamsPtr spur)
Definition: command_joint.c:92
ipcmd_open_tcp
int ipcmd_open_tcp(struct ipcmd_t *ipcmd, char *host, int port)
Definition: ipcommunication.c:137
_spur_user_params::theta
double theta
Definition: command.h:78
set_torque_com
void set_torque_com(double *data, SpurUserParamsPtr spur)
Definition: command_set.c:136
YPSpur_msg::pid
long pid
Definition: ypparam.h:485
get_wheel_vel_com
void get_wheel_vel_com(double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:122
ipcmd_t::flush
void(* flush)(struct ipcmd_t *ipcmd)
Definition: ypparam.h:512
_spur_user_params::wheel_accel
double wheel_accel[YP_PARAM_MAX_MOTOR_NUM]
Definition: command.h:71
p
double p(YPSpur_param id, enum motor_id motor)
Definition: param.c:79
YPSpur_msg::type
int type
Definition: ypparam.h:486
set_adjust_com
void set_adjust_com(int cs, double *data, SpurUserParamsPtr spur)
Definition: command_set.c:91
get_wheel_vref_com
void get_wheel_vref_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:61
_spur_user_params::before_freeze
int before_freeze
Definition: command.h:84
command_loop_cleanup
void command_loop_cleanup(void *data)
Definition: command.c:58
YP_PARAM_NUM
パラメータの最大値
Definition: ypparam.h:207
param_set_com
void param_set_com(int cs, double *data, SpurUserParamsPtr spur)
Definition: command_param.c:51
set_GL_on_GL_com
void set_GL_on_GL_com(double *data, SpurUserParamsPtr spur)
Definition: command_set.c:80
set_pos_com
void set_pos_com(int cs, double *data, SpurUserParamsPtr spur)
Definition: command_set.c:49
_spur_user_params::wheel_mode
MotorControlMode wheel_mode[YP_PARAM_MAX_MOTOR_NUM]
Definition: command.h:76
set_tilt_com
void set_tilt_com(int cs, double *data, SpurUserParamsPtr spur)
Definition: command_set.c:170
get_spur_user_param_ptr
SpurUserParamsPtr get_spur_user_param_ptr()
Definition: command.c:53
_spur_user_params::wref
double wref
Definition: command.h:62
_spur_user_params::radius
double radius
Definition: command.h:79
joint_vel_com
void joint_vel_com(int id, double *data, SpurUserParamsPtr spur)
Definition: command_joint.c:58
wheel_vel_com
void wheel_vel_com(double *data, SpurUserParamsPtr spur)
Definition: command_run.c:146
near_pos_com
int near_pos_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:148
_spur_user_params::wheel_angle
double wheel_angle[YP_PARAM_MAX_MOTOR_NUM]
Definition: command.h:74
get_error_state_com
void get_error_state_com(double *data, double *resdata)
Definition: command_aux.c:72
_spur_user_params::y
double y
Definition: command.h:67
get_vref_com
void get_vref_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:72
get_pos_com
void get_pos_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:48
over_line_com
int over_line_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:199
near_ang_com
int near_ang_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:173
_spur_user_params::wheel_vel
double wheel_vel[YP_PARAM_MAX_MOTOR_NUM]
Definition: command.h:72
get_wheel_ang_com
void get_wheel_ang_com(double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:135
_spur_user_params
Definition: command.h:55
spin_com
void spin_com(int cs, double *data, SpurUserParamsPtr spur)
Definition: command_run.c:96
get_ad_com
void get_ad_com(double *data, double *resdata)
Definition: command_aux.c:48
line_com
void line_com(int cs, double *data, SpurUserParamsPtr spur)
Definition: command_run.c:48
YPSpur_msg
Definition: ypparam.h:482
ipcmd_t::recv
int(* recv)(struct ipcmd_t *ipcmd, YPSpur_msg *data)
Definition: ypparam.h:511
set_accel_com
void set_accel_com(double *data, SpurUserParamsPtr spur)
Definition: command_set.c:152
get_vel_com
void get_vel_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:83
YPSpur_msg::msg_type
long msg_type
Definition: ypparam.h:484
YP_PARAM_MAX_MOTOR_NUM
#define YP_PARAM_MAX_MOTOR_NUM
Definition: ypparam.h:430
YPSpur_msg::cs
int cs
Definition: ypparam.h:487
wheel_angle_com
void wheel_angle_com(double *data, SpurUserParamsPtr spur)
Definition: command_run.c:153
_spur_user_params::before_run_mode
SpurRunMode before_run_mode
Definition: command.h:87
param_get_com
int param_get_com(int cs, double *resdata, SpurUserParamsPtr spur)
Definition: command_param.c:60
_spur_user_params::tilt
double tilt
Definition: command.h:80
yprintf
void yprintf(ParamOutputLv level, const char *format,...)
Definition: yprintf.c:32
YPSpur_msg::data
double data[4]
Definition: ypparam.h:488
_parameters::motor_enable
int motor_enable[YP_PARAM_MAX_MOTOR_NUM]
Definition: param.h:82
_spur_user_params::v
double v
Definition: command.h:59
_spur_user_params::dw
double dw
Definition: command.h:58
YPSPUR_MSG_SIZE
#define YPSPUR_MSG_SIZE
Definition: ypparam.h:517
set_io_dir_com
void set_io_dir_com(double *data, double *resdata)
Definition: command_aux.c:56
set_vel_com
void set_vel_com(double *data, SpurUserParamsPtr spur)
Definition: command_set.c:127
joint_ang_vel_com
void joint_ang_vel_com(int id, double *data, SpurUserParamsPtr spur)
Definition: command_joint.c:80
_spur_user_params::mutex
pthread_mutex_t mutex
Definition: command.h:88
_spur_user_params::dv
double dv
Definition: command.h:57
_parameters
Definition: param.h:71
set_joint_vel_com
void set_joint_vel_com(int id, double *data, SpurUserParamsPtr spur)
Definition: command_joint.c:102
get_force_com
void get_force_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:96
circle_com
void circle_com(int cs, double *data, SpurUserParamsPtr spur)
Definition: command_run.c:80
option
int option(ParamOptions option)
Definition: param.c:99
get_joint_ang_com
void get_joint_ang_com(int id, double *data, SpurUserParamsPtr spur)
Definition: command_joint.c:142
_spur_user_params::x
double x
Definition: command.h:66
stop_line_com
void stop_line_com(int cs, double *data, SpurUserParamsPtr spur)
Definition: command_run.c:64
set_ang_vel_com
void set_ang_vel_com(double *data, SpurUserParamsPtr spur)
Definition: command_set.c:143
init_command_thread
void init_command_thread(pthread_t *thread)
Definition: command.c:462
joint_ang_com
void joint_ang_com(int id, double *data, SpurUserParamsPtr spur)
Definition: command_joint.c:69
_spur_user_params::vref
double vref
Definition: command.h:61
YP_PARAM_NAME
#define YP_PARAM_NAME
Definition: ypparam.h:211
vel_com
void vel_com(double *data, SpurUserParamsPtr spur)
Definition: command_run.c:139
joint_torque_com
void joint_torque_com(int id, double *data, SpurUserParamsPtr spur)
Definition: command_joint.c:47
ipcmd_close
void ipcmd_close(struct ipcmd_t *ipcmd)
Definition: ipcommunication.c:332
ipcmd_t::send
int(* send)(struct ipcmd_t *ipcmd, YPSpur_msg *data)
Definition: ypparam.h:510
get_param_ptr
ParametersPtr get_param_ptr()
Definition: param.c:111
stop_com
void stop_com(double *data, SpurUserParamsPtr spur)
Definition: command_run.c:124
get_joint_vref_com
void get_joint_vref_com(int id, double *data, SpurUserParamsPtr spur)
Definition: command_joint.c:127
get_joint_vel_com
void get_joint_vel_com(int id, double *data, SpurUserParamsPtr spur)
Definition: command_joint.c:112
ipcmd_open_msq
int ipcmd_open_msq(struct ipcmd_t *ipcmd, int key, int creat)
Definition: ipcommunication.c:59
get_wheel_torque_com
void get_wheel_torque_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:109
set_wheel_vel_com
void set_wheel_vel_com(double *data, SpurUserParamsPtr spur)
Definition: command_set.c:190
orient_com
void orient_com(int cs, double *data, SpurUserParamsPtr spur)
Definition: command_run.c:110
ipcmd_t
Definition: ypparam.h:493
_spur_user_params::freeze
int freeze
Definition: command.h:83
openfree_com
void openfree_com(double *data, SpurUserParamsPtr spur)
Definition: command_run.c:134
_spur_user_params::run_mode_cnt
int run_mode_cnt
Definition: command.h:86

yp-spur
Author(s):
autogenerated on Sat May 11 2019 02:08:24