| 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 20 // konstanta pro derivacni regulator *0,01 (20) |
14 |
#define COND 20 // konstanta pro derivacni regulator *0,01 (20) |
| 15 |
|
15 |
|
| 16 |
#define SPD_LO 130 // zaklad pro vypocet rychlosti pomalejsiho motoru (130) |
16 |
#define SPD_LO 255 // zaklad pro vypocet rychlosti pomalejsiho motoru (130) |
| 17 |
#define SPD_HI 155 // zaklad pro vypocetrychlosti rychlejsiho motoru (155) |
17 |
#define SPD_HI 255 // zaklad pro vypocetrychlosti rychlejsiho motoru (155) |
| 18 |
#define SPD_MAX 240 // rychlost po rovince (240) |
18 |
#define SPD_MAX 255 // rychlost po rovince (240) |
| 19 |
|
19 |
|
| 20 |
int8 reg_out; |
20 |
int8 reg_out; |
| 21 |
int8 err1; // odchylka prvni hodnoty |
21 |
int8 err1; // odchylka prvni hodnoty |
| 22 |
int8 err2; |
22 |
int8 err2; |
| 23 |
int8 err3; |
23 |
int8 err3; |
| Line 623... |
Line 623... |
| 623 |
calc_error(); |
623 |
calc_error(); |
| 624 |
calc_regulator(); |
624 |
calc_regulator(); |
| 625 |
//printf("regulator: %u\r\n",reg_out); |
625 |
//printf("regulator: %u\r\n",reg_out); |
| 626 |
if(position<50) // prepocet regulatoru pro motory, pokud je cara vlevo |
626 |
if(position<50) // prepocet regulatoru pro motory, pokud je cara vlevo |
| 627 |
{ |
627 |
{ |
| 628 |
lm_speed=SPD_LO-reg_out; |
628 |
lm_speed=SPD_LO-(2*reg_out); |
| 629 |
rm_speed=SPD_HI+reg_out; |
629 |
rm_speed=SPD_HI; |
| 630 |
} |
630 |
} |
| 631 |
if(position==50) // nastaveni rychlosti, pokud je cara uprostred |
631 |
if(position==50) // nastaveni rychlosti, pokud je cara uprostred |
| 632 |
{ |
632 |
{ |
| 633 |
lm_speed=SPD_MAX; |
633 |
lm_speed=SPD_MAX; |
| 634 |
rm_speed=SPD_MAX; |
634 |
rm_speed=SPD_MAX; |
| 635 |
} |
635 |
} |
| 636 |
if(position>50) // prepocet regulatoru pro motory, pokud je cara vpravo |
636 |
if(position>50) // prepocet regulatoru pro motory, pokud je cara vpravo |
| 637 |
{ |
637 |
{ |
| 638 |
lm_speed=SPD_HI+reg_out; |
638 |
lm_speed=SPD_HI; |
| 639 |
rm_speed=SPD_LO-reg_out; |
639 |
rm_speed=SPD_LO-(2*reg_out); |
| 640 |
} |
640 |
} |
| 641 |
if((sharp_lev>PROBLEM)&&(DET_EN)) // zachycen odraz na dalkomeru |
641 |
if((sharp_lev>PROBLEM)&&(DET_EN)) // zachycen odraz na dalkomeru |
| 642 |
{ |
642 |
{ |
| 643 |
p_count++; // pocita, jak dlouho je videt |
643 |
p_count++; // pocita, jak dlouho je videt |
| 644 |
output_low(LED1); |
644 |
output_low(LED1); |