pendview.cpp

上次我传的倒立摆的控制程序,不是我的最终的程序,本程序采用VC++编程,定时器为多媒体控制器,(控制时间可到达5ms),控制算法用的是最优控制,

// PendView.cpp : implementation of the CPendView class
//

#include "stdafx.h"
#include "Pend.h"
#include "GT400.h"
#include "PendDoc.h"
#include "PendView.h"
#include "MMSystem.h"


#include "afxtempl.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

/////////////////////////////////////////////////////////////////////////////
// CPendView
    int poscontrol=0;
    double cappos[10000];
	double capangle[10000];
    int datacontrol=0;//控制数据采集的频率
	double wzchange=0.0;
	double v_ctrl=0.0;
	double v=0;
	double angle1=0.0;
	double angle0=0.0;
    double posdot=0.0;
    double angledot=0.0;
	double pos=0.0;
	double angle=0.0;
    double pos0=0.0;
	CArray<double,double> m_arArray1;
    CArray<double,double> m_arArray2;
	void PASCAL CALLBACK  OneMilliSecondProc(UINT wTimerID, UINT msg,DWORD dwUser,DWORD dwl,DWORD dw2);
    inline void error(short rtn);
	void DataCapturn();
	void Control();
IMPLEMENT_DYNCREATE(CPendView, CView)

BEGIN_MESSAGE_MAP(CPendView, CView)
	//{{AFX_MSG_MAP(CPendView)
	ON_WM_CREATE()
	ON_COMMAND(IDR_START, OnStart)
	ON_COMMAND(IDR_STOP, OnStop)
	ON_WM_CHAR()
	ON_COMMAND(IDR_PAINT, OnPaint)
	ON_COMMAND(IDR_LEFT, OnLeft)
	ON_COMMAND(IDR_RIGHT, OnRight)
	//}}AFX_MSG_MAP
	// Standard printing commands
	ON_COMMAND(ID_FILE_PRINT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_DIRECT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_PREVIEW, CView::OnFilePrintPreview)
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CPendView construction/destruction

CPendView::CPendView()
{
	wznum=1;
    jdnum=1;// TODO: add construction code here

}

CPendView::~CPendView()
{
}

BOOL CPendView::PreCreateWindow(CREATESTRUCT& cs)
{
	// TODO: Modify the Window class or styles here by modifying
	//  the CREATESTRUCT cs

	return CView::PreCreateWindow(cs);
}

/////////////////////////////////////////////////////////////////////////////
// CPendView drawing

