command_set.c
Go to the documentation of this file.
00001 // Copyright (c) 2010-2016 The YP-Spur Authors, except where otherwise indicated.
00002 //
00003 // Permission is hereby granted, free of charge, to any person obtaining a copy
00004 // of this software and associated documentation files (the "Software"), to
00005 // deal in the Software without restriction, including without limitation the
00006 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
00007 // sell copies of the Software, and to permit persons to whom the Software is
00008 // furnished to do so, subject to the following conditions:
00009 //
00010 // The above copyright notice and this permission notice shall be included in
00011 // all copies or substantial portions of the Software.
00012 //
00013 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00014 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00015 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
00016 // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00017 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00018 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
00019 // SOFTWARE.
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 /* yp-spur用 */
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   /* ロボット(FS)がGL上のx,y,thetaに見えるとするとき */
00102   /* FSからGLがどこに見えるか(GL->FS => FS->GL) */
00103   CS_turn_base(&x, &y, &theta);
00104   /* それはBS上のどこか */
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   /* ロボット(FS)がGL上のx,y,thetaに見えるとするとき */
00116   /* FSからGLがどこに見えるか(GL->FS => FS->GL) */
00117   CS_turn_base(&x, &y, &theta);
00118   /* それはBS上のどこか */
00119   cstrans_xy(CS_FS, CS_BS, &x, &y, &theta);
00120 
00121   /* SPをセット */
00122   /* GLをセット */
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 }


yp-spur
Author(s):
autogenerated on Fri May 10 2019 02:52:19