Rev 920 Rev 923
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 DET_EN 1 // povoluje nebo zakazuje vyhodnoceni SHARP
9   10  
10 // regulator 11 // regulator
11 #define CONP 2 // konstanta pro proporcionalni regulator (2) 12 #define CONP 2 // konstanta pro proporcionalni regulator (2)
12 #define CONI 45 // konstanta pro integracni regulator *0,01 (45) 13 #define CONI 45 // konstanta pro integracni regulator *0,01 (45)
13 #define COND 20 // konstanta pro derivacni regulator *0,01 (20) 14 #define COND 20 // konstanta pro derivacni regulator *0,01 (20)
Line 86... Line 87...
86 int8 line_l; // uklada hodnotu leveho senzoru 87 int8 line_l; // uklada hodnotu leveho senzoru
87 int8 line_r; // uklada hodnotu praveho senzoru 88 int8 line_r; // uklada hodnotu praveho senzoru
88   89  
89 // ================================ DALKOMER =================================== 90 // ================================ DALKOMER ===================================
90 #define SHARP 2 // analogovy kanal pro SHARP 91 #define SHARP 2 // analogovy kanal pro SHARP
91 #define PROBLEM 40 // rozhodovaci uroven, kdy hrozi prekazka 92 #define PROBLEM 50 // rozhodovaci uroven, kdy hrozi prekazka
92 #define BLOCK 70 // rozhodovaci uroven, kdy je jiste prekazka 93 #define BLOCK 90 // rozhodovaci uroven, kdy je jiste prekazka
93 #define DANGER 200 // je urcite prekazka? 94 #define DANGER 50 // pocita, jak dlouho je detekovan problem
94   95  
95 int8 p_count; 96 int8 p_count;
96 int8 sharp_lev; // uklada hodnotu sharp 97 int8 sharp_lev; // uklada hodnotu sharp
97   98  
98 // ================================== MOTORY =================================== 99 // ================================== MOTORY ===================================
Line 460... Line 461...
460 r_motor_off(); 461 r_motor_off();
461 printf("KONEC TESTU MOTORU\r\n"); 462 printf("KONEC TESTU MOTORU\r\n");
462 delay_ms(1000); 463 delay_ms(1000);
463 } 464 }
464   465  
-   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 }
465 // ================================ DIAGNOSTIKA ================================ 484 // ================================ DIAGNOSTIKA ================================
466   485  
467 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru 486 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
468 { 487 {
469 read_blue_sensors(); // cteni nouzovych senzoru 488 read_blue_sensors(); // cteni nouzovych senzoru
Line 617... Line 636...
617 if(position>50) // prepocet regulatoru pro motory, pokud je cara vpravo 636 if(position>50) // prepocet regulatoru pro motory, pokud je cara vpravo
618 { 637 {
619 lm_speed=SPD_HI+reg_out; 638 lm_speed=SPD_HI+reg_out;
620 rm_speed=SPD_LO-reg_out; 639 rm_speed=SPD_LO-reg_out;
621 } 640 }
622 if(sharp_lev>PROBLEM) // zachycen odraz na dalkomeru 641 if((sharp_lev>PROBLEM)&&(DET_EN)) // zachycen odraz na dalkomeru
623 { 642 {
624 p_count++; // pocitej, jak dlouho vidis odraz 643 p_count++; // pocita, jak dlouho je videt
-   644 output_low(LED1);
625 if(sharp_lev>BLOCK) // odraz je nebezpecne blizko 645 if(p_count>DANGER) // pokud je odraz videt nebezpecne dlouho, zpomali
626 { 646 {
627 lm_speed=0; 647 lm_speed=(lm_speed/2);
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; 648 rm_speed=(rm_speed/2);
634 } 649 }
635 } 650 }
636 else // pokud jiz neni detekoven odraz, vynuluj pocitadlo 651 else // pokud jiz neni detekoven odraz, vynuluj pocitadlo
637 { 652 {
638 p_count=0; 653 p_count=0;
-   654 output_high(LED1);
-   655 }
-   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 }
639 } 666 }
640 l_motor_fwd(lm_speed); 667 l_motor_fwd(lm_speed);
641 r_motor_fwd(rm_speed); 668 r_motor_fwd(rm_speed);
642 } 669 }
643 } 670 }