Rev 874 Rev 879
Line 6... Line 6...
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 // konstanty a promenne pro sledovani cary
11 #define SPACE 10 // urcuje, za jak dlouho robot vi, ze nic nevidi 11 #define SPACE 15 // urcuje, za jak dlouho robot vi, ze nic nevidi
12 #define SPD1 100 // rychlost pomalejsiho motoru, kdyz je cara hodne z osy 12 #define SPD1 100 // rychlost pomalejsiho motoru, kdyz je cara hodne z osy
13 #define SPD2 200 // rychlost pomalejsiho motoru, kdyz je cara z osy 13 #define SPD2 150 // rychlost pomalejsiho motoru, kdyz je cara z osy
14 #define SPD3 200 // rychlost rychlejsiho motoru pro vsechny pripady 14 #define SPD3 200 // rychlost rychlejsiho motoru pro vsechny pripady
15 #define SPD4 255 // pro oba motory - rovinka 15 #define SPD4 255 // pro oba motory - rovinka
16 // oblasti cary 16 // oblasti cary
17 #define L1 20 // nejvice vlevo 17 #define L1 25 // nejvice vlevo
18 #define L2 40 18 #define L2 30
19 #define L3 48 19 #define L3 45
20 #define R1 52 20 #define R1 55
21 #define R2 60 21 #define R2 70
22 #define R3 80 // nejvice vpravo 22 #define R3 85 // nejvice vpravo
23   23  
24 int8 action; // akce pro nastaveni motoru 24 int8 action; // akce pro nastaveni motoru
25   25  
26 // univerzalni LED diody 26 // univerzalni LED diody
27 #define LED1 PIN_E0 27 #define LED1 PIN_E0
Line 490... Line 490...
490 // =============================== SLEDOVANI CARY ============================== 490 // =============================== SLEDOVANI CARY ==============================
491 old_position=position; // zaznamena predhozi polohu cary 491 old_position=position; // zaznamena predhozi polohu cary
492 read_olsa(); // precte a ulozi hodnoty z olsa 492 read_olsa(); // precte a ulozi hodnoty z olsa
493 olsa_position(); // vyhodnoti pozici cary 493 olsa_position(); // vyhodnoti pozici cary
494 read_blue_sensors(); // cte nouzove senzory 494 read_blue_sensors(); // cte nouzove senzory
495 if(line_l==0) // robot najede na levy nouzovy senzor 495 if((line_l==0)&&(gap>SPACE)) // robot najede na levy nouzovy senzor
496 { 496 {
497 l_motor_bwd(150); // prudce zatoc doleva 497 l_motor_bwd(150); // prudce zatoc doleva
498 r_motor_fwd(255); 498 r_motor_fwd(255);
499 line_sector=LEFT; // pamatuj, kde je cara 499 line_sector=LEFT; // pamatuj, kde je cara
500 delay_ms(200); 500 delay_ms(200);
501 } 501 }
502 if(line_r==0) // robot najede na pravy nouzovy senzor 502 if((line_r==0)&&(gap>SPACE)) // robot najede na pravy nouzovy senzor
503 { 503 {
504 l_motor_fwd(255); // prudce zatoc doprava 504 l_motor_fwd(255); // prudce zatoc doprava
505 r_motor_bwd(150); 505 r_motor_bwd(150);
506 line_sector=RIGHT; // pamatuj, kde je cara 506 line_sector=RIGHT; // pamatuj, kde je cara
507 delay_ms(200); 507 delay_ms(200);
Line 514... Line 514...
514 else // pokud je videt cara 514 else // pokud je videt cara
515 { 515 {
516 gap=0; // vynuluje citani 516 gap=0; // vynuluje citani
517 m=0; // pry svetsovani hulu zataceni 517 m=0; // pry svetsovani hulu zataceni
518 } 518 }
519 if(gap>SPACE) // dlouho neni nic videt -  
520 { -  
521 if(line_sector==LEFT) // cara byla naposledy vlevo -  
522 { -  
523 l_motor_bwd(50+m); // zatacej doleva -  
524 r_motor_fwd(200); -  
525 m++; -  
526 } -  
527 else // cara byla naposledy vpravo -  
528 { -  
529 r_motor_bwd(50+m); // zatacej doprava -  
530 l_motor_fwd(200); -  
531 m++; -  
532 } -  
533 } -  
534 if(position>0) // urceni akci pro rizeni, pokud cara je videt 519 if(position>0) // urceni akci pro rizeni, pokud cara je videt
535 { 520 {
536 if(position>L1) // cara je hodne vlevo 521 if(position>L1) // cara je hodne vlevo
537 { 522 {
538 if(position>L2) // cara je vlevo 523 if(position>L2) // cara je vlevo
Line 570... Line 555...
570 } 555 }
571 else 556 else
572 { 557 {
573 action=1; 558 action=1;
574 } 559 }
575 } 560 }
576 if(L3<position>R1) // cara je uprostred 561 if(L3<=position>=R1) // cara je uprostred
577 { 562 {
578 action=7; 563 action=7;
579 } 564 }
580 switch(action) // reakce na pohyb cary 565 switch(action) // reakce na pohyb cary
581 { 566 {
Line 616... Line 601...
616 case 7: // cara je uprostred 601 case 7: // cara je uprostred
617 l_motor_fwd(SPD4); 602 l_motor_fwd(SPD4);
618 r_motor_fwd(SPD4); 603 r_motor_fwd(SPD4);
619 break; 604 break;
620 } 605 }
-   606 if((line_sector==LEFT)&&(position==0))
-   607 {
-   608 r_motor_fwd(255);
-   609 l_motor_off();
-   610 }
-   611 if((line_sector==RIGHT)&&(position==0))
-   612 {
-   613 l_motor_fwd(255);
-   614 r_motor_off();
-   615 }
621 } 616 }
622 } 617 }
623 } 618 }
624 } 619 }
625   620