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