?固高環形倒立擺GRIP2002是基于GT-400-SV-PCI運動控制卡的一個二級環形倒立擺（擺桿和連桿兩根桿的擺），固高公司提供了一個DOS環境下的Demo和MATLAB 7.0的simulink的Demo，但DOS版本貌似不能用，下面是在VS2008+SP1平臺下用VC做的控制程序，中間拖延了一陣子，今天算是全部完成，這個小實驗平臺提供了三個基本實驗，第一個是編碼器測試，第二個是測試伺服電機，第三個是主要的，通過LQR算法控制倒立擺的桿豎起來，即Swing-up Control實驗。。。

實驗效果圖

?

?

?

?

?

Swing-up Control 用VC控制的原理就是設置一個定時器，在相應函數里對倒立擺施加控制，這里的定時由于是5ms，非常下的時間，一般的方法不行，需要利用多媒體定時器。

//啟動定時

UINT CGRIP2002DemoDlg::CreateTimer()

{

//create the timer

// Create a periodic timer

timeBeginPeriod(1);

? ? gl_uTimerID = timeSetEvent(5,1,TimerHandler,(DWORD)this,TIME_PERIODIC); //5ms定時，最小是1ms

return gl_uTimerID;

}

//取消定時

void CGRIP2002DemoDlg::DestroyTimer()

{

if (m_bShiYanFlag)

{

timeKillEvent(gl_uTimerID);

timeEndPeriod(1);

m_bShiYanFlag = FALSE;

m_start = 0;

m_safety = 0;

m_pend = 0;

//offset = 0;

m_vel = 0;

m_acc = 0;

TRACE0("swing-up control is stoped...\n");

}

}

//

//多媒體定時器回調函數，這里面控制倒立擺

void CALLBACK ?TimerHandler(UINT uTimerID, UINT uMsg, DWORD dwUser, DWORD dw1, DWORD dw2)

{

short rtn;

long actl_pos;

CGRIP2002DemoDlg* pThis = (CGRIP2002DemoDlg *)dwUser;

GT_Axis(2);

GT_GetAtlPos(&actl_pos); ? ? ? ? ? ??//get angle of axis 2?

pThis->m_angle = ENCODE2*actl_pos; ??

?GT_Axis(3);

?rtn=GT_GetAtlPos(&actl_pos);//error_msg(rtn);

?pThis->m_angle2 = ENCODE2 * actl_pos;

?GT_Axis(1);

?GT_GetAtlPos(&actl_pos); ? ? ? ? ? ? ? /* get position of axis 1 */

?pThis->m_pos=ENCODE1*actl_pos;?

//TRACE1("get pos of axis 1 where is %f\n",pThis->m_pos);

??// get speed

?pThis->m_posDot = (pThis->m_pos - pThis->m_pos0) / INTPERIOD;

pThis->m_angleDot = (pThis->m_angle - pThis->m_angle0) / INTPERIOD;

?pThis->m_angleDot2 = (pThis->m_angle2 -pThis->m_angle02 ) / INTPERIOD;

?

?pThis->m_pos0 = pThis->m_pos;

?pThis->m_angle0 = pThis->m_angle;

?pThis->m_angle02 = pThis->m_angle2;

//安全檢查

if (pThis->handle_safety() ==-1) return;

//歸一化擺桿1角度，使之在0~2*PI之間

pThis->m_ang_2pi = pThis->m_angle;

while (pThis->m_ang_2pi < 0)

{pThis->m_ang_2pi += 2 * M_PI;};

while (pThis->m_ang_2pi >= (2*M_PI))

{pThis->m_ang_2pi -= 2 * M_PI;};

//歸一化擺桿2角度

pThis->m_ang_2pi2 = pThis->m_angle2 + pThis->m_ang_2pi - M_PI;

//?pThis->m_ang1=pThis->m_ang2; ? ? ? ? ? ? ? ? ? ? ? ? ? ?//保存最近四次的角度值,都為負值

//?pThis->m_ang2=pThis->m_ang3;

//?pThis->m_ang3=pThis->m_ang4;

//?pThis->m_ang4=-(pThis->m_angle);

if(pThis->m_bShiYanFlag) ? ? ? ? ? ? ? ? ? ? ? ? //進入控制程序

{

pThis->m_vel=0;

pThis->m_acc=0;

?

TRACE1("m_safety = %d\n",pThis->m_safety);

switch(pThis->m_start)

{

case 1: ? ? ? ? ? ? ? ??

pThis->m_start = 2;

pThis->enable_servo();

break;

case 2:

if ((fabs(pThis->m_ang_2pi-M_PI) <= (pThis->m_entryAngle)) && (fabs(pThis->m_ang_2pi2) <= (pThis->m_entryAngle)))

{

pThis->anti_cran_two();

pThis->m_start = 5;

pThis->m_pend = 1;

}

break;

case 5:

if (pThis->m_pend == 1)

pThis->anti_cran_two();

break;

}

?

//控制器輸出/

TRACE1("m_ang_2pi2 = %f,,,,,,, \n",pThis->m_ang_2pi2);

TRACE3("m_start=%d,m_acc=%f,m_vel=%f\n",pThis->m_start,pThis->m_acc,pThis->m_vel);

?

? ?GT_ClrSts();

? ?GT_SetVel(pThis->m_vel);

? ?GT_SetAcc(pThis->m_acc);

? ?GT_Update();?

}

}

