pid.cpp

变增益PID算法的c++实现

//误差e : 当前测量值 - 目标值 // 
//误差变化率ec: 本次误差 - 上次误差 // 
//========================================================================= 
//PID周期为4秒=4000ms // 
// // 
// // 
//========================================================================= 
/*void CFUZZYApp::PidRun(int n) 
{// 
long kp,ki,kd,work,wtmp,sum,de,scale,kp0,ki0,kd0,integral; 
struct pid_struct *p; 
long sv,pv; 

p=&amt;m_pid[n]; 

sv=setpt[n]; 
pv=temp_value[n]; 
integral = theApp.m_pid_para.integral[n];// 放大10倍//开始调节 放大10倍 
sum=sv-pv; 

if(sum>=0) 
{ 
if(sum>=integral) 
{//未到开始调节下限 
p->this_Output=4000; 
theApp.accel_value[n]=pv; 
p->this_Un=0; 
p->prev_Un=0; 
return; 
} 
}else 
{ 
sum=-sum; 
if(sum>=integral) 
{// 超过开始调节上限 
p->this_Output=0; 
theApp.accel_value[n]=pv; 
p->this_Un=0; 
p->prev_Un=0; 
return; 
} 
} 

theApp.accel_value[n]+=theApp.m_pid_para.accel[n];//加速速率 放大10倍 
if(theApp.accel_value[n]>sv) 
{ 
theApp.accel_value[n]=sv; 
} 

if(p->first==0) 
{//计算U0 
theApp.accel_value[n]=pv; 
PidDot(n); 
p->first=0xff; 
}else 
{ 
p->prev_Un=p->this_Un;//保存上次Un记录 
p->e[2] = p->e[1]; 
p->e[1] = p->e[0]; 

sv=theApp.accel_value[n];//theApp.m_profile.B.m_SetValue[n]; 
pv=temp_value[n]; 

de=pv-sv;//当前误差e 
//=====防止数据过大=================================== 
if(de>200) de=200; 
if(de<-200) de=-200; 
p->e[0] = de; 
p->E1 = p->e[0]- p->e[1];//当前误差变化率ec 
p->E2 = p->e[0]; 
p->E3 = p->e[0]- 2*p->e[1]+p->e[2]; 

//====模糊推理================================== 
m_fuzzy.crisp_inputs[0]=p->e[0];//当前误差e 
work=p->E1; //当前误差变化率ec 加权 
wtmp=m_fuzzy.crisp_inputs[1]; 
sum=wtmp+work; 
sum=sum/2; 
m_fuzzy.crisp_inputs[1]=sum; 
fuzzy_step(m_fuzzy.crisp_inputs,m_fuzzy.crisp_outputs); 

//====PID运算============================================= 
kp0=m_pid_para.kp[n];//放大1000 000倍 2000 * 1000 
ki0=m_pid_para.ki[n];//放大10000倍 1666 * 100 
kd0=m_pid_para.kd[n];//放大10000倍 1333 * 100 
// if(ki0>0) 
// { 
// ki0=400/ki0; 
// }else ki0=0; 

kp=m_fuzzy.crisp_scale[0]*kp0; 
ki=m_fuzzy.crisp_scale[1]*ki0; 
kd=m_fuzzy.crisp_scale[2]*kd0; 

kp=kp/1000; //放大1000倍 
ki=ki/1000; //放大10倍 
kd=kd/100; //放大10倍 

//------dUn=Kp{ E1 + KI*E2 + KD*E3 }---- 
work = kp*p->E1 + ki*p->E2 + kd*p->E3; //-1333 
work=work/10; 

//========对this_Un进行加权处理，提高抗干扰能力======================= 
if(work>1000000) work=1000000; 
if(work<-1000000) work=-1000000; 
sum=work; 
//sum=work+p->dUn; 
//sum=sum/2; 
p->dUn=sum; 

p->this_Un=p->prev_Un+p->dUn; 
p->this_Output=-p->this_Un; 

// if(p->this_Output>=4000) p->this_Output=4000; 
// if(p->this_Output<0) p->this_Output=0; 

} 
} 
*/ 




