Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <stdio.h>
00022 #include <unistd.h>
00023 #include <stdlib.h>
00024 #include <math.h>
00025 #include <ypspur.h>
00026
00027 int main(int argc, char *argv[])
00028 {
00029 double refvel;
00030 double torque, force;
00031 double torque_sum;
00032 double torque1;
00033 double alpha;
00034 int torque_num;
00035
00036
00037 if (Spur_init() < 0)
00038 {
00039 fprintf(stderr, "ERROR : cannot open spur.\n");
00040 return -1;
00041 }
00042
00043 if (argc != 3)
00044 {
00045 fprintf(stderr, "USAGE: %s maximum_velocity second\n", argv[0]);
00046 return -1;
00047 }
00048
00049 refvel = atoi(argv[1]);
00050 alpha = refvel / atoi(argv[2]);
00051
00052 usleep(1000000);
00053 Spur_set_vel(0);
00054 Spur_set_accel(0);
00055 Spur_set_angvel(refvel);
00056 Spur_set_angaccel(alpha);
00057
00058 Spur_vel(0, M_PI);
00059 torque_sum = 0;
00060 torque_num = 0;
00061 torque1 = 0;
00062
00063 Spur_set_pos_GL(0, 0, 0);
00064
00065 printf("# torque force torque_r, torque_l vel w\n");
00066 while (1)
00067 {
00068 double vel, w;
00069
00070 Spur_get_vel(&vel, &w);
00071 if (w > refvel * 0.9)
00072 break;
00073 if (w > refvel * 0.4)
00074 {
00075 double tr, tl;
00076 Spur_get_force(&force, &torque);
00077 if (torque1 == 0)
00078 torque1 = torque;
00079 YP_get_wheel_torque(&tr, &tl);
00080 torque_sum += torque;
00081 torque_num++;
00082 printf("%f %f %f %f %f %f\n", torque, force, tr, tl, vel, w);
00083 }
00084 usleep(5000);
00085 }
00086 Spur_set_angaccel(2);
00087 Spur_spin_GL(0);
00088
00089 fprintf(stderr, "INFO: Every robot parameter MUST BE TUNED FINELY!\n");
00090 fprintf(stderr, "INFO: This estimation is susceptible to ERROR!\n");
00091
00092 if (fabs((torque1 - torque) / torque1) > 0.5)
00093 {
00094 if (torque1 > torque)
00095 {
00096 fprintf(stderr, "WARNING: TORQUE_VISCOS or 1/MOTOR_VC might be too large\n");
00097 }
00098 else
00099 {
00100 fprintf(stderr, "WARNING: TORQUE_VISCOS or 1/MOTOR_VC might be too small\n");
00101 }
00102 }
00103
00104 {
00105 double I, N;
00106
00107 if (alpha < 0.4)
00108 {
00109 fprintf(stderr, "WARNING: Too small Angular acceleration\n");
00110 }
00111
00112 N = (torque_sum / torque_num);
00113
00114
00115 I = N / alpha;
00116
00117 fprintf(stderr, "%f[Nm], %f[rad/s^2] %f[Nm^2]\n", N, alpha, I);
00118 }
00119
00120 return 0;
00121 }