| Line 9... |
Line 9... |
| 9 |
#define DET_EN 0 // 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 40 // konstanta pro integracni regulator *0,01 (45) |
13 |
#define CONI 40 // konstanta pro integracni regulator *0,01 (45) |
| 14 |
#define COND 20 // konstanta pro derivacni regulator *0,01 (20) |
14 |
#define COND 10 // konstanta pro derivacni regulator *0,01 (20) |
| 15 |
|
15 |
|
| 16 |
#define SPD_LO 220 // zaklad pro vypocet rychlosti pomalejsiho motoru (130) |
16 |
#define SPD_LO 220 // zaklad pro vypocet rychlosti pomalejsiho motoru (130) |
| 17 |
#define SPD_HI 220 // zaklad pro vypocetrychlosti rychlejsiho motoru (155) |
17 |
#define SPD_HI 220 // zaklad pro vypocetrychlosti rychlejsiho motoru (155) |
| 18 |
#define SPD_MAX 220 // rychlost po rovince (240) |
18 |
#define SPD_MAX 220 // rychlost po rovince (240) |
| 19 |
|
19 |
|
| 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 5 // jak dlouho robot smi nic nevidet (8) |
29 |
#define SPACE 10 // jak dlouho robot smi nic nevidet (8) |
| 30 |
#define CONT 17 // kontrast, kdy nic nevidime |
30 |
#define CONT 30 // 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 615... |
Line 615... |
| 615 |
} |
615 |
} |
| 616 |
} |
616 |
} |
| 617 |
calc_error(); |
617 |
calc_error(); |
| 618 |
calc_regulator(); |
618 |
calc_regulator(); |
| 619 |
//printf("regulator: %u\r\n",reg_out); |
619 |
//printf("regulator: %u\r\n",reg_out); |
| 620 |
if(position<46) // prepocet regulatoru pro motory, pokud je cara vlevo |
620 |
if(position<47) // prepocet regulatoru pro motory, pokud je cara vlevo |
| 621 |
{ |
621 |
{ |
| 622 |
lm_speed=SPD_LO-(2*reg_out); |
622 |
lm_speed=SPD_LO-(2*reg_out); |
| 623 |
rm_speed=SPD_HI; |
623 |
rm_speed=SPD_HI; |
| 624 |
line_sector=LEFT; |
624 |
line_sector=LEFT; |
| 625 |
} |
625 |
} |
| 626 |
if((position>45)&&(position<55)) // nastaveni rychlosti, pokud je cara uprostred |
626 |
if((position>46)&&(position<54)) // nastaveni rychlosti, pokud je cara uprostred |
| 627 |
{ |
627 |
{ |
| 628 |
lm_speed=SPD_MAX; |
628 |
lm_speed=SPD_MAX; |
| 629 |
rm_speed=SPD_MAX; |
629 |
rm_speed=SPD_MAX; |
| 630 |
} |
630 |
} |
| 631 |
if(position>54) // prepocet regulatoru pro motory, pokud je cara vpravo |
631 |
if(position>53) // prepocet regulatoru pro motory, pokud je cara vpravo |
| 632 |
{ |
632 |
{ |
| 633 |
lm_speed=SPD_HI; |
633 |
lm_speed=SPD_HI; |
| 634 |
rm_speed=SPD_LO-(2*reg_out); |
634 |
rm_speed=SPD_LO-(2*reg_out); |
| 635 |
line_sector=RIGHT; |
635 |
line_sector=RIGHT; |
| 636 |
} |
636 |
} |