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