void CPendView::OnDraw(CDC* pDC)
{
	CPendDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);

	CPen pen,pen1,pen2,*pOldPen;
	CRect rect;
	GetClientRect(rect);
	//画方框图
	pen.CreatePen(PS_SOLID,1,RGB(0,0,0));
	pOldPen=pDC->SelectObject(&pen);
	int x1=rect.Width()/10;
	int y1=rect.Height()/10;
	int x2=rect.Width()/10;
	int y2=rect.Height()*9/10;
	int y22=rect.Height()/2;
	int x3=rect.Width()*9/10;
	int y3=rect.Height()/2;
	int x4=rect.Width()/10;
	int y4=rect.Height()*8/10;
	int x5=rect.Width()/10;
	int y5=rect.Height()*7/10;
	int x6=rect.Width()/10;
	int y6=rect.Height()*6/10;
	int x7=rect.Width()/10;
	int y7=rect.Height()*5/10;
	int x8=rect.Width()/10;
	int y8=rect.Height()*4/10;
	int x9=rect.Width()/10;
	int y9=rect.Height()*3/10;
	int x10=rect.Width()/10;
	int y10=rect.Height()*2/10;

	pDC->MoveTo(x1,y1);
	pDC->LineTo(x2,y2);
	pDC->MoveTo(x2,y22);
	pDC->LineTo(x3,y3);
	pDC->MoveTo(x1,y1);
	pDC->LineTo(x1+5,y1+5);
	pDC->MoveTo(x1,y1);
	pDC->LineTo(x1-5,y1+5);
	pDC->MoveTo(x3,y3);
	pDC->LineTo(x3-5,y3-5);
	pDC->MoveTo(x3,y3);
	pDC->LineTo(x3-5,y3+5);
	pDC->MoveTo(x4,y4);
	pDC->LineTo(x4+5,y4);
	pDC->TextOut(x4-60,y4,"-0.24 (-PI/2)");
	pDC->MoveTo(x5,y5);
	pDC->LineTo(x5+5,y5);
	pDC->TextOut(x5-60,y5,"-016 (0)");
	pDC->MoveTo(x6,y6);
	pDC->LineTo(x6+5,y6);
	pDC->TextOut(x6-60,y6,"-0.08 (PI/2)");
	pDC->MoveTo(x7,y7);
	pDC->LineTo(x7+5,y7);
	pDC->TextOut(x7-60,y7,"0(PI)");
	pDC->MoveTo(x8,y8);
	pDC->LineTo(x8+5,y8);
	pDC->TextOut(x8-60,y8,"0.08 (3PI/2)");
	pDC->MoveTo(x9,y9);
	pDC->LineTo(x9+5,y9);
	pDC->TextOut(x9-60,y9,"0.16 (2PI)");
	pDC->MoveTo(x10,y10);
	pDC->LineTo(x10+5,y10);
	pDC->TextOut(x10-60,y10,"0.24 (5PI/2)");
    pDC->SelectObject(pOldPen);


   //画具体的角度和位置的图象
	pen1.CreatePen(PS_SOLID,1,RGB(0,255,0));
	pOldPen=pDC->SelectObject(&pen1);
    pDC->TextOut(10,10,"位置");
	pDC->MoveTo(40,20);
	pDC->LineTo(80,20);
	for(;wznum<poscontrol;wznum++)//画 位置曲线
	{
    int weizhi1,weizhi2,i;
	i=wznum;
	     while(i>=4*rect.Width()/15) 
		 { i=i-(int)(4*rect.Width()/15);};
	     if((i+2)>4*rect.Width()/15)
		 {
	        wznum=wznum+2;
			break;
		 }
	weizhi1=(int)(rect.Height()/2-cappos[wznum-1]*(rect.Height()/10)/0.08);
	weizhi2=(int)(rect.Height()/2-cappos[wznum]*(rect.Height()/10)/0.08);//每 格 为0.08
	     if(weizhi1<=0)  weizhi1=1;
	     if(weizhi1>=rect.Height()) weizhi1=rect.Height()-3;//确定画的曲线在 范围之内
	     if(weizhi2<=0)  weizhi1=1;
	     if(weizhi2>=rect.Height()) weizhi1=rect.Height()-3;
	pDC->MoveTo((int)(rect.Width()/10+3*i),weizhi1);
	pDC->LineTo((int)(rect.Width()/10+3*i+3),weizhi2);
	if(wznum%10==0)
	{   char str[12];
	    int time=wznum/10;
		sprintf(str,"%d",time);
		pDC->TextOut((int)(rect.Width()/10+3*i),rect.Height()/2,str);

	}
	}
	pDC->SelectObject(pOldPen);


    //画角度曲线
	pen2.CreatePen(PS_SOLID,1,RGB(255,0,0));
	pOldPen=pDC->SelectObject(&pen2);
	pDC->TextOut(100,10,"角度");
	pDC->MoveTo(130,20);
	pDC->LineTo(170,20);
	for(;jdnum<poscontrol;jdnum++)
	{
    int jiaodu1,jiaodu2,j;
	   j=jdnum;
       while(j>=4*rect.Width()/15)  
	   { j=j-(int)(4*rect.Width()/15);};
	   if((j+2)>4*rect.Width()/15)
	   {
          jdnum=jdnum+2;
	      Sleep(5000);
	      Invalidate();
		  break;
	   }
	jiaodu1=(int)(rect.Height()/2-(capangle[jdnum-1]-3.1415)*(rect.Height()/10)/(3.1415/2));//每个格为PI/2
	jiaodu2=(int)(rect.Height()/2-(capangle[jdnum]-3.1415)*(rect.Height()/10)/(3.1415/2));
	   if(jiaodu1<=0)  jiaodu1=1;
	   if(jiaodu1>=rect.Height()) jiaodu1=rect.Height()-3;//确定画的曲线在 范围之内
	   if(jiaodu2<=0)  jiaodu2=1;
	   if(jiaodu2>=rect.Height()) jiaodu2=rect.Height()-3;
	pDC->MoveTo(rect.Width()/10+3*j,jiaodu1);
	pDC->LineTo(rect.Width()/10+3*j+3,jiaodu2);//画直线
	}
	pDC->SelectObject(pOldPen);

	// TODO: add draw code for native data here
}

/////////////////////////////////////////////////////////////////////////////
// CPendView printing

BOOL CPendView::OnPreparePrinting(CPrintInfo* pInfo)
{
	// default preparation
	return DoPreparePrinting(pInfo);
}

void CPendView::OnBeginPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add extra initialization before printing
}

