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