Rev 872 Rev 873
Line 1... Line 1...
1 #include "main.h" 1 #include "main.h"
2   2  
3 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI 3 // NEPOUZIVAT PINY B6 A B7, JSOU VYHRAZENY PRO SERIOVOU KOMUNIKACI
4 // BAUD RATE = 9600 4 // BAUD RATE = 9600
5   -  
-   5 // ========================== PRIPRAVA DAT A VYSTUPU ===========================
6 // pomocne promenne 6 // pomocne promenne
7 #define LEFT 0 7 #define LEFT 0
8 #define RIGHT 1 8 #define RIGHT 1
9 // univerzalni LED diody 9 // univerzalni LED diody
10 #define LED1 PIN_E0 10 #define LED1 PIN_E0
Line 45... Line 45...
45 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101) 45 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
46 int8 *lp; // ukazatel pro levou polovinu radky 46 int8 *lp; // ukazatel pro levou polovinu radky
47 int8 *rp; // ukazatel pro levou polovinu radky 47 int8 *rp; // ukazatel pro levou polovinu radky
48   48  
49 int8 position; // ulozeni pozice cary 49 int8 position; // ulozeni pozice cary
-   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
50   54  
51 //naraznik -  
-   55 // ================================= NARAZNIK ==================================
52 #define BUMPL input(PIN_D6) 56 #define BUMPL input(PIN_D6)
53 #define BUMPR input(PIN_D7) 57 #define BUMPR input(PIN_D7)
54   58  
55 //nouzove senzory 59 // ============================= NOUZOVE SENZORY ===============================
56 #define LINEL 0 60 #define LINEL 0
57 #define LINER 1 61 #define LINER 1
58 #define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna) 62 #define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna)
59 int8 line_l; 63 int8 line_l;
60 int8 line_r; 64 int8 line_r;
61 int1 line_sector; -  
62   65  
63 // motory -  
-   66 // ================================== MOTORY ===================================
64 #define LMF PIN_D0 67 #define LMF PIN_D0
65 #define LMB PIN_D1 68 #define LMB PIN_D1
66 #define RMF PIN_D2 69 #define RMF PIN_D2
67 #define RMB PIN_D3 70 #define RMB PIN_D3
68   71  
69 int8 lm_speed; 72 int8 lm_speed;
70 int8 rm_speed; 73 int8 rm_speed;
71   74  
72 //PODPROGRAMY 75 // =============================== PODPROGRAMY =================================
73 //SENZORY 76  
-   77 // ================================= OLSA01A ===================================
74 //OLSA01A 78  
75 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku 79 void olsa_pulses(int count) // vytvori impulzy pro ridici logiku
76 { 80 {
77 int8 ct; 81 int8 ct;
78 for(ct=0;ct<=count;ct++) 82 for(ct=0;ct<=count;ct++)
79 { 83 {
Line 215... Line 219...
215 protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje 219 protect_count=0; // pokud nasleduje bila, pocet cernych pixelu vynuluje
216 } 220 }
217 if(protect_count>LINE_PX) // vidim caru 221 if(protect_count>LINE_PX) // vidim caru
218 { 222 {
219 position=searchp; // zapis presnou pozici 223 position=searchp; // zapis presnou pozici
220 line_sector=RIGHT; // cara je v leve polovine 224 line_sector=LEFT; // cara je v leve polovine
221 searchp=55; // ukonci hledani 225 searchp=55; // ukonci hledani
222 } 226 }
223 } 227 }
224 for(searchp=0;searchp<52;searchp++) // prohlizi pravou cast cary 228 for(searchp=0;searchp<52;searchp++) // prohlizi pravou cast cary
225 { 229 {
Line 239... Line 243...
239 searchp=55; // ukonci hledani 243 searchp=55; // ukonci hledani
240 } 244 }
241 } 245 }
242 } 246 }
243   247  
244 //ZACHRANNE SENZORY 248 // ============================ ZACHRANNE SENZORY ==============================
-   249  
245 void read_blue_sensors() // cteni nouzovych senzoru 250 void read_blue_sensors() // cteni nouzovych senzoru
246 { 251 {
247 set_adc_channel(LINEL); // cti levy nouzovy senzor 252 set_adc_channel(LINEL); // cti levy nouzovy senzor
248 delay_us(10); 253 delay_us(10);
249 line_l=read_adc(); 254 line_l=read_adc();
250 set_adc_channel(LINER); // cti pravy nouzovy senzor 255 set_adc_channel(LINER); // cti pravy nouzovy senzor
251 delay_us(10); 256 delay_us(10);
252 line_r=read_adc(); 257 line_r=read_adc();
253 } 258 }
254 259
-   260 // ================================== PIPAK ====================================
255 //PIPAK 261  
256 void beep(int16 period,int16 length) 262 void beep(int16 period,int16 length)
257 { 263 {
258 int16 bp; // promenna pro nastaveni delky 264 int16 bp; // promenna pro nastaveni delky
259 for(bp=length;bp>0;bp--) // prepina vystupy tolikrat, jakou jsme zadali delku 265 for(bp=length;bp>0;bp--) // prepina vystupy tolikrat, jakou jsme zadali delku
260 { 266 {
Line 264... Line 270...
264 output_high(SOUND_LO); 270 output_high(SOUND_LO);
265 output_low(SOUND_HI); 271 output_low(SOUND_HI);
266 delay_us(period); 272 delay_us(period);
267 } 273 }
268 } 274 }
-   275  
-   276 // ================================== MOTORY ===================================
269 //MOTORY 277  
270 void l_motor_fwd(int8 speedl) // levy motor dopredu 278 void l_motor_fwd(int8 speedl) // levy motor dopredu
271 { 279 {
272 output_high(LMF); 280 output_high(LMF);
273 output_low(LMB); 281 output_low(LMB);
274 set_pwm2_duty(speedl); 282 set_pwm2_duty(speedl);
Line 375... Line 383...
375 r_motor_off(); 383 r_motor_off();
376 printf("KONEC TESTU MOTORU\r\n"); 384 printf("KONEC TESTU MOTORU\r\n");
377 delay_ms(1000); 385 delay_ms(1000);
378 } 386 }
379   387  
-   388 // ================================ DIAGNOSTIKA ================================
-   389  
380 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru 390 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
381 { 391 {
382 read_blue_sensors(); 392 read_blue_sensors();
383 read_olsa(); 393 read_olsa();
384 olsa_position(); 394 olsa_position();
Line 424... Line 434...
424 } 434 }
425 } 435 }
426 } 436 }
427 } 437 }
428   438  
429 // HLAVNI SMYCKA 439 // ============================== HLAVNI SMYCKA ================================
-   440  
430 void main() 441 void main()
431 { 442 {
432 printf("POWER ON \r\n"); 443 printf("POWER ON \r\n");
433 // NASTAVENI > provede se pouze pri zapnuti 444 // NASTAVENI > provede se pouze pri zapnuti
434 setup_adc_ports(sAN0-sAN1-sAN2); // aktivní analogové vstupy RA0, RA1 a RA2 445 setup_adc_ports(sAN0-sAN1-sAN2); // aktivní analogové vstupy RA0, RA1 a RA2
435 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik 446 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
436 setup_spi(SPI_SS_DISABLED); 447 setup_spi(SPI_SS_DISABLED);
437 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); 448 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
438 setup_timer_1(T1_DISABLED); 449 setup_timer_1(T1_DISABLED);
439 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM 450 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
440 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2 451 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
441 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1 452 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
442 setup_comparator(NC_NC_NC_NC); 453 setup_comparator(NC_NC_NC_NC);
443 setup_vref(FALSE); 454 setup_vref(FALSE);
444 l_motor_off(); // vypne levy motor 455 l_motor_off(); // vypne levy motor
445 r_motor_off(); // vypne pravy motor 456 r_motor_off(); // vypne pravy motor
446 output_high(LED1); // zhasne LED1 457 output_high(LED1); // zhasne LED1
447 output_high(LED2); // zhasne LED2 458 output_high(LED2); // zhasne LED2
448 olsa_reset(); 459 olsa_reset();
449 olsa_setup(); 460 olsa_setup();
450 beep(100,500); // pipni pri startu 461 beep(100,500); // pipni pri startu
451 printf("OK! \r\n"); 462 printf("OK! \r\n");
452 delay_ms(500); 463 delay_ms(500);
453 printf("VYBRAT MOD... \r\n"); 464 printf("VYBRAT MOD... \r\n");
454 while(true) 465 while(true)
455 { 466 {
-   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
456 read_olsa(); // precte a ulozi hodnoty z olsa 475 read_olsa(); // precte a ulozi hodnoty z olsa
457 olsa_position(); // vyhodnoti pozici cary 476 olsa_position(); // vyhodnoti pozici cary
458 read_blue_sensors(); // cte nouzove senzory 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 {
459 //printf("poloha: %u\r\n",position); // tiskne pozici - zakomentovat pro hledani 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 }
460 } 511 }
461 } 512 }
462   513  
463   514