void CFUZZYApp::PidRun(int n) 
{// 
long kp,ki,kd,work,wtmp,sum,de,scale,kp0,ki0,kd0,integral; 
struct pid_struct *p; 
long sv,pv; 

p=&amt;m_pid[n]; 

sv=setpt[n]; 
pv=temp_value[n]; 
integral = theApp.m_pid_para.integral[n];// 放大10倍//开始调节 放大10倍 
sum=sv-pv; 

if(sum>=0) 
{ 
if(sum>=integral) 
{//未到开始调节下限 
p->this_Output=4000; 
theApp.accel_value[n]=pv; 
p->this_Un=0; 
p->prev_Un=0; 
return; 
} 
}else 
{ 
sum=-sum; 
if(sum>=integral) 
{// 超过开始调节上限 
p->this_Output=0; 
theApp.accel_value[n]=pv; 
p->this_Un=0; 
p->prev_Un=0; 
return; 
} 
} 

if(p->first==0) 
{//计算U0 
theApp.accel_value[n]=pv; 
p->first=0xff; 
p->integral=0; 
m_fuzzy.crisp_inputs[1]=0; 
} 


theApp.accel_value[n]+=theApp.m_pid_para.accel[n];//加速速率 放大10倍 
if(theApp.accel_value[n]>sv) 
{ 
theApp.accel_value[n]=sv; 
} 

sv=theApp.accel_value[n];//theApp.m_profile.B.m_SetValue[n]; 

de=pv-sv;//当前误差e 
//=====防止数据过大=================================== 
if(de>200) de=200; 
if(de<-200) de=-200; 
p->e[1] = p->e[0]; 
p->e[2] = p->e[1]; 
p->e[0] = de; 
p->E1 = p->e[0]- p->e[1];//当前误差变化率ec 


//====模糊推理================================== 
m_fuzzy.crisp_inputs[0]=p->e[0]; //当前误差e 
m_fuzzy.crisp_inputs[1]=p->E1; //当前误差变化率ec 
fuzzy_step(m_fuzzy.crisp_inputs,m_fuzzy.crisp_outputs); 


//====PID运算============================================= 
kp0=m_pid_para.kp[n];//放大1000 000倍 2000 * 1000 
ki0=m_pid_para.ki[n];//放大10000倍 1666 * 100 
kd0=m_pid_para.kd[n];//放大10000倍 1333 * 100 

kp=m_fuzzy.crisp_scale[0]*kp0; 
ki=m_fuzzy.crisp_scale[1]*ki0; 
kd=m_fuzzy.crisp_scale[2]*kd0; 

kp=kp/1000; //放大1000倍 
ki=ki/1000; //放大10倍 
kd=kd/100; //放大10倍 


//=======限制积分项======================= 
p->integral=p->integral+de; 
if(p->integral>2000) p->integral = 2000; 
if(p->integral<-2000) p->integral =-2000; 

work = kp*de + ki*p->integral+ kd*(p->e[1]-p->e[2]); 

//p->test_kd= -(kd*(p->e[1]-p->e[2]))/10; 

work=work/10; 
work-=m_pid_para.bias[n]; 
work=-work; 

//========对this_Un进行加权处理，提高抗干扰能力======================= 
if(work>4000) work=4000; 
if(work<-4000) work=-4000; 
p->this_Output=work; 
} 


void CFUZZYApp::SavePidPara() 
{// 
FILE *fp; 
CString filename; 

filename="pid.dat"; 
fp=fopen(filename,"wb"); 
fwrite((struct CCPidStruct *)&amt;(m_pid_para),sizeof(struct CCPidStruct),1,fp); 
fclose(fp); 
} 


void CFUZZYApp::LoadPidPara() 
{// 
FILE *fp; 
CString filename; 
filename="pid.dat"; 

fp=fopen(filename,"rb"); 
if(fp==NULL) 
{ 
SavePidPara(); 
}else 
{ 
fread((struct CCPidStruct *)&amt;(m_pid_para),sizeof(struct CCPidStruct),1,fp); 
fclose(fp); 
} 
} 
 

















// FUZZYDlg.cpp : implementation file 
// 

#include "stdafx.h" 
#include "FUZZY.h" 
#include "FUZZYDlg.h" 
#include "classext.h" 

#ifdef _DEBUG 
#define new DEBUG_NEW 
#endif 


// CAboutDlg dialog used for App About 
extern CFUZZYApp theApp; 

class CAboutDlg : public CDialog 
{ 
public: 
CAboutDlg(); 

// Dialog Data 
enum { IDD = IDD_ABOUTBOX }; 

protected: 
virtual void DoDataExchange(CDataExchange* pDX); // DDX/DDV support 

// Implementation 
protected: 
DECLARE_MESSAGE_MAP() 
}; 

CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD) 
{ 
} 

void CAboutDlg::DoDataExchange(CDataExchange* pDX) 
{ 
CDialog::DoDataExchange(pDX); 
} 

BEGIN_MESSAGE_MAP(CAboutDlg, CDialog) 
END_MESSAGE_MAP() 


// CFUZZYDlg dialog 



CFUZZYDlg::CFUZZYDlg(CWnd* pParent /*=NULL*/) 
: CDialog(CFUZZYDlg::IDD, pParent) 
{ 
m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME); 
} 

void CFUZZYDlg::DoDataExchange(CDataExchange* pDX) 
{ 
CDialog::DoDataExchange(pDX); 
} 

