固高环形倒立摆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);
}
