MOI-estimate.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 <stdio.h>
22 #include <unistd.h>
23 #include <stdlib.h>
24 #include <math.h>
25 #include <ypspur.h>
26 
27 int main(int argc, char *argv[])
28 {
29  double refvel;
30  double torque, force;
31  double torque_sum;
32  double torque1;
33  double alpha;
34  int torque_num;
35 
36  /* 初期化 */
37  if (Spur_init() < 0)
38  {
39  fprintf(stderr, "ERROR : cannot open spur.\n");
40  return -1;
41  }
42 
43  if (argc != 3)
44  {
45  fprintf(stderr, "USAGE: %s maximum_velocity second\n", argv[0]);
46  return -1;
47  }
48 
49  refvel = atoi(argv[1]);
50  alpha = refvel / atoi(argv[2]);
51 
52  usleep(1000000);
53  Spur_set_vel(0);
54  Spur_set_accel(0);
55  Spur_set_angvel(refvel);
56  Spur_set_angaccel(alpha);
57 
58  Spur_vel(0, M_PI);
59  torque_sum = 0;
60  torque_num = 0;
61  torque1 = 0;
62 
63  Spur_set_pos_GL(0, 0, 0);
64 
65  printf("# torque force torque_r, torque_l vel w\n");
66  while (1)
67  {
68  double vel, w;
69 
70  Spur_get_vel(&vel, &w);
71  if (w > refvel * 0.9)
72  break;
73  if (w > refvel * 0.4)
74  {
75  double tr, tl;
76  Spur_get_force(&force, &torque);
77  if (torque1 == 0)
78  torque1 = torque;
79  YP_get_wheel_torque(&tr, &tl);
80  torque_sum += torque;
81  torque_num++;
82  printf("%f %f %f %f %f %f\n", torque, force, tr, tl, vel, w);
83  }
84  usleep(5000);
85  }
87  Spur_spin_GL(0);
88 
89  fprintf(stderr, "INFO: Every robot parameter MUST BE TUNED FINELY!\n");
90  fprintf(stderr, "INFO: This estimation is susceptible to ERROR!\n");
91 
92  if (fabs((torque1 - torque) / torque1) > 0.5)
93  {
94  if (torque1 > torque)
95  {
96  fprintf(stderr, "WARNING: TORQUE_VISCOS or 1/MOTOR_VC might be too large\n");
97  }
98  else
99  {
100  fprintf(stderr, "WARNING: TORQUE_VISCOS or 1/MOTOR_VC might be too small\n");
101  }
102  }
103 
104  {
105  double I, N;
106 
107  if (alpha < 0.4)
108  {
109  fprintf(stderr, "WARNING: Too small Angular acceleration\n");
110  }
111 
112  N = (torque_sum / torque_num); //[Nm]
113 
114  // I alpha = N
115  I = N / alpha;
116 
117  fprintf(stderr, "%f[Nm], %f[rad/s^2] %f[Nm^2]\n", N, alpha, I);
118  }
119 
120  return 0;
121 }
#define Spur_init()
Definition: ypspur.h:31
#define Spur_set_pos_GL(x, y, th)
Definition: ypspur.h:71
int main(int argc, char *argv[])
Definition: MOI-estimate.c:27
#define Spur_get_force(trans, angular)
Definition: ypspur.h:92
#define Spur_vel(v, w)
Definition: ypspur.h:109
#define Spur_set_angaccel(w)
Definition: ypspur.h:78
#define Spur_set_accel(v)
Definition: ypspur.h:77
#define Spur_spin_GL(th)
Definition: ypspur.h:59
#define Spur_set_angvel(w)
Definition: ypspur.h:76
#define Spur_set_vel(v)
Definition: ypspur.h:75
double YP_get_wheel_torque(double *torque_r, double *torque_l)
Definition: libypspur.c:205
#define Spur_get_vel(v, w)
Definition: ypspur.h:91


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