【代码】发个PID的代码

村口张si傅 · 发表于 2014-12-13 15:26:02

吓尿！

Te_Amo_Jing · 发表于 2014-12-13 18:52:00

给汇编跪了

慎独 · 发表于 2014-12-13 22:45:31

本帖最后由慎独于 2014-12-13 22:47 编辑

#include <stdio.h>
#include <math.h>
struct PID
{
float SetSpeed;
float ActualSpeed;
float err;
float err_last;
float Kp,Ki,Kd;
float voltage;
float integral;
float umax;
float umin;
}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.1;
pid.Kd=0.2;
pid.umax=400;
pid.umin=-200;
printf("PID_init end \n");
}
float PID_realize(float speed)
{
int index;
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
if(pid.ActualSpeed>pid.umax)//抗积分饱和
{
if(fabs(pid.err)>200) //积分分离过程
index=0;
else
{
index=1;
if(pid.err<0)
pid.integral+=pid.err;
}
}
else if(pid.ActualSpeed<pid.umin)
{
if(fabs(pid.err)>200) //积分分离过程
index=0;
else if(fabs(pid.err)<180)
{
index=1;
if(pid.err>0)
pid.integral+=pid.err;
}
else
{
index=(int)(200-fabs(pid.err))/20;
if(pid.err>0)
pid.integral+=pid.err;
}
}
else
{
if(fabs(pid.err)>200)//积分分离过程
index=0;
else
{
index=1;
if(pid.err>0)
pid.integral+=pid.err;
}
}
pid.voltage=pid.Kp*pid.err+index*pid.Ki*pid.integral/2+pid.Kd*(pid.err-pid.err_last);
pid.err_last=pid.err;
pid.ActualSpeed=pid.voltage*1.0;
return pid.ActualSpeed;
}以前自己写的PID，感觉还行，在大神面前献丑一番