验证中...
8月18日(周六)成都源创会火热报名中,四位一线行业大牛与你面对面,探讨区块链技术热潮下的冷思考。
简单模拟位置式PID算法
原始数据 复制代码
/*
P控制——用于对精度要求不高的场合
PI控制——积分环境可消除静态误差,用于高精度场合
PD控制——微分有预测能力,用于带有时间滞后的场合(如本例模拟类巡线小车运动)
PID控制——综合上述
此程序面向过程,使用pid结构体
*/
#include <iostream>
#include <cmath>
#include <Windows.h>
#include <conio.h>
using namespace std;
#define OFFSET 55 //中轴位置
//#define f 0.1 //阻尼系数
#define OUT_0 0 //偏差为0时(没)有控制量
#define DIV 20
////PID参数///////
#define kp 5 //5
#define ki 0 //0.01
#define kd 800 //800
int offset=35; //起点位置
double m=3; //惯性系数(质量)
double F=0; //合力
double a=0; //加速度
double v=0; //速度
int T=100; //新行刷新时间 ms
double t=(1.0*T/500)/DIV; //两点间等效时间间隔s (若系数大于1,则省略了一些点)
void outputspace(int n) //输出空格
{
for(int i=0;i<n;i++)
cout<<" ";
}
void outputstar(int p,int os) //根据中轴位置和偏差量作图
{
if(os>0)
{
outputspace(p);
cout<<"*";
outputspace(os);
cout<<"*"<<endl;
}
if(os<0)
{
outputspace(p+os);
cout<<"*";
outputspace(-1*os-1);
cout<<"*"<<endl;
}
if(os==0)
outputspace(p),cout<<"*"<<endl;
}
int main()
{
int n;//n是一个判断标志
double d=0;//d路程
struct PidStructure
{
double ref;//调节量的给定值(初始偏差),初始化OFFSET
double fdb;//调节量的反馈值(调节后的偏差) ,初始化0
double P;
double I;
double D;
double Ek;//当前的偏差
double Sk;//历史偏差(积分)
double Dk;//最近两次偏差值之差
double Ek_1;//上次偏差
double Ek_2;//本次偏差
double out;//输出控制量
double outMax;//最大100
double outMin;//最小-100
public:
PidStructure(double REF,double FDB,double p,double i,double d,double EK,double SK,double DK,double EK1,double EK2,double O,double OMax,double OMin)
{
ref=REF,fdb=FDB;
P=p,I=i,D=d;
Ek=EK,SK=SK,Dk=DK;
Ek_1=EK1,Ek_2=EK2;
out=O,outMax=OMax,outMin=OMin;
}
};
PidStructure PID(1.0*OFFSET,0.0,1.0*kp,1.0*ki,1.0*kd,0.0,0.0,0.0,0.0,0.0,0.0,100.0,-100.0);
while(1)
{
for(int i=0;i<DIV;i++)
{
PID.fdb=(PID.ref+offset)-PID.ref-d;
PID.Ek=1.0*(PID.fdb);
PID.Sk+=PID.Ek;
PID.out=1.0*(PID.P*PID.Ek+PID.I*PID.Sk+PID.D*PID.Dk+OUT_0);
if(PID.out>PID.outMax)
PID.out=PID.outMax;
if(PID.out<PID.outMin)
PID.out=PID.outMin;
a=PID.out/m;
d+=0.5*a*t*t+v*t;
v+=a*t;
if(n==0)//处理Ek1和Ek2
PID.Ek_1=PID.Ek,n++;
else if(n==1)
PID.Ek_2=PID.Ek,n++;
else
PID.Ek_1=PID.Ek_2, PID.Ek_2=PID.Ek, PID.Dk=PID.Ek_2-PID.Ek_1;
}
if(kbhit())//这段代码用于模拟扰动,任意时刻输入一个数字(0~9) ,偏差会增大相应数值,按住不放偏差将持续增大
{
a=_getch();
if(a>47&&a<58)//用ASCII码判断 0~9对应48~57
{
if(PID.Ek>=0)
d-=(a-48);
else
d+=(a-48);
}
}
outputstar(OFFSET,PID.Ek);
Sleep(T);//延时ms
}
return 0;
}

评论列表( 0 )

你可以在登录后,发表评论