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