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