00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <math.h>
00022 #include <stdio.h>
00023 #include <strings.h>
00024 #include <unistd.h>
00025
00026 #include <fcntl.h>
00027 #include <sys/time.h>
00028 #include <sys/types.h>
00029 #include <time.h>
00030
00031 #ifdef HAVE_CONFIG_H
00032 #include <config.h>
00033 #endif // HAVE_CONFIG_H
00034
00035
00036 #include <shvel-param.h>
00037
00038
00039 #include <serial.h>
00040 #include <param.h>
00041 #include <control.h>
00042 #include <command.h>
00043 #include <odometry.h>
00044
00045
00046 #include <ypspur.h>
00047 #include <cartesian2d.h>
00048
00049 void set_pos_com(int cs, double *data, SpurUserParamsPtr spur)
00050 {
00051 double x, y, theta;
00052 CSptr cs_src;
00053 CSptr cs_fs;
00054
00055 x = data[0];
00056 y = data[1];
00057 theta = data[2];
00058
00059 cs_src = get_cs_pointer(cs);
00060 cs_fs = get_cs_pointer(CS_FS);
00061
00062 if (cs_src->parent != NULL)
00063 {
00064 CS_turn_base(&x, &y, &theta);
00065 CS_recursive_trans(cs_src->parent, cs_fs, &x, &y, &theta);
00066 set_cs(cs, x, y, theta);
00067 }
00068 else if (option(OPTION_ENABLE_SET_BS))
00069 {
00070 OdometryPtr odm;
00071
00072 odm = get_odometry_ptr();
00073 odm->x = x;
00074 odm->y = y;
00075 odm->theta = theta;
00076 set_cs(CS_FS, x, y, theta);
00077 }
00078 }
00079
00080 void set_GL_on_GL_com(double *data, SpurUserParamsPtr spur)
00081 {
00082 double x, y, theta;
00083 x = data[0];
00084 y = data[1];
00085 theta = data[2];
00086
00087 cstrans_xy(CS_GL, CS_BS, &x, &y, &theta);
00088 set_cs(CS_GL, x, y, theta);
00089 }
00090
00091 void set_adjust_com(int cs, double *data, SpurUserParamsPtr spur)
00092 {
00093 double x, y, theta;
00094 double xgl, ygl, thetagl;
00095
00096 x = data[0];
00097 y = data[1];
00098 theta = data[2];
00099
00100 cstrans_xy(cs, CS_GL, &x, &y, &theta);
00101
00102
00103 CS_turn_base(&x, &y, &theta);
00104
00105 cstrans_xy(CS_FS, CS_BS, &x, &y, &theta);
00106 xgl = x;
00107 ygl = y;
00108 thetagl = theta;
00109
00110
00111 x = data[0];
00112 y = data[1];
00113 theta = data[2];
00114 cstrans_xy(cs, CS_SP, &x, &y, &theta);
00115
00116
00117 CS_turn_base(&x, &y, &theta);
00118
00119 cstrans_xy(CS_FS, CS_BS, &x, &y, &theta);
00120
00121
00122
00123 set_cs(CS_GL, xgl, ygl, thetagl);
00124 set_cs(CS_SP, x, y, theta);
00125 }
00126
00127 void set_vel_com(double *data, SpurUserParamsPtr spur)
00128 {
00129 spur->v = data[0];
00130 if (spur->v > p(YP_PARAM_MAX_VEL, 0))
00131 spur->v = p(YP_PARAM_MAX_VEL, 0);
00132 if (spur->v < -p(YP_PARAM_MAX_VEL, 0))
00133 spur->v = -p(YP_PARAM_MAX_VEL, 0);
00134 }
00135
00136 void set_torque_com(double *data, SpurUserParamsPtr spur)
00137 {
00138 spur->torque[1] = data[1];
00139 spur->torque[0] = data[0];
00140 spur->run_mode = RUN_WHEEL_TORQUE;
00141 }
00142
00143 void set_ang_vel_com(double *data, SpurUserParamsPtr spur)
00144 {
00145 spur->w = data[0];
00146 if (spur->w > p(YP_PARAM_MAX_W, 0))
00147 spur->w = p(YP_PARAM_MAX_W, 0);
00148 if (spur->w < 0)
00149 spur->w = 0;
00150 }
00151
00152 void set_accel_com(double *data, SpurUserParamsPtr spur)
00153 {
00154 spur->dv = data[0];
00155 if (spur->dv > p(YP_PARAM_MAX_ACC_V, 0))
00156 spur->dv = p(YP_PARAM_MAX_ACC_V, 0);
00157 if (spur->dv < 0)
00158 spur->dv = 0;
00159 }
00160
00161 void set_ang_accel_com(double *data, SpurUserParamsPtr spur)
00162 {
00163 spur->dw = data[0];
00164 if (spur->dw > p(YP_PARAM_MAX_ACC_W, 0))
00165 spur->dw = p(YP_PARAM_MAX_ACC_W, 0);
00166 if (spur->dw < 0)
00167 spur->dw = 0;
00168 }
00169
00170 void set_tilt_com(int cs, double *data, SpurUserParamsPtr spur)
00171 {
00172 double x, y, theta;
00173
00174 x = 0;
00175 y = 0;
00176 theta = data[0];
00177
00178 cstrans_xy(CS_GL, cs, &x, &y, &theta);
00179
00180 spur->dir = theta;
00181 spur->tilt = data[1];
00182 }
00183
00184 void set_wheel_accel_com(double *data, SpurUserParamsPtr spur)
00185 {
00186 spur->wheel_accel[1] = data[1];
00187 spur->wheel_accel[0] = data[0];
00188 }
00189
00190 void set_wheel_vel_com(double *data, SpurUserParamsPtr spur)
00191 {
00192 spur->wheel_vel[1] = data[1];
00193 spur->wheel_vel[0] = data[0];
00194 }