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 ===========================
865 cizelu 6 // pomocne promenne
7 #define LEFT 0
8 #define RIGHT 1
842 cizelu 9 // univerzalni LED diody
830 cizelu 10 #define LED1 PIN_E0
11 #define LED2 PIN_E1
700 cizelu 12  
842 cizelu 13 // piezo pipak
716 cizelu 14 #DEFINE SOUND_HI PIN_B4
15 #DEFINE SOUND_LO PIN_B5
700 cizelu 16  
842 cizelu 17 // radkovy senzor
745 cizelu 18 #define SDIN PIN_D4 // seriovy vstup
830 cizelu 19 #define SDOUT input(PIN_C5) // seriovy vystup
745 cizelu 20 #define SCLK PIN_D5 // takt
727 cizelu 21  
865 cizelu 22 #define LINE_PX 5 // pocet pixelu pro jiste urceni cary
868 cizelu 23 #define OLSA_LEV 0x60 // rozhodovaci uroven (cca 10 odpovida cerne)
840 cizelu 24  
842 cizelu 25 // pro komunikaci s OLSA, prvni se posila LSB
829 cizelu 26 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B
27 int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F
28 int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00
730 cizelu 29  
829 cizelu 30 int left_offset[8]={0,0,0,0,0,0,1,0}; // offset leveho segmentu senzoru 0x40
31 int mid_offset[8]={0,1,0,0,0,0,1,0}; // offset prostredniho segmentu senzoru 0x42
32 int right_offset[8]={0,0,1,0,0,0,1,0}; // offset praveho segmentu senzoru 0x44
33 int offset[8]={1,0,0,0,0,0,0,1}; // minus jedna - pouzit pro vsechny segmenty 0x81
741 cizelu 34  
829 cizelu 35 int left_gain[8]={1,0,0,0,0,0,1,0}; // zisk leveho segmentu 0x41
36 int mid_gain[8]={1,1,0,0,0,0,1,0}; // zisk leveho segmentu 0x43
37 int right_gain[8]={1,0,1,0,0,0,1,0}; // zisk leveho segmentu 0x45
38 int gain[8]={1,0,1,0,0,0,0,0}; // zisk = 5 - pouzit pro vsechny segmenty 0x5
771 kaklik 39  
829 cizelu 40 int start_int[8]={0,0,0,1,0,0,0,0}; // zacatek integrace 0x08
41 int stop_int[8]={0,0,0,0,1,0,0,0}; // konec integrace 0x10
42 int readout[8]={0,1,0,0,0,0,0,0}; // cteni senzoru 0x02
789 cizelu 43  
829 cizelu 44 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
45 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
837 cizelu 46 int8 *lp; // ukazatel pro levou polovinu radky
47 int8 *rp; // ukazatel pro levou polovinu radky
812 cizelu 48  
870 cizelu 49 int8 position; // ulozeni pozice cary
873 cizelu 50 int8 old_position; // ulozeni predchozi pozice cary
51 int1 line_sector; // cara je vlevo/vpravo
52 int8 change; // zmena oproti predchozi poloze
53 int8 gap; // pocita, jak dlouho neni videt cara
870 cizelu 54  
873 cizelu 55 // ================================= NARAZNIK ==================================
708 cizelu 56 #define BUMPL input(PIN_D6)
57 #define BUMPR input(PIN_D7)
700 cizelu 58  
873 cizelu 59 // ============================= NOUZOVE SENZORY ===============================
708 cizelu 60 #define LINEL 0
61 #define LINER 1
842 cizelu 62 #define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna)
789 cizelu 63 int8 line_l;
64 int8 line_r;
700 cizelu 65  
873 cizelu 66 // ================================== MOTORY ===================================
708 cizelu 67 #define LMF PIN_D0
68 #define LMB PIN_D1
69 #define RMF PIN_D2
70 #define RMB PIN_D3
842 cizelu 71  
72 int8 lm_speed;
790 cizelu 73 int8 rm_speed;
74  
873 cizelu 75 // =============================== PODPROGRAMY =================================
76  
77 // ================================= OLSA01A ===================================
78  
870 cizelu 79 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
829 cizelu 80 {
741 cizelu 81 int8 ct;
82 for(ct=0;ct<=count;ct++)
829 cizelu 83 {
730 cizelu 84 output_high(SCLK);
85 output_low(SCLK);
829 cizelu 86 }
87 }
730 cizelu 88  
870 cizelu 89 void olsa_pulse() // vytvori jeden impulz
829 cizelu 90 {
741 cizelu 91 output_high(SCLK);
92 output_low(SCLK);
829 cizelu 93 }
741 cizelu 94  
870 cizelu 95 void olsa_send(int8 info[8]) // USART komunikace s modulem OLSA01A - poslani zpravy
829 cizelu 96 {
870 cizelu 97 int *ip; // ukazatel na pole s informaci
98 int8 i; // pomocna promenna pro nastaveni 0 nebo 1 na SDIN
99 output_low(SDIN); // start bit
741 cizelu 100 olsa_pulse();
870 cizelu 101 for(ip=0;ip<8;ip++) // predani informace - 8 bit, LSB prvni > MSB posledni
829 cizelu 102 {
870 cizelu 103 i=info[ip]; // ziskani hodnoty z pole
104 if(i==1) // vyhodnoceni obsahu informace - nastav 1
829 cizelu 105 {
730 cizelu 106 output_high(SDIN);
829 cizelu 107 }
870 cizelu 108 else // vyhodnoceni obsahu informace - nastav 0
829 cizelu 109 {
730 cizelu 110 output_low(SDIN);
829 cizelu 111 }
741 cizelu 112 olsa_pulse();
829 cizelu 113 }
870 cizelu 114 output_high(SDIN); // stop bit
741 cizelu 115 olsa_pulse();
829 cizelu 116 }
730 cizelu 117  
870 cizelu 118 void olsa_reset() // hlavni RESET - provadi se po zapnuti
829 cizelu 119 {
741 cizelu 120 output_low(SDIN);
121 output_low(SCLK);
870 cizelu 122 olsa_pulses(30); // reset radkoveho senzoru
741 cizelu 123 output_high(SDIN);
870 cizelu 124 olsa_pulses(10); // start bit - synchronizace
741 cizelu 125 olsa_send(main_reset);
126 olsa_pulses(5);
127 olsa_send(set_mode_rg);
128 olsa_send(clear_mode_rg);
829 cizelu 129 }
789 cizelu 130  
870 cizelu 131 void olsa_setup() // kompletni nastaveni, provadi se po resetu
829 cizelu 132 {
870 cizelu 133 olsa_send(left_offset); // nastaveni leveho segmentu (offset a zisk)
789 cizelu 134 olsa_send(offset);
135 olsa_send(left_gain);
136 olsa_send(gain);
870 cizelu 137 olsa_send(mid_offset); // nastaveni prostredniho segmentu (offset a zisk)
789 cizelu 138 olsa_send(offset);
139 olsa_send(mid_gain);
140 olsa_send(gain);
870 cizelu 141 olsa_send(right_offset); // nastaveni praveho segmentu (offset a zisk)
789 cizelu 142 olsa_send(offset);
143 olsa_send(right_gain);
144 olsa_send(gain);
829 cizelu 145 }
790 cizelu 146  
870 cizelu 147 void olsa_integration() // snimani pixelu
829 cizelu 148 {
870 cizelu 149 olsa_send(start_int); // zacatek integrace senzoru
790 cizelu 150 olsa_pulses(22);
870 cizelu 151 olsa_send(stop_int); // konec integrace senzoru
790 cizelu 152 olsa_pulses(5);
829 cizelu 153 }
796 cizelu 154  
837 cizelu 155 void read_olsa()
156 {
870 cizelu 157 int8 cpixel; // pocet prectenych pixelu
158 int8 cbit; // pocet prectenych bitu
159 int8 pixel; // hodnota precteneho pixelu
837 cizelu 160 cpixel=0;
161 lp=0;
162 rp=0;
163 olsa_integration();
164 olsa_send(readout);
870 cizelu 165 do // precte 102 pixelu
837 cizelu 166 {
870 cizelu 167 if(!SDOUT) // zacatek prenosu - zachycen start bit
837 cizelu 168 {
169 pixel=0;
870 cizelu 170 for(cbit=0;cbit<8;cbit++) // cte jednotlive bity (8 bitu - 0 az 7)
837 cizelu 171 {
870 cizelu 172 olsa_pulse(); // impulz pro generovani dalsiho bitu
173 if(SDOUT) // zachycena 1
837 cizelu 174 {
870 cizelu 175 pixel|=1; // zapise do bitu pixelu 1 - OR
837 cizelu 176 }
870 cizelu 177 else // zachycena 0
837 cizelu 178 {
870 cizelu 179 pixel|=0; // zapise do bitu pixelu 0 - OR
837 cizelu 180 }
870 cizelu 181 pixel<<=1; // posune pixel
837 cizelu 182 }
870 cizelu 183 olsa_pulse(); // generuje stop bit
184 if(cpixel<52) // ulozeni do pole
837 cizelu 185 {
870 cizelu 186 olsa_lseg[lp]=pixel; // leva polovina radky - leve pole
837 cizelu 187 lp++;
188 }
189 else
190 {
870 cizelu 191 olsa_rseg[rp]=pixel; // prava polovina cary - prave pole
837 cizelu 192 rp++;
193 }
194 cpixel++;
195 }
196 else
197 {
870 cizelu 198 olsa_pulse(); // generuje start bit, nebyl-li poslan
837 cizelu 199 }
200 }
870 cizelu 201 while(cpixel<102); // precte 102 pixelu
837 cizelu 202 }
203  
870 cizelu 204 void olsa_position() // vyhodnoti pozici cary
869 cizelu 205 {
206 int8 searchp; // ukazatel na pole
207 int8 search; // ulozeni prectene hodnoty
208 int8 protect_count; // opravdu vidime caru
870 cizelu 209 position=0; // nuluje pozici, pokud cara neni, ulozena 0
869 cizelu 210 for(searchp=0;searchp<52;searchp++) // prohlizi levou cast cary
211 {
212 search=olsa_lseg[searchp]; // vybira pixel
213 if(search==OLSA_LEV) // cerna nebo bila?
214 {
215 protect_count++; // pokud nasleduje cerna, pricte 1 k poctu cernych pixelu
216 }
217 else
218 {
219 protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje
220 }
221 if(protect_count>LINE_PX) // vidim caru
222 {
223 position=searchp; // zapis presnou pozici
873 cizelu 224 line_sector=LEFT; // cara je v leve polovine
870 cizelu 225 searchp=55; // ukonci hledani
869 cizelu 226 }
227 }
870 cizelu 228 for(searchp=0;searchp<52;searchp++) // prohlizi pravou cast cary
869 cizelu 229 {
870 cizelu 230 search=olsa_rseg[searchp]; // vybira pixel
869 cizelu 231 if(search==OLSA_LEV)
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+50); // zapis presnou pozici
872 cizelu 242 line_sector=RIGHT; // cara je v prave polovine
870 cizelu 243 searchp=55; // ukonci hledani
869 cizelu 244 }
245 }
246 }
247  
873 cizelu 248 // ============================ ZACHRANNE SENZORY ==============================
249  
870 cizelu 250 void read_blue_sensors() // cteni nouzovych senzoru
829 cizelu 251 {
870 cizelu 252 set_adc_channel(LINEL); // cti levy nouzovy senzor
700 cizelu 253 delay_us(10);
745 cizelu 254 line_l=read_adc();
870 cizelu 255 set_adc_channel(LINER); // cti pravy nouzovy senzor
700 cizelu 256 delay_us(10);
257 line_r=read_adc();
829 cizelu 258 }
730 cizelu 259  
873 cizelu 260 // ================================== PIPAK ====================================
261  
716 cizelu 262 void beep(int16 period,int16 length)
829 cizelu 263 {
870 cizelu 264 int16 bp; // promenna pro nastaveni delky
265 for(bp=length;bp>0;bp--) // prepina vystupy tolikrat, jakou jsme zadali delku
829 cizelu 266 {
730 cizelu 267 output_high(SOUND_HI);
268 output_low(SOUND_LO);
269 delay_us(period);
270 output_high(SOUND_LO);
271 output_low(SOUND_HI);
272 delay_us(period);
829 cizelu 273 }
274 }
873 cizelu 275  
276 // ================================== MOTORY ===================================
277  
870 cizelu 278 void l_motor_fwd(int8 speedl) // levy motor dopredu
829 cizelu 279 {
708 cizelu 280 output_high(LMF);
281 output_low(LMB);
282 set_pwm2_duty(speedl);
829 cizelu 283 }
700 cizelu 284  
870 cizelu 285 void l_motor_bwd(int8 speedl) // levy motor dozadu
829 cizelu 286 {
708 cizelu 287 output_high(LMB);
288 output_low(LMF);
289 set_pwm2_duty(speedl);
829 cizelu 290 }
708 cizelu 291  
870 cizelu 292 void r_motor_fwd(int8 speedr) // pravy motor dopredu
829 cizelu 293 {
708 cizelu 294 output_high(RMF);
295 output_low(RMB);
296 set_pwm1_duty(speedr);
829 cizelu 297 }
708 cizelu 298  
870 cizelu 299 void r_motor_bwd(int8 speedr) // pravy motor dozadu
829 cizelu 300 {
708 cizelu 301 output_high(RMB);
302 output_low(RMF);
303 set_pwm1_duty(speedr);
829 cizelu 304 }
708 cizelu 305  
870 cizelu 306 void l_motor_off() // levy motor vypnut
829 cizelu 307 {
708 cizelu 308 output_low(LMF);
309 output_low(LMB);
310 set_pwm2_duty(0);
829 cizelu 311 }
730 cizelu 312  
870 cizelu 313 void r_motor_off() // pravy motor vypnut
829 cizelu 314 {
708 cizelu 315 output_low(RMF);
316 output_low(RMB);
317 set_pwm1_duty(0);
829 cizelu 318 }
708 cizelu 319  
870 cizelu 320 void motor_test() // test motoru
829 cizelu 321 {
708 cizelu 322 int8 i;
716 cizelu 323 beep(100,200);
799 cizelu 324 printf("TEST MOTORU\r\n");
708 cizelu 325 delay_ms(1000);
799 cizelu 326 printf("LEVY MOTOR DOPREDU\r\n");
818 cizelu 327 delay_ms(1000);
870 cizelu 328 for(i=0;i<255;i++) // levy motor dopredu - zrychluje
829 cizelu 329 {
708 cizelu 330 l_motor_fwd(i);
799 cizelu 331 printf("RYCHLOST: %u\r\n",i);
716 cizelu 332 delay_ms(5);
829 cizelu 333 }
870 cizelu 334 for(i=255;i>0;i--) // levy motor dopredu - zpomaluje
829 cizelu 335 {
708 cizelu 336 l_motor_fwd(i);
799 cizelu 337 printf("RYCHLOST: %u\r\n",i);
716 cizelu 338 delay_ms(5);
829 cizelu 339 }
870 cizelu 340 printf("LEVY MOTOR DOZADU\r\n"); // levy motor dozadu - zrychluje
818 cizelu 341 delay_ms(1000);
708 cizelu 342 for(i=0;i<255;i++)
829 cizelu 343 {
708 cizelu 344 l_motor_bwd(i);
799 cizelu 345 printf("RYCHLOST: %u\r\n",i);
716 cizelu 346 delay_ms(5);
829 cizelu 347 }
870 cizelu 348 for(i=255;i>0;i--) // levy motor dozadu - zpomaluje
829 cizelu 349 {
708 cizelu 350 l_motor_bwd(i);
799 cizelu 351 printf("RYCHLOST: %u\r\n",i);
716 cizelu 352 delay_ms(5);
829 cizelu 353 }
870 cizelu 354 printf("PRAVY MOTOR DOPREDU\r\n");
818 cizelu 355 delay_ms(1000);
870 cizelu 356 for(i=0;i<255;i++) // pravy motor dopredu - zrychluje
829 cizelu 357 {
708 cizelu 358 r_motor_fwd(i);
799 cizelu 359 printf("RYCHLOST: %u\r\n",i);
716 cizelu 360 delay_ms(5);
829 cizelu 361 }
870 cizelu 362 for(i=255;i>0;i--) // pravy motor dopredu - zpomaluje
829 cizelu 363 {
708 cizelu 364 r_motor_fwd(i);
799 cizelu 365 printf("RYCHLOST: %u\r\n",i);
716 cizelu 366 delay_ms(5);
829 cizelu 367 }
799 cizelu 368 printf("PRAVY MOTOR DOZADU\r\n");
818 cizelu 369 delay_ms(1000);
870 cizelu 370 for(i=0;i<255;i++) // pravy motor dozadu - zrychluje
829 cizelu 371 {
708 cizelu 372 r_motor_bwd(i);
799 cizelu 373 printf("RYCHLOST: %u\r\n",i);
716 cizelu 374 delay_ms(5);
829 cizelu 375 }
870 cizelu 376 for(i=255;i>0;i--) // pravy motor dozadu - zpomaluje
829 cizelu 377 {
708 cizelu 378 r_motor_bwd(i);
799 cizelu 379 printf("RYCHLOST: %u\r\n",i);
716 cizelu 380 delay_ms(5);
829 cizelu 381 }
870 cizelu 382 l_motor_off(); // po ukonceni testu vypnout motory
818 cizelu 383 r_motor_off();
870 cizelu 384 printf("KONEC TESTU MOTORU\r\n");
818 cizelu 385 delay_ms(1000);
829 cizelu 386 }
708 cizelu 387  
873 cizelu 388 // ================================ DIAGNOSTIKA ================================
389  
870 cizelu 390 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
708 cizelu 391 {
392 read_blue_sensors();
870 cizelu 393 read_olsa();
394 olsa_position();
395 printf("LEVA: %u \t",line_l); // tiskne z leveho senzoru
396 printf("PRAVA: %u \t",line_r); // tiskne z praveho senzoru
871 cizelu 397 printf("POLOHA: %u\t",position); // tiskne pozici OLSA
870 cizelu 398 printf("L_NARAZ: %u \t",BUMPL); // leve tlacitko narazniku
399 printf("P_NARAZ: %u \r\n",BUMPR); // prave tlacitko narazniku
400 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
708 cizelu 401 {
868 cizelu 402 beep(100,1000);
865 cizelu 403 printf("Levy naraznik - test OLSA\r\n");
404 printf("Pravy naraznik - test motoru\r\n");
405 delay_ms(500);
868 cizelu 406 while(true)
865 cizelu 407 {
868 cizelu 408 if(BUMPR)
843 cizelu 409 {
870 cizelu 410 beep(100,500); // pipni pri startu
868 cizelu 411 motor_test();
412 }
413 if(BUMPL)
414 {
415 beep(100,500);
416 printf("TEST OLSA\r\n");
417 while(true)
843 cizelu 418 {
868 cizelu 419 int8 tisk;
420 int8 *tiskp;
421 read_olsa();
422 printf("cteni\r\n"); // po precteni vsech pixelu odradkuje
423 for(tiskp=0;tiskp<52;tiskp++) // tisk leve casti radky
424 {
425 tisk=olsa_lseg[tiskp];
426 printf("%x ",tisk);
427 }
428 for(tiskp=0;tiskp<52;tiskp++) // tisk prave casti radky
429 {
430 tisk=olsa_rseg[tiskp];
431 printf("%x ",tisk);
432 }
865 cizelu 433 }
434 }
435 }
436 }
708 cizelu 437 }
438  
873 cizelu 439 // ============================== HLAVNI SMYCKA ================================
440  
842 cizelu 441 void main()
829 cizelu 442 {
799 cizelu 443 printf("POWER ON \r\n");
842 cizelu 444 // NASTAVENI > provede se pouze pri zapnuti
873 cizelu 445 setup_adc_ports(sAN0-sAN1-sAN2); // aktivní analogové vstupy RA0, RA1 a RA2
446 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
842 cizelu 447 setup_spi(SPI_SS_DISABLED);
448 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
449 setup_timer_1(T1_DISABLED);
873 cizelu 450 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
451 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
452 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
842 cizelu 453 setup_comparator(NC_NC_NC_NC);
454 setup_vref(FALSE);
873 cizelu 455 l_motor_off(); // vypne levy motor
456 r_motor_off(); // vypne pravy motor
457 output_high(LED1); // zhasne LED1
458 output_high(LED2); // zhasne LED2
842 cizelu 459 olsa_reset();
460 olsa_setup();
873 cizelu 461 beep(100,500); // pipni pri startu
842 cizelu 462 printf("OK! \r\n");
463 delay_ms(500);
464 printf("VYBRAT MOD... \r\n");
465 while(true)
865 cizelu 466 {
873 cizelu 467 // ============================ HLAVNI CAST PROGRAMU ===========================
468 if(true) // spusteni hledani pravym naraznikem
469 {
470 beep(100,500);
471 while(true)
472 {
473 // =============================== SLEDOVANI CARY ==============================
474 old_position=position; // zaznamena predhozi polohu cary
475 read_olsa(); // precte a ulozi hodnoty z olsa
476 olsa_position(); // vyhodnoti pozici cary
477 read_blue_sensors(); // cte nouzove senzory
478 if(line_l==0) // robot najede na levy nouzovy senzor
479 {
480 l_motor_bwd(255); // prudce zatoc doleva
481 r_motor_fwd(255);
482 line_sector=LEFT; // pamatuj, kde je cara
483 delay_ms(20);
484 }
485 if(line_r==0) // robot najede na pravy nouzovy senzor
486 {
487 l_motor_fwd(255); // prudce zatoc doprava
488 r_motor_bwd(255);
489 line_sector=RIGHT; // pamatuj, kde je cara
490 delay_ms(20);
491 }
492 if(position>old_position) // pocita absolutni rozdil mezi soucasnou a predchozi pozici
493 {
494 change=(position-old_position); // pozice ma vyssi cislo nez predchozi pozice
495 }
496 else
497 {
498 change=(old_position-position); // pozice ma vyssi cislo nez predchozi pozice
499 }
500 if(position==0) // pokud neni videt cara
501 {
502 position=old_position; // nastav predchozi pozici
503 gap++; // pocita, jak dlouho neni videt cara
504 }
505 else // pokud je videt cara
506 {
507 gap=0; // vynuluje citani
508 }
509 }
510 }
730 cizelu 511 }
829 cizelu 512 }
865 cizelu 513  
869 cizelu 514