Rev 933 Rev 954
Line 25... Line 25...
25 int8 err5; // odchylka posledni hodnoty 25 int8 err5; // odchylka posledni hodnoty
26 int8 errp; // prumer chyb 26 int8 errp; // prumer chyb
27   27  
28 // mezera 28 // mezera
29 #define SPACE 8 // jak dlouho robot smi nic nevidet (8) 29 #define SPACE 8 // jak dlouho robot smi nic nevidet (8)
-   30 #define CONT 20 // kontrast, kdy nic nevidime
30   31  
31 // univerzalni LED diody 32 // univerzalni LED diody
32 #define LED1 PIN_E1 33 #define LED1 PIN_E1
33 #define LED2 PIN_E0 34 #define LED2 PIN_E0
34   35  
Line 68... Line 69...
68 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50) 69 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
69 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101) 70 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
70 int8 *lp; // ukazatel pro levou polovinu radky 71 int8 *lp; // ukazatel pro levou polovinu radky
71 int8 *rp; // ukazatel pro levou polovinu radky 72 int8 *rp; // ukazatel pro levou polovinu radky
72   73  
-   74 int8 contrast;
73 int8 position; // ulozeni pozice cary 75 int8 position; // ulozeni pozice cary
74 int8 old_position; // ulozeni predchozi pozice cary 76 int8 old_position; // ulozeni predchozi pozice cary
75 int1 line_sector; // cara je vlevo/vpravo 77 int1 line_sector; // cara je vlevo/vpravo
76 int8 gap; // pocita, jak dlouho neni videt cara 78 int8 gap; // pocita, jak dlouho neni videt cara
77   79  
Line 87... Line 89...
87 int8 line_l; // uklada hodnotu leveho senzoru 89 int8 line_l; // uklada hodnotu leveho senzoru
88 int8 line_r; // uklada hodnotu praveho senzoru 90 int8 line_r; // uklada hodnotu praveho senzoru
89   91  
90 // ================================ DALKOMER =================================== 92 // ================================ DALKOMER ===================================
91 #define SHARP 2 // analogovy kanal pro SHARP 93 #define SHARP 2 // analogovy kanal pro SHARP
92 #define PROBLEM 50 // rozhodovaci uroven, kdy hrozi prekazka 94 #define PROBLEM 55 // rozhodovaci uroven, kdy hrozi prekazka
93 #define BLOCK 70 // rozhodovaci uroven, kdy je jiste prekazka 95 #define BLOCK 65 // rozhodovaci uroven, kdy je jiste prekazka
94 #define DANGER 40 // pocita, jak dlouho je detekovan problem 96 #define DANGER 10 // pocita, jak dlouho je detekovan problem
95   97  
96 int8 p_count; 98 int8 p_count;
97 int8 sharp_lev; // uklada hodnotu sharp 99 int8 sharp_lev; // uklada hodnotu sharp
98   100  
99 // ================================== MOTORY =================================== 101 // ================================== MOTORY ===================================
Line 201... Line 203...
201 { 203 {
202 pixel=0; 204 pixel=0;
203 for(cbit=0;cbit<8;cbit++) // cte jednotlive bity (8 bitu - 0 az 7) 205 for(cbit=0;cbit<8;cbit++) // cte jednotlive bity (8 bitu - 0 az 7)
204 { 206 {
205 olsa_pulse(); // impulz pro generovani dalsiho bitu 207 olsa_pulse(); // impulz pro generovani dalsiho bitu
-   208
206 if(SDOUT) // zachycena 1 209 if(SDOUT) // zachycena 1
207 { 210 {
208 pixel|=1; // zapise do bitu pixelu 1 - OR 211 bit_set(pixel,cbit); // zapise do bitu pixelu 1 - OR
209 } 212 }
210 else // zachycena 0 -  
211 { -  
212 pixel|=0; // zapise do bitu pixelu 0 - OR -  
213 } -  
214 pixel<<=1; // posune pixel -  
215 } 213 }
216 olsa_pulse(); // generuje stop bit 214 olsa_pulse(); // generuje stop bit
217 if(cpixel<52) // ulozeni do pole 215 if(cpixel<52) // ulozeni do pole
218 { 216 {
219 olsa_lseg[lp]=pixel; // leva polovina radky - leve pole 217 olsa_lseg[lp]=pixel; // leva polovina radky - leve pole
Line 236... Line 234...
236   234  
237 void olsa_position() // vyhodnoti pozici cary 235 void olsa_position() // vyhodnoti pozici cary
238 { 236 {
239 int8 searchp; // ukazatel na pole 237 int8 searchp; // ukazatel na pole
240 int8 search; // ulozeni prectene hodnoty 238 int8 search; // ulozeni prectene hodnoty
241 int8 protect_count; // opravdu vidime caru 239 int8 dark; // nejtmavsi pixel
242 position=0; // nuluje pozici, pokud cara neni, ulozena 0 240 int8 bright; // nejsvetlejsi pixel
-   241 dark=0xff;
-   242 bright=0x00;
243 for(searchp=0;searchp<52;searchp++) // prohlizi levou cast cary 243 for(searchp=0;searchp<52;searchp++) // prohlizi levou cast radky
244 { 244 {
245 search=olsa_lseg[searchp]; // vybira pixel 245 search=olsa_lseg[searchp]; // vybira pixel
246 if(search==OLSA_LEV) // cerna nebo bila? 246 if((search<dark)&&(search>0x00)) // porovna pixel s doposud nejtmavsim
247 { -  
248 protect_count++; // pokud nasleduje cerna, pricte 1 k poctu cernych pixelu -  
249 } -  
250 else -  
251 { 247 {
-   248 dark=search; // ulozi nejtmavsi pixel
252 protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje 249 position=searchp; // ulozi polohu nejtmavsiho pixelu
253 } 250 }
254 if(protect_count>LINE_PX) // vidim caru 251 if((search>bright)&&(search<0xff))
255 { 252 {
256 position=searchp; // zapis presnou pozici 253 bright=search; // ulozi nejsvetlejsi pixel
257 line_sector=LEFT; // cara je v leve polovine -  
258 searchp=55; // ukonci hledani -  
259 } 254 }
260 } 255 }
261 for(searchp=0;searchp<52;searchp++) // prohlizi pravou cast cary 256 for(searchp=0;searchp<52;searchp++) // prohlizi pravou cast radky
262 { 257 {
263 search=olsa_rseg[searchp]; // vybira pixel 258 search=olsa_rseg[searchp]; // vybira pixel
264 if(search==OLSA_LEV) 259 if((search<dark)&&(search>0x00)) // porovna pixel s doposud nejtmavsim
265 { 260 {
266 protect_count++; // pokud nasleduje cerna, pricte 1 k poctu cernych pixelu 261 dark=search; // ulozi nejtmavsi pixel
-   262 position=(searchp+50); // ulozi polohu nejtmavsiho pixelu
267 } 263 }
268 else -  
269 { -  
270 protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje -  
271 } -  
272 if(protect_count>LINE_PX) // vidim caru 264 if((search>bright)&&(search<0xff))
273 { 265 {
274 position=(searchp+50); // zapis presnou pozici -  
275 line_sector=RIGHT; // cara je v prave polovine -  
276 searchp=55; // ukonci hledani 266 bright=search; // ulozi nejsvetlejsi pixel
277 } 267 }
278 } 268 }
-   269 contrast=(bright-dark);
-   270 if(contrast<CONT)
-   271 {
-   272 position=0;
-   273 }
279 } 274 }
280   275  
281 // ============================ ZACHRANNE SENZORY ============================== 276 // ============================ ZACHRANNE SENZORY ==============================
282   277  
283 void read_blue_sensors() // cteni nouzovych senzoru 278 void read_blue_sensors() // cteni nouzovych senzoru
Line 467... Line 462...
467   462  
468 void detour() // po detekci prekazky zacne objizdeni 463 void detour() // po detekci prekazky zacne objizdeni
469 { 464 {
470 l_motor_bwd(255); // zatoc doleva 465 l_motor_bwd(255); // zatoc doleva
471 r_motor_fwd(255); 466 r_motor_fwd(255);
472 delay_ms(500); 467 delay_ms(350);
473 l_motor_fwd(255); // jed rovne 468 l_motor_fwd(255); // jed rovne
474 delay_ms(1000); 469 delay_ms(1000);
475 r_motor_bwd(255); // zatoc doprava 470 r_motor_bwd(255); // zatoc doprava
476 delay_ms(450); 471 delay_ms(350);
477 r_motor_fwd(255); // jed rovne 472 r_motor_fwd(255); // jed rovne
478 delay_ms(1500); 473 delay_ms(1500);
479 r_motor_bwd(255); // zatoc doprava 474 r_motor_bwd(255); // zatoc doprava
480 delay_ms(450); 475 delay_ms(300);
481 r_motor_fwd(255); // jed rovne 476 r_motor_fwd(255); // jed rovne
482 delay_ms(500); 477 delay_ms(200);
-   478 position=40;
483 } 479 }
484 // ================================ DIAGNOSTIKA ================================ 480 // ================================ DIAGNOSTIKA ================================
485   481  
486 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru 482 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
487 { 483 {
Line 491... Line 487...
491 olsa_position(); 487 olsa_position();
492 printf("LEVA: %u \t",line_l); // tiskne z leveho senzoru 488 printf("LEVA: %u \t",line_l); // tiskne z leveho senzoru
493 printf("PRAVA: %u \t",line_r); // tiskne z praveho senzoru 489 printf("PRAVA: %u \t",line_r); // tiskne z praveho senzoru
494 printf("SHARP: %u \t",sharp_lev); // tiskne z dalkomeru 490 printf("SHARP: %u \t",sharp_lev); // tiskne z dalkomeru
495 printf("POLOHA: %u\t",position); // tiskne pozici OLSA 491 printf("POLOHA: %u\t",position); // tiskne pozici OLSA
-   492 printf("KONTRAST: %u \t", contrast); // tiskne kontrast z OLSA
496 printf("L_NARAZ: %u \t",BUMPL); // leve tlacitko narazniku 493 printf("L_NARAZ: %u \t",BUMPL); // leve tlacitko narazniku
497 printf("P_NARAZ: %u \r\n",BUMPR); // prave tlacitko narazniku 494 printf("P_NARAZ: %u \r\n",BUMPR); // prave tlacitko narazniku
498 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru 495 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
499 { 496 {
500 beep(100,1000); 497 beep(100,1000);