Rev 919 Rev 920
Line 4... Line 4...
4 // BAUD RATE = 9600 4 // BAUD RATE = 9600
5 // ========================== PRIPRAVA DAT A VYSTUPU =========================== 5 // ========================== PRIPRAVA DAT A VYSTUPU ===========================
6 // pomocne konstanty 6 // pomocne konstanty
7 #define LEFT 0 7 #define LEFT 0
8 #define RIGHT 1 8 #define RIGHT 1
9 #define BLACK 0 -  
10 #define WHITE 255 -  
11   9  
12 // regulator 10 // regulator
13 #define CONP 2 // konstanta pro proporcionalni regulator (2) 11 #define CONP 2 // konstanta pro proporcionalni regulator (2)
14 #define CONI 45 // konstanta pro integracni regulator *0,01 (45) 12 #define CONI 45 // konstanta pro integracni regulator *0,01 (45)
15 #define COND 20 // konstanta pro derivacni regulator *0,01 (20) 13 #define COND 20 // konstanta pro derivacni regulator *0,01 (20)
Line 81... Line 79...
81 #define BUMPR input(PIN_D7) 79 #define BUMPR input(PIN_D7)
82   80  
83 // ============================= NOUZOVE SENZORY =============================== 81 // ============================= NOUZOVE SENZORY ===============================
84 #define LINEL 0 // analogovy kanal pro levy senzor 82 #define LINEL 0 // analogovy kanal pro levy senzor
85 #define LINER 1 // analogovy kanal pro pravy senzor 83 #define LINER 1 // analogovy kanal pro pravy senzor
86 #define BLUE_LEV 200 // rozhodovaci uroven (zmereno 0 - cerna, 255 cerna) 84 #define WHITE 100 // rozhodovaci uroven pro nouzove senzory
-   85  
87 int8 line_l; // uklada hodnotu leveho senzoru 86 int8 line_l; // uklada hodnotu leveho senzoru
88 int8 line_r; // uklada hodnotu praveho senzoru 87 int8 line_r; // uklada hodnotu praveho senzoru
89   88  
90 // ================================ DALKOMER =================================== 89 // ================================ DALKOMER ===================================
91 #define SHARP 2 // analogovy kanal pro SHARP 90 #define SHARP 2 // analogovy kanal pro SHARP
-   91 #define PROBLEM 40 // rozhodovaci uroven, kdy hrozi prekazka
92 #define BLOCK 300 // rozhodovaci uroven, kdy je prekazka 92 #define BLOCK 70 // rozhodovaci uroven, kdy je jiste prekazka
-   93 #define DANGER 200 // je urcite prekazka?
93   94  
-   95 int8 p_count;
94 int8 sharp_lev; // uklada hodnotu sharp 96 int8 sharp_lev; // uklada hodnotu sharp
95   97  
96 // ================================== MOTORY =================================== 98 // ================================== MOTORY ===================================
97 #define LMF PIN_D0 99 #define LMF PIN_D0
98 #define LMB PIN_D1 100 #define LMB PIN_D1
Line 284... Line 286...
284 line_l=read_adc(); 286 line_l=read_adc();
285 set_adc_channel(LINER); // cti pravy nouzovy senzor 287 set_adc_channel(LINER); // cti pravy nouzovy senzor
286 delay_us(10); 288 delay_us(10);
287 line_r=read_adc(); 289 line_r=read_adc();
288 } 290 }
-   291  
-   292 // ================================= DALKOMER =================================
-   293  
-   294 void read_sharp() // cteni z dalkomeru
-   295 {
-   296 set_adc_channel(SHARP); // cteni z dalkomeru
-   297 delay_us(10);
-   298 sharp_lev=read_adc(); // ulozeni hodnoty
-   299 }
289 300
290 // ================================== PIPAK ==================================== 301 // ================================== PIPAK ====================================
291   302  
292 void beep(int16 period,int16 length) 303 void beep(int16 period,int16 length)
293 { 304 {
Line 453... Line 464...
453   464  
454 // ================================ DIAGNOSTIKA ================================ 465 // ================================ DIAGNOSTIKA ================================
455   466  
456 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru 467 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
457 { 468 {
458 read_blue_sensors(); 469 read_blue_sensors(); // cteni nouzovych senzoru
459 read_olsa(); 470 read_sharp(); // cteni dalkomeru
-   471 read_olsa(); // cteni z optickeho radkoveho senzoru
460 olsa_position(); 472 olsa_position();
461 printf("LEVA: %u \t",line_l); // tiskne z leveho senzoru 473 printf("LEVA: %u \t",line_l); // tiskne z leveho senzoru
462 printf("PRAVA: %u \t",line_r); // tiskne z praveho senzoru 474 printf("PRAVA: %u \t",line_r); // tiskne z praveho senzoru
-   475 printf("SHARP: %u \t",sharp_lev); // tiskne z dalkomeru
463 printf("POLOHA: %u\t",position); // tiskne pozici OLSA 476 printf("POLOHA: %u\t",position); // tiskne pozici OLSA
464 printf("L_NARAZ: %u \t",BUMPL); // leve tlacitko narazniku 477 printf("L_NARAZ: %u \t",BUMPL); // leve tlacitko narazniku
465 printf("P_NARAZ: %u \r\n",BUMPR); // prave tlacitko narazniku 478 printf("P_NARAZ: %u \r\n",BUMPR); // prave tlacitko narazniku
466 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru 479 if(BUMPL&&BUMPR) // po zmacknuti stran narazniku spusti test motoru
467 { 480 {
Line 505... Line 518...
505 // ============================== HLAVNI SMYCKA ================================ 518 // ============================== HLAVNI SMYCKA ================================
506   519  
507 void main() 520 void main()
508 { 521 {
509 printf("POWER ON \r\n"); 522 printf("POWER ON \r\n");
510 // NASTAVENI > provede se pouze pri zapnuti 523 // NASTAVENI > provede se pouze pri zapnuti
511 setup_adc_ports(sAN0-sAN1-sAN2); // aktivní analogové vstupy RA0, RA1 a RA2 -  
512 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik 524 setup_adc(ADC_CLOCK_INTERNAL); // interni hodniny pro AD prevodnik
-   525 setup_adc_ports(ALL_ANALOG); // aktivní analogové vstupy RA0, RA1 a RA2
513 setup_spi(SPI_SS_DISABLED); 526 setup_spi(SPI_SS_DISABLED);
514 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); 527 setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
515 setup_timer_1(T1_DISABLED); 528 setup_timer_1(T1_DISABLED);
516 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM 529 setup_timer_2(T2_DIV_BY_16,255,1); // casovac pro PWM
517 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2 530 setup_ccp1(CCP_PWM); // povoli PWM na pinu RC2
Line 520... Line 533...
520 setup_vref(FALSE); 533 setup_vref(FALSE);
521 l_motor_off(); // vypne levy motor 534 l_motor_off(); // vypne levy motor
522 r_motor_off(); // vypne pravy motor 535 r_motor_off(); // vypne pravy motor
523 olsa_reset(); 536 olsa_reset();
524 olsa_setup(); 537 olsa_setup();
525 beep(100,500); // pipni pri startu 538 beep(350,300); // pipni pri startu
526 printf("OK! \r\n"); 539 printf("OK! \r\n");
527 delay_ms(500); 540 delay_ms(500);
528 printf("VYBRAT MOD... \r\n"); 541 printf("VYBRAT MOD... \r\n");
529 // ============================ HLAVNI CAST PROGRAMU =========================== 542 // ============================ HLAVNI CAST PROGRAMU ===========================
530 while(true) 543 while(true)
Line 547... Line 560...
547 // ================================ DIAGNOSTIKA ================================ 560 // ================================ DIAGNOSTIKA ================================
548 if(BUMPL) 561 if(BUMPL)
549 { 562 {
550 output_low(LED1); 563 output_low(LED1);
551 output_high(LED2); 564 output_high(LED2);
552 beep(100,500); 565 beep(200,500);
553 while(true) 566 while(true)
554 { 567 {
555 diag(); 568 diag();
556 } 569 }
557 } 570 }
558 // =============================== SLEDOVANI CARY ============================== 571 // =============================== SLEDOVANI CARY ==============================
559 if(BUMPR) // spusteni hledani pravym naraznikem 572 if(BUMPR) // spusteni hledani pravym naraznikem
560 { 573 {
561 output_low(LED2); 574 output_low(LED2);
562 output_high(LED1); 575 output_high(LED1);
563 beep(100,500); 576 beep(300,500);
564 while(true) 577 while(true)
565 { 578 {
566 old_position=position; // zaznamena predhozi polohu cary 579 old_position=position; // zaznamena predhozi polohu cary
567 read_olsa(); // precte a ulozi hodnoty z olsa 580 read_olsa(); // precte a ulozi hodnoty z olsa
568 olsa_position(); // vyhodnoti pozici cary 581 olsa_position(); // vyhodnoti pozici cary
569 read_blue_sensors(); // cte nouzove senzory 582 read_blue_sensors(); // cte nouzove senzory
-   583 read_sharp(); // cte dalkomer
570 if(position==0) // pokud neni videt cara 584 if(position==0) // pokud neni videt cara
571 { 585 {
572 position=old_position; // nastav predchozi pozici 586 position=old_position; // nastav predchozi pozici
573 gap++; // pocita, jak dlouho neni videt cara 587 gap++; // pocita, jak dlouho neni videt cara
574 } 588 }
Line 576... Line 590...
576 { 590 {
577 gap=0; // gap je roven nule 591 gap=0; // gap je roven nule
578 } 592 }
579 if(gap>space) // cara neni urcite videt 593 if(gap>space) // cara neni urcite videt
580 { 594 {
581 if(line_l==BLACK) // cara videna levym modrym senzorem 595 if(line_l<WHITE) // cara videna levym modrym senzorem
582 { 596 {
583 position=1; 597 position=1;
584 } 598 }
585 if(line_r==BLACK) // cara videna pravym modrym senzorem 599 if(line_r<WHITE) // cara videna pravym modrym senzorem
586 { 600 {
587 position=99; 601 position=99;
588 } 602 }
589 } 603 }
590 calc_error(); 604 calc_error();
591 calc_regulator(); 605 calc_regulator();
592 //printf("regulator: %u\r\n",reg_out); 606 //printf("regulator: %u\r\n",reg_out);
593 if(position<50) 607 if(position<50) // prepocet regulatoru pro motory, pokud je cara vlevo
594 { 608 {
595 lm_speed=SPD_LO-reg_out; 609 lm_speed=SPD_LO-reg_out;
596 rm_speed=SPD_HI+reg_out; 610 rm_speed=SPD_HI+reg_out;
597 } 611 }
598 if(position==50) 612 if(position==50) // nastaveni rychlosti, pokud je cara uprostred
599 { 613 {
600 lm_speed=SPD_MAX; 614 lm_speed=SPD_MAX;
601 rm_speed=SPD_MAX; 615 rm_speed=SPD_MAX;
602 } 616 }
603 if(position>50) 617 if(position>50) // prepocet regulatoru pro motory, pokud je cara vpravo
604 { 618 {
605 lm_speed=SPD_HI+reg_out; 619 lm_speed=SPD_HI+reg_out;
606 rm_speed=SPD_LO-reg_out; 620 rm_speed=SPD_LO-reg_out;
607 } 621 }
-   622 if(sharp_lev>PROBLEM) // zachycen odraz na dalkomeru
-   623 {
-   624 p_count++; // pocitej, jak dlouho vidis odraz
-   625 if(sharp_lev>BLOCK) // odraz je nebezpecne blizko
-   626 {
-   627 lm_speed=0;
-   628 rm_speed=0;
-   629 }
-   630 if(p_count>DANGER) // zjistuje, zda se nejedna o nahodny odraz
-   631 {
-   632 lm_speed=0;
-   633 rm_speed=0;
-   634 }
-   635 }
-   636 else // pokud jiz neni detekoven odraz, vynuluj pocitadlo
-   637 {
-   638 p_count=0;
-   639 }
608 l_motor_fwd(lm_speed); 640 l_motor_fwd(lm_speed);
609 r_motor_fwd(rm_speed); 641 r_motor_fwd(rm_speed);
610 } 642 }
611 } 643 }
612 } 644 }
613 } 645 }
614   646