int Position_PID (int shiji,int Target)
{
// float Position_KP=-80,Position_KI=0,Position_KD=-170;
float Position_KP=-40,Position_KI=0,Position_KD=-50;
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=shiji-Target; //計算偏差
Integral_bias+=Bias; //求出偏差的積分
Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias); //位置式PID控制器
Last_Bias=Bias; //保存上一次偏差
if(Pwm>7200) Pwm=7200;
else if(Pwm<-7200) Pwm=-7200;
return Pwm; //增量輸出
}
int coder;
// int speed;
int speed_s=0;
int speed_po=0;
int SET_Pos=0;
int pwm_out=0;
void TIM1_UP_IRQHandler(void)
{
// 檢查更新中斷標志位
if (TIM_GetITStatus(TIM1, TIM_IT_Update) == SET)
{
// 執行定時器中斷處理操作
coder=Encoder_Get();
speed_po=Position_PID(coder,SET_Pos);
//speed_s=pid_speed(coder,10);
//pwm_out=speed_s+speed_po;
if(pwm_out>7200) pwm_out=7200;
else if(pwm_out<-7200) pwm_out=-7200;
if(pwm_out>0) motorA_stata (fan,speed_po,enable);
else if(pwm_out<=0) motorA_stata (zhen,-speed_po,enable);
// 清除更新中斷標志位
TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
}
}
以上是我實現速度環的電機控制, 脈沖數是一直累計的,獲取后不清零處理,,,后面我該這么加入速度環呢? 我直接把速度環計算出的PWM和位置環輸出的PWM相加輸出給電機,這樣沒有成功.
該這么才能實現控制位置環和速度環串級PID呢? (小白求助,謝謝回復,謝謝你~)
|
