root/branches/atorf/NXC/MotorControl2/SpeedFromPosLookup.nxc @ 588

Revision 588, 9.7 KB (checked in by atorf, 4 years ago)
Line 
1#ifndef __SPEEDFROMPOSLOOKUP_NXC__
2#define __SPEEDFROMPOSLOOKUP_NXC__
3
4
5#define SPEEDLUTSIZE 301
6#define SPEEDLUTSCALING 10000 // scaled * 100, + to get percentages * 100 ...
7
8int SpeedFromPosLUT[SPEEDLUTSIZE];
9
10
11long GetIdealSpeedFromPos(long x, long maxSpeed, long brakingDist) {
12    int i;
13    //long tmp;
14   
15    i = (x * SPEEDLUTSIZE) / brakingDist;
16    if (i < 0) {
17        i = 0;
18    } else if (i > (SPEEDLUTSIZE-1)) {
19        i = SPEEDLUTSIZE-1;
20    }//end if
21   
22
23    // convert to long
24    //tmp = ;
25   
26    return ((SpeedFromPosLUT[i] * maxSpeed) / SPEEDLUTSCALING);  // scaling *100 and another *100 to get percentages
27
28}//end function
29
30
31
32
33void InitSpeedFromPosLUT()  {
34        SpeedFromPosLUT[0] = 10000;
35        SpeedFromPosLUT[1] = 9983;
36        SpeedFromPosLUT[2] = 9967;
37        SpeedFromPosLUT[3] = 9950;
38        SpeedFromPosLUT[4] = 9933;
39        SpeedFromPosLUT[5] = 9916;
40        SpeedFromPosLUT[6] = 9899;
41        SpeedFromPosLUT[7] = 9883;
42        SpeedFromPosLUT[8] = 9866;
43        SpeedFromPosLUT[9] = 9849;
44        SpeedFromPosLUT[10] = 9832;
45        SpeedFromPosLUT[11] = 9815;
46        SpeedFromPosLUT[12] = 9798;
47        SpeedFromPosLUT[13] = 9781;
48        SpeedFromPosLUT[14] = 9764;
49        SpeedFromPosLUT[15] = 9747;
50        SpeedFromPosLUT[16] = 9730;
51        SpeedFromPosLUT[17] = 9713;
52        SpeedFromPosLUT[18] = 9695;
53        SpeedFromPosLUT[19] = 9678;
54        SpeedFromPosLUT[20] = 9661;
55        SpeedFromPosLUT[21] = 9644;
56        SpeedFromPosLUT[22] = 9626;
57        SpeedFromPosLUT[23] = 9609;
58        SpeedFromPosLUT[24] = 9592;
59        SpeedFromPosLUT[25] = 9574;
60        SpeedFromPosLUT[26] = 9557;
61        SpeedFromPosLUT[27] = 9539;
62        SpeedFromPosLUT[28] = 9522;
63        SpeedFromPosLUT[29] = 9504;
64        SpeedFromPosLUT[30] = 9487;
65        SpeedFromPosLUT[31] = 9469;
66        SpeedFromPosLUT[32] = 9452;
67        SpeedFromPosLUT[33] = 9434;
68        SpeedFromPosLUT[34] = 9416;
69        SpeedFromPosLUT[35] = 9399;
70        SpeedFromPosLUT[36] = 9381;
71        SpeedFromPosLUT[37] = 9363;
72        SpeedFromPosLUT[38] = 9345;
73        SpeedFromPosLUT[39] = 9327;
74        SpeedFromPosLUT[40] = 9309;
75        SpeedFromPosLUT[41] = 9292;
76        SpeedFromPosLUT[42] = 9274;
77        SpeedFromPosLUT[43] = 9256;
78        SpeedFromPosLUT[44] = 9238;
79        SpeedFromPosLUT[45] = 9220;
80        SpeedFromPosLUT[46] = 9201;
81        SpeedFromPosLUT[47] = 9183;
82        SpeedFromPosLUT[48] = 9165;
83        SpeedFromPosLUT[49] = 9147;
84        SpeedFromPosLUT[50] = 9129;
85        SpeedFromPosLUT[51] = 9110;
86        SpeedFromPosLUT[52] = 9092;
87        SpeedFromPosLUT[53] = 9074;
88        SpeedFromPosLUT[54] = 9055;
89        SpeedFromPosLUT[55] = 9037;
90        SpeedFromPosLUT[56] = 9018;
91        SpeedFromPosLUT[57] = 9000;
92        SpeedFromPosLUT[58] = 8981;
93        SpeedFromPosLUT[59] = 8963;
94        SpeedFromPosLUT[60] = 8944;
95        SpeedFromPosLUT[61] = 8926;
96        SpeedFromPosLUT[62] = 8907;
97        SpeedFromPosLUT[63] = 8888;
98        SpeedFromPosLUT[64] = 8869;
99        SpeedFromPosLUT[65] = 8851;
100        SpeedFromPosLUT[66] = 8832;
101        SpeedFromPosLUT[67] = 8813;
102        SpeedFromPosLUT[68] = 8794;
103        SpeedFromPosLUT[69] = 8775;
104        SpeedFromPosLUT[70] = 8756;
105        SpeedFromPosLUT[71] = 8737;
106        SpeedFromPosLUT[72] = 8718;
107        SpeedFromPosLUT[73] = 8699;
108        SpeedFromPosLUT[74] = 8679;
109        SpeedFromPosLUT[75] = 8660;
110        SpeedFromPosLUT[76] = 8641;
111        SpeedFromPosLUT[77] = 8622;
112        SpeedFromPosLUT[78] = 8602;
113        SpeedFromPosLUT[79] = 8583;
114        SpeedFromPosLUT[80] = 8563;
115        SpeedFromPosLUT[81] = 8544;
116        SpeedFromPosLUT[82] = 8524;
117        SpeedFromPosLUT[83] = 8505;
118        SpeedFromPosLUT[84] = 8485;
119        SpeedFromPosLUT[85] = 8466;
120        SpeedFromPosLUT[86] = 8446;
121        SpeedFromPosLUT[87] = 8426;
122        SpeedFromPosLUT[88] = 8406;
123        SpeedFromPosLUT[89] = 8386;
124        SpeedFromPosLUT[90] = 8367;
125        SpeedFromPosLUT[91] = 8347;
126        SpeedFromPosLUT[92] = 8327;
127        SpeedFromPosLUT[93] = 8307;
128        SpeedFromPosLUT[94] = 8287;
129        SpeedFromPosLUT[95] = 8266;
130        SpeedFromPosLUT[96] = 8246;
131        SpeedFromPosLUT[97] = 8226;
132        SpeedFromPosLUT[98] = 8206;
133        SpeedFromPosLUT[99] = 8185;
134        SpeedFromPosLUT[100] = 8165;
135        SpeedFromPosLUT[101] = 8145;
136        SpeedFromPosLUT[102] = 8124;
137        SpeedFromPosLUT[103] = 8103;
138        SpeedFromPosLUT[104] = 8083;
139        SpeedFromPosLUT[105] = 8062;
140        SpeedFromPosLUT[106] = 8042;
141        SpeedFromPosLUT[107] = 8021;
142        SpeedFromPosLUT[108] = 8000;
143        SpeedFromPosLUT[109] = 7979;
144        SpeedFromPosLUT[110] = 7958;
145        SpeedFromPosLUT[111] = 7937;
146        SpeedFromPosLUT[112] = 7916;
147        SpeedFromPosLUT[113] = 7895;
148        SpeedFromPosLUT[114] = 7874;
149        SpeedFromPosLUT[115] = 7853;
150        SpeedFromPosLUT[116] = 7832;
151        SpeedFromPosLUT[117] = 7810;
152        SpeedFromPosLUT[118] = 7789;
153        SpeedFromPosLUT[119] = 7767;
154        SpeedFromPosLUT[120] = 7746;
155        SpeedFromPosLUT[121] = 7724;
156        SpeedFromPosLUT[122] = 7703;
157        SpeedFromPosLUT[123] = 7681;
158        SpeedFromPosLUT[124] = 7659;
159        SpeedFromPosLUT[125] = 7638;
160        SpeedFromPosLUT[126] = 7616;
161        SpeedFromPosLUT[127] = 7594;
162        SpeedFromPosLUT[128] = 7572;
163        SpeedFromPosLUT[129] = 7550;
164        SpeedFromPosLUT[130] = 7528;
165        SpeedFromPosLUT[131] = 7506;
166        SpeedFromPosLUT[132] = 7483;
167        SpeedFromPosLUT[133] = 7461;
168        SpeedFromPosLUT[134] = 7439;
169        SpeedFromPosLUT[135] = 7416;
170        SpeedFromPosLUT[136] = 7394;
171        SpeedFromPosLUT[137] = 7371;
172        SpeedFromPosLUT[138] = 7348;
173        SpeedFromPosLUT[139] = 7326;
174        SpeedFromPosLUT[140] = 7303;
175        SpeedFromPosLUT[141] = 7280;
176        SpeedFromPosLUT[142] = 7257;
177        SpeedFromPosLUT[143] = 7234;
178        SpeedFromPosLUT[144] = 7211;
179        SpeedFromPosLUT[145] = 7188;
180        SpeedFromPosLUT[146] = 7165;
181        SpeedFromPosLUT[147] = 7141;
182        SpeedFromPosLUT[148] = 7118;
183        SpeedFromPosLUT[149] = 7095;
184        SpeedFromPosLUT[150] = 7071;
185        SpeedFromPosLUT[151] = 7047;
186        SpeedFromPosLUT[152] = 7024;
187        SpeedFromPosLUT[153] = 7000;
188        SpeedFromPosLUT[154] = 6976;
189        SpeedFromPosLUT[155] = 6952;
190        SpeedFromPosLUT[156] = 6928;
191        SpeedFromPosLUT[157] = 6904;
192        SpeedFromPosLUT[158] = 6880;
193        SpeedFromPosLUT[159] = 6856;
194        SpeedFromPosLUT[160] = 6831;
195        SpeedFromPosLUT[161] = 6807;
196        SpeedFromPosLUT[162] = 6782;
197        SpeedFromPosLUT[163] = 6758;
198        SpeedFromPosLUT[164] = 6733;
199        SpeedFromPosLUT[165] = 6708;
200        SpeedFromPosLUT[166] = 6683;
201        SpeedFromPosLUT[167] = 6658;
202        SpeedFromPosLUT[168] = 6633;
203        SpeedFromPosLUT[169] = 6608;
204        SpeedFromPosLUT[170] = 6583;
205        SpeedFromPosLUT[171] = 6557;
206        SpeedFromPosLUT[172] = 6532;
207        SpeedFromPosLUT[173] = 6506;
208        SpeedFromPosLUT[174] = 6481;
209        SpeedFromPosLUT[175] = 6455;
210        SpeedFromPosLUT[176] = 6429;
211        SpeedFromPosLUT[177] = 6403;
212        SpeedFromPosLUT[178] = 6377;
213        SpeedFromPosLUT[179] = 6351;
214        SpeedFromPosLUT[180] = 6325;
215        SpeedFromPosLUT[181] = 6298;
216        SpeedFromPosLUT[182] = 6272;
217        SpeedFromPosLUT[183] = 6245;
218        SpeedFromPosLUT[184] = 6218;
219        SpeedFromPosLUT[185] = 6191;
220        SpeedFromPosLUT[186] = 6164;
221        SpeedFromPosLUT[187] = 6137;
222        SpeedFromPosLUT[188] = 6110;
223        SpeedFromPosLUT[189] = 6083;
224        SpeedFromPosLUT[190] = 6055;
225        SpeedFromPosLUT[191] = 6028;
226        SpeedFromPosLUT[192] = 6000;
227        SpeedFromPosLUT[193] = 5972;
228        SpeedFromPosLUT[194] = 5944;
229        SpeedFromPosLUT[195] = 5916;
230        SpeedFromPosLUT[196] = 5888;
231        SpeedFromPosLUT[197] = 5859;
232        SpeedFromPosLUT[198] = 5831;
233        SpeedFromPosLUT[199] = 5802;
234        SpeedFromPosLUT[200] = 5774;
235        SpeedFromPosLUT[201] = 5745;
236        SpeedFromPosLUT[202] = 5715;
237        SpeedFromPosLUT[203] = 5686;
238        SpeedFromPosLUT[204] = 5657;
239        SpeedFromPosLUT[205] = 5627;
240        SpeedFromPosLUT[206] = 5598;
241        SpeedFromPosLUT[207] = 5568;
242        SpeedFromPosLUT[208] = 5538;
243        SpeedFromPosLUT[209] = 5508;
244        SpeedFromPosLUT[210] = 5477;
245        SpeedFromPosLUT[211] = 5447;
246        SpeedFromPosLUT[212] = 5416;
247        SpeedFromPosLUT[213] = 5385;
248        SpeedFromPosLUT[214] = 5354;
249        SpeedFromPosLUT[215] = 5323;
250        SpeedFromPosLUT[216] = 5292;
251        SpeedFromPosLUT[217] = 5260;
252        SpeedFromPosLUT[218] = 5228;
253        SpeedFromPosLUT[219] = 5196;
254        SpeedFromPosLUT[220] = 5164;
255        SpeedFromPosLUT[221] = 5132;
256        SpeedFromPosLUT[222] = 5099;
257        SpeedFromPosLUT[223] = 5066;
258        SpeedFromPosLUT[224] = 5033;
259        SpeedFromPosLUT[225] = 5000;
260        SpeedFromPosLUT[226] = 4967;
261        SpeedFromPosLUT[227] = 4933;
262        SpeedFromPosLUT[228] = 4899;
263        SpeedFromPosLUT[229] = 4865;
264        SpeedFromPosLUT[230] = 4830;
265        SpeedFromPosLUT[231] = 4796;
266        SpeedFromPosLUT[232] = 4761;
267        SpeedFromPosLUT[233] = 4726;
268        SpeedFromPosLUT[234] = 4690;
269        SpeedFromPosLUT[235] = 4655;
270        SpeedFromPosLUT[236] = 4619;
271        SpeedFromPosLUT[237] = 4583;
272        SpeedFromPosLUT[238] = 4546;
273        SpeedFromPosLUT[239] = 4509;
274        SpeedFromPosLUT[240] = 4472;
275        SpeedFromPosLUT[241] = 4435;
276        SpeedFromPosLUT[242] = 4397;
277        SpeedFromPosLUT[243] = 4359;
278        SpeedFromPosLUT[244] = 4320;
279        SpeedFromPosLUT[245] = 4282;
280        SpeedFromPosLUT[246] = 4243;
281        SpeedFromPosLUT[247] = 4203;
282        SpeedFromPosLUT[248] = 4163;
283        SpeedFromPosLUT[249] = 4123;
284        SpeedFromPosLUT[250] = 4082;
285        SpeedFromPosLUT[251] = 4041;
286        SpeedFromPosLUT[252] = 4000;
287        SpeedFromPosLUT[253] = 3958;
288        SpeedFromPosLUT[254] = 3916;
289        SpeedFromPosLUT[255] = 3873;
290        SpeedFromPosLUT[256] = 3830;
291        SpeedFromPosLUT[257] = 3786;
292        SpeedFromPosLUT[258] = 3742;
293        SpeedFromPosLUT[259] = 3697;
294        SpeedFromPosLUT[260] = 3651;
295        SpeedFromPosLUT[261] = 3606;
296        SpeedFromPosLUT[262] = 3559;
297        SpeedFromPosLUT[263] = 3512;
298        SpeedFromPosLUT[264] = 3464;
299        SpeedFromPosLUT[265] = 3416;
300        SpeedFromPosLUT[266] = 3367;
301        SpeedFromPosLUT[267] = 3317;
302        SpeedFromPosLUT[268] = 3266;
303        SpeedFromPosLUT[269] = 3215;
304        SpeedFromPosLUT[270] = 3162;
305        SpeedFromPosLUT[271] = 3109;
306        SpeedFromPosLUT[272] = 3055;
307        SpeedFromPosLUT[273] = 3000;
308        SpeedFromPosLUT[274] = 2944;
309        SpeedFromPosLUT[275] = 2887;
310        SpeedFromPosLUT[276] = 2828;
311        SpeedFromPosLUT[277] = 2769;
312        SpeedFromPosLUT[278] = 2708;
313        SpeedFromPosLUT[279] = 2646;
314        SpeedFromPosLUT[280] = 2582;
315        SpeedFromPosLUT[281] = 2517;
316        SpeedFromPosLUT[282] = 2449;
317        SpeedFromPosLUT[283] = 2380;
318        SpeedFromPosLUT[284] = 2309;
319        SpeedFromPosLUT[285] = 2236;
320        SpeedFromPosLUT[286] = 2160;
321        SpeedFromPosLUT[287] = 2082;
322        SpeedFromPosLUT[288] = 2000;
323        SpeedFromPosLUT[289] = 1915;
324        SpeedFromPosLUT[290] = 1826;
325        SpeedFromPosLUT[291] = 1732;
326        SpeedFromPosLUT[292] = 1633;
327        SpeedFromPosLUT[293] = 1528;
328        SpeedFromPosLUT[294] = 1414;
329        SpeedFromPosLUT[295] = 1291;
330        SpeedFromPosLUT[296] = 1155;
331        SpeedFromPosLUT[297] = 1000;
332        SpeedFromPosLUT[298] = 816;
333        SpeedFromPosLUT[299] = 577;
334        SpeedFromPosLUT[300] = 0;
335}//end void
336
337
338
339
340
341#endif
Note: See TracBrowser for help on using the browser.