Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1085 | pj | 1 | /* |
2 | * Project: S.Ha.R.K. |
||
3 | * |
||
4 | * Coordinators: |
||
5 | * Giorgio Buttazzo <giorgio@sssup.it> |
||
6 | * Paolo Gai <pj@gandalf.sssup.it> |
||
7 | * |
||
8 | * Authors : |
||
9 | * Paolo Gai <pj@gandalf.sssup.it> |
||
10 | * Massimiliano Giorgi <massy@gandalf.sssup.it> |
||
11 | * Luca Abeni <luca@gandalf.sssup.it> |
||
12 | * (see the web pages for full authors list) |
||
13 | * |
||
14 | * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
||
15 | * |
||
16 | * http://www.sssup.it |
||
17 | * http://retis.sssup.it |
||
18 | * http://shark.sssup.it |
||
19 | */ |
||
20 | |||
21 | |||
22 | /** |
||
23 | ------------ |
||
24 | CVS : $Id: sensor.c,v 1.1.1.1 2002-09-02 09:37:42 pj Exp $ |
||
25 | |||
26 | File: $File$ |
||
27 | Revision: $Revision: 1.1.1.1 $ |
||
28 | Last update: $Date: 2002-09-02 09:37:42 $ |
||
29 | ------------ |
||
30 | **/ |
||
31 | |||
32 | /* |
||
33 | * Copyright (C) 2000 Marco Dallera and Marco Fiocca |
||
34 | * |
||
35 | * This program is free software; you can redistribute it and/or modify |
||
36 | * it under the terms of the GNU General Public License as published by |
||
37 | * the Free Software Foundation; either version 2 of the License, or |
||
38 | * (at your option) any later version. |
||
39 | * |
||
40 | * This program is distributed in the hope that it will be useful, |
||
41 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
42 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
43 | * GNU General Public License for more details. |
||
44 | * |
||
45 | * You should have received a copy of the GNU General Public License |
||
46 | * along with this program; if not, write to the Free Software |
||
47 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
||
48 | * |
||
49 | */ |
||
50 | |||
51 | /* |
||
52 | * AUTO |
||
53 | * |
||
54 | * Another Unuseful Track simulatOr |
||
55 | * |
||
56 | * Authors: Marco Dallera |
||
57 | * Marco Fiocca |
||
58 | * |
||
59 | */ |
||
60 | |||
61 | /* ------------ */ |
||
62 | /* Camera car */ |
||
63 | /* ------------ */ |
||
64 | |||
65 | #include "include/auto.h" |
||
66 | #include "include/const.h" |
||
67 | #include "include/utils.h" |
||
68 | #include <drivers/glib.h> |
||
69 | |||
70 | #define RADIUS 1 |
||
71 | #define TOLL_FACTOR 4.0 |
||
72 | #define MIN_DIFF 3 |
||
73 | #define ROAD_COLOR 27 |
||
74 | #define OFFROAD_COLOR 0 |
||
75 | #define DEBUG_COLOR GREEN |
||
76 | #define OUT_OF_TRACK 230 |
||
77 | #define STRIP_DEFAULT TEL_WIDTH+1 |
||
78 | |||
79 | extern sem_t grx_mutex; |
||
80 | extern car_params cars[MAX_CAR_NUMBER]; |
||
81 | |||
82 | void scanline(point_f *buffer, float x1, float y1, float x2, float y2, int factor); |
||
83 | void get_camera_car(BYTE *buffer, car_status status); |
||
84 | int find_track(BYTE *image, road_info *info); |
||
85 | int find_opps(BYTE *image, road_info *info); |
||
86 | void find_collision(BYTE *image, road_info *info, car_status status); |
||
87 | |||
88 | /* ----------------------------------------------------------------- */ |
||
89 | |||
90 | TASK sensor(int index) { |
||
91 | car_status actual_car_status; // the actual car state |
||
92 | road_info actual_road_info; // the actual road informations |
||
93 | BYTE image[TEL_WIDTH*TEL_HEIGHT]; // buffer contenente la visione della telecamera ruotata |
||
94 | |||
95 | point sq1, sq2; |
||
96 | point im1, im2; |
||
97 | |||
98 | CAB road_status_cab = cars[index].road_status_cab; |
||
99 | CAB car_status_cab = cars[index].car_status_cab; |
||
100 | |||
101 | // Draws display squares |
||
102 | sq1.x = TRACK_X1 + TRACK_WIDTH + TEL_HEIGHT; |
||
103 | sq1.y = TEL_HEIGHT*index + 1 + index; |
||
104 | sq2.x = sq1.x + TEL_WIDTH + 1; |
||
105 | sq2.y = sq1.y + TEL_HEIGHT + 1; |
||
106 | |||
107 | // Sets viewer coords |
||
108 | im1.x = sq1.x + 1; |
||
109 | im1.y = sq1.y + 1; |
||
110 | im2.x = im1.x + TEL_WIDTH - 1; |
||
111 | im2.y = im1.y + TEL_HEIGHT - 1; |
||
112 | |||
113 | // Inizializza il cab della strada |
||
114 | actual_road_info.left_border = TEL_WIDTH/2; |
||
115 | actual_road_info.right_border = TEL_WIDTH/2; |
||
116 | actual_road_info.distance = TEL_HEIGHT; |
||
117 | actual_road_info.curve = 0.0; |
||
118 | actual_road_info.opps_number = 0; |
||
119 | actual_road_info.flag = ROAD_OK; |
||
120 | actual_road_info.collision = NO_COLL; |
||
121 | set_road_info(actual_road_info, road_status_cab); |
||
122 | |||
123 | task_endcycle(); |
||
124 | |||
125 | while(1) { |
||
126 | /* reads car informations */ |
||
127 | actual_car_status = get_car_status(car_status_cab); |
||
128 | |||
129 | /* implementazione riconoscimento strada (sti cazzi!!!) */ |
||
130 | get_camera_car(image, actual_car_status); |
||
131 | find_track(image, &actual_road_info); |
||
132 | find_opps(image, &actual_road_info); |
||
133 | find_collision(image, &actual_road_info, actual_car_status); |
||
134 | |||
135 | sem_wait(&grx_mutex); |
||
136 | grx_rect(sq1.x, sq1.y, sq2.x, sq2.y, cars[index].color); |
||
137 | grx_putimage(im1.x, im1.y, im2.x, im2.y, image); |
||
138 | sem_post(&grx_mutex); |
||
139 | |||
140 | /* sends informations to control task */ |
||
141 | set_road_info(actual_road_info, road_status_cab); |
||
142 | task_endcycle(); |
||
143 | } |
||
144 | |||
145 | } |
||
146 | |||
147 | /* ----------------------------------------------------------------- */ |
||
148 | |||
149 | void find_collision(BYTE *image, road_info *info, car_status status) |
||
150 | { |
||
151 | point_f lt,rt,lb,rb,ct; |
||
152 | /* the lines used to scan the quadrilate */ |
||
153 | point_f height_line[CAR_HEIGHT]; |
||
154 | int i; |
||
155 | BYTE pix; |
||
156 | |||
157 | info->collision = NO_COLL; |
||
158 | |||
159 | ct.x = status.pos.x + (float)(CAR_HEIGHT/2 + 1) * cos(degree_to_rad(status.orient)); |
||
160 | ct.y = status.pos.y - (float)(CAR_WIDTH/2 + 1) * sin(degree_to_rad(status.orient)); |
||
161 | |||
162 | lb.x = ct.x - (float)(CAR_WIDTH/2.0) * cos(degree_to_rad(90-status.orient)); |
||
163 | rb.x = ct.x + (float)(CAR_WIDTH/2.0) * cos(degree_to_rad(90-status.orient)); |
||
164 | lt.x = lb.x - (float)(CAR_HEIGHT) * cos(degree_to_rad(status.orient)); |
||
165 | rt.x = rb.x - (float)(CAR_HEIGHT) * cos(degree_to_rad(status.orient)); |
||
166 | |||
167 | lb.y = ct.y - (float)(CAR_WIDTH/2.0) * sin(degree_to_rad(90-status.orient)); |
||
168 | rb.y = ct.y + (float)(CAR_WIDTH/2.0) * sin(degree_to_rad(90-status.orient)); |
||
169 | lt.y = lb.y + (float)(CAR_HEIGHT) * sin(degree_to_rad(status.orient)); |
||
170 | rt.y = rb.y + (float)(CAR_HEIGHT) * sin(degree_to_rad(status.orient)); |
||
171 | |||
172 | sem_wait(&grx_mutex); |
||
173 | scanline(height_line,rb.x,rb.y,rt.x,rt.y,CAR_HEIGHT); |
||
174 | for (i=0; i < CAR_HEIGHT; i++) { |
||
175 | pix = grx_getpixel(round(height_line[i].x),round(height_line[i].y)); |
||
176 | if (pix != OFFROAD_COLOR && pix != ROAD_COLOR) |
||
177 | info->collision = COLLISION_RIGHT; |
||
178 | } |
||
179 | scanline(height_line,lb.x,lb.y,lt.x,lt.y,CAR_HEIGHT); |
||
180 | for (i=0; i < CAR_HEIGHT; i++) { |
||
181 | pix = grx_getpixel(round(height_line[i].x),round(height_line[i].y)); |
||
182 | if (pix != OFFROAD_COLOR && pix != ROAD_COLOR) |
||
183 | info->collision = COLLISION_LEFT; |
||
184 | } |
||
185 | sem_post(&grx_mutex); |
||
186 | |||
187 | } |
||
188 | |||
189 | /* ----------------------------------------------------------------- */ |
||
190 | |||
191 | /* Puts in a buffer the image viewed from the camera according to the car status */ |
||
192 | void get_camera_car(BYTE *buffer, car_status status) { |
||
193 | /* |
||
194 | * lt ------ rt |
||
195 | * | | |
||
196 | * | | |
||
197 | * lb --ct-- rb |
||
198 | */ |
||
199 | point_f lt,rt,lb,rb,ct; |
||
200 | /* the lines used to scan the quadrilate */ |
||
201 | point_f width_line[TEL_WIDTH]; |
||
202 | point_f height_line[TEL_HEIGHT]; |
||
203 | int i,j; |
||
204 | int tmpx,tmpy,tx,ty; |
||
205 | float x,y; |
||
206 | BYTE *row; |
||
207 | |||
208 | ct.x = status.pos.x + (float)(CAR_WIDTH/2 + 1) * cos(degree_to_rad(status.orient)); |
||
209 | ct.y = status.pos.y - (float)(CAR_WIDTH/2 + 1) * sin(degree_to_rad(status.orient)); |
||
210 | |||
211 | /* for now the visual starts from the center of the car */ |
||
212 | lb.x = ct.x - (float)(TEL_WIDTH/2.0) * cos(degree_to_rad(90-status.orient)); |
||
213 | rb.x = ct.x + (float)(TEL_WIDTH/2.0) * cos(degree_to_rad(90-status.orient)); |
||
214 | lt.x = lb.x + (float)(TEL_HEIGHT) * cos(degree_to_rad(status.orient)); |
||
215 | rt.x = rb.x + (float)(TEL_HEIGHT) * cos(degree_to_rad(status.orient)); |
||
216 | |||
217 | lb.y = ct.y - (float)(TEL_WIDTH/2.0) * sin(degree_to_rad(90-status.orient)); |
||
218 | rb.y = ct.y + (float)(TEL_WIDTH/2.0) * sin(degree_to_rad(90-status.orient)); |
||
219 | lt.y = lb.y - (float)(TEL_HEIGHT) * sin(degree_to_rad(status.orient)); |
||
220 | rt.y = rb.y - (float)(TEL_HEIGHT) * sin(degree_to_rad(status.orient)); |
||
221 | |||
222 | /* scan only 2 sense thanks to parallelism */ |
||
223 | /* ^ <-(height) |
||
224 | * ! lt |
||
225 | * ! | |
||
226 | * ! | |
||
227 | * ! lb ------ rb ) |
||
228 | * 0 -------------> <-(width |
||
229 | */ |
||
230 | /* the width_line coordinates are relative, so in the for cycle |
||
231 | they are summarized to absolute height_line values */ |
||
232 | scanline(width_line,0,0,rb.x - lb.x,rb.y - lb.y,TEL_WIDTH); |
||
233 | scanline(height_line,lb.x,lb.y,lt.x,lt.y,TEL_HEIGHT); |
||
234 | |||
235 | /* First fill row TEL_HEIGHT-1 of image and so on until row 0 */ |
||
236 | sem_wait(&grx_mutex); |
||
237 | for (i=0; i < TEL_HEIGHT; i++) { |
||
238 | x = height_line[i].x; |
||
239 | y = height_line[i].y; |
||
240 | row = &buffer[TEL_WIDTH * (TEL_HEIGHT-1-i)]; |
||
241 | for (j=0; j < TEL_WIDTH; j++) { |
||
242 | tmpx = round(x + width_line[j].x); |
||
243 | tmpy = round(y + width_line[j].y); |
||
244 | |||
245 | // Test rispetto ai bordi |
||
246 | if (tmpx >= 0 && tmpx < SCREEN_WIDTH && tmpy >= 0 && tmpy < SCREEN_HEIGHT) { |
||
247 | tx = tmpx - TRACK_X1-1; |
||
248 | ty = tmpy - TRACK_Y1-1; |
||
249 | if (tx >= 0 && tx < TRACK_WIDTH && ty >= 0 && ty < TRACK_HEIGHT) |
||
250 | row[j] = grx_getpixel(tmpx,tmpy); |
||
251 | else |
||
252 | row[j] = OUT_OF_TRACK; |
||
253 | } else { |
||
254 | row[j] = OUT_OF_TRACK; |
||
255 | } |
||
256 | } |
||
257 | } |
||
258 | sem_post(&grx_mutex); |
||
259 | } |
||
260 | |||
261 | /* ----------------------------------------------------------------- */ |
||
262 | |||
263 | void scanline(point_f *buffer, float x1, float y1, float x2, float y2, int factor) { |
||
264 | int i; |
||
265 | float x,y; |
||
266 | float xstep,ystep; |
||
267 | |||
268 | xstep = (x2 - x1) / factor; |
||
269 | ystep = (y2 - y1) / factor; |
||
270 | |||
271 | // Start scan at <x1, y1> |
||
272 | x = x1; |
||
273 | y = y1; |
||
274 | |||
275 | for (i = 0; i < factor; i++) { |
||
276 | buffer[i].x = x; |
||
277 | buffer[i].y = y; |
||
278 | x += xstep; |
||
279 | y += ystep; |
||
280 | } |
||
281 | } |
||
282 | |||
283 | /* ----------------------------------------------------------------- */ |
||
284 | |||
285 | /* |
||
286 | * Approssima la curva quando non è più possibile farlo con la scansione |
||
287 | * orizzontale |
||
288 | * - image |
||
289 | * - index l'indice da cui cominciare la ricerca della curva |
||
290 | * - distance la distanza dalla curva |
||
291 | */ |
||
292 | float approx_curve(BYTE *image, int index, int distance, int d_center, road_strip *strips) { |
||
293 | BYTE *p,tmp; |
||
294 | int i,j,k,ctrl=0; |
||
295 | point ms_center,ct; |
||
296 | |||
297 | // Decide se la curva e' verso sinistra o verso destra |
||
298 | p = &image[(TEL_HEIGHT - index - 1) * TEL_WIDTH]; |
||
299 | |||
300 | ms_center.x = strips[index-1].left; |
||
301 | for (i = 0; i < min(3,strips[index-1].left); i++) { |
||
302 | // La curva è a destra |
||
303 | tmp = *(p + round(ms_center.x) - i); |
||
304 | if (tmp != ROAD_COLOR) { |
||
305 | ms_center.x = strips[index-1].right; |
||
306 | break; |
||
307 | } |
||
308 | } |
||
309 | |||
310 | p += round(ms_center.x); |
||
311 | // Scandisce la colonna fino alla fine della strada |
||
312 | for (i = index; i < TEL_HEIGHT; i++, p -= TEL_WIDTH) { |
||
313 | if (*p != ROAD_COLOR) break; |
||
314 | } |
||
315 | |||
316 | // Imposta il nuovo max_scost empirico |
||
317 | ms_center.y = (i-1 + index) / 2; |
||
318 | |||
319 | // i-1 è l'indice dell'ultima riga bianca |
||
320 | p = &image[(TEL_HEIGHT - (i-1) - 1) * TEL_WIDTH + round(ms_center.x)]; |
||
321 | // scandisce da destra a sinistra |
||
322 | if (round(ms_center.x) > 0) { |
||
323 | for (j = 0; j < (round(ms_center.x) - strips[index-1].left); j++, p--) { |
||
324 | if (*p != ROAD_COLOR) break; |
||
325 | // scandisce dall'alto in basso finchè non trova nero |
||
326 | for (k = i-1; k > index; k--) { |
||
327 | tmp = *(p + (i-1-k)*TEL_WIDTH); |
||
328 | if (tmp != ROAD_COLOR) { |
||
329 | ms_center.y = (k + index) / 2; |
||
330 | ctrl = 1; |
||
331 | break; |
||
332 | } |
||
333 | } |
||
334 | if (ctrl) break; |
||
335 | } |
||
336 | ct.x = ms_center.x - j/2; |
||
337 | ct.y = ms_center.y; |
||
338 | } else { |
||
339 | // scandisce da sinistra a destra |
||
340 | for (j = 0; j < (strips[index-1].right - round(ms_center.x)); j++, p++) { |
||
341 | if (*p != ROAD_COLOR) break; |
||
342 | // scandisce dall'alto in basso finchè non trova nero |
||
343 | for (k = i-1; k > index; k--) { |
||
344 | tmp = *(p + (i-1-k)*TEL_WIDTH); |
||
345 | if (tmp != ROAD_COLOR) { |
||
346 | ms_center.y = (k + index) / 2; |
||
347 | ctrl = 1; |
||
348 | break; |
||
349 | } |
||
350 | } |
||
351 | if (ctrl) break; |
||
352 | } |
||
353 | ct.x = ms_center.x + j/2; |
||
354 | ct.y = ms_center.y; |
||
355 | } |
||
356 | |||
357 | return rad_to_degree(atan2((double)(ct.y - distance),(double)(ct.x - d_center))); |
||
358 | } |
||
359 | |||
360 | /* ----------------------------------------------------------------- */ |
||
361 | |||
362 | int find_distance(road_strip *strips, int max_scost, int rett_tol) { |
||
363 | int i, distance; |
||
364 | int center_car = TEL_WIDTH/2; |
||
365 | |||
366 | // La distanza non puo' essere maggiore del max_scost |
||
367 | distance = max_scost; |
||
368 | |||
369 | for (i=1; i < max_scost; i++) { |
||
370 | // Calcola la distanza dalla curva |
||
371 | if (abs(strips[i].center - center_car) > rett_tol) { |
||
372 | distance = i; |
||
373 | break; |
||
374 | } |
||
375 | } |
||
376 | return distance; |
||
377 | } |
||
378 | |||
379 | /* ----------------------------------------------------------------- */ |
||
380 | |||
381 | /* Scandisce la linea row e inserisce nella strip index le informazioni. |
||
382 | * Ritorna 1 se trova entrambi i bordi, altrimenti 0 |
||
383 | */ |
||
384 | int scan_strip(BYTE *row, int index, road_strip *strips) { |
||
385 | // Tiene conto della posizione dei bordi precedenti per trattare |
||
386 | // meglio il caso di curve molto strette, dove possono esistere |
||
387 | // 2 bordi destri o sinistri |
||
388 | |||
389 | // Per evitare problemi nei casi in cui siano possibili 2 |
||
390 | // bordi destri o sinistri |
||
391 | int lfound = 0; |
||
392 | int rfound = 0; |
||
393 | int center_car = TEL_WIDTH/2; |
||
394 | int i; |
||
395 | |||
396 | // Parte dal secondo pixel e finisce al penultimo |
||
397 | for(i=1; i < (TEL_WIDTH-1); i++) { |
||
398 | // Se trova la strada controlla se si tratta di bordo |
||
399 | // destro o sinistro |
||
400 | if (row[i] == ROAD_COLOR) { |
||
401 | // bordo sinistro |
||
402 | if (row[i - 1] != ROAD_COLOR) { |
||
403 | // Se ho già trovato questo tipo di bordo cerco |
||
404 | // quello + plausibile ... |
||
405 | if (lfound) { |
||
406 | // Test sulla striscia precedente |
||
407 | if (index > 0) { |
||
408 | if (abs(i - strips[index-1].left) < |
||
409 | abs(strips[index].left - strips[index-1].left)) |
||
410 | strips[index].left = i; |
||
411 | } else { |
||
412 | // Test sul centro della macchina |
||
413 | if (abs(i - center_car) < abs(strips[0].left - center_car)) |
||
414 | strips[index].left = i; |
||
415 | } |
||
416 | lfound++; |
||
417 | } else { |
||
418 | // ... altrimenti lo inizializzo |
||
419 | strips[index].left = i; |
||
420 | lfound = 1; |
||
421 | } |
||
422 | // bordo destro |
||
423 | } else if ((row[i + 1] != ROAD_COLOR) && lfound) { |
||
424 | // Se ho già trovato questo tipo di bordo cerco |
||
425 | // quello + plausibile ... |
||
426 | if (rfound) { |
||
427 | // Test sulla striscia precedente |
||
428 | if (index > 0) { |
||
429 | if (abs(i - strips[index-1].right) < |
||
430 | abs(strips[index].right - strips[index-1].right)) |
||
431 | strips[index].right = i; |
||
432 | } else { |
||
433 | // Test sul centro della macchina |
||
434 | if (abs(i - center_car) < abs(strips[0].right - center_car)) |
||
435 | strips[index].right = i; |
||
436 | } |
||
437 | rfound++; |
||
438 | } else { |
||
439 | // ... altrimenti lo inizializzo |
||
440 | strips[index].right = i; |
||
441 | rfound = 1; |
||
442 | } |
||
443 | } |
||
444 | } |
||
445 | } |
||
446 | |||
447 | // Calcola il centro della strada se trova i bordi |
||
448 | if (lfound && rfound) |
||
449 | strips[index].center = (strips[index].left + strips[index].right) / 2; |
||
450 | |||
451 | return (lfound && rfound); |
||
452 | } |
||
453 | |||
454 | /* ----------------------------------------------------------------- */ |
||
455 | |||
456 | void where_am_i(BYTE *row, road_strip strip, road_info *info) { |
||
457 | int i; |
||
458 | int lfound = 0; |
||
459 | int rfound = 0; |
||
460 | int car_center = TEL_WIDTH/2; |
||
461 | |||
462 | for(i=1; i < (TEL_WIDTH-1); i++) { |
||
463 | // Se trova la strada controlla se si tratta di bordo |
||
464 | // destro o sinistro |
||
465 | if (row[i] == ROAD_COLOR) { |
||
466 | // bordo sinistro |
||
467 | if (row[i - 1] != ROAD_COLOR) { |
||
468 | strip.left = i; |
||
469 | lfound = 1; |
||
470 | break; |
||
471 | } else if (row[i + 1] != ROAD_COLOR) { |
||
472 | strip.right = i; |
||
473 | rfound = 1; |
||
474 | break; |
||
475 | } |
||
476 | } |
||
477 | } |
||
478 | |||
479 | if (lfound && rfound) { |
||
480 | info->flag = ROAD_OK; |
||
481 | } |
||
482 | else if (lfound && !rfound) info->flag = LEFT_ONLY; |
||
483 | else if (!lfound && rfound) info->flag = RIGHT_ONLY; |
||
484 | else { |
||
485 | if (row[car_center] == OFFROAD_COLOR) info->flag = NO_ROAD; |
||
486 | else info->flag = ROAD_OK; |
||
487 | } |
||
488 | info->left_border = car_center - strip.left; |
||
489 | info->right_border = strip.right - car_center; |
||
490 | } |
||
491 | |||
492 | /* ----------------------------------------------------------------- */ |
||
493 | |||
494 | /* Analizza l'immagine vista dalla telecamera inserendo le informazioni |
||
495 | * necessarie in info, ovvero la distanza dai bordi destro e sinistro, |
||
496 | * la distanza dalla prossima curva e il suo angolo di curvatura. |
||
497 | * Per il momento ritorna -1 se non riesce a trovare entrambi i bordi della |
||
498 | * strada; in futuro gestiremo anche questa situazione. |
||
499 | */ |
||
500 | int find_track(BYTE *image, road_info *info) { |
||
501 | // vettore contenente le informazioni su ogni striscia |
||
502 | road_strip strips[TEL_HEIGHT]; |
||
503 | BYTE *row; |
||
504 | int car_center = TEL_WIDTH/2; |
||
505 | int max_scost=0; |
||
506 | int once_scost=0; |
||
507 | int j, rett_tol=0; |
||
508 | |||
509 | info->flag = ROAD_OK; |
||
510 | |||
511 | strips[0].left = STRIP_DEFAULT; |
||
512 | strips[0].right = STRIP_DEFAULT; |
||
513 | /* Tratta in modo personalizzato il primo tratto di strada per |
||
514 | inizializzare alcune variabili e per capire situazioni particolari */ |
||
515 | row = &image[(TEL_HEIGHT - 1)*TEL_WIDTH]; |
||
516 | // Se trova i bordi ... |
||
517 | if (scan_strip(row,0,strips)) { |
||
518 | // ... calcola i bordi della strada e ... |
||
519 | info->left_border = car_center - strips[0].left; |
||
520 | info->right_border = strips[0].right - car_center; |
||
521 | // ... inizializza la tolleranza per il rettilineo. |
||
522 | rett_tol = (strips[0].right - strips[0].left) / TOLL_FACTOR; |
||
523 | } else { |
||
524 | // Se non trova il bordo ... |
||
525 | // Mi sono perso, cercami la strada |
||
526 | where_am_i(row,strips[0],info); |
||
527 | return 1; |
||
528 | } |
||
529 | |||
530 | /* Ora controlla le altre strisce della strada */ |
||
531 | |||
532 | for(j=1; j < TEL_HEIGHT ; j++) { |
||
533 | row = &image[(TEL_HEIGHT - j - 1)*TEL_WIDTH]; |
||
534 | // Se trova i bordi ... |
||
535 | if (scan_strip(row, j, strips)) { |
||
536 | // ... aggiorna l'indice di max scostamento. |
||
537 | if (abs(strips[j].center - car_center) > |
||
538 | abs(strips[max_scost].center - car_center) |
||
539 | && abs(strips[j].center - car_center) > rett_tol) { |
||
540 | |||
541 | max_scost = j; |
||
542 | once_scost=1; // ha trovato almeno un max scost |
||
543 | } |
||
544 | } else { |
||
545 | // Se max_scost non è mai stato settato, prendo il max |
||
546 | if (!once_scost) { |
||
547 | max_scost = j-1; |
||
548 | once_scost = 1; |
||
549 | } |
||
550 | |||
551 | // se la distanza e il max_scost sono troppo vicini approssima la curva |
||
552 | info->distance = find_distance(strips, max_scost, rett_tol); |
||
553 | if ((max_scost - info->distance) < MIN_DIFF) { |
||
554 | info->curve = approx_curve(image, j, info->distance, |
||
555 | strips[info->distance].center,strips) - 90.0; |
||
556 | return 1; |
||
557 | } |
||
558 | break; |
||
559 | } |
||
560 | } |
||
561 | |||
562 | // Se max_scost non è mai stato settato, prendo il max |
||
563 | if (!once_scost) max_scost = TEL_HEIGHT-1; |
||
564 | max_scost = min(max_scost,TEL_HEIGHT-1); |
||
565 | |||
566 | info->distance = find_distance(strips,max_scost,rett_tol); |
||
567 | |||
568 | // calcola l'angolo di curvatura solo se il conto e' attendibile |
||
569 | if ((info->distance < (TEL_HEIGHT*0.9)) && |
||
570 | ((max_scost - info->distance) >= MIN_DIFF)) |
||
571 | info->curve = rad_to_degree(atan2((double)(max_scost - info->distance), |
||
572 | (double)(strips[max_scost].center - strips[info->distance].center))); |
||
573 | else { |
||
574 | // se i conti sono empirici, si guarda la posizione dei due centri |
||
575 | if (strips[max_scost].center > strips[info->distance].center) |
||
576 | info->curve = 88.0; |
||
577 | else if (strips[max_scost].center < strips[info->distance].center) |
||
578 | info->curve = 92.0; |
||
579 | else |
||
580 | info->curve = 90.0; |
||
581 | } |
||
582 | info->curve -= 90.0; |
||
583 | return 0; |
||
584 | } |
||
585 | |||
586 | /* ----------------------------------------------------------------- */ |
||
587 | |||
588 | int find_opps(BYTE *image, road_info *info) |
||
589 | { |
||
590 | int i, j; |
||
591 | int l_limit, r_limit; // the opposite empiric limits |
||
592 | BYTE *row; |
||
593 | int car_center = TEL_WIDTH/2; |
||
594 | int opps_number = 0; |
||
595 | |||
596 | // by default it needs to scan the entire row |
||
597 | l_limit = TEL_WIDTH - 1; |
||
598 | r_limit = 0; |
||
599 | |||
600 | // Scans one row at a time |
||
601 | for(j=1; j<TEL_HEIGHT; j++) { |
||
602 | row = &image[(TEL_HEIGHT - j - 1) * TEL_WIDTH]; |
||
603 | |||
604 | // if some of the previous row has been identified as a car... |
||
605 | if (opps_number > 0 && |
||
606 | j < (info->opps_list[opps_number - 1].y + CAR_HEIGHT + 1)) { |
||
607 | // excludes CAR_HEIGHT lines from the bottom if the car |
||
608 | l_limit = (car_center + info->opps_list[opps_number - 1].x) - CAR_WIDTH; |
||
609 | r_limit = (car_center + info->opps_list[opps_number - 1].x) + CAR_WIDTH; |
||
610 | } |
||
611 | else { |
||
612 | // it needs to scan the entire row |
||
613 | l_limit = TEL_WIDTH - 1; |
||
614 | r_limit = 0; |
||
615 | } |
||
616 | |||
617 | // Scans the current row |
||
618 | for (i=0; i<TEL_WIDTH; i++) { |
||
619 | if( (i < l_limit || i > r_limit) && |
||
620 | row[i] != ROAD_COLOR && row[i] != OFFROAD_COLOR && |
||
621 | row[i] != DEBUG_COLOR && row[i] != OUT_OF_TRACK) { |
||
622 | // there is an opponent car!! |
||
623 | // stores its position |
||
624 | info->opps_list[opps_number].x = i - car_center; // left -, right + |
||
625 | info->opps_list[opps_number].y = j; |
||
626 | |||
627 | // increases number of opposites car in the sensor view range |
||
628 | opps_number++; |
||
629 | } |
||
630 | } |
||
631 | } |
||
632 | |||
633 | info->opps_number = opps_number; |
||
634 | |||
635 | return 0; |
||
636 | } |
||
637 | |||
638 | |||
639 | |||
640 | |||
641 | |||
642 | |||
643 | |||
644 | |||
645 | |||
646 | |||
647 | |||
648 | |||
649 | |||
650 |