root / branches / encoders / code / behaviors / spline / server / speed_map.c @ 1344
History | View | Annotate | Download (1 KB)
1 |
#include "speed_map.h" |
---|---|
2 |
#include <stdlib.h> |
3 |
|
4 |
double robot_1[41] = |
5 |
{ |
6 |
0,
|
7 |
0.495185695, |
8 |
0.935064935, |
9 |
1.385681293, |
10 |
1.834862385, |
11 |
2.278481013, |
12 |
2.739726027, |
13 |
3.20855615, |
14 |
3.629032258, |
15 |
4.295942721, |
16 |
4.7, |
17 |
5.013927577, |
18 |
5.487804878, |
19 |
5.940594059, |
20 |
6.382978723, |
21 |
6.844106464, |
22 |
7.317073171, |
23 |
7.75862069, |
24 |
8.181818182, |
25 |
8.653846154, |
26 |
9.183673469, |
27 |
9.677419355, |
28 |
10.05586592, |
29 |
10.34482759, |
30 |
10.97560976, |
31 |
11.39240506, |
32 |
11.84210526, |
33 |
12.4137931, |
34 |
12.76595745, |
35 |
13.23529412, |
36 |
13.63636364, |
37 |
14.17322835, |
38 |
14.51612903, |
39 |
15,
|
40 |
15.65217391, |
41 |
15.78947368, |
42 |
16.51376147, |
43 |
16.66666667, |
44 |
17.47572816, |
45 |
17.84, |
46 |
18.3 |
47 |
}; |
48 |
|
49 |
int robot_velocity(int robot, double velocity) |
50 |
{ |
51 |
if(velocity > 0) |
52 |
return (int)((velocity+0.2757)/0.4645); |
53 |
|
54 |
velocity = -velocity; |
55 |
return -(int)((velocity+0.2757)/0.4645); |
56 |
} |
57 |
|
58 |
double real_velocity(int robot, int velocity) |
59 |
{ |
60 |
if(velocity > 40 || velocity < -40) |
61 |
return velocity < 0?-18.3:18.3; |
62 |
|
63 |
switch(robot)
|
64 |
{ |
65 |
case 1: |
66 |
return velocity < 0?-robot_1[-velocity]:robot_1[velocity]; |
67 |
default:
|
68 |
exit(-15410);
|
69 |
} |
70 |
|
71 |
} |