Rev 902 Rev 912
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 // regulator 10 // regulator
11 #define CONP 12 // konstanta pro proporcionalni regulator 11 #define CONP 2 // konstanta pro proporcionalni regulator
12 #define CONI 2 // konstanta pro integracni regulator 12 #define CONI 0 // konstanta pro integracni regulator
13 #define COND 1 // konstanta pro derivacni regulator 13 #define COND 0 // konstanta pro derivacni regulator
-   14  
-   15 #define SPD_LO 120 // zaklad pro vypocet rychlosti pomalejsiho motoru
-   16 #define SPD_HI 150 // zaklad pro vypocetrychlosti rychlejsiho motoru
14   17  
15 int8 reg_out; 18 int8 reg_out;
16 int8 err1; // odchylka prvni hodnoty 19 int8 err1; // odchylka prvni hodnoty
17 int8 err2; 20 int8 err2;
18 int8 err3; 21 int8 err3;
Line 22... Line 25...
22   25  
23 // mezera 26 // mezera
24 #define SPACE 20 27 #define SPACE 20
25   28  
26 // univerzalni LED diody 29 // univerzalni LED diody
27 #define LED1 PIN_E0 30 #define LED1 PIN_E1
28 #define LED2 PIN_E1 31 #define LED2 PIN_E0
-   32  
-   33 int16 blink;
29   34  
30 // piezo pipak 35 // piezo pipak
31 #DEFINE SOUND_HI PIN_B4 36 #DEFINE SOUND_HI PIN_B4
32 #DEFINE SOUND_LO PIN_B5 37 #DEFINE SOUND_LO PIN_B5
33   38  
Line 506... Line 511...
506 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1 511 setup_ccp2(CCP_PWM); // povolí PWM na pinu RC1
507 setup_comparator(NC_NC_NC_NC); 512 setup_comparator(NC_NC_NC_NC);
508 setup_vref(FALSE); 513 setup_vref(FALSE);
509 l_motor_off(); // vypne levy motor 514 l_motor_off(); // vypne levy motor
510 r_motor_off(); // vypne pravy motor 515 r_motor_off(); // vypne pravy motor
511 output_high(LED1); // zhasne LED1 -  
512 output_high(LED2); // zhasne LED2 -  
513 olsa_reset(); 516 olsa_reset();
514 olsa_setup(); 517 olsa_setup();
515 beep(100,500); // pipni pri startu 518 beep(100,500); // pipni pri startu
516 printf("OK! \r\n"); 519 printf("OK! \r\n");
517 delay_ms(500); 520 delay_ms(500);
518 printf("VYBRAT MOD... \r\n"); 521 printf("VYBRAT MOD... \r\n");
519 // ============================ HLAVNI CAST PROGRAMU =========================== 522 // ============================ HLAVNI CAST PROGRAMU ===========================
520 while(true) 523 while(true)
521 { 524 {
-   525 if(blink<4000)
-   526 {
-   527 output_low(LED1);
-   528 output_high(LED2);
-   529 }
-   530 else
-   531 {
-   532 output_low(LED2);
-   533 output_high(LED1);
-   534 }
-   535 if (blink==8000)
-   536 {
-   537 blink=0;
-   538 }
-   539 blink++;
522 // ================================ DIAGNOSTIKA ================================ 540 // ================================ DIAGNOSTIKA ================================
523 if(BUMPL) 541 if(BUMPL)
524 { 542 {
-   543 output_low(LED1);
-   544 output_high(LED2);
525 beep(100,500); 545 beep(100,500);
526 while(true) 546 while(true)
527 { 547 {
528 diag(); 548 diag();
529 } 549 }
530 } 550 }
531 // =============================== SLEDOVANI CARY ============================== 551 // =============================== SLEDOVANI CARY ==============================
532 if(BUMPR) // spusteni hledani pravym naraznikem 552 if(BUMPR) // spusteni hledani pravym naraznikem
533 { 553 {
-   554 output_low(LED2);
-   555 output_high(LED1);
534 beep(100,500); 556 beep(100,500);
535 while(true) 557 while(true)
536 { 558 {
537 old_position=position; // zaznamena predhozi polohu cary 559 old_position=position; // zaznamena predhozi polohu cary
538 read_olsa(); // precte a ulozi hodnoty z olsa 560 read_olsa(); // precte a ulozi hodnoty z olsa
Line 547... Line 569...
547 { 569 {
548 gap=0; // gap je roven nule 570 gap=0; // gap je roven nule
549 } 571 }
550 calc_error(); 572 calc_error();
551 calc_regulator(); 573 calc_regulator();
552 printf("regulator: %u\r\n",reg_out); 574 //printf("regulator: %u\r\n",reg_out);
-   575 if(position<50)
-   576 {
-   577 lm_speed=SPD_LO-reg_out;
-   578 rm_speed=SPD_HI+reg_out;
-   579 }
-   580 if(position==50)
-   581 {
-   582 lm_speed=200;
-   583 rm_speed=200;
-   584 }
-   585 if(position>50)
-   586 {
-   587 lm_speed=SPD_HI+reg_out;
-   588 rm_speed=SPD_LO-reg_out;
-   589 }
-   590 l_motor_fwd(lm_speed);
-   591 r_motor_fwd(rm_speed);
553 } 592 }
554 } 593 }
555 } 594 }
556 } 595 }
557   596