00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #ifndef YPSPUR_H
00022 #define YPSPUR_H
00023
00024 #include <ypparam.h>
00025
00026 #ifdef __cplusplus
00027 extern "C" {
00028 #endif // __cplusplus
00029
00030
00031 #define Spur_init() YPSpur_init()
00032 #define Spur_initex(msq) YPSpur_initex(msq)
00033 #define Spur_init_socket(ip, port) YPSpur_init_socket(ip, port)
00034 #define Spur_stop() YPSpur_stop()
00035 #define Spur_free() YPSpur_free()
00036
00037 #define Spur_freeze() YPSpur_freeze()
00038 #define Spur_isfreeze() YPSpur_isfreeze()
00039 #define Spur_unfreeze() YPSpur_unfreeze()
00040
00041 #define Spur_line_GL(x, y, th) YPSpur_line(CS_GL, x, y, th)
00042 #define Spur_line_LC(x, y, th) YPSpur_line(CS_LC, x, y, th)
00043 #define Spur_line_FS(x, y, th) YPSpur_line(CS_FS, x, y, th)
00044 #define Spur_line_BS(x, y, th) YPSpur_line(CS_BS, x, y, th)
00045 #define Spur_line_BL(x, y, th) YPSpur_line(CS_BL, x, y, th)
00046
00047 #define Spur_stop_line_GL(x, y, th) YPSpur_stop_line(CS_GL, x, y, th)
00048 #define Spur_stop_line_LC(x, y, th) YPSpur_stop_line(CS_LC, x, y, th)
00049 #define Spur_stop_line_FS(x, y, th) YPSpur_stop_line(CS_FS, x, y, th)
00050 #define Spur_stop_line_BS(x, y, th) YPSpur_stop_line(CS_BS, x, y, th)
00051 #define Spur_stop_line_BL(x, y, th) YPSpur_stop_line(CS_BL, x, y, th)
00052
00053 #define Spur_circle_GL(x, y, d) YPSpur_circle(CS_GL, x, y, d)
00054 #define Spur_circle_LC(x, y, d) YPSpur_circle(CS_LC, x, y, d)
00055 #define Spur_circle_FS(x, y, d) YPSpur_circle(CS_FS, x, y, d)
00056 #define Spur_circle_BS(x, y, d) YPSpur_circle(CS_BS, x, y, d)
00057 #define Spur_circle_BL(x, y, d) YPSpur_circle(CS_BL, x, y, d)
00058
00059 #define Spur_spin_GL(th) YPSpur_spin(CS_GL, th)
00060 #define Spur_spin_LC(th) YPSpur_spin(CS_LC, th)
00061 #define Spur_spin_FS(th) YPSpur_spin(CS_FS, th)
00062 #define Spur_spin_BS(th) YPSpur_spin(CS_BS, th)
00063 #define Spur_spin_BL(th) YPSpur_spin(CS_BL, th)
00064
00065 #define Spur_orient_GL(th) YPSpur_orient(CS_GL, th)
00066 #define Spur_orient_LC(th) YPSpur_orient(CS_LC, th)
00067 #define Spur_orient_FS(th) YPSpur_orient(CS_FS, th)
00068 #define Spur_orient_BS(th) YPSpur_orient(CS_BS, th)
00069 #define Spur_orient_BL(th) YPSpur_orient(CS_BL, th)
00070
00071 #define Spur_set_pos_GL(x, y, th) YPSpur_set_pos(CS_GL, x, y, th)
00072 #define Spur_set_pos_LC(x, y, th) YPSpur_set_pos(CS_LC, x, y, th)
00073 #define Spur_set_pos_BL(x, y, th) YPSpur_set_pos(CS_BL, x, y, th)
00074
00075 #define Spur_set_vel(v) YPSpur_set_vel(v)
00076 #define Spur_set_angvel(w) YPSpur_set_angvel(w)
00077 #define Spur_set_accel(v) YPSpur_set_accel(v)
00078 #define Spur_set_angaccel(w) YPSpur_set_angaccel(w)
00079
00080 #define Spur_adjust_pos_GL(x, y, th) YPSpur_adjust_pos(CS_GL, x, y, th)
00081 #define Spur_adjust_pos_LC(x, y, th) YPSpur_adjust_pos(CS_LC, x, y, th)
00082 #define Spur_adjust_pos_FS(x, y, th) YPSpur_adjust_pos(CS_FS, x, y, th)
00083 #define Spur_adjust_pos_BS(x, y, th) YPSpur_adjust_pos(CS_BS, x, y, th)
00084 #define Spur_adjust_pos_BL(x, y, th) YPSpur_adjust_pos(CS_BL, x, y, th)
00085
00086 #define Spur_get_pos_GL(x, y, th) YPSpur_get_pos(CS_GL, x, y, th)
00087 #define Spur_get_pos_LC(x, y, th) YPSpur_get_pos(CS_LC, x, y, th)
00088 #define Spur_get_pos_BS(x, y, th) YPSpur_get_pos(CS_BS, x, y, th)
00089 #define Spur_get_pos_BL(x, y, th) YPSpur_get_pos(CS_BL, x, y, th)
00090
00091 #define Spur_get_vel(v, w) YPSpur_get_vel(v, w)
00092 #define Spur_get_force(trans, angular) YPSpur_get_force(trans, angular)
00093
00094 #define Spur_near_pos_GL(x, y, r) YPSpur_near_pos(CS_GL, x, y, r)
00095 #define Spur_near_pos_LC(x, y, r) YPSpur_near_pos(CS_LC, x, y, r)
00096 #define Spur_near_pos_BS(x, y, r) YPSpur_near_pos(CS_BS, x, y, r)
00097 #define Spur_near_pos_BL(x, y, r) YPSpur_near_pos(CS_BL, x, y, r)
00098
00099 #define Spur_near_ang_GL(th, d) YPSpur_near_ang(CS_GL, th, d)
00100 #define Spur_near_ang_LC(th, d) YPSpur_near_ang(CS_LC, th, d)
00101 #define Spur_near_ang_BS(th, d) YPSpur_near_ang(CS_BS, th, d)
00102 #define Spur_near_ang_BL(th, d) YPSpur_near_ang(CS_BL, th, d)
00103
00104 #define Spur_over_line_GL(x, y, th) YPSpur_over_line(CS_GL, x, y, th)
00105 #define Spur_over_line_LC(x, y, th) YPSpur_over_line(CS_LC, x, y, th)
00106 #define Spur_over_line_BS(x, y, th) YPSpur_over_line(CS_BS, x, y, th)
00107 #define Spur_over_line_BL(x, y, th) YPSpur_over_line(CS_BL, x, y, th)
00108
00109 #define Spur_vel(v, w) YPSpur_vel(v, w)
00110
00111 #define Spur_tilt_GL(d, t) YPSpur_tilt(CS_GL, d, t)
00112 #define Spur_tilt_LC(d, t) YPSpur_tilt(CS_LC, d, t)
00113 #define Spur_tilt_FS(d, t) YPSpur_tilt(CS_FS, d, t)
00114 #define Spur_tilt_BS(d, t) YPSpur_tilt(CS_BS, d, t)
00115 #define Spur_tilt_BL(d, t) YPSpur_tilt(CS_BL, d, t)
00116
00118 int YPSpur_init(void);
00119 int YPSpur_initex(int msq_key);
00120 int YPSpur_init_socket(char *ip, int port);
00121
00123 int YPSpur_isfreeze(void);
00124 int YPSpur_freeze(void);
00125 int YPSpur_unfreeze(void);
00126
00128 int YPSpur_stop(void);
00129 int YPSpur_free(void);
00130 int YP_openfree(void);
00131
00133 int YPSpur_line(int cs, double x, double y, double theta);
00134 int YPSpur_stop_line(int cs, double x, double y, double theta);
00135 int YPSpur_circle(int cs, double x, double y, double r);
00136 int YPSpur_spin(int cs, double theta);
00137 int YPSpur_orient(int cs, double theta);
00138
00140 int YPSpur_set_pos(int cs, double x, double y, double theta);
00141 int YPSpur_adjust_pos(int cs, double x, double y, double theta);
00142 int YPSpur_set_vel(double v);
00143 int YPSpur_set_angvel(double w);
00144 int YPSpur_set_accel(double v);
00145 int YPSpur_set_angaccel(double w);
00146
00148 double YPSpur_get_pos(int cs, double *x, double *y, double *theta);
00149 double YPSpur_get_vel(double *v, double *w);
00150 double YPSpur_get_force(double *trans, double *angular);
00151
00153 int YPSpur_near_pos(int cs, double x, double y, double r);
00154 int YPSpur_near_ang(int cs, double th, double d);
00155 int YPSpur_over_line(int cs, double x, double y, double theta);
00156
00158 int YPSpur_vel(double v, double w);
00159
00161 int YPSpur_tilt(int cs, double dir, double tilt);
00162
00163
00164 int YP_get_error_state(void);
00165
00166 int YP_set_parameter(int param_id, double value);
00167 int YP_set_parameter_array(int param_id, double *value);
00168 int YP_get_parameter(int param_id, double *value);
00169 int YP_get_parameter_array(int param_id, double *value);
00170 int YP_set_control_state(int control_id, int state);
00171
00172 int YP_get_ad_value(int num);
00173 int YP_set_io_dir(unsigned char dir);
00174 int YP_set_io_data(unsigned char data);
00175 double YP_get_device_error_state(int id, int *err);
00176 int YP_wheel_vel(double r, double l);
00177 int YP_wheel_torque(double r, double l);
00178 double YP_get_wheel_vel(double *wr, double *wl);
00179 double YP_get_wheel_ang(double *theta_r, double *theta_l);
00180 double YP_get_wheel_torque(double *torque_r, double *torque_l);
00181 int YP_wheel_ang(double r, double l);
00182 int YP_set_wheel_accel(double r, double l);
00183 int YP_set_wheel_vel(double r, double l);
00184 double YP_get_vref(double *vref, double *wref);
00185 double YP_get_wheel_vref(double *wrref, double *wlref);
00186
00187 int YP_joint_torque(int id, double t);
00188 int YP_joint_vel(int id, double v);
00189 int YP_joint_ang(int id, double a);
00190 int YP_joint_ang_vel(int id, double a, double v);
00191 int YP_set_joint_accel(int id, double a);
00192 int YP_set_joint_vel(int id, double v);
00193 double YP_get_joint_vel(int id, double *v);
00194 double YP_get_joint_vref(int id, double *v);
00195 double YP_get_joint_ang(int id, double *a);
00196 double YP_get_joint_torque(int id, double *t);
00197
00198 #define YPSPUR_JOINT_SUPPORT 1
00199 #define YPSPUR_JOINT_ANG_VEL_SUPPORT 1
00200 #define YPSPUR_GET_DEVICE_ERROR_STATE_SUPPORT 1
00201
00202 #ifdef __cplusplus
00203 }
00204 #endif // __cplusplus
00205 #endif // YPSPUR_H