| 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 |
#define DET_EN 0 // povoluje nebo zakazuje vyhodnoceni SHARP |
| 10 |
|
10 |
|
| 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 30 // konstanta pro derivacni regulator *0,01 (20) |
14 |
#define COND 10 // konstanta pro derivacni regulator *0,01 (20) |
| 15 |
|
15 |
|
| 16 |
#define SPD_LO 200 // zaklad pro vypocet rychlosti pomalejsiho motoru (130) |
16 |
#define SPD_LO 200 // zaklad pro vypocet rychlosti pomalejsiho motoru (130) |
| 17 |
#define SPD_HI 200 // zaklad pro vypocetrychlosti rychlejsiho motoru (155) |
17 |
#define SPD_HI 200 // 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 |
|
| Line 599... |
Line 599... |
| 599 |
} |
599 |
} |
| 600 |
else // pokud je videt |
600 |
else // pokud je videt |
| 601 |
{ |
601 |
{ |
| 602 |
gap=0; // gap je roven nule |
602 |
gap=0; // gap je roven nule |
| 603 |
} |
603 |
} |
| 604 |
if(line_l<WHITE) // cara videna levym modrym senzorem |
- |
|
| 605 |
{ |
- |
|
| 606 |
position=1; |
- |
|
| 607 |
line_sector=LEFT; |
- |
|
| 608 |
} |
- |
|
| 609 |
if(line_r<WHITE) // cara videna pravym modrym senzorem |
- |
|
| 610 |
{ |
- |
|
| 611 |
position=99; |
- |
|
| 612 |
line_sector=RIGHT; |
- |
|
| 613 |
} |
- |
|
| 614 |
if(gap>SPACE) |
604 |
if(gap>SPACE) |
| 615 |
{ |
605 |
{ |
| - |
|
606 |
if(line_l<WHITE) // cara videna levym modrym senzorem |
| - |
|
607 |
{ |
| - |
|
608 |
position=1; |
| - |
|
609 |
line_sector=LEFT; |
| - |
|
610 |
} |
| - |
|
611 |
if(line_r<WHITE) // cara videna pravym modrym senzorem |
| - |
|
612 |
{ |
| - |
|
613 |
position=100; |
| - |
|
614 |
line_sector=RIGHT; |
| - |
|
615 |
} |
| 616 |
if(line_sector==LEFT) |
616 |
/*if(line_sector==LEFT) |
| 617 |
{ |
617 |
{ |
| 618 |
position=1; |
618 |
position=1; |
| 619 |
line_sector=LEFT; |
619 |
line_sector=LEFT; |
| 620 |
} |
620 |
} |
| 621 |
if(line_sector==RIGHT) |
621 |
if(line_sector==RIGHT) |
| 622 |
{ |
622 |
{ |
| 623 |
position=99; |
623 |
position=99; |
| 624 |
line_sector=RIGHT; |
624 |
line_sector=RIGHT; |
| 625 |
} |
625 |
}*/ |
| 626 |
} |
- |
|
| 627 |
if(position<20) // pro ostre zataceni |
- |
|
| 628 |
{ |
- |
|
| 629 |
position=1; |
- |
|
| 630 |
} |
- |
|
| 631 |
if(position>80) // pro ostre zataceni |
- |
|
| 632 |
{ |
- |
|
| 633 |
position=100; |
- |
|
| 634 |
} |
626 |
} |
| 635 |
calc_error(); |
627 |
calc_error(); |
| 636 |
calc_regulator(); |
628 |
calc_regulator(); |
| 637 |
//printf("regulator: %u\r\n",reg_out); |
629 |
//printf("regulator: %u\r\n",reg_out); |
| 638 |
if(position<50) // prepocet regulatoru pro motory, pokud je cara vlevo |
630 |
if(position<50) // prepocet regulatoru pro motory, pokud je cara vlevo |