经过很久的战争,终于PID被我拿下,下面我将就增量式PID介绍,位置式的就是公式不一样,套一下就可以了
PID控制算法的C语言,增量型PID的C语言实现
这里直接给出代码了。
#include<stdio.h>
#include<stdlib.h>
struct _pid{
float SetSpeed; //定义设定值
float ActualSpeed; //定义实际值
float err; //定义偏差值
float err_next; //定义上一个偏差值
float err_last; //定义最上前的偏差值
float Kp,Ki,Kd; //定义比例、积分、微分系数
}pid;
void PID_init(){
pid.SetSpeed=0.0;
pid.ActualSpeed=0.0;
pid.err=0.0;
pid.err_last=0.0;
pid.err_next=0.0;
pid.Kp=0.2;
pid.Ki=0.015;
pid.Kd=0.2;
}
float PID_realize(float speed){
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
float incrementSpeed=pid.Kp*(pid.err-pid.err_next)+pid.Ki*pid.err+pid.Kd*(pid.err-2*pid.err_next+pid.err_last);
pid.ActualSpeed+=incrementSpeed;
pid.err_last=pid.err_next;
pid.err_next=pid.err;
return pid.ActualSpeed;
}
int main(){
PID_init();
int count=0;
while(count<1000)
{
float speed=PID_realize(200.0);
printf("%f\n",speed);
count++;
}
return 0;
}
这个只是c语言程序,还要修改一下成为单片机的,但是我们可以参考她的公式及思路,
下面是我窜眼很久写出来的pid代码,虽然不好,但大致可以实现控制电机,加了限幅滤波算法
废话不多说,直接上代码,不足之处望大佬指正
采用298驱动的,板子型号是Arduino UNO,以下是接线(为了省事,298的IN1和IN2我没有通过程序给出高低电平,直接接在arduino UNO的5v和gnd,)
/*
IN1-----+5V;
IN2------GND;
ENA-----D9;
光耦测速模块D0-----2;
*/
#include <MsTimer2.h>
int pwm = 9;
float kp = 8,ki = 4,kd = 2;
float e,e1,e2;
int cnt; //存储电机触发的脉冲次数
int m; //记录进入定时器中断的次数
int v; //存储电机500ms内触发的脉冲次数
int uk,uk1,duk,out=20;
int SpeedSet = 200;
int v1;//用于滤波记录当前值
void setup()
{
Serial.begin(9600);
pinMode(9,OUTPUT);
pinMode(2,INPUT);
attachInterrupt(0, blink, CHANGE);
MsTimer2::set(2, flash);
MsTimer2::start(); //开始计时
}
void PIDControl()
{
e=SpeedSet-v;
duk=(kp*(e-e1)+ki*e+kd*(e-2*e1+e2))/50; //增量式PID输出
uk=uk1+duk;
out=(int)uk;
if(out>1000)
{
out=1000;
}
else if(out<0)
{
out=0;
}
uk1=uk;
e2=e1;
e1=e;
}
// 限幅滤波法(又称程序判断滤波法)
int Filter()
{
int NewV;
NewV = v;
if(abs(NewV - v) > 10)
return v;
else
return NewV;
}
void loop()
{
v1 = Filter();
v = v1;
Serial.println(v1);
analogWrite(pwm,out);
}
void blink()
{
cnt++;
}
void flash()
{
m++;
if(m>100)
{
v = 2.5*cnt;//计算500ms内的脉冲数
PIDControl();
cnt = 0;
m = 0;
}
}
电机通过L298N驱动,这里为了省事,298的IN1和IN2我没有通过程序给出高低电平,直接接在arduino UNO的5v和gnd,298的ENA接在D9脚,才用光藕计脉冲,接在2脚
把程序上传到你的板子上,通过串口监视器就可以看到变化啦,当然pid参数需要根据你的实际情况调整。
希望能帮到大家! |