void CPendView::OnEndPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add cleanup after printing
}

/////////////////////////////////////////////////////////////////////////////
// CPendView diagnostics

#ifdef _DEBUG
void CPendView::AssertValid() const
{
	CView::AssertValid();
}

void CPendView::Dump(CDumpContext& dc) const
{
	CView::Dump(dc);
}

CPendDoc* CPendView::GetDocument() // non-debug version is inline
{
	ASSERT(m_pDocument->IsKindOf(RUNTIME_CLASS(CPendDoc)));
	return (CPendDoc*)m_pDocument;
}
#endif //_DEBUG

/////////////////////////////////////////////////////////////////////////////
// CPendView message handlers



int CPendView::OnCreate(LPCREATESTRUCT lpCreateStruct) 
{
	if (CView::OnCreate(lpCreateStruct) == -1)
		return -1;
	
   short rtn;
   rtn = GT_Open();                                error(rtn); 
   rtn = GT_Reset();                               error(rtn); 
   rtn = GT_ClrSts();                              error(rtn); 
   rtn = GT_SetIntrMsk(127);                       error(rtn); 
   rtn = GT_SetIntrTm(1);                          error(rtn); 
   rtn = GT_LmtSns(255);                           error(rtn); 
   rtn = GT_LmtsOn();                              error(rtn); 
   rtn = GT_EncSns(0);                             error(rtn); 
   rtn = GT_Axis(1);                               error(rtn); 
   rtn = GT_PrflT();                               error(rtn); 
   rtn = GT_ClrSts();                              error(rtn); 
   rtn = GT_SetKp(3);                              error(rtn); 
   rtn = GT_SetKi(0);                              error(rtn); 
   rtn = GT_SetKd(0);                              error(rtn); 
   rtn = GT_SetKvff(0);                            error(rtn); 
   rtn = GT_SetKaff(0);                            error(rtn); 
   rtn = GT_SetILmt(5000);                         error(rtn); 
   rtn = GT_SetMtrBias(440);                       error(rtn); 
   rtn = GT_Update();                              error(rtn); 
   rtn = GT_ClrSts();                              error(rtn); 
   rtn = GT_SetPos(0);                             error(rtn); 
   rtn = GT_SetVel(0);                             error(rtn); 
   rtn = GT_SetAcc(0);                             error(rtn); 
   rtn = GT_Update();                              error(rtn); 
   rtn = GT_Axis(1);                               error(rtn); 
   rtn = GT_PrflV();                               error(rtn); 
   rtn = GT_Update();                              error(rtn); 

   
   // TODO: Add your specialized creation code here
	
	return 0;
}

void error(short rtn)
{
   switch (rtn)
   {
      case -1:
      //MessageBox(NULL," communciation error\n","错误信息",MB_OK|MB_ICONERROR);   
		  break;
      case 0:
	   /* No error */    
		  break;
      case 1:
	   //MessageBox(NULL," command error\n","错误信息",MB_OK|MB_ICONERROR);   
		  break;
      case 2:
	  //MessageBox(NULL," radius is zero or too large\n","错误信息",MB_OK|MB_ICONERROR);   
		  break;
      case 3:
	  //MessageBox(NULL," the length of line is zero or too large\n","错误信息",MB_OK|MB_ICONERROR);   
		  break;
      case 4:
	  //MessageBox(NULL," acceleration or velocity is zero or too large\n","错误信息",MB_OK|MB_ICONERROR);   
		  break;
      case 5:
  	  //MessageBox(NULL," parameter conflict\n","错误信息",MB_OK|MB_ICONERROR);   
		  break;
      case 6:
 	  //MessageBox(NULL,"  nonexistence solution for the equation\n","错误信息",MB_OK|MB_ICONERROR);   
		  break;
      case 7:
	  //MessageBox(NULL," process parameter error\n","错误信息",MB_OK|MB_ICONERROR);   
		  break;
      default:
	      break;
   }
}


