Rev Author Line No. Line
799 cizelu 1 #include "main.h"
842 cizelu 2  
708 cizelu 3 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
842 cizelu 4 // BAUD RATE = 9600
873 cizelu 5 // ========================== PRIPRAVA DAT A VYSTUPU ===========================
874 cizelu 6 // pomocne konstanty
865 cizelu 7 #define LEFT 0
8 #define RIGHT 1
874 cizelu 9  
10 // konstanty a promenne pro sledovani cary
900 cizelu 11 #define SPACE 20 // urcuje, za jak dlouho robot vi, ze nic nevidi
874 cizelu 12 #define SPD1 100 // rychlost pomalejsiho motoru, kdyz je cara hodne z osy
879 cizelu 13 #define SPD2 150 // rychlost pomalejsiho motoru, kdyz je cara z osy
874 cizelu 14 #define SPD3 200 // rychlost rychlejsiho motoru pro vsechny pripady
15 #define SPD4 255 // pro oba motory - rovinka
16 // oblasti cary
879 cizelu 17 #define L1 25 // nejvice vlevo
18 #define L2 30
19 #define L3 45
20 #define R1 55
21 #define R2 70
22 #define R3 85 // nejvice vpravo
874 cizelu 23  
24 int8 action; // akce pro nastaveni motoru
25  
842 cizelu 26 // univerzalni LED diody
830 cizelu 27 #define LED1 PIN_E0
28 #define LED2 PIN_E1
700 cizelu 29  
842 cizelu 30 // piezo pipak
716 cizelu 31 #DEFINE SOUND_HI PIN_B4
32 #DEFINE SOUND_LO PIN_B5
700 cizelu 33  
842 cizelu 34 // radkovy senzor
745 cizelu 35 #define SDIN PIN_D4 // seriovy vstup
830 cizelu 36 #define SDOUT input(PIN_C5) // seriovy vystup
745 cizelu 37 #define SCLK PIN_D5 // takt
727 cizelu 38  
874 cizelu 39 #define LINE_PX 4 // pocet pixelu pro jiste urceni cary
868 cizelu 40 #define OLSA_LEV 0x60 // rozhodovaci uroven (cca 10 odpovida cerne)
840 cizelu 41  
842 cizelu 42 // pro komunikaci s OLSA, prvni se posila LSB
829 cizelu 43 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B
44 int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F
45 int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00
730 cizelu 46  
829 cizelu 47 int left_offset[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40
48 int mid_offset[8]={0,1,0,0,0,0,1,0}; // offset prostredniho segmentu senzoru 0x42
49 int right_offset[8]={0,0,1,0,0,0,1,0}; // offset praveho segmentu senzoru 0x44
50 int offset[8]={1,0,0,0,0,0,0,1}; // minus jedna - pouzit pro vsechny segmenty 0x81
741 cizelu 51  
829 cizelu 52 int left_gain[8]={1,0,0,0,0,0,1,0}; // zisk leveho segmentu 0x41
53 int mid_gain[8]={1,1,0,0,0,0,1,0}; // zisk leveho segmentu 0x43
54 int right_gain[8]={1,0,1,0,0,0,1,0}; // zisk leveho segmentu 0x45
55 int gain[8]={1,0,1,0,0,0,0,0}; // zisk = 5 - pouzit pro vsechny segmenty 0x5
771 kaklik 56  
829 cizelu 57 int start_int[8]={0,0,0,1,0,0,0,0}; // zacatek integrace 0x08
58 int stop_int[8]={0,0,0,0,1,0,0,0}; // konec integrace 0x10
59 int readout[8]={0,1,0,0,0,0,0,0}; // cteni senzoru 0x02
789 cizelu 60  
829 cizelu 61 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
62 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
837 cizelu 63 int8 *lp; // ukazatel pro levou polovinu radky
64 int8 *rp; // ukazatel pro levou polovinu radky
812 cizelu 65  
870 cizelu 66 int8 position; // ulozeni pozice cary
873 cizelu 67 int8 old_position; // ulozeni predchozi pozice cary
68 int1 line_sector; // cara je vlevo/vpravo
69 int8 gap; // pocita, jak dlouho neni videt cara
870 cizelu 70  
873 cizelu 71 // ================================= NARAZNIK ==================================
708 cizelu 72 #define BUMPL input(PIN_D6)
73 #define BUMPR input(PIN_D7)
700 cizelu 74  
873 cizelu 75 // ============================= NOUZOVE SENZORY ===============================
708 cizelu 76 #define LINEL 0
77 #define LINER 1
842 cizelu 78 #define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna)
789 cizelu 79 int8 line_l;
80 int8 line_r;
700 cizelu 81  
873 cizelu 82 // ================================== MOTORY ===================================
708 cizelu 83 #define LMF PIN_D0
84 #define LMB PIN_D1
85 #define RMF PIN_D2
86 #define RMB PIN_D3
842 cizelu 87  
88 int8 lm_speed;
790 cizelu 89 int8 rm_speed;
874 cizelu 90 int8 m;
790 cizelu 91  
873 cizelu 92 // =============================== PODPROGRAMY =================================
93  
94 // ================================= OLSA01A ===================================
95  
870 cizelu 96 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
829 cizelu 97 {
741 cizelu 98 int8 ct;
99 for(ct=0;ct<=count;ct++)
829 cizelu 100 {
730 cizelu 101 output_high(SCLK);
102 output_low(SCLK);
829 cizelu 103 }
104 }
730 cizelu 105  
870 cizelu 106 void olsa_pulse() // vytvori jeden impulz
829 cizelu 107 {
741 cizelu 108 output_high(SCLK);
109 output_low(SCLK);
829 cizelu 110 }
741 cizelu 111  
870 cizelu 112 void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy
829 cizelu 113 {
870 cizelu 114 int *ip; // ukazatel na pole s informaci
115 int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
116 output_low(SDIN); // start bit
741 cizelu 117 olsa_pulse();
870 cizelu 118 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
829 cizelu 119 {
870 cizelu 120 i=info[ip]; // ziskani hodnoty z pole
121 if(i==1) // vyhodnoceni obsahu informace - nastav 1
829 cizelu 122 {
730 cizelu 123 output_high(SDIN);
829 cizelu 124 }
870 cizelu 125 else // vyhodnoceni obsahu informace - nastav 0
829 cizelu 126 {
730 cizelu 127 output_low(SDIN);
829 cizelu 128 }
741 cizelu 129 olsa_pulse();
829 cizelu 130 }
870 cizelu 131 output_high(SDIN); // stop bit
741 cizelu 132 olsa_pulse();
829 cizelu 133 }
730 cizelu 134  
870 cizelu 135 void olsa_reset() // hlavni RESET - provadi se po zapnuti
829 cizelu 136 {
741 cizelu 137 output_low(SDIN);
138 output_low(SCLK);
870 cizelu 139 olsa_pulses(30); // reset radkoveho senzoru
741 cizelu 140 output_high(SDIN);
870 cizelu 141 olsa_pulses(10); // start bit - synchronizace
741 cizelu 142 olsa_send(main_reset);
143 olsa_pulses(5);
144 olsa_send(set_mode_rg);
145 olsa_send(clear_mode_rg);
829 cizelu 146 }
789 cizelu 147  
870 cizelu 148 void olsa_setup() // kompletni nastaveni, provadi se po resetu
829 cizelu 149 {
870 cizelu 150 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk)
789 cizelu 151 olsa_send(offset);
152 olsa_send(left_gain);
153 olsa_send(gain);
870 cizelu 154 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk)
789 cizelu 155 olsa_send(offset);
156 olsa_send(mid_gain);
157 olsa_send(gain);
870 cizelu 158 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk)
789 cizelu 159 olsa_send(offset);
160 olsa_send(right_gain);
161 olsa_send(gain);
829 cizelu 162 }
790 cizelu 163  
870 cizelu 164 void olsa_integration() // snimani pixelu
829 cizelu 165 {
870 cizelu 166 olsa_send(start_int); // zacatek integrace senzoru
790 cizelu 167 olsa_pulses(22);
870 cizelu 168 olsa_send(stop_int); // konec integrace senzoru
790 cizelu 169 olsa_pulses(5);
829 cizelu 170 }
796 cizelu 171  
837 cizelu 172 void read_olsa()
173 {
870 cizelu 174 int8 cpixel; // pocet prectenych pixelu
175 int8 cbit; // pocet prectenych bitu
176 int8 pixel; // hodnota precteneho pixelu
837 cizelu 177 cpixel=0;
178 lp=0;
179 rp=0;
180 olsa_integration();
181 olsa_send(readout);
870 cizelu 182 do // precte 102 pixelu
837 cizelu 183 {
870 cizelu 184 if(!SDOUT) // zacatek prenosu - zachycen start bit
837 cizelu 185 {
186 pixel=0;
870 cizelu 187 for(cbit=0;cbit<8;cbit++) // cte jednotlive bity (8 bitu - 0 az 7)
837 cizelu 188 {
870 cizelu 189 olsa_pulse(); // impulz pro generovani dalsiho bitu
190 if(SDOUT) // zachycena 1
837 cizelu 191 {
870 cizelu 192 pixel|=1; // zapise do bitu pixelu 1 - OR
837 cizelu 193 }
870 cizelu 194 else // zachycena 0
837 cizelu 195 {
870 cizelu 196 pixel|=0; // zapise do bitu pixelu 0 - OR
837 cizelu 197 }
870 cizelu 198 pixel<<=1; // posune pixel
837 cizelu 199 }
870 cizelu 200 olsa_pulse(); // generuje stop bit
201 if(cpixel<52) // ulozeni do pole
837 cizelu 202 {
870 cizelu 203 olsa_lseg[lp]=pixel; // leva polovina radky - leve pole
837 cizelu 204 lp++;
205 }
206 else
207 {
870 cizelu 208 olsa_rseg[rp]=pixel; // prava polovina cary - prave pole
837 cizelu 209 rp++;
210 }
211 cpixel++;
212 }
213 else
214 {
870 cizelu 215 olsa_pulse(); // generuje start bit, nebyl-li poslan
837 cizelu 216 }
217 }
870 cizelu 218 while(cpixel<102); // precte 102 pixelu
837 cizelu 219 }
220  
870 cizelu 221 void olsa_position() // vyhodnoti pozici cary
869 cizelu 222 {
223 int8 searchp; // ukazatel na pole
224 int8 search; // ulozeni prectene hodnoty
225 int8 protect_count; // opravdu vidime caru
870 cizelu 226 position=0; // nuluje pozici, pokud cara neni, ulozena 0
869 cizelu 227 for(searchp=0;searchp<52;searchp++) // prohlizi levou cast cary
228 {
229 search=olsa_lseg[searchp]; // vybira pixel
230 if(search==OLSA_LEV) // cerna nebo bila?
231 {
232 protect_count++; // pokud nasleduje cerna, pricte 1 k poctu cernych pixelu
233 }
234 else
235 {
236 protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje
237 }
238 if(protect_count>LINE_PX) // vidim caru
239 {
240 position=searchp; // zapis presnou pozici
873 cizelu 241 line_sector=LEFT; // cara je v leve polovine
870 cizelu 242 searchp=55; // ukonci hledani
869 cizelu 243 }
244 }
870 cizelu 245 for(searchp=0;searchp<52;searchp++) // prohlizi pravou cast cary
869 cizelu 246 {
870 cizelu 247 search=olsa_rseg[searchp]; // vybira pixel
869 cizelu 248 if(search==OLSA_LEV)
249 {
250 protect_count++; // pokud nasleduje cerna, pricte 1 k poctu cernych pixelu
251 }
252 else
253 {
254 protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje
255 }
256 if(protect_count>LINE_PX) // vidim caru
257 {
258 position=(searchp+50); // zapis presnou pozici
872 cizelu 259 line_sector=RIGHT; // cara je v prave polovine
870 cizelu 260 searchp=55; // ukonci hledani
869 cizelu 261 }
262 }
263 }
264  
873 cizelu 265 // ============================ ZACHRANNE SENZORY ==============================
266  
874 cizelu 267 void read_blue_sensors() // cteni nouzovych senzoru
829 cizelu 268 {
874 cizelu 269 set_adc_channel(LINEL); // cti levy nouzovy senzor
700 cizelu 270 delay_us(10);
745 cizelu 271 line_l=read_adc();
874 cizelu 272 set_adc_channel(LINER); // cti pravy nouzovy senzor
700 cizelu 273 delay_us(10);
274 line_r=read_adc();
829 cizelu 275 }
730 cizelu 276  
873 cizelu 277 // ================================== PIPAK ====================================
278  
716 cizelu 279 void beep(int16 period,int16 length)
829 cizelu 280 {
874 cizelu 281 int16 bp; // promenna pro nastaveni delky
282 for(bp=length;bp>0;bp--) // prepina vystupy tolikrat, jakou jsme zadali delku
829 cizelu 283 {
730 cizelu 284 output_high(SOUND_HI);
285 output_low(SOUND_LO);
286 delay_us(period);
287 output_high(SOUND_LO);
288 output_low(SOUND_HI);
289 delay_us(period);
829 cizelu 290 }
291 }
873 cizelu 292  
293 // ================================== MOTORY ===================================
294  
874 cizelu 295 void l_motor_fwd(int8 speedl) // levy motor dopredu
829 cizelu 296 {
708 cizelu 297 output_high(LMF);
298 output_low(LMB);
299 set_pwm2_duty(speedl);
829 cizelu 300 }
700 cizelu 301  
874 cizelu 302 void l_motor_bwd(int8 speedl) // levy motor dozadu
829 cizelu 303 {
708 cizelu 304 output_high(LMB);
305 output_low(LMF);
306 set_pwm2_duty(speedl);
829 cizelu 307 }
708 cizelu 308  
874 cizelu 309 void r_motor_fwd(int8 speedr) // pravy motor dopredu
829 cizelu 310 {
708 cizelu 311 output_high(RMF);
312 output_low(RMB);
313 set_pwm1_duty(speedr);
829 cizelu 314 }
708 cizelu 315  
874 cizelu 316 void r_motor_bwd(int8 speedr) // pravy motor dozadu
829 cizelu 317 {
708 cizelu 318 output_high(RMB);
319 output_low(RMF);
320 set_pwm1_duty(speedr);
829 cizelu 321 }
708 cizelu 322  
874 cizelu 323 void l_motor_off() // levy motor vypnut
829 cizelu 324 {
708 cizelu 325 output_low(LMF);
326 output_low(LMB);
327 set_pwm2_duty(0);
829 cizelu 328 }
730 cizelu 329  
874 cizelu 330 void r_motor_off() // pravy motor vypnut
829 cizelu 331 {
708 cizelu 332 output_low(RMF);
333 output_low(RMB);
334 set_pwm1_duty(0);
829 cizelu 335 }
708 cizelu 336  
874 cizelu 337 void motor_test() // TEST MOTORU
829 cizelu 338 {
708 cizelu 339 int8 i;
716 cizelu 340 beep(100,200);
799 cizelu 341 printf("TEST MOTORU\r\n");
708 cizelu 342 delay_ms(1000);
799 cizelu 343 printf("LEVY MOTOR DOPREDU\r\n");
818 cizelu 344 delay_ms(1000);
874 cizelu 345 for(i=0;i<255;i++) // levy motor dopredu - zrychluje
829 cizelu 346 {
708 cizelu 347 l_motor_fwd(i);
799 cizelu 348 printf("RYCHLOST: %u\r\n",i);
716 cizelu 349 delay_ms(5);
829 cizelu 350 }
874 cizelu 351 for(i=255;i>0;i--) // levy motor dopredu - zpomaluje
829 cizelu 352 {
708 cizelu 353 l_motor_fwd(i);
799 cizelu 354 printf("RYCHLOST: %u\r\n",i);
716 cizelu 355 delay_ms(5);
829 cizelu 356 }
874 cizelu 357 printf("LEVY MOTOR DOZADU\r\n"); // levy motor dozadu - zrychluje
818 cizelu 358 delay_ms(1000);
708 cizelu 359 for(i=0;i<255;i++)
829 cizelu 360 {
708 cizelu 361 l_motor_bwd(i);
799 cizelu 362 printf("RYCHLOST: %u\r\n",i);
716 cizelu 363 delay_ms(5);
829 cizelu 364 }
874 cizelu 365 for(i=255;i>0;i--) // levy motor dozadu - zpomaluje
829 cizelu 366 {
708 cizelu 367 l_motor_bwd(i);
799 cizelu 368 printf("RYCHLOST: %u\r\n",i);
716 cizelu 369 delay_ms(5);
829 cizelu 370 }
870 cizelu 371 printf("PRAVY MOTOR DOPREDU\r\n");
818 cizelu 372 delay_ms(1000);
874 cizelu 373 for(i=0;i<255;i++) // pravy motor dopredu - zrychluje
829 cizelu 374 {
708 cizelu 375 r_motor_fwd(i);
799 cizelu 376 printf("RYCHLOST: %u\r\n",i);
716 cizelu 377 delay_ms(5);
829 cizelu 378 }
874 cizelu 379 for(i=255;i>0;i--) // pravy motor dopredu - zpomaluje
829 cizelu 380 {
708 cizelu 381 r_motor_fwd(i);
799 cizelu 382 printf("RYCHLOST: %u\r\n",i);
716 cizelu 383 delay_ms(5);
829 cizelu 384 }
799 cizelu 385 printf("PRAVY MOTOR DOZADU\r\n");
818 cizelu 386 delay_ms(1000);
874 cizelu 387 for(i=0;i<255;i++) // pravy motor dozadu - zrychluje
829 cizelu 388 {
708 cizelu 389 r_motor_bwd(i);
799 cizelu 390 printf("RYCHLOST: %u\r\n",i);
716 cizelu 391 delay_ms(5);
829 cizelu 392 }
874 cizelu 393 for(i=255;i>0;i--) // pravy motor dozadu - zpomaluje
829 cizelu 394 {
708 cizelu 395 r_motor_bwd(i);
799 cizelu 396 printf("RYCHLOST: %u\r\n",i);
716 cizelu 397 delay_ms(5);
829 cizelu 398 }
874 cizelu 399 l_motor_off(); // po ukonceni testu vypnout motory
818 cizelu 400 r_motor_off();
870 cizelu 401 printf("KONEC TESTU MOTORU\r\n");
818 cizelu 402 delay_ms(1000);
829 cizelu 403 }
708 cizelu 404  
873 cizelu 405 // ================================ DIAGNOSTIKA ================================
406  
874 cizelu 407 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
708 cizelu 408 {
409 read_blue_sensors();
870 cizelu 410 read_olsa();
411 olsa_position();
874 cizelu 412 printf("LEVA: %u \t",line_l); // tiskne z leveho senzoru
413 printf("PRAVA: %u \t",line_r); // tiskne z praveho senzoru
414 printf("POLOHA: %u\t",position); // tiskne pozici OLSA
415 printf("L_NARAZ: %u \t",BUMPL); // leve tlacitko narazniku
416 printf("P_NARAZ: %u \r\n",BUMPR); // prave tlacitko narazniku
417 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
708 cizelu 418 {
868 cizelu 419 beep(100,1000);
865 cizelu 420 printf("Levy naraznik - test OLSA\r\n");
421 printf("Pravy naraznik - test motoru\r\n");
422 delay_ms(500);
868 cizelu 423 while(true)
865 cizelu 424 {
868 cizelu 425 if(BUMPR)
843 cizelu 426 {
870 cizelu 427 beep(100,500); // pipni pri startu
868 cizelu 428 motor_test();
429 }
430 if(BUMPL)
431 {
432 beep(100,500);
433 printf("TEST OLSA\r\n");
434 while(true)
843 cizelu 435 {
868 cizelu 436 int8 tisk;
437 int8 *tiskp;
438 read_olsa();
439 printf("cteni\r\n"); // po precteni vsech pixelu odradkuje
440 for(tiskp=0;tiskp<52;tiskp++) // tisk leve casti radky
441 {
442 tisk=olsa_lseg[tiskp];
443 printf("%x ",tisk);
444 }
445 for(tiskp=0;tiskp<52;tiskp++) // tisk prave casti radky
446 {
447 tisk=olsa_rseg[tiskp];
448 printf("%x ",tisk);
449 }
865 cizelu 450 }
451 }
452 }
453 }
708 cizelu 454 }
455  
873 cizelu 456 // ============================== HLAVNI SMYCKA ================================
457  
842 cizelu 458 void main()
829 cizelu 459 {
799 cizelu 460 printf("POWER ON \r\n");
842 cizelu 461 // NASTAVENI > provede se pouze pri zapnuti
873 cizelu 462 setup_adc_ports(sAN0-sAN1-sAN2); // aktivní analogové vstupy RA0, RA1 a RA2
463 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
842 cizelu 464 setup_spi(SPI_SS_DISABLED);
465 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
466 setup_timer_1(T1_DISABLED);
873 cizelu 467 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
468 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
469 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
842 cizelu 470 setup_comparator(NC_NC_NC_NC);
471 setup_vref(FALSE);
873 cizelu 472 l_motor_off(); // vypne levy motor
473 r_motor_off(); // vypne pravy motor
474 output_high(LED1); // zhasne LED1
475 output_high(LED2); // zhasne LED2
842 cizelu 476 olsa_reset();
477 olsa_setup();
873 cizelu 478 beep(100,500); // pipni pri startu
842 cizelu 479 printf("OK! \r\n");
480 delay_ms(500);
481 printf("VYBRAT MOD... \r\n");
900 cizelu 482 // ============================ HLAVNI CAST PROGRAMU ===========================
842 cizelu 483 while(true)
865 cizelu 484 {
900 cizelu 485 // ================================ DIAGNOSTIKA ================================
486 if(BUMPL)
487 {
488 beep(100,500);
489 while(true)
490 {
491 diag();
492 }
493 }
494 // =============================== SLEDOVANI CARY ==============================
874 cizelu 495 if(BUMPR) // spusteni hledani pravym naraznikem
873 cizelu 496 {
497 beep(100,500);
498 while(true)
499 {
500 old_position=position; // zaznamena predhozi polohu cary
501 read_olsa(); // precte a ulozi hodnoty z olsa
502 olsa_position(); // vyhodnoti pozici cary
503 read_blue_sensors(); // cte nouzove senzory
879 cizelu 504 if((line_l==0)&&(gap>SPACE)) // robot najede na levy nouzovy senzor
873 cizelu 505 {
874 cizelu 506 l_motor_bwd(150); // prudce zatoc doleva
873 cizelu 507 r_motor_fwd(255);
508 line_sector=LEFT; // pamatuj, kde je cara
900 cizelu 509 delay_ms(300);
873 cizelu 510 }
879 cizelu 511 if((line_r==0)&&(gap>SPACE)) // robot najede na pravy nouzovy senzor
873 cizelu 512 {
513 l_motor_fwd(255); // prudce zatoc doprava
874 cizelu 514 r_motor_bwd(150);
873 cizelu 515 line_sector=RIGHT; // pamatuj, kde je cara
900 cizelu 516 delay_ms(300);
873 cizelu 517 }
518 if(position==0) // pokud neni videt cara
519 {
520 position=old_position; // nastav predchozi pozici
521 gap++; // pocita, jak dlouho neni videt cara
522 }
523 else // pokud je videt cara
524 {
525 gap=0; // vynuluje citani
874 cizelu 526 m=0; // pry svetsovani hulu zataceni
873 cizelu 527 }
874 cizelu 528 if(position>0) // urceni akci pro rizeni, pokud cara je videt
529 {
530 if(position>L1) // cara je hodne vlevo
531 {
532 if(position>L2) // cara je vlevo
533 {
534 if(position>L3) // cara je mirne vlevo
535 {
536 if(position>R1) // cara je mirne vpravo
537 {
538 if(position>R2) // cara je vpravo
539 {
540 if(position>R3) // cara je hodne vpravo
541 {
542 action=6;
543 }
544 }
545 else
546 {
547 action=5;
548 }
549 }
550 else
551 {
552 action=4;
553 }
554 }
555 else
556 {
557 action=3;
558 }
559 }
560 else
561 {
562 action=2;
563 }
564 }
565 else
566 {
567 action=1;
568 }
879 cizelu 569 }
570 if(L3<=position>=R1) // cara je uprostred
874 cizelu 571 {
572 action=7;
573 }
574 switch(action) // reakce na pohyb cary
575 {
576 case 1: // cara je hodne vlevo
577 rm_speed=(SPD3+position);
578 lm_speed=(SPD1+position);
579 r_motor_fwd(rm_speed);
580 l_motor_bwd(lm_speed);
581 break;
582 case 2: // cara je vlevo
583 rm_speed=(SPD3+position);
584 r_motor_fwd(rm_speed);
585 l_motor_off();
586 break;
587 case 3: // cara je mirne vlevo
588 rm_speed=(SPD3+position);
589 lm_speed=(SPD2-position);
590 r_motor_fwd(rm_speed);
591 l_motor_fwd(lm_speed);
592 break;
593 case 4: // cara je mirne vpravo
594 lm_speed=(SPD3+(position/2));
595 rm_speed=(SPD2-(position/2));
596 l_motor_fwd(lm_speed);
597 r_motor_fwd(rm_speed);
598 break;
599 case 5: // cara je vpravo
600 lm_speed=(SPD3+(position/2));
601 l_motor_fwd(lm_speed);
602 r_motor_off();
603 break;
604 case 6: // cara je hodne vpravo
605 lm_speed=(SPD3+(position/2));
606 rm_speed=(SPD2-(position/2));
607 l_motor_fwd(lm_speed);
608 r_motor_bwd(rm_speed);
609 break;
610 case 7: // cara je uprostred
611 l_motor_fwd(SPD4);
612 r_motor_fwd(SPD4);
613 break;
614 }
879 cizelu 615 if((line_sector==LEFT)&&(position==0))
616 {
617 r_motor_fwd(255);
618 l_motor_off();
619 }
620 if((line_sector==RIGHT)&&(position==0))
621 {
622 l_motor_fwd(255);
623 r_motor_off();
624 }
873 cizelu 625 }
626 }
730 cizelu 627 }
829 cizelu 628 }
865 cizelu 629  
869 cizelu 630