#include "pid.h"
#include "stdio.h"
#include "stdlib.h"
#include "stm32f10x.h"
struct _pid
{
int set_x,set_y; //設定值
int now_x,now_y; //現在值
int last_x,last_y; //上一次的值
int prve_x,prve_y; //上兩次的值
double sum_x,sum_y; //誤差積累
float err_x,err_y; //誤差值
float last_err_x,last_err_y; //上一次誤差值
float prve_err_x,prve_err_y; //上兩次誤差值
float Kp,Ki,Kd;
}pid;
void PID_init() //初始化函數
{
pid.err_x=0.0;
pid.err_y=0.0;
pid.Kd=0.2;
pid.Ki=0.015;
pid.Kp=0.2;
pid.set_x=0;
pid.set_y=0; //初始化設定值
pid.now_x=0;
pid.now_y=0; //初始化實際值
pid.last_x=0;
pid.last_y=0; //初始化上次的值
pid.prve_x=0;
pid.prve_y=0; //初始化上兩次的值
pid.last_err_x=0.0;
pid.last_err_y=0.0;
pid.prve_err_x=0.0;
pid.prve_err_y=0.0;
pid.sum_x=0;
pid.sum_y=0; //初始化積累值
}
float PID_GET(float x,float y)
{
float sumx,sumy;
pid.now_x=x;
pid.now_y=y;
pid.err_x=pid.set_x-pid.now_x;
pid.err_y=pid.set_y-pid.now_y;
pid.sum_x=pid.Kp*(pid.err_x-pid.last_err_x)+ pid.Ki*pid.err_x +pid.Kd*(pid.err_x-2*pid.last_err_x+pid.prve_err_x);
pid.sum_y=pid.Kp*(pid.err_y-pid.last_err_y)+ pid.Ki*pid.err_y +pid.Kd*(pid.err_y-2*pid.last_err_y+pid.prve_err_y);
sumx=pid.sum_x;
sumy=pid.sum_y;
pid.prve_err_x=pid.last_err_x;
pid.prve_err_y=pid.last_err_y;
pid.last_err_x=pid.err_x;
pid.last_err_y=pid.err_y;
return sumx,sumy;
}
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
