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