Rev 900 Rev 901
Line 5... Line 5...
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   9  
10 // konstanty a promenne pro sledovani cary 10 // regulator
11 #define SPACE 20 // urcuje, za jak dlouho robot vi, ze nic nevidi 11 #define CONP 10 // konstanta pro proporcionalni regulator
12 #define SPD1 100 // rychlost pomalejsiho motoru, kdyz je cara hodne z osy -  
13 #define SPD2 150 // rychlost pomalejsiho motoru, kdyz je cara z osy 12 #define CONI 2 // konstanta pro integracni regulator
14 #define SPD3 200 // rychlost rychlejsiho motoru pro vsechny pripady -  
15 #define SPD4 255 // pro oba motory - rovinka 13 #define COND 2 // konstanta pro derivacni regulator
-   14  
16 // oblasti cary 15 int8 reg_out;
17 #define L1 25 // nejvice vlevo 16 int8 err1; // odchylka prvni hodnoty
18 #define L2 30 17 int8 err2;
19 #define L3 45 18 int8 err3;
20 #define R1 55 19 int8 err4;
21 #define R2 70 20 int8 err5; // odchylka posledni hodnoty
22 #define R3 85 // nejvice vpravo 21 int8 errp; // prumer chyb
23   22  
-   23 // mezera
24 int8 action; // akce pro nastaveni motoru 24 #define SPACE 20
25   25  
26 // univerzalni LED diody 26 // univerzalni LED diody
27 #define LED1 PIN_E0 27 #define LED1 PIN_E0
28 #define LED2 PIN_E1 28 #define LED2 PIN_E1
29   29  
Line 35... Line 35...
35 #define SDIN PIN_D4 // seriovy vstup 35 #define SDIN PIN_D4 // seriovy vstup
36 #define SDOUT input(PIN_C5) // seriovy vystup 36 #define SDOUT input(PIN_C5) // seriovy vystup
37 #define SCLK PIN_D5 // takt 37 #define SCLK PIN_D5 // takt
38   38  
39 #define LINE_PX 4 // pocet pixelu pro jiste urceni cary 39 #define LINE_PX 4 // pocet pixelu pro jiste urceni cary
40 #define OLSA_LEV 0x60 // rozhodovaci uroven (cca 10 odpovida cerne) 40 #define OLSA_LEV 0x10 // rozhodovaci uroven (cca 10 odpovida cerne)
41   41  
42 // pro komunikaci s OLSA, prvni se posila LSB 42 // pro komunikaci s OLSA, prvni se posila LSB
43 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B 43 int main_reset[8]={1,1,0,1,1,0,0,0}; // hlavni reset 0x1B
44 int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F 44 int set_mode_rg[8]={1,1,1,1,1,0,1,0}; // zapis do MODE registru 0x5F
45 int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00 45 int clear_mode_rg[8]={0,0,0,0,0,0,0,0}; // nulovani MODE registru 0x00
Line 63... Line 63...
63 int8 *lp; // ukazatel pro levou polovinu radky 63 int8 *lp; // ukazatel pro levou polovinu radky
64 int8 *rp; // ukazatel pro levou polovinu radky 64 int8 *rp; // ukazatel pro levou polovinu radky
65   65  
66 int8 position; // ulozeni pozice cary 66 int8 position; // ulozeni pozice cary
67 int8 old_position; // ulozeni predchozi pozice cary 67 int8 old_position; // ulozeni predchozi pozice cary
-   68 int8 l_position;
-   69 int8 r_position;
68 int1 line_sector; // cara je vlevo/vpravo 70 int1 line_sector; // cara je vlevo/vpravo
69 int8 gap; // pocita, jak dlouho neni videt cara 71 int8 gap; // pocita, jak dlouho neni videt cara
70   72  
71 // ================================= NARAZNIK ================================== 73 // ================================= NARAZNIK ==================================
72 #define BUMPL input(PIN_D6) 74 #define BUMPL input(PIN_D6)
Line 83... Line 85...
83 #define LMF PIN_D0 85 #define LMF PIN_D0
84 #define LMB PIN_D1 86 #define LMB PIN_D1
85 #define RMF PIN_D2 87 #define RMF PIN_D2
86 #define RMB PIN_D3 88 #define RMB PIN_D3
87   89  
88 int8 lm_speed; 90 int8 lm_speed; // rychlost leveho motoru
89 int8 rm_speed; 91 int8 rm_speed; // rychlost praveho motoru
90 int8 m; -  
91   92  
92 // =============================== PODPROGRAMY ================================= 93 // =============================== PODPROGRAMY =================================
93   94  
94 // ================================= OLSA01A =================================== 95 // ================================= OLSA01A ===================================
95   96  
Line 286... Line 287...
286 delay_us(period); 287 delay_us(period);
287 output_high(SOUND_LO); 288 output_high(SOUND_LO);
288 output_low(SOUND_HI); 289 output_low(SOUND_HI);
289 delay_us(period); 290 delay_us(period);
290 } 291 }
-   292 }
-   293  
-   294 // ================================= REGULATOR =================================
-   295  
-   296 void calc_error()
-   297 {
-   298 err1=err2; // ulozeni chyb
-   299 err2=err3;
-   300 err3=err4;
-   301 err4=err5;
-   302 if(position<50) // ulozeni a vypocet aktualni absolutni chyby
291 } 303 {
-   304 err5=(50-position); // pokud je cara vlevo
-   305 }
-   306 else
-   307 {
-   308 err5=(position-50); // pokud je cara vpravo
-   309 }
-   310 errp=((err1+err2+err3+err4+err5)/5); // vypocet chyby pro integracni regulator
-   311 }
-   312 void calc_regulator()
-   313 {
-   314 int8 p_reg;
-   315 int8 i_reg;
-   316 int8 d_reg;
-   317 p_reg=(CONP*err5); // vypocet proporcionalni slozky
-   318 i_reg=(CONI*errp); // vypocet integracni slozky
-   319 if(position>old_position) // vypocet derivace
-   320 {
-   321 d_reg=(COND*(position-old_position)); // pokud je aktualni pozice vetsi nez predesla
-   322 }
-   323 else
-   324 {
-   325 d_reg=(COND*(old_position-position)); // pokud je aktualni pozice mensi nez predesla
-   326 }
-   327 reg_out=(p_reg+i_reg+d_reg); // vypocet celeho regulatoru
-   328 }
292   329  
293 // ================================== MOTORY =================================== 330 // ================================== MOTORY ===================================
294   331  
295 void l_motor_fwd(int8 speedl) // levy motor dopredu 332 void l_motor_fwd(int8 speedl) // levy motor dopredu
296 { 333 {
Line 499... Line 536...
499 { 536 {
500 old_position=position; // zaznamena predhozi polohu cary 537 old_position=position; // zaznamena predhozi polohu cary
501 read_olsa(); // precte a ulozi hodnoty z olsa 538 read_olsa(); // precte a ulozi hodnoty z olsa
502 olsa_position(); // vyhodnoti pozici cary 539 olsa_position(); // vyhodnoti pozici cary
503 read_blue_sensors(); // cte nouzove senzory 540 read_blue_sensors(); // cte nouzove senzory
504 if((line_l==0)&&(gap>SPACE)) // robot najede na levy nouzovy senzor -  
505 { -  
506 l_motor_bwd(150); // prudce zatoc doleva -  
507 r_motor_fwd(255); -  
508 line_sector=LEFT; // pamatuj, kde je cara -  
509 delay_ms(300); -  
510 } -  
511 if((line_r==0)&&(gap>SPACE)) // robot najede na pravy nouzovy senzor -  
512 { -  
513 l_motor_fwd(255); // prudce zatoc doprava -  
514 r_motor_bwd(150); -  
515 line_sector=RIGHT; // pamatuj, kde je cara -  
516 delay_ms(300); -  
517 } -  
518 if(position==0) // pokud neni videt cara 541 if(position==0) // pokud neni videt cara
519 { 542 {
520 position=old_position; // nastav predchozi pozici 543 position=old_position; // nastav predchozi pozici
521 gap++; // pocita, jak dlouho neni videt cara 544 gap++; // pocita, jak dlouho neni videt cara
522 } 545 }
523 else // pokud je videt cara 546 else // pokud je videt
524 { -  
525 gap=0; // vynuluje citani -  
526 m=0; // pry svetsovani hulu zataceni -  
527 } -  
528 if(position>0) // urceni akci pro rizeni, pokud cara je videt -  
529 { -  
530 if(position>L1) // cara je hodne vlevo -  
531 { -  
532 if(position>L2) // cara je vlevo -  
533 { -  
534 if(position>L3) // cara je mirne vlevo -  
535 { -  
536 if(position>R1) // cara je mirne vpravo -  
537 { -  
538 if(position>R2) // cara je vpravo -  
539 { -  
540 if(position>R3) // cara je hodne vpravo -  
541 { -  
542 action=6; -  
543 } -  
544 } -  
545 else -  
546 { -  
547 action=5; -  
548 } -  
549 } -  
550 else -  
551 { -  
552 action=4; -  
553 } -  
554 } -  
555 else -  
556 { -  
557 action=3; -  
558 } -  
559 } -  
560 else -  
561 { -  
562 action=2; -  
563 } -  
564 } -  
565 else -  
566 { -  
567 action=1; -  
568 } -  
569 } -  
570 if(L3<=position>=R1) // cara je uprostred -  
571 { -  
572 action=7; -  
573 } -  
574 switch(action) // reakce na pohyb cary -  
575 { -  
576 case 1: // cara je hodne vlevo -  
577 rm_speed=(SPD3+position); -  
578 lm_speed=(SPD1+position); -  
579 r_motor_fwd(rm_speed); -  
580 l_motor_bwd(lm_speed); -  
581 break; -  
582 case 2: // cara je vlevo -  
583 rm_speed=(SPD3+position); -  
584 r_motor_fwd(rm_speed); -  
585 l_motor_off(); -  
586 break; -  
587 case 3: // cara je mirne vlevo -  
588 rm_speed=(SPD3+position); -  
589 lm_speed=(SPD2-position); -  
590 r_motor_fwd(rm_speed); -  
591 l_motor_fwd(lm_speed); -  
592 break; -  
593 case 4: // cara je mirne vpravo -  
594 lm_speed=(SPD3+(position/2)); -  
595 rm_speed=(SPD2-(position/2)); -  
596 l_motor_fwd(lm_speed); -  
597 r_motor_fwd(rm_speed); -  
598 break; -  
599 case 5: // cara je vpravo -  
600 lm_speed=(SPD3+(position/2)); -  
601 l_motor_fwd(lm_speed); -  
602 r_motor_off(); -  
603 break; -  
604 case 6: // cara je hodne vpravo -  
605 lm_speed=(SPD3+(position/2)); -  
606 rm_speed=(SPD2-(position/2)); -  
607 l_motor_fwd(lm_speed); -  
608 r_motor_bwd(rm_speed); -  
609 break; -  
610 case 7: // cara je uprostred -  
611 l_motor_fwd(SPD4); -  
612 r_motor_fwd(SPD4); -  
613 break; -  
614 } -  
615 if((line_sector==LEFT)&&(position==0)) -  
616 { -  
617 r_motor_fwd(255); -  
618 l_motor_off(); -  
619 } -  
620 if((line_sector==RIGHT)&&(position==0)) -  
621 { 547 {
622 l_motor_fwd(255); 548 gap=0; // gap je roven nule
623 r_motor_off(); -  
624 } 549 }
-   550 calc_error();
-   551 calc_regulator();
-   552 printf("regulator: %u\r\n",reg_out);
625 } 553 }
626 } 554 }
627 } 555 }
628 } 556 }
629   557