#include <stdio.h>
#include <math.h>
int index;
struct _pid{
float SetSpeed; //定義設定值
float ActualSpeed; //定義實際值
float err; //定義偏差值
float err_last; //定義上一個偏差值
float Kp,Ki,Kd; //定義比例、積分、微分系數
float voltage; //定義電壓值（控制執行器的變量）
float integral; //定義積分值
}pid;
//項目中獲取到的參數
void PID_init(){
printf("PID_init begin \n");
pid.SetSpeed=0.0;
pid.ActualSpeed=0.0;
pid.err=0.0;
pid.err_last=0.0;
pid.voltage=0.0;
pid.integral=0.0;
pid.Kp=0.2; //自己設定
pid.Ki=0.04; //自己設定
pid.Kd=0.2; //自己設定
printf("PID_init end \n");
}
float PID_realize(float speed){
pid.SetSpeed=speed; //設定值
pid.err=pid.SetSpeed-pid.ActualSpeed; //設定值-實際值
if(abs(pid.err)>200)
{
index=0;
}
else
{
index=1;
pid.integral+=pid.err; //積分值，偏差累加
}
pid.voltage=pid.Kp*pid.err+pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
pid.err_last=pid.err; //上一個偏差值
pid.ActualSpeed=pid.voltage*1.0; //算出實際值
return pid.ActualSpeed; //返回
}
int main(){
printf("System begin \n");
PID_init();
int count=0;
while(count<1000)
{
float speed=PID_realize(200.0);
printf("%d %f\n",count,speed);
count++;
}
return 0;
}
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
