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