#define PID_P 0.3
-#define PID_P_S 0.7
+#define PID_P_S 0.9 /*2.3 kmita*/ /*1.4 vhodne jen pro P regulator*/
+#define PID_I_S 0.01
#define PRIOR_KERN 50
#define PRIOR_HIGH 49
.pos_reg_ena=0,
.desired_spd=0,
.spd_reg_ena=0,
- .old_pos={0}
+ .old_pos={0},
+ .spd_err_sum=0
};
/**
*/
inline void spd_pid(){
int duty_tmp;
- duty_tmp = PID_P_S*(rps.desired_spd - rps.speed);
+ int error;
+ error=rps.desired_spd - rps.speed;
+ rps.spd_err_sum+=error;
+ duty_tmp = PID_P_S*error+PID_I_S*rps.spd_err_sum;
if (duty_tmp>MAX_DUTY){
rps.duty=MAX_DUTY;
}else if (duty_tmp<-MAX_DUTY){