个人写的关于PID的代码 -

标题: 个人写的关于PID的代码 [打印本页]

作者: 哒哒哒阿达 时间: 2018-7-2 20:41
标题: 个人写的关于PID的代码
#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;

}

欢迎光临 (http://www.51hei.com/bbs/)