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