其他還有很多代碼。。。

//這是 啟動 按鈕的代碼

void CGRIP2002DemoDlg::OnOK()

{

if (m_bAlarmOn)?

{

AfxMessageBox(_T("當前有軸報警，必須消除才能繼續運行！"));

return;

}

short rtn=0;

int nID;

int nPage;

long pos;

double vel;

double acc;

TCHAR str[20];

KillTimer(1);

nID = GetCheckedRadioButton(IDC_RADIO3,IDC_RADIO5);

nPage = m_wndTab.GetActiveTab();

switch(nID)

{

case IDC_RADIO3: ?//encoder test

SetTimer(1, 10, NULL); ? //定時器

break;

case IDC_RADIO4: ? //servo motor control

if (nPage == 0)

{

//T

m_wndPage1.GetDlgItemText(IDC_EDIT1,str,20);

pos = _tstol(str);

m_wndPage1.GetDlgItemText(IDC_EDIT2,str,20);

vel = _tstof(str);

m_wndPage1.GetDlgItemText(IDC_EDIT3,str,20);

acc = _tstof(str);

rtn+=GT_PrflT();

rtn+=GT_SetPos(pos);

rtn+=GT_SetVel(vel);

rtn+=GT_SetAcc(acc);

rtn+=GT_Update();

}?

else

{

//V

m_wndPage2.GetDlgItemText(IDC_EDIT2,str,20);

vel = _tstol(str);

m_wndPage2.GetDlgItemText(IDC_EDIT3,str,20);

acc = _tstof(str);

rtn+=GT_PrflV();

rtn+=GT_SetVel(vel);

rtn+=GT_SetAcc(acc);

rtn+=GT_Update();

}

Sleep(200);

break;

case IDC_RADIO5: ?//swing-up control

if (m_safety == 0 && m_start == 0)

{

TRACE0("swing-up control\n");?

CreateTimer();

//注冊空格鍵鍵為緊急鍵

::RegisterHotKey(GetSafeHwnd(),WM_EMERGENCY_HOTKEY,/*MOD_ALT | MOD_CONTROL*/NULL,VK_SPACE); ?

SetWindowText(_T("環形二級倒立擺實驗平臺(Swing-up Control 急停按空格鍵)"));

//

m_acc = 0;

m_vel = 0;

m_pend = 0;

m_offset = 0;

m_pos = m_pos0 = 0;

m_angle = m_angle0 = 0;

m_angle2 = m_angle02 = 0;

m_posDot = m_angleDot = 0;

m_bShiYanFlag = TRUE;

m_bSuanFaFlag = TRUE; ?//LQR

m_start = 1;

//Sleep(1000); ?//延遲1秒

}

break;

default:?

break;

}

error_msg(rtn);

}

總結

以上是生活随笔為你收集整理的MFC的固高环形倒立摆GRIP2002实验平台的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。