Rev 954 Rev 955
Line 11... Line 11...
11 // regulator 11 // regulator
12 #define CONP 2 // konstanta pro proporcionalni regulator (2) 12 #define CONP 2 // konstanta pro proporcionalni regulator (2)
13 #define CONI 45 // konstanta pro integracni regulator *0,01 (45) 13 #define CONI 45 // konstanta pro integracni regulator *0,01 (45)
14 #define COND 20 // konstanta pro derivacni regulator *0,01 (20) 14 #define COND 20 // konstanta pro derivacni regulator *0,01 (20)
15   15  
16 #define SPD_LO 255 // zaklad pro vypocet rychlosti pomalejsiho motoru (130) 16 #define SPD_LO 250 // zaklad pro vypocet rychlosti pomalejsiho motoru (130)
17 #define SPD_HI 255 // zaklad pro vypocetrychlosti rychlejsiho motoru (155) 17 #define SPD_HI 250 // zaklad pro vypocetrychlosti rychlejsiho motoru (155)
18 #define SPD_MAX 255 // rychlost po rovince (240) 18 #define SPD_MAX 255 // rychlost po rovince (240)
19   19  
20 int8 reg_out; 20 int8 reg_out;
21 int8 err1; // odchylka prvni hodnoty 21 int8 err1; // odchylka prvni hodnoty
22 int8 err2; 22 int8 err2;
Line 24... Line 24...
24 int8 err4; 24 int8 err4;
25 int8 err5; // odchylka posledni hodnoty 25 int8 err5; // odchylka posledni hodnoty
26 int8 errp; // prumer chyb 26 int8 errp; // prumer chyb
27   27  
28 // mezera 28 // mezera
29 #define SPACE 8 // jak dlouho robot smi nic nevidet (8) 29 #define SPACE 10 // jak dlouho robot smi nic nevidet (8)
30 #define CONT 20 // kontrast, kdy nic nevidime 30 #define CONT 15 // kontrast, kdy nic nevidime
31   31  
32 // univerzalni LED diody 32 // univerzalni LED diody
33 #define LED1 PIN_E1 33 #define LED1 PIN_E1
34 #define LED2 PIN_E0 34 #define LED2 PIN_E0
35   35  
Line 42... Line 42...
42 // radkovy senzor 42 // radkovy senzor
43 #define SDIN PIN_D4 // seriovy vstup 43 #define SDIN PIN_D4 // seriovy vstup
44 #define SDOUT input(PIN_C5) // seriovy vystup 44 #define SDOUT input(PIN_C5) // seriovy vystup
45 #define SCLK PIN_D5 // takt 45 #define SCLK PIN_D5 // takt
46   46  
47 #define LINE_PX 4 // pocet pixelu pro jiste urceni cary (4) -  
48 #define OLSA_LEV 0x60 // rozhodovaci uroven (cca 10 odpovida cerne) (0x10 nebo 0x60) -  
49   -  
50 // pro komunikaci s OLSA, prvni se posila LSB 47 // pro komunikaci s OLSA, prvni se posila LSB
51 int MAIN_RESET[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B 48 int MAIN_RESET[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B
52 int SET_MODE_RG[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F 49 int SET_MODE_RG[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F
53 int CLEAR_MODE_RG[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00 50 int CLEAR_MODE_RG[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00
54   51  
Line 69... Line 66...
69 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50) 66 int olsa_lseg[51]={0}; // leva cast radky (pixely 0 - 50)
70 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101) 67 int olsa_rseg[51]={0}; // prava cast radky (pixely 51 - 101)
71 int8 *lp; // ukazatel pro levou polovinu radky 68 int8 *lp; // ukazatel pro levou polovinu radky
72 int8 *rp; // ukazatel pro levou polovinu radky 69 int8 *rp; // ukazatel pro levou polovinu radky
73   70  
74 int8 contrast; 71 int8 contrast; // rozdil mezi nejsvetlejsim a nejtmavsi mistem
75 int8 position; // ulozeni pozice cary 72 int8 position; // ulozeni pozice cary
76 int8 old_position; // ulozeni predchozi pozice cary 73 int8 old_position; // ulozeni predchozi pozice cary
77 int1 line_sector; // cara je vlevo/vpravo 74 int1 line_sector; // cara je vlevo/vpravo
78 int8 gap; // pocita, jak dlouho neni videt cara 75 int8 gap; // pocita, jak dlouho neni videt cara
79   76  
Line 82... Line 79...
82 #define BUMPR input(PIN_D7) 79 #define BUMPR input(PIN_D7)
83   80  
84 // ============================= NOUZOVE SENZORY =============================== 81 // ============================= NOUZOVE SENZORY ===============================
85 #define LINEL 0 // analogovy kanal pro levy senzor 82 #define LINEL 0 // analogovy kanal pro levy senzor
86 #define LINER 1 // analogovy kanal pro pravy senzor 83 #define LINER 1 // analogovy kanal pro pravy senzor
87 #define WHITE 100 // rozhodovaci uroven pro nouzove senzory 84 #define WHITE 30 // rozhodovaci uroven pro nouzove senzory
88   85  
89 int8 line_l; // uklada hodnotu leveho senzoru 86 int8 line_l; // uklada hodnotu leveho senzoru
90 int8 line_r; // uklada hodnotu praveho senzoru 87 int8 line_r; // uklada hodnotu praveho senzoru
91   88  
92 // ================================ DALKOMER =================================== 89 // ================================ DALKOMER ===================================
Line 468... Line 465...
468 l_motor_fwd(255); // jed rovne 465 l_motor_fwd(255); // jed rovne
469 delay_ms(1000); 466 delay_ms(1000);
470 r_motor_bwd(255); // zatoc doprava 467 r_motor_bwd(255); // zatoc doprava
471 delay_ms(350); 468 delay_ms(350);
472 r_motor_fwd(255); // jed rovne 469 r_motor_fwd(255); // jed rovne
473 delay_ms(1500); 470 delay_ms(1000);
474 r_motor_bwd(255); // zatoc doprava 471 r_motor_bwd(255); // zatoc doprava
475 delay_ms(300); 472 delay_ms(300);
476 r_motor_fwd(255); // jed rovne 473 r_motor_fwd(255); // jed rovne
477 delay_ms(200); 474 delay_ms(800);
478 position=40; 475 position=40;
479 } 476 }
480 // ================================ DIAGNOSTIKA ================================ 477 // ================================ DIAGNOSTIKA ================================
481   478  
482 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru 479 void diag() // diagnostika - vypis senzoru s moznosti prepnuti na test motoru
Line 604... Line 601...
604 } 601 }
605 else // pokud je videt 602 else // pokud je videt
606 { 603 {
607 gap=0; // gap je roven nule 604 gap=0; // gap je roven nule
608 } 605 }
-   606 if(line_l<WHITE) // cara videna levym modrym senzorem
-   607 {
-   608 position=1;
-   609 line_sector=LEFT;
-   610 }
609 if(gap>space) // cara neni urcite videt 611 if(line_r<WHITE) // cara videna pravym modrym senzorem
-   612 {
-   613 position=99;
-   614 line_sector=RIGHT;
-   615 }
-   616 if(gap>SPACE)
610 { 617 {
611 if(line_l<WHITE) // cara videna levym modrym senzorem 618 if(line_sector==LEFT)
612 { 619 {
613 position=1; 620 position=1;
614 } 621 }
615 if(line_r<WHITE) // cara videna pravym modrym senzorem 622 if(line_sector==RIGHT)
616 { 623 {
617 position=99; 624 position=99;
618 } 625 }
619 } 626 }
620 calc_error(); 627 calc_error();
Line 622... Line 629...
622 //printf("regulator: %u\r\n",reg_out); 629 //printf("regulator: %u\r\n",reg_out);
623 if(position<50) // prepocet regulatoru pro motory, pokud je cara vlevo 630 if(position<50) // prepocet regulatoru pro motory, pokud je cara vlevo
624 { 631 {
625 lm_speed=SPD_LO-(2*reg_out); 632 lm_speed=SPD_LO-(2*reg_out);
626 rm_speed=SPD_HI; 633 rm_speed=SPD_HI;
-   634 line_sector=LEFT;
627 } 635 }
628 if(position==50) // nastaveni rychlosti, pokud je cara uprostred 636 if(position==50) // nastaveni rychlosti, pokud je cara uprostred
629 { 637 {
630 lm_speed=SPD_MAX; 638 lm_speed=SPD_MAX;
631 rm_speed=SPD_MAX; 639 rm_speed=SPD_MAX;
632 } 640 }
633 if(position>50) // prepocet regulatoru pro motory, pokud je cara vpravo 641 if(position>50) // prepocet regulatoru pro motory, pokud je cara vpravo
634 { 642 {
635 lm_speed=SPD_HI; 643 lm_speed=SPD_HI;
636 rm_speed=SPD_LO-(2*reg_out); 644 rm_speed=SPD_LO-(2*reg_out);
-   645 line_sector=RIGHT;
637 } 646 }
638 if((sharp_lev>PROBLEM)&&(DET_EN)) // zachycen odraz na dalkomeru 647 if((sharp_lev>PROBLEM)&&(DET_EN)) // zachycen odraz na dalkomeru
639 { 648 {
640 p_count++; // pocita, jak dlouho je videt 649 p_count++; // pocita, jak dlouho je videt
641 output_low(LED1); 650 output_low(LED1);