📄 robotdlg.cpp
字号:
glTranslatef(1.0-0.06,0.0,0.0);
glRotatef(three+90,0.0,0.0,1.0);//沿z轴第三个关节
glTranslatef(1.5+0.138,0.0,0.0);
glPushMatrix();
glColor3f(0.0,1.0,0.0);
glScalef(3.276,0.4,0.6);//第二根杆长度为1638
glutSolidCube(1.0);
glPopMatrix();
glTranslatef(1.5+0.138,0.0,0.0);
glRotatef(four,0.0,1.0,0.0);//第四个关节沿y轴
glTranslatef(0.158,0.0,0.0);
glPushMatrix();
glColor3f(0.0,0.0,1.0);
glScalef(0.316,0.3,0.6);//第三根杆长度为158
glutSolidCube(1.0);
glPopMatrix();
glTranslatef(0.25-0.092,0.0,0.0);
glRotatef(five,0.0,0.0,1.0);//第五个关节沿z轴
glTranslatef(0.25-0.15,0.0,0.0);
glPushMatrix();
glColor3f(1.0,1.0,0.0);
glScalef(0.2,0.4,0.62);//第四根杆长度为0.2
glutSolidCube(1.0);
glPopMatrix();
glTranslatef(0.25-0.1,0.0,0.0);
glRotatef(six,1.0,0.0,0.0);//第六个关节沿x轴
glTranslatef(0.5-0.1,0.0,0.0);
glPushMatrix();
glColor3f(0.0,1.0,1.0);
glRotatef(90,0.0,1.0,0.0);
glTranslatef(0.0,0.0,-0.5+0.05);
gluCylinder(qobj,0.2,0.2,0.2,20,2);//0.2
glTranslatef(0.0,0.0,0.2);
gluDisk(qobj,0,0.2,20,2);
//glTranslatef(0.15,0.0,0.0);
gluCylinder(qobj,0.05,0.05,0.32,20,2);//第五根杆长度为360
glPopMatrix();
glPopMatrix();
glPopMatrix();
UpdateData(false);
SwapBuffers(::wglGetCurrentDC());
}
void CRobotDlg::OnUpButton()
{
// TODO: Add your control notification handler code here
m_upanddown+=0.1;
m_opengl.Invalidate(false);
}
void CRobotDlg::OnDownButton()
{
// TODO: Add your control notification handler code here
m_upanddown-=0.1;
m_opengl.Invalidate(false);
}
void CRobotDlg::OnFrontButton()
{
// TODO: Add your control notification handler code here
m_behind=m_behind+0.2;
m_opengl.Invalidate(false);
}
void CRobotDlg::OnBackButton()
{
// TODO: Add your control notification handler code here
m_behind=m_behind-0.2;
m_opengl.Invalidate(false);
}
void CRobotDlg::OnLeftButton()
{
// TODO: Add your control notification handler code here
m_leftandright=m_leftandright-0.2;
m_opengl.Invalidate(false);
}
void CRobotDlg::OnRightButton()
{
// TODO: Add your control notification handler code here
m_leftandright=m_leftandright+0.2;
m_opengl.Invalidate(false);
}
void CRobotDlg::OnReleasedcaptureSliderX(NMHDR* pNMHDR, LRESULT* pResult)
{
// TODO: Add your control notification handler code here
UpdateData(true);
m_opengl.Invalidate(false);
*pResult = 0;
}
void CRobotDlg::OnReleasedcaptureSliderY(NMHDR* pNMHDR, LRESULT* pResult)
{
// TODO: Add your control notification handler code here
m_opengl.Invalidate(false);
*pResult = 0;
}
void CRobotDlg::OnReleasedcaptureSliderZ(NMHDR* pNMHDR, LRESULT* pResult)
{
// TODO: Add your control notification handler code here
m_opengl.Invalidate(false);
*pResult = 0;
}
void CRobotDlg::OnDeltaposSpinFirst(NMHDR* pNMHDR, LRESULT* pResult)
{
NM_UPDOWN* pNMUpDown = (NM_UPDOWN*)pNMHDR;
// TODO: Add your control notification handler code here
UpdateData(true);
if(m_tongxun)
{
Sleep(2000);
MessageBox("无法与机器人连接,请检查线路!");
return;
}
m_firstangel=pNMUpDown->iPos;
UpdateData(false);
if(m_hand)
OPGLUpdate();
*pResult = 0;
}
void CRobotDlg::OnDeltaposSpinFive(NMHDR* pNMHDR, LRESULT* pResult)
{
NM_UPDOWN* pNMUpDown = (NM_UPDOWN*)pNMHDR;
// TODO: Add your control notification handler code here
UpdateData(true);
if(m_tongxun)
{
Sleep(2000);
MessageBox("无法与机器人连接,请检查线路!");
return;
}
m_fiveangel=pNMUpDown->iPos;
UpdateData(false);
if(m_hand)
OPGLUpdate();
*pResult = 0;
}
void CRobotDlg::OnDeltaposSpinFour(NMHDR* pNMHDR, LRESULT* pResult)
{
NM_UPDOWN* pNMUpDown = (NM_UPDOWN*)pNMHDR;
// TODO: Add your control notification handler code here
UpdateData(true);
if(m_tongxun)
{
Sleep(2000);
MessageBox("无法与机器人连接,请检查线路!");
return;
}
m_fourangel=pNMUpDown->iPos;
UpdateData(false);
if(m_hand)
OPGLUpdate();
*pResult = 0;
}
void CRobotDlg::OnDeltaposSpinSecond(NMHDR* pNMHDR, LRESULT* pResult)
{
NM_UPDOWN* pNMUpDown = (NM_UPDOWN*)pNMHDR;
// TODO: Add your control notification handler code here
UpdateData(true);
if(m_tongxun)
{
Sleep(2000);
MessageBox("无法与机器人连接,请检查线路!");
return;
}
m_secondangel=pNMUpDown->iPos;
UpdateData(false);
if(m_hand)
OPGLUpdate();
*pResult = 0;
}
void CRobotDlg::OnDeltaposSpinSix(NMHDR* pNMHDR, LRESULT* pResult)
{
NM_UPDOWN* pNMUpDown = (NM_UPDOWN*)pNMHDR;
// TODO: Add your control notification handler code here
UpdateData(true);
if(m_tongxun)
{
Sleep(2000);
MessageBox("无法与机器人连接,请检查线路!");
return;
}
m_sixangel=pNMUpDown->iPos;
UpdateData(false);
if(m_hand)
OPGLUpdate();
*pResult = 0;
}
void CRobotDlg::OnDeltaposSpinThree(NMHDR* pNMHDR, LRESULT* pResult)
{
NM_UPDOWN* pNMUpDown = (NM_UPDOWN*)pNMHDR;
// TODO: Add your control notification handler code here
UpdateData(true);
if(m_tongxun)
{
Sleep(2000);
MessageBox("无法与机器人连接,请检查线路!");
return;
}
m_threeangel=pNMUpDown->iPos;
UpdateData(false);
if(m_hand)
OPGLUpdate();
*pResult = 0;
}
void CRobotDlg::OPGLUpdate()
{
UpdateData(true);
one=m_firstangel;
two=m_secondangel;
three=m_threeangel;
four=m_fourangel;
five=m_fiveangel;
six=m_sixangel;
m_opengl.Invalidate(false);
UpdateData(false);
}
void CRobotDlg::OnInitButton()
{
// TODO: Add your control notification handler code here
int iHostID=1;
int m_uDevNum=0;
int iBaudRate;
bool checkinit=1;
m_BTR0=0x3;
m_BTR1=0x1c;/* baud rate 125Kbps*/
m_AcpMask=0xfd;
m_AcpCode=0x02;
m_OutCtrlCode=0;
m_IntMask=0;/*0 means poiling*/
if (CANPortOpen( m_uDevNum, (WORD *)&m_Port, (WORD *)&iHostID,
(WORD *)&iBaudRate) != SUCCESS) {
MessageBox( "CAN port open error!", "CANOpen ( )", MB_ICONHAND|MB_ICONSTOP|MB_ICONERROR);
checkinit=0;
}
////////////canhwreset/////
CANHwReset( m_Port );
///////////cansetbaud/////
CANSetBaud( m_Port, m_BTR0, m_BTR1);
//////////cansetacp///////////
CANSetAcp( m_Port, m_AcpCode, m_AcpMask );
//////////cansetoutctrl//////////
CANSetOutCtrl( m_Port, m_OutCtrlCode);
//////////canint//////////////
if (CANInit( m_Port, m_BTR0, m_BTR1, (UCHAR)m_IntMask ) != SUCCESS)
{
MessageBox( "CAN port open error!", "CANInit ( )", MB_ICONHAND|MB_ICONSTOP|MB_ICONERROR);
checkinit=0;
return;
}
/////////cansetnormal//////////////////
if (CANSetNormal( m_Port ) != SUCCESS)
{
MessageBox( "CAN port CANSetNormal error!", "CANSetNormal ( )", MB_ICONHAND|MB_ICONSTOP|MB_ICONERROR);
checkinit=0;
return;
}
if(checkinit)
{
MessageBox("CAN初始化成功!");
CanInitSuccess=1;
//CWnd* pWnd=GetDlgItem(IDC_BUTTON_SEND);
//pWnd->EnableWindow;
UpdateData(false);}
}
void CRobotDlg::OnRobotResetButton()
{
// TODO: Add your control notification handler code here
UpdateData(true);
KillTimer(1);
KillTimer(3);
one=0;
two=0;
three=0;
four=0;
five=0;
six=0;
m_xrot=0;
m_yrot=0;
m_zrot=0;
m_firstangel=0;
m_secondangel=0;
m_threeangel=0;
m_fourangel=0;
m_fiveangel=0;
m_sixangel=0;
UpdateData(false);
m_opengl.Invalidate(false);
if(m_tongxun)
{
if(!CanInitSuccess)
{
MessageBox("请先初始化!!", "初始化错误!",
MB_OK | MB_ICONERROR );
return;
}
UCHAR ucIdent1,ucIdent2,ucRTRData1, ucRTRData2;
char cTxBuf1[30],cTxBuf2[30];
ucIdent1 =16;//关节5复位
ucRTRData1 = 0x03;
cTxBuf1[0] = ucIdent1;
cTxBuf1[1] = ucRTRData1;
cTxBuf1[2] = 1;
cTxBuf1[3] = 12;
cTxBuf1[4] = 1;
if(CANSendMsg( m_Port, (UCHAR*)cTxBuf1, FALSE)==0) ;
else MessageBox("数据发送错误!", "Transmit Error !",MB_OK | MB_ICONERROR );
Sleep(300);
ucIdent2 =8;//关节4复位
ucRTRData2 = 0x03;
cTxBuf2[0] = ucIdent2;
cTxBuf2[1] = ucRTRData2;
cTxBuf2[2] = 1;
cTxBuf2[3] = 12;
cTxBuf2[4] = 1;
if(CANSendMsg( m_Port, (UCHAR*)cTxBuf2, FALSE)==0) ;
else MessageBox("数据发送错误!", "Transmit Error !",MB_OK | MB_ICONERROR );
SetTimer(3,33,NULL);
}
}
void CRobotDlg::OnDirectControlButton()
{
// TODO: Add your control notification handler code here
UpdateData(true);
UCHAR ucRTRData;
UCHAR ucIdent;
if(m_robotnumber=="关节 1")
ucIdent=1;
else if(m_robotnumber=="关节 2")
ucIdent=2;
else if(m_robotnumber=="关节 3")
ucIdent=4;
else if(m_robotnumber=="关节 4")
ucIdent=8;
else if(m_robotnumber=="关节 5")
ucIdent=16;
else if(m_robotnumber=="关节 6")
ucIdent=32;
else if(m_robotnumber=="关节 7")
ucIdent=128;
UpdateData(FALSE);
ucRTRData = 0x03;/*00000011*/
UpdateData(true);
if(!CanInitSuccess)
{
MessageBox("请先初始化!!", "初始化错误!",
MB_OK | MB_ICONERROR );
return;
}
if ((m_robotangel>105)||(m_robotangel<-105))
{
MessageBox("请输入一个-105度到105度的数!", "角度错误!",
MB_OK | MB_ICONERROR );
return;
}
char angel1,angel2;
float angel;
angel=m_robotangel;
if(angel>0)
angel=angel+(float)0.001;
if(angel<0)
angel=angel-(float)0.001;
angel1=(char)angel;//整数位
angel2=(char)((int(angel*100))%100);//小数点后两位
if (angel2==0)
angel2=1;
cTxBuf[0] = ucIdent;
cTxBuf[1] = ucRTRData;
cTxBuf[2] = 2;
cTxBuf[3] = angel1;
cTxBuf[4] = angel2;
if(CANSendMsg( m_Port, (UCHAR*)cTxBuf, FALSE)==0) ;
else MessageBox("数据发送错误!", "Transmit Error !",MB_OK | MB_ICONERROR );
////////////////////////////调用时钟中断 3
int iIstallResult;
iIstallResult=SetTimer(3,
200,
NULL);
if(iIstallResult==0)
{
MessageBox("Cannot install timer 3");
}
}
void CRobotDlg::OnTimer(UINT nIDEvent)
{
// TODO: Add your message handler code here and/or call default
CString str;
UpdateData(true);
if(nIDEvent==3)//单关节控制,从下位机接收数据
{
LPSTR cstr1;
int bReady;
int angel1,angel2,fankui1,fankui2;
float angel,fankui;
CString m_RxBuf;
bReady = FALSE;
CANQueryMsg( m_Port, (int *)&bReady, (UCHAR*)cReBuf ); // query CAN message
if ( bReady )
{
// Get ID
m_RxID = (WORD)cReBuf[0];
m_RxID <<= 3;
m_RxID |= ((cReBuf[1] >> 5) & 0x07);
// Get message
cstr1 = m_RxBuf.GetBuffer(20);
strcpy( cstr1, &cReBuf[2] );
angel1=(int)(*cstr1);
angel2=(int)(*(cstr1+1));
fankui1=(int)(*(cstr1+2));
fankui2=(int)(*(cstr1+3));
angel=(float)angel1+(float)angel2/100;
fankui=(float)fankui1+(float)fankui2/100;
if(m_RxID==8)
{
one=fankui;
}
if(m_RxID==9)
{
two=fankui;
}
if(m_RxID==11)
{
three=-fankui;
}
if(m_RxID==12)
{
m_RxID4=m_RxID;
m_rmsg0=angel;
m_rmsg1=fankui;
m_rmsg2=*(cstr1+4);
m_rmsg3=*(cstr1+5);
m_rmsg4=*(cstr1+6);
m_rmsg5=*(cstr1+7);
m_v4=(float)((*(cstr1+4))*0x100+(*(cstr1+5)))*9.58/0xfff;
}
if(m_RxID==13)
{
m_RxID5=m_RxID;
m_rmsg51=angel;
m_rmsg52=fankui;
m_rsmg53=*(cstr1+4);
m_rsmg54=*(cstr1+5);
m_rsmg55=*(cstr1+6);
m_rsmg56=*(cstr1+7);
m_v5=(float)((*(cstr1+4))*0x100+(*(cstr1+5)))*9.58/0xfff;
}
if(m_RxID==14)
{
six=fankui;
}
if(m_RxID==15)
{
m_seven=fankui;
}
if(m_RxID==10)
{
m_P1=angel1;
m_control=angel2;
}
four=-m_rmsg1;
five=m_rmsg52;
//*/////////////////////////////////2004
m_firstangel=one;
m_secondangel=two;
m_threeangel=three;
m_fourangel=-four;
m_fiveangel=five;
m_sixangel=six;
//*//////////////////////////////////2004
UpdateData(FALSE); // Update display
m_RxBuf.ReleaseBuffer();
Ttime=Ttime++;
if(Ttime%100==0)
{
start_time++;
Ttime=0;
for(int i=0;i<101;i++)
yResult[i]=0;
CRobotDlg::OnPaint();
}
////////////////////////////////////数据采集
yResult[Ttime]=two;
CRobotDlg::OnGraph();
}
else {KillTimer(3);MessageBox("请检查机器人通讯是否正常!", "没有可用信息!",MB_OK | MB_ICONERROR );}
}
else if(nIDEvent==10)//按运动规划曲线进行控制,并从下位机接收数据
{
UCHAR ucIdent;
UCHAR ucRTRData;
ucRTRData = 0x03;/*00000011*/
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -