| Line 56... |
Line 56... |
| 56 |
#define LINEL 0 |
56 |
#define LINEL 0 |
| 57 |
#define LINER 1 |
57 |
#define LINER 1 |
| 58 |
#define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna) |
58 |
#define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna) |
| 59 |
int8 line_l; |
59 |
int8 line_l; |
| 60 |
int8 line_r; |
60 |
int8 line_r; |
| 61 |
int1 line_position; |
61 |
int1 line_sector; |
| 62 |
|
62 |
|
| 63 |
// motory |
63 |
// motory |
| 64 |
#define LMF PIN_D0 |
64 |
#define LMF PIN_D0 |
| 65 |
#define LMB PIN_D1 |
65 |
#define LMB PIN_D1 |
| 66 |
#define RMF PIN_D2 |
66 |
#define RMF PIN_D2 |
| Line 199... |
Line 199... |
| 199 |
|
199 |
|
| 200 |
void olsa_position() // vyhodnoti pozici cary |
200 |
void olsa_position() // vyhodnoti pozici cary |
| 201 |
{ |
201 |
{ |
| 202 |
int8 searchp; // ukazatel na pole |
202 |
int8 searchp; // ukazatel na pole |
| 203 |
int8 search; // ulozeni prectene hodnoty |
203 |
int8 search; // ulozeni prectene hodnoty |
| 204 |
int1 segment; // cara je vlevo nebo vpravo |
- |
|
| 205 |
int8 protect_count; // opravdu vidime caru |
204 |
int8 protect_count; // opravdu vidime caru |
| 206 |
position=0; // nuluje pozici, pokud cara neni, ulozena 0 |
205 |
position=0; // nuluje pozici, pokud cara neni, ulozena 0 |
| 207 |
for(searchp=0;searchp<52;searchp++) // prohlizi levou cast cary |
206 |
for(searchp=0;searchp<52;searchp++) // prohlizi levou cast cary |
| 208 |
{ |
207 |
{ |
| 209 |
search=olsa_lseg[searchp]; // vybira pixel |
208 |
search=olsa_lseg[searchp]; // vybira pixel |
| Line 216... |
Line 215... |
| 216 |
protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje |
215 |
protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje |
| 217 |
} |
216 |
} |
| 218 |
if(protect_count>LINE_PX) // vidim caru |
217 |
if(protect_count>LINE_PX) // vidim caru |
| 219 |
{ |
218 |
{ |
| 220 |
position=searchp; // zapis presnou pozici |
219 |
position=searchp; // zapis presnou pozici |
| 221 |
segment=LEFT; // cara je v leve polovine |
220 |
line_sector=RIGHT; // cara je v leve polovine |
| 222 |
searchp=55; // ukonci hledani |
221 |
searchp=55; // ukonci hledani |
| 223 |
} |
222 |
} |
| 224 |
} |
223 |
} |
| 225 |
for(searchp=0;searchp<52;searchp++) // prohlizi pravou cast cary |
224 |
for(searchp=0;searchp<52;searchp++) // prohlizi pravou cast cary |
| 226 |
{ |
225 |
{ |
| Line 234... |
Line 233... |
| 234 |
protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje |
233 |
protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje |
| 235 |
} |
234 |
} |
| 236 |
if(protect_count>LINE_PX) // vidim caru |
235 |
if(protect_count>LINE_PX) // vidim caru |
| 237 |
{ |
236 |
{ |
| 238 |
position=(searchp+50); // zapis presnou pozici |
237 |
position=(searchp+50); // zapis presnou pozici |
| 239 |
segment=RIGHT; // cara je v prave polovine |
238 |
line_sector=RIGHT; // cara je v prave polovine |
| 240 |
searchp=55; // ukonci hledani |
239 |
searchp=55; // ukonci hledani |
| 241 |
} |
240 |
} |
| 242 |
} |
241 |
} |
| 243 |
// printf("poloha: %u\r\n",position); // tiskne pozici - zakomentovat pro hledani |
- |
|
| 244 |
} |
242 |
} |
| 245 |
|
243 |
|
| 246 |
//ZACHRANNE SENZORY |
244 |
//ZACHRANNE SENZORY |
| 247 |
void read_blue_sensors() // cteni nouzovych senzoru |
245 |
void read_blue_sensors() // cteni nouzovych senzoru |
| 248 |
{ |
246 |
{ |
| Line 453... |
Line 451... |
| 453 |
printf("OK! \r\n"); |
451 |
printf("OK! \r\n"); |
| 454 |
delay_ms(500); |
452 |
delay_ms(500); |
| 455 |
printf("VYBRAT MOD... \r\n"); |
453 |
printf("VYBRAT MOD... \r\n"); |
| 456 |
while(true) |
454 |
while(true) |
| 457 |
{ |
455 |
{ |
| - |
|
456 |
read_olsa(); // precte a ulozi hodnoty z olsa |
| - |
|
457 |
olsa_position(); // vyhodnoti pozici cary |
| 458 |
diag(); |
458 |
read_blue_sensors(); // cte nouzove senzory |
| - |
|
459 |
//printf("poloha: %u\r\n",position); // tiskne pozici - zakomentovat pro hledani |
| 459 |
} |
460 |
} |
| 460 |
} |
461 |
} |
| 461 |
|
462 |
|
| 462 |
|
463 |
|