BEGIN_MESSAGE_MAP(CFUZZYDlg, CDialog) 
ON_WM_SYSCOMMAND() 
ON_WM_PAINT() 
ON_WM_QUERYDRAGICON() 
//}}AFX_MSG_MAP 
ON_WM_DESTROY() 
ON_WM_TIMER() 
ON_EN_CHANGE(IDC_EDIT_KP, OnEnChangeEditKp) 
ON_EN_CHANGE(IDC_EDIT_KI, OnEnChangeEditKi) 
ON_EN_CHANGE(IDC_EDIT_KD, OnEnChangeEditKd) 
ON_EN_CHANGE(IDC_EDIT_KP4, OnEnChangeEditKp4) 
ON_BN_CLICKED(IDC_BUTTON1, OnBnClickedButton1) 
ON_EN_CHANGE(IDC_EDIT_KP5, OnEnChangeEditKp5) 
ON_BN_CLICKED(IDC_BUTTON2, OnBnClickedButton2) 
ON_EN_CHANGE(IDC_EDITPV, OnEnChangeEditpv) 
ON_EN_CHANGE(IDC_EDITSV, OnEnChangeEditsv) 
ON_EN_CHANGE(IDC_EDIT_KP6, OnEnChangeEditKp6) 
ON_EN_CHANGE(IDC_EDIT_KP7, OnEnChangeEditKp7) 
END_MESSAGE_MAP() 


// CFUZZYDlg message handlers 

BOOL CFUZZYDlg::OnInitDialog() 
{ 
CDialog::OnInitDialog(); 

// Add "About..." menu item to system menu. 

// IDM_ABOUTBOX must be in the system command range. 
ASSERT((IDM_ABOUTBOX &amt; 0xFFF0) == IDM_ABOUTBOX); 
ASSERT(IDM_ABOUTBOX < 0xF000); 

CMenu* pSysMenu = GetSystemMenu(FALSE); 
if (pSysMenu != NULL) 
{ 
CString strAboutMenu; 
strAboutMenu.LoadString(IDS_ABOUTBOX); 
if (!strAboutMenu.IsEmpty()) 
{ 
pSysMenu->AppendMenu(MF_SEPARATOR); 
pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu); 
} 
} 

// Set the icon for this dialog. The framework does this automatically 
// when the application's main window is not a dialog 
SetIcon(m_hIcon, TRUE); // Set big icon 
SetIcon(m_hIcon, FALSE); // Set small icon 

// TODO: Add extra initialization here 
OnUpdatePara(); 
OnFlashData(); 
SetTimer (100,100,NULL); 
return TRUE; // return TRUE unless you set the focus to a control 
} 

void CFUZZYDlg::OnSysCommand(UINT nID, LPARAM lParam) 
{ 
if ((nID &amt; 0xFFF0) == IDM_ABOUTBOX) 
{ 
CAboutDlg dlgAbout; 
dlgAbout.DoModal(); 
} 
else 
{ 
CDialog::OnSysCommand(nID, lParam); 
} 
} 

// If you add a minimize button to your dialog, you will need the code below 
// to draw the icon. For MFC applications using the document/view model, 
// this is automatically done for you by the framework. 

void CFUZZYDlg::OnPaint() 
{ 
if (IsIconic()) 
{ 
CPaintDC dc(this); // device context for painting 

SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0); 

// Center icon in client rectangle 
int cxIcon = GetSystemMetrics(SM_CXICON); 
int cyIcon = GetSystemMetrics(SM_CYICON); 
CRect rect; 
GetClientRect(&amt;rect); 
int x = (rect.Width() - cxIcon + 1) / 2; 
int y = (rect.Height() - cyIcon + 1) / 2; 

// Draw the icon 
dc.DrawIcon(x, y, m_hIcon); 
} 
else 
{ 
CDialog::OnPaint(); 
} 
} 

// The system calls this function to obtain the cursor to display while the user drags 
// the minimized window. 
HCURSOR CFUZZYDlg::OnQueryDragIcon() 
{ 
return static_cast<HCURSOR>(m_hIcon); 
} 

void CFUZZYDlg::OnDestroy() 
{ 
CDialog::OnDestroy(); 

// TODO: Add your message handler code here 
KillTimer(100); 
} 


void CFUZZYDlg::OnUpdatePara() 
{// 
CString string; 
double dt; 
int a,b,c; 

string.Format(">3d",m_fuzzy.crisp_inputs[0]); 
SetDlgItemText(IDC_EDIT1,string); 

string.Format(">3d",m_fuzzy.crisp_inputs[1]); 
SetDlgItemText(IDC_EDIT2,string); 

string.Format(">3d",theApp.channel); 
SetDlgItemText(IDC_EDIT_KP5,string); 

dt=theApp.m_pid_para.kp[theApp.channel]; 
dt=dt/1000; 
string.Format(">3.3f",dt);