| Line 7... |
Line 7... |
| 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 2 // konstanta pro proporcionalni regulator |
11 |
#define CONP 2 // konstanta pro proporcionalni regulator |
| 12 |
#define CONI 0 // konstanta pro integracni regulator |
12 |
#define CONI 45 // konstanta pro integracni regulator *0,01 |
| 13 |
#define COND 0 // konstanta pro derivacni regulator |
13 |
#define COND 20 // konstanta pro derivacni regulator *0,01 |
| 14 |
|
14 |
|
| 15 |
#define SPD_LO 120 // zaklad pro vypocet rychlosti pomalejsiho motoru |
15 |
#define SPD_LO 120 // zaklad pro vypocet rychlosti pomalejsiho motoru |
| 16 |
#define SPD_HI 150 // zaklad pro vypocetrychlosti rychlejsiho motoru |
16 |
#define SPD_HI 150 // zaklad pro vypocetrychlosti rychlejsiho motoru |
| 17 |
|
17 |
|
| 18 |
int8 reg_out; |
18 |
int8 reg_out; |
| Line 318... |
Line 318... |
| 318 |
{ |
318 |
{ |
| 319 |
int8 p_reg; |
319 |
int8 p_reg; |
| 320 |
int8 i_reg; |
320 |
int8 i_reg; |
| 321 |
int8 d_reg; |
321 |
int8 d_reg; |
| 322 |
p_reg=(CONP*err5); // vypocet proporcionalni slozky |
322 |
p_reg=(CONP*err5); // vypocet proporcionalni slozky |
| 323 |
i_reg=(CONI*errp); // vypocet integracni slozky |
323 |
i_reg=(CONI*(errp/100)); // vypocet integracni slozky |
| 324 |
if(position>old_position) // vypocet derivace |
324 |
if(position>old_position) // vypocet derivacni slozky |
| 325 |
{ |
325 |
{ |
| 326 |
d_reg=(COND*(position-old_position)); // pokud je aktualni pozice vetsi nez predesla |
326 |
d_reg=(COND*((position-old_position)/100)); // pokud je aktualni pozice vetsi nez predesla |
| 327 |
} |
327 |
} |
| 328 |
else |
328 |
else |
| 329 |
{ |
329 |
{ |
| 330 |
d_reg=(COND*(old_position-position)); // pokud je aktualni pozice mensi nez predesla |
330 |
d_reg=(COND*((old_position-position)/100)); // pokud je aktualni pozice mensi nez predesla |
| 331 |
} |
331 |
} |
| 332 |
reg_out=(p_reg+i_reg+d_reg); // vypocet celeho regulatoru |
332 |
reg_out=(p_reg+i_reg+d_reg); // vypocet celeho regulatoru |
| 333 |
} |
333 |
} |
| 334 |
|
334 |
|
| 335 |
// ================================== MOTORY =================================== |
335 |
// ================================== MOTORY =================================== |