void PASCAL CALLBACK OneMilliSecondProc(UINT wTimerID, UINT msg, DWORD dwUser, DWORD dwl, DWORD dw2)
{   
DataCapturn();//采集位置，速度，角度，角速度
Control();//对倒立摆进行实时控制

}
void DataCapturn()
{   short rtn;
    long actl_pos;
	GT_Axis(1);
	rtn=GT_GetAtlPos(&actl_pos);                    error(rtn); 
    pos=0.46*actl_pos/37500;//获得小车的位置

	GT_Axis(2);
	rtn=GT_GetAtlPos(&actl_pos);                    error(rtn); 
	angle1=-2*3.1415926*actl_pos/2400;
	posdot=(pos-pos0)/0.01;
	angledot=(angle1-angle0)/0.01;//获得摆杆的角度

	pos0=pos;angle0=angle1;angle=angle1;

    while(angle<0)	
	{angle=angle+2*3.1415926;};
	while(angle>2*3.1415926)
	{angle=angle-2*3.1415926;};//使摆杆的角度控制在0到2PI
	//为画图程序提供数据
	//采集的频率降低
	 if(datacontrol<10)
	 {
	    datacontrol++;
	 }
	 else{
            if(poscontrol<9999)
			{
		         cappos[poscontrol]=pos;
			     capangle[poscontrol++]=angle;
			}
		datacontrol=0;
	 }
}
void Control()
{
	double vel,acc;
	short rtn;
    unsigned short swt=0;
	rtn=GT_GetSts(&swt);                             error(rtn);
   if(((swt&0x20)==0x20)||((swt&0x40)==0x40)||(pos>0.25)||(pos<-0.25))//限为位控制
   {
           	rtn=GT_Axis(1);
            rtn=GT_AxisOff();                         error(rtn);
            rtn=GT_Close();                           error(rtn);  	               
		    rtn=GT_Update();                          error(rtn);
   }
   else
   {
	     if(angle>3.04432613888889&&angle<3.238859061111111)
		 {      v_ctrl=-31.62*(pos-wzchange)-20.95*posdot+77.80*(angle-3.1415926)+11.76*angledot;
           	    v=-v_ctrl*0.285;
	            acc = v * 0.0004 * 0.0004 * 37500/ 0.46;//最大加速度
				if(acc<0)  acc=-acc;
				if(v<-0)  vel = -(0.0004 * 10 *37500 / 0.46);
				if(v>0) vel = 0.0004 * 10 * 37500/ 0.46;//速度控制
                  rtn=GT_Axis(1);                     error(rtn);
                  rtn=GT_ClrSts();                    error(rtn);
                  rtn=GT_AxisOn();                    error(rtn);
                  rtn = GT_SetVel(vel);               error(rtn); 
                  rtn = GT_SetAcc(acc);               error(rtn); 
                  rtn=GT_Update();                    error(rtn); 
		   }
	       else
		   {

               	rtn=GT_Axis(1);
               	rtn=GT_AxisOff();                      error(rtn);
                rtn=GT_Close();                        error(rtn);  	               
				rtn=GT_Update();                       error(rtn);
		   }


   }
}
void CPendView::OnStart() 
{
TIMECAPS tc;

if(timeGetDevCaps(&tc,sizeof(TIMECAPS))==TIMERR_NOERROR)
{
   wAccuracy=min(max(tc.wPeriodMin,1),tc.wPeriodMax);
   timeBeginPeriod(wAccuracy);
  if((TimerID_7ms=timeSetEvent(10,
   wAccuracy,
   (LPTIMECALLBACK ) OneMilliSecondProc,
   (DWORD)this,
   TIME_CALLBACK_FUNCTION|TIME_PERIODIC))==0)
  {
	MessageBox("不能定时");
  }
 
}	// TODO: Add your command handler code here
	
}

void CPendView::OnStop() 
{
    short rtn;
    timeKillEvent(TimerID_7ms);
    timeEndPeriod(wAccuracy);
	rtn=GT_Axis(1);
	rtn=GT_AxisOff();            error(rtn);
    rtn=GT_Close();              error(rtn);  
    rtn=GT_Update();             error(rtn);
	// TODO: Add your command handler code here
	
}

void CPendView::OnChar(UINT nChar, UINT nRepCnt, UINT nFlags) 
{
	short rtn;
	switch(nChar)
	{
	case VK_SPACE:
        timeKillEvent(TimerID_7ms);
        timeEndPeriod(wAccuracy);
	    rtn=GT_Axis(1);
	    rtn=GT_AxisOff();            error(rtn);
        rtn=GT_Close();              error(rtn);  
        rtn=GT_Update();             error(rtn);
        break;
	}// TODO: Add your message handler code here and/or call default
	
	CView::OnChar(nChar, nRepCnt, nFlags);
}

void CPendView::OnPaint() 
{
Invalidate();	// TODO: Add your command handler code here
	
}

void CPendView::OnLeft() 
{
wzchange=wzchange-0.03;	// TODO: Add your command handler code here
	
}

void CPendView::OnRight() 
{
wzchange=wzchange+0.03;	// TODO: Add your command handler code here
	
}