command_get.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 <sys/types.h>
29 #include <time.h>
30 
31 #ifdef HAVE_CONFIG_H
32 #include <config.h>
33 #endif // HAVE_CONFIG_H
34 
35 /* ボディパラメータ */
36 #include <shvel-param.h>
37 
38 /* yp-spur用 */
39 #include <serial.h>
40 #include <param.h>
41 #include <control.h>
42 #include <command.h>
43 
44 /* ライブラリ用 */
45 #include <ypspur.h>
46 #include <cartesian2d.h>
47 
48 void get_pos_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
49 {
51 
52  cstrans_odometry(cs, &odometry);
53 
54  resdata[0] = odometry.x;
55  resdata[1] = odometry.y;
56  resdata[2] = odometry.theta;
57  resdata[3] = odometry.time;
58  // printf( "get %f %f %f\n", x, y, theta );
59 }
60 
61 void get_wheel_vref_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
62 {
64 
65  odometry = get_odometry_ptr();
66 
67  resdata[0] = spur->wvelref[0];
68  resdata[1] = spur->wvelref[1];
69  resdata[2] = odometry->time;
70 }
71 
72 void get_vref_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
73 {
75 
76  odometry = get_odometry_ptr();
77 
78  resdata[0] = spur->vref_smooth;
79  resdata[1] = spur->wref_smooth;
80  resdata[2] = odometry->time;
81 }
82 
83 void get_vel_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
84 {
86 
87  odometry = get_odometry_ptr();
88 
89  resdata[0] = odometry->v;
90  resdata[1] = odometry->w;
91  resdata[2] = odometry->time;
92 
93  // printf("getvel %f %f %f\n",);
94 }
95 
96 void get_force_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
97 {
99 
100  odometry = get_odometry_ptr();
101 
102  resdata[0] = odometry->torque_trans;
103  resdata[1] = odometry->torque_angular;
104  resdata[2] = odometry->time;
105 
106  // printf("getvel %f %f %f\n",);
107 }
108 
109 void get_wheel_torque_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
110 {
112 
113  odometry = get_odometry_ptr();
114 
115  resdata[0] = odometry->wtorque[0];
116  resdata[1] = odometry->wtorque[1];
117  resdata[2] = odometry->time;
118 
119  // printf("getvel %f %f %f\n",);
120 }
121 
122 void get_wheel_vel_com(double *data, double *resdata, SpurUserParamsPtr spur)
123 {
125 
126  odometry = get_odometry_ptr();
127 
128  resdata[0] = odometry->wvel[0];
129  resdata[1] = odometry->wvel[1];
130  resdata[2] = odometry->time;
131 
132  // printf("getvel %f %f %f\n",);
133 }
134 
135 void get_wheel_ang_com(double *data, double *resdata, SpurUserParamsPtr spur)
136 {
138 
139  odometry = get_odometry_ptr();
140 
141  resdata[0] = odometry->wang[0];
142  resdata[1] = odometry->wang[1];
143  resdata[2] = odometry->time;
144 
145  // printf("getvel %f %f %f\n",);
146 }
147 
148 int near_pos_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
149 {
150  double x, y, theta, cx, cy;
151  double dist;
153 
154  odometry = get_odometry_ptr();
155 
156  x = odometry->x;
157  y = odometry->y;
158  theta = odometry->theta;
159 
160  cstrans_xy(CS_BS, cs, &x, &y, &theta);
161 
162  cx = data[0];
163  cy = data[1];
164  dist = sqrt((cx - x) * (cx - x) + (cy - y) * (cy - y));
165  resdata[0] = dist;
166 
167  if (dist < data[2])
168  return 1;
169  else
170  return 0;
171 }
172 
173 int near_ang_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
174 {
175  double x, y, theta;
176  double dist;
178 
179  odometry = get_odometry_ptr();
180 
181  x = odometry->x;
182  y = odometry->y;
183  theta = odometry->theta;
184 
185  cstrans_xy(CS_BS, cs, &x, &y, &theta);
186 
187  dist = theta - data[0];
188  while (dist > M_PI)
189  dist -= 2 * M_PI;
190  while (dist < -M_PI)
191  dist += 2 * M_PI;
192  resdata[0] = dist;
193  if (fabs(dist) < data[1])
194  return 1;
195  else
196  return 0;
197 }
198 
199 int over_line_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
200 {
201  double x, y, theta;
202  double dist;
204 
205  odometry = get_odometry_ptr();
206 
207  x = odometry->x;
208  y = odometry->y;
209  theta = odometry->theta;
210 
211  cstrans_xy(CS_BS, cs, &x, &y, &theta);
212 
213  dist = (x - data[0]) * cos(data[2]) + (y - data[1]) * sin(data[2]);
214 
215  resdata[0] = dist;
216  if (dist > 0)
217  return 1;
218  else
219  return 0;
220 }
void get_force_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:96
int near_ang_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:173
double torque_angular
Definition: odometry.h:45
double y
Definition: odometry.h:34
void get_pos_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:48
void cstrans_xy(YPSpur_cs src, YPSpur_cs dest, double *x, double *y, double *theta)
Definition: odometry.c:132
double wvelref[YP_PARAM_MAX_MOTOR_NUM]
Definition: command.h:65
void odometry(OdometryPtr xp, short *cnt, short *pwm, double dt, double time)
Definition: odometry.c:145
double time
Definition: odometry.h:38
double wang[YP_PARAM_MAX_MOTOR_NUM]
Definition: odometry.h:40
double wtorque[YP_PARAM_MAX_MOTOR_NUM]
Definition: odometry.h:43
void get_vref_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:72
static YPSpur spur
Definition: libypspur.c:42
double w
Definition: odometry.h:37
void cstrans_odometry(YPSpur_cs cs, OdometryPtr dst_odm)
Definition: odometry.c:345
double v
Definition: odometry.h:36
double theta
Definition: odometry.h:35
double wvel[YP_PARAM_MAX_MOTOR_NUM]
Definition: odometry.h:39
double torque_trans
Definition: odometry.h:44
void get_wheel_torque_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:109
double vref_smooth
Definition: command.h:63
OdometryPtr get_odometry_ptr()
Definition: odometry.c:361
void get_wheel_vref_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:61
int over_line_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:199
int near_pos_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:148
double wref_smooth
Definition: command.h:64
void get_vel_com(int cs, double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:83
void get_wheel_vel_com(double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:122
Definition: ypparam.h:490
double x
Definition: odometry.h:33
void get_wheel_ang_com(double *data, double *resdata, SpurUserParamsPtr spur)
Definition: command_get.c:135


yp-spur
Author(s):
autogenerated on Fri May 7 2021 02:12:17