Rev 912 Rev 913
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 ===================================