00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #ifndef YPPARAM_H
00022 #define YPPARAM_H
00023
00024 #ifdef __cplusplus
00025 extern "C" {
00026 #endif // __cplusplus
00027
00028 #if defined(_WIN32)
00029 #if !defined(_WIN64)
00030 typedef int pid_t;
00031 #endif // !defined(_WIN64)
00032 #else
00033 #include <unistd.h>
00034 #endif // defined(_WIN32)
00035
00036
00037 enum
00038 {
00039 YPSPUR_FREE = 0x10,
00040 YPSPUR_SERVO,
00041 YPSPUR_OPENFREE,
00042
00043 YPSPUR_VEL = 0x20,
00044 YPSPUR_LINE,
00045 YPSPUR_CIRCLE,
00046 YPSPUR_WHEEL_VEL,
00047 YPSPUR_WHEEL_TORQUE,
00048
00049 YPSPUR_ORIENT = 0x30,
00050
00051 YPSPUR_SPIN = 0x40,
00052 YPSPUR_STOP_LINE,
00053 YPSPUR_STOP,
00054 YPSPUR_WHEEL_ANGLE,
00055
00056 YPSPUR_SET_VEL = 0x50,
00057 YPSPUR_SET_ANGVEL,
00058 YPSPUR_SET_ACCEL,
00059 YPSPUR_SET_ANGACCEL,
00060 YPSPUR_SET_POS,
00061 YPSPUR_SET_GL_GL,
00062 YPSPUR_SET_TILT,
00063 YPSPUR_ADJUST,
00064 YPSPUR_SET_WHEEL_VEL,
00065 YPSPUR_SET_WHEEL_ACCEL,
00066
00067 YPSPUR_GET_POS = 0x70,
00068 YPSPUR_GET_VEL,
00069 YPSPUR_NEAR_POS,
00070 YPSPUR_NEAR_ANG,
00071 YPSPUR_OVER_LINE,
00072 YPSPUR_GET_WHEEL_VEL,
00073 YPSPUR_GET_WHEEL_ANG,
00074 YPSPUR_GET_FORCE,
00075 YPSPUR_GET_VREF,
00076 YPSPUR_GET_WHEEL_VREF,
00077
00078 YPSPUR_PARAM_SET = 0x100,
00079 YPSPUR_PARAM_GET,
00080 YPSPUR_PARAM_STATE,
00081 YPSPUR_GET_WHEEL_TORQUE,
00082
00083 YPSPUR_FREEZE = 0x200,
00084 YPSPUR_UNFREEZE,
00085 YPSPUR_ISFREEZE,
00086
00087 YPSPUR_GETAD = 0x500,
00088 YPSPUR_SETIODIR,
00089 YPSPUR_SETIODATA,
00090 YPSPUR_GET_ERROR_STATE,
00091
00092 YPSPUR_JOINT_TORQUE = 0x800,
00093 YPSPUR_JOINT_VEL,
00094 YPSPUR_JOINT_ANG,
00095 YPSPUR_JOINT_ANG_VEL,
00096
00097 YPSPUR_SET_JOINT_ACCEL = 0x810,
00098 YPSPUR_SET_JOINT_VEL,
00099
00100 YPSPUR_GET_JOINT_VEL = 0x820,
00101 YPSPUR_GET_JOINT_VREF,
00102 YPSPUR_GET_JOINT_ANG,
00103 YPSPUR_GET_JOINT_TORQUE,
00104 };
00105
00106
00107
00108
00109 typedef enum
00110 {
00111
00112 YP_PARAM_VERSION = 0,
00113
00114 YP_PARAM_TORQUE_UNIT,
00115 YP_PARAM_TORQUE_FINENESS,
00116 YP_PARAM_PWM_MAX,
00117
00118
00119 YP_PARAM_COUNT_REV,
00120 YP_PARAM_ENCODER_TYPE,
00121 YP_PARAM_VOLT,
00122 YP_PARAM_CYCLE,
00123 YP_PARAM_GEAR,
00124 YP_PARAM_MOTOR_R,
00125 YP_PARAM_MOTOR_TC,
00126 YP_PARAM_MOTOR_VC,
00127 YP_PARAM_MOTOR_PHASE,
00128 YP_PARAM_PHASE_OFFSET,
00129
00130
00131 YP_PARAM_RADIUS,
00132 YP_PARAM_RADIUS_R,
00133 YP_PARAM_RADIUS_L,
00134 YP_PARAM_TREAD,
00135
00136
00137 YP_PARAM_CONTROL_CYCLE,
00138 YP_PARAM_MAX_VEL,
00139 YP_PARAM_MAX_W,
00140 YP_PARAM_MAX_ACC_V,
00141 YP_PARAM_MAX_ACC_W,
00142 YP_PARAM_MAX_CENTRIFUGAL_ACC,
00143
00144
00145 YP_PARAM_L_C1,
00146 YP_PARAM_L_K1,
00147 YP_PARAM_L_K2,
00148 YP_PARAM_L_K3,
00149 YP_PARAM_L_DIST,
00150
00151
00152 YP_PARAM_GAIN_KP,
00153 YP_PARAM_GAIN_KI,
00154 YP_PARAM_TORQUE_MAX,
00155 YP_PARAM_TORQUE_NEWTON,
00156 YP_PARAM_TORQUE_VISCOS,
00157 YP_PARAM_TORQUE_NEWTON_NEG,
00158 YP_PARAM_TORQUE_VISCOS_NEG,
00159 YP_PARAM_INTEGRAL_MAX,
00160 YP_PARAM_TORQUE_OFFSET,
00161 YP_PARAM_TORQUE_LIMIT,
00162
00163
00164 YP_PARAM_MASS,
00165 YP_PARAM_MOMENT_INERTIA,
00166 YP_PARAM_MOTOR_M_INERTIA,
00167 YP_PARAM_TIRE_M_INERTIA,
00168
00169
00170 YP_PARAM_SIZE_FRONT,
00171 YP_PARAM_SIZE_REAR,
00172 YP_PARAM_SIZE_LEFT,
00173 YP_PARAM_SIZE_RIGHT,
00174
00175
00176 YP_PARAM_GAIN_A,
00177 YP_PARAM_GAIN_B,
00178 YP_PARAM_GAIN_C,
00179 YP_PARAM_GAIN_D,
00180 YP_PARAM_GAIN_E,
00181 YP_PARAM_GAIN_F,
00182
00183
00184 YP_PARAM_STOP_LINEAR,
00185 YP_PARAM_SPIN_LINEAR,
00186 YP_PARAM_WHEEL_ANG_LINEAR,
00187
00188 YP_PARAM_MIN_WHEEL_ANGLE,
00189 YP_PARAM_MAX_WHEEL_ANGLE,
00190
00191 YP_PARAM_VEHICLE_CONTROL,
00192 YP_PARAM_CONTROL_MODE_RESEND,
00193
00194
00195 YP_PARAM_INERTIA_SELF,
00196 YP_PARAM_INERTIA_CROSS,
00197
00198
00199 YP_PARAM_ENCODER_DIV,
00200 YP_PARAM_ENCODER_DENOMINATOR,
00201
00202
00203 YP_PARAM_INDEX_RISE_ANGLE,
00204 YP_PARAM_INDEX_FALL_ANGLE,
00205 YP_PARAM_INDEX_GEAR,
00206
00207 YP_PARAM_NUM
00208 } YPSpur_param;
00209
00210
00211 #define YP_PARAM_NAME \
00212 { \
00213 "VERSION", \
00214 "_TORQUE_UNIT", \
00215 "TORQUE_FINENESS", \
00216 "_PWM_RESOLUTION", \
00217 "COUNT_REV", \
00218 "ENCODER_TYPE", \
00219 "VOLT", \
00220 "CYCLE", \
00221 "GEAR", \
00222 "MOTOR_R", \
00223 "MOTOR_TC", \
00224 "MOTOR_VC", \
00225 "MOTOR_PHASE", \
00226 "PHASE_OFFSET", \
00227 "RADIUS", \
00228 "RADIUS_R", \
00229 "RADIUS_L", \
00230 "TREAD", \
00231 "CONTROL_CYCLE", \
00232 "MAX_VEL", \
00233 "MAX_W", \
00234 "MAX_ACC_V", \
00235 "MAX_ACC_W", \
00236 "MAX_CENTRI_ACC", \
00237 "L_C1", \
00238 "L_K1", \
00239 "L_K2", \
00240 "L_K3", \
00241 "L_DIST", \
00242 "GAIN_KP", \
00243 "GAIN_KI", \
00244 "TORQUE_MAX", \
00245 "TORQUE_NEWTON", \
00246 "TORQUE_VISCOS", \
00247 "-TORQUE_NEWTON", \
00248 "-TORQUE_VISCOS", \
00249 "INTEGRAL_MAX", \
00250 "TORQUE_OFFSET", \
00251 "TORQUE_LIMIT", \
00252 "MASS", \
00253 "MOMENT_INERTIA", \
00254 "MOTOR_M_INERTIA", \
00255 "TIRE_M_INERTIA", \
00256 "SIZE_FRONT", \
00257 "SIZE_REAR", \
00258 "SIZE_LEFT", \
00259 "SIZE_RIGHT", \
00260 "_GAIN_A", \
00261 "_GAIN_B", \
00262 "_GAIN_C", \
00263 "_GAIN_D", \
00264 "_GAIN_E", \
00265 "_GAIN_F", \
00266 "STOP_LINEAR", \
00267 "SPIN_LINEAR", \
00268 "WHEEL_ANG_LINEAR", \
00269 "MIN_WHEEL_ANGLE", \
00270 "MAX_WHEEL_ANGLE", \
00271 "VEHICLE_CONTROL", \
00272 "CONTROL_MODE_RESEND", \
00273 "_INERTIA_SELF", \
00274 "_INERTIA_CROSS", \
00275 "ENCODER_DIV", \
00276 "ENCODER_DENOMINATOR", \
00277 "INDEX_RISE_ANGLE", \
00278 "INDEX_FALL_ANGLE", \
00279 "INDEX_GEAR", \
00280 }
00281
00282 #define YP_PARAM_NECESSARY \
00283 { \
00284 1, \
00285 0, \
00286 1, \
00287 0, \
00288 1, \
00289 0, \
00290 1, \
00291 1, \
00292 1, \
00293 1, \
00294 1, \
00295 1, \
00296 0, \
00297 0, \
00298 1, \
00299 0, \
00300 0, \
00301 1, \
00302 1, \
00303 1, \
00304 1, \
00305 1, \
00306 1, \
00307 1, \
00308 1, \
00309 1, \
00310 1, \
00311 1, \
00312 1, \
00313 1, \
00314 1, \
00315 1, \
00316 0, \
00317 0, \
00318 0, \
00319 0, \
00320 1, \
00321 0, \
00322 0, \
00323 1, \
00324 1, \
00325 1, \
00326 1, \
00327 0, \
00328 0, \
00329 0, \
00330 0, \
00331 0, \
00332 0, \
00333 0, \
00334 0, \
00335 0, \
00336 0, \
00337 0, \
00338 0, \
00339 0, \
00340 0, \
00341 0, \
00342 0, \
00343 0, \
00344 0, \
00345 0, \
00346 0, \
00347 0, \
00348 0, \
00349 0, \
00350 0, \
00351 }
00352
00353 #define YP_PARAM_COMMENT \
00354 { \
00355 "Parameter file version", \
00356 "[Integer Nm/Nm] Fixed-point position of PC-MCU communication", \
00357 "[Nm] Calculation fineness of torque control", \
00358 "[Counts] PWM cycle", \
00359 "[Counts/rev] Encoder specification", \
00360 "Encoder type (2:2-phase incremental, 3:3-phase incremental)", \
00361 "[V] Power source voltage", \
00362 "[s] Velocity control cycle", \
00363 "[in/out] Gear ratio", \
00364 "[ohm] Motor internal resistance", \
00365 "[Nm/A] Motor torque constant", \
00366 "[rpm/V] Motor speed constant", \
00367 "Motor type (0:DC, 3:3phase-AC)", \
00368 "[rad] Offset angle of AC motor phase", \
00369 "[m] Wheel radius", \
00370 "[m] Right wheel radius", \
00371 "[m] Left wheel radius", \
00372 "[m] Tread", \
00373 "[s] Trajectory control cycle", \
00374 "[m/s] Maximum velocity", \
00375 "[rad/s] Maximum angular velocity", \
00376 "[m/ss] Maximum acceleration", \
00377 "[rad/ss] Maximum angular acceleration", \
00378 "[m/ss] Centrifugal acceleration limit", \
00379 "[m/s / rad/s] Deacceleration factor of trajectory control", \
00380 "[rad/ss / m] Feedback gain for distance error", \
00381 "[rad/ss / rad] Feedback gain for angular error", \
00382 "[rad/ss / rad/s] Feedback gain for angular velocity", \
00383 "[m] Clipping value of line following control", \
00384 "[1/s] PI control parameter Kp", \
00385 "[1/ss] PI control parameter Ki", \
00386 "[Nm] Motor maximum torque", \
00387 "[Nm] Constant friction", \
00388 "[Nm/(rad/s)] Viscous friction", \
00389 "[Nm] Constant friction (neg-direction)", \
00390 "[Nm/(rad/s)] Viscous friction (neg-direction)", \
00391 "[Nm] Motor torque limit", \
00392 "[rev] Maximum integrated error of PI control", \
00393 "[Nm] Offset value of motor torque", \
00394 "[kg] Robot weight", \
00395 "[kgm^2] Robot moment of inertia", \
00396 "[kgm^2] Rotor moment of inertia of motor", \
00397 "[kgm^2] Tire moment of inertia", \
00398 "[m] Robot size of front", \
00399 "[m] Robot size of rear", \
00400 "[m] Robot size of left", \
00401 "[m] Robot size of right", \
00402 "PWS parameter A", \
00403 "PWS parameter B", \
00404 "PWS parameter C", \
00405 "PWS parameter D", \
00406 "PWS parameter E", \
00407 "PWS parameter F", \
00408 "[m] Linear feedback area of stop command", \
00409 "[rad] Linear feedback area of spin command", \
00410 "[rad] Linear feedback area of wheel_ang command", \
00411 "[rad] Minimum wheel angle (for wheel_angle command)", \
00412 "[rad] Maximum wheel angle (for wheel_angle command)", \
00413 "Used for vehicle control (0: false, 1: true)", \
00414 "[s] Time-span to resend control mode (0: don't resend)", \
00415 "Motor load inertia", \
00416 "Motor load cross inertia", \
00417 "Encoder count divider 2^x (e.g. 4 means divide by 16)", \
00418 "Encoder count denominator" \
00419 " (COUNT_REV/ENCODER_DENOMINATOR is encoder resolution for one electrical revolution)", \
00420 "[rad] Index signal rising edge angle at CW rotation", \
00421 "[rad] Index signal falling edge angle at CW rotation", \
00422 "[in/out] Index signal gear ratio", \
00423 }
00424
00425 enum motor_id
00426 {
00427 MOTOR_RIGHT = 0,
00428 MOTOR_LEFT,
00429 };
00430 #define YP_PARAM_MAX_MOTOR_NUM 16
00431
00432 #define YP_PARAM_ALIAS_NUM 2
00433
00434 #define YP_PARAM_ALIAS \
00435 { \
00436 { \
00437 YP_PARAM_RADIUS_L, YP_PARAM_RADIUS, MOTOR_LEFT \
00438 }, \
00439 { \
00440 YP_PARAM_RADIUS_R, YP_PARAM_RADIUS, MOTOR_RIGHT \
00441 } \
00442 }
00443
00444
00445 #define YP_PARAM_REQUIRED_VERSION 4.0
00446 #define YP_PARAM_SUPPORTED_VERSION 5.0
00447
00448
00449
00450 typedef enum
00451 {
00452 YP_STATE_MOTOR = 0,
00453 YP_STATE_VELOCITY,
00454 YP_STATE_BODY,
00455 YP_STATE_TRACKING,
00456 YP_STATE_GRAVITY,
00457
00458 YP_STATE_NUM
00459 } YPSpur_state;
00460
00462 typedef enum
00463 {
00464 CS_BS = 0,
00465 CS_SP,
00466 CS_GL,
00467 CS_LC,
00468 CS_FS,
00469 CS_BL,
00470 CS_MAX
00471 } YPSpur_cs;
00472
00473 static const char YPSpur_CSName[CS_MAX][16] = {
00474 { "BS" },
00475 { "SP" },
00476 { "GL" },
00477 { "LC" },
00478 { "FS" },
00479 { "BL" },
00480 };
00481
00482 typedef struct
00483 {
00484 long msg_type;
00485 long pid;
00486 int type;
00487 int cs;
00488 double data[4];
00489 } YPSpur_msg;
00490
00491 #define YPSPUR_MAX_SOCKET 64
00492
00493 struct ipcmd_t
00494 {
00495 enum ipcmd_type_t
00496 {
00497 IPCMD_MSQ,
00498 IPCMD_TCP
00499 } type;
00500 enum ipcmd_tcp_type_t
00501 {
00502 IPCMD_TCP_SERVER,
00503 IPCMD_TCP_CLIENT
00504 } tcp_type;
00505 int socket;
00506 int clients[YPSPUR_MAX_SOCKET];
00507 int connection_error;
00508 pid_t pid;
00509 pid_t pids[YPSPUR_MAX_SOCKET];
00510 int (*send)(struct ipcmd_t *ipcmd, YPSpur_msg *data);
00511 int (*recv)(struct ipcmd_t *ipcmd, YPSpur_msg *data);
00512 void (*flush)(struct ipcmd_t *ipcmd);
00513 };
00514
00515 #define YPSPUR_MSQ_KEY 0x7045
00516 #define YPSPUR_MSG_CMD 1
00517 #define YPSPUR_MSG_SIZE (sizeof(YPSpur_msg) - sizeof(long))
00518
00519 #define ENABLE 1
00520 #define DISABLE 0
00521
00522 #ifdef __cplusplus
00523 }
00524 #endif // __cplusplus
00525
00526 #endif // YPPARAM_H