📄 robotdlg.cpp
字号:
m_x1ctrl = 0;
//////////////////////先送关节2的参数
char angel1,angel2;
float angel;
//angel = iplantimes;
angel = r2data[iplantimes];
m_x2ctrl =angel;
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;
ucIdent = 2;//关节2
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("关节2数据发送错误!", "Transmit Error !",MB_OK | MB_ICONERROR );
//*/
//////////////////////再送关节3的参数
//angel = -iplantimes;
angel = r3data[iplantimes];
m_x3ctrl =angel;
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;
ucIdent = 4;//关节3
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("关节3数据发送错误!", "Transmit Error !",MB_OK | MB_ICONERROR );
//*/
if(iplantimes <= 20) //共41个数据点
iplantimes++;
else
{ iplantimes = 0;//
CRobotDlg::OnButtonStop();
}
////////////////////////////调用时钟中断 3
int iIstallResult;
iIstallResult = SetTimer(3,
50,
NULL);
if(iIstallResult==0)
{
MessageBox("Cannot install timer 3");
}
/*
one = m_x1ctrl;
two = m_x2ctrl;
three = m_x3ctrl;
four =0;
five =0;
six =0;
*/
UpdateData(FALSE);
m_opengl.Invalidate(false);
}
//m_opengl.Invalidate(false);
CDialog::OnTimer(nIDEvent);
}
void CRobotDlg::OnRobotStopButton()
{
// TODO: Add your control notification handler code here
KillTimer(3);
KillTimer(1);
UpdateData(false);
}
void CRobotDlg::OnZhengComputeButton()
{
// TODO: Add your control notification handler code here
UpdateData(true);
char tbuf[80];
if (m_r1<-60||m_r1>30)
{
sprintf(tbuf,"r1应该在-60度到30度之间!!");
AfxMessageBox(tbuf);
}
else if (m_r2<-40||m_r2>50)
{
sprintf(tbuf,"r2应该在-40度到50度之间!!");
AfxMessageBox(tbuf);
}
else if (m_r3<-40||m_r3>50)
{
sprintf(tbuf,"r3应该在-40度到50度之间!!");
AfxMessageBox(tbuf);
}
else if (m_r4<-105||m_r4>105)
{
sprintf(tbuf,"r1应该在-105度到105度之间!!");
AfxMessageBox(tbuf);
}
else if (m_r5<-105||m_r5>105)
{
sprintf(tbuf,"r1应该在-105度到105度之间!!");
AfxMessageBox(tbuf);
}
else if (m_r6<-140||m_r6>140)
{
sprintf(tbuf,"r1应该在-140度到140度之间!!");
AfxMessageBox(tbuf);
}
else
{
mwArray Result=zhjie(m_r1*pi/180,m_r2*pi/180-pi/2,m_r3*pi/180+pi/2,m_r4*pi/180,m_r5*pi/180,m_r6*pi/180);
double array[16];
for(int i=0;i<4;i++)
for(int j=0;j<4;j++)
{
array[4*i+j]=Result(i+1,j+1);
CString str;
str.Format("%f",array[4*i+j]);
}
m_positionX=(float)Result(1,4);
m_positionY=(float)Result(2,4);
m_positionZ=(float)Result(3,4);
}
UpdateData(false);
}
void CRobotDlg::OnZhengResetButton()
{
// TODO: Add your control notification handler code here
UpdateData(true);
m_r1 = 0.0;
m_r2 = 0.0;
m_r3 = 0.0;
m_r4 = 0.0;
m_r5 = 0.0;
m_r6 = 0.0;
m_positionX=1796;
m_positionY=0;
m_positionZ=940;
UpdateData(false);
}
void CRobotDlg::OnZhengResultButton()
{
// TODO: Add your control notification handler code here
}
mwArray CRobotDlg::zhjie(double r1, double r2, double r3, double r4, double r5, double r6)
{
double AB1 [] ={cos(r1),sin(r1),0,0,0,0,-1,0,-sin(r1),cos(r1),0,0,0,0,0,1};
double AB2 [] ={cos(r2),sin(r2),0,0,-sin(r2),cos(r2),0,0,0,0,1,0,940*cos(r2),940*sin(r2),0,1};
double AB3 [] ={cos(r3),sin(r3),0,0,0,0,1,0,sin(r3),-cos(r3),0,0,1638*cos(r3),1638*sin(r3),0,1};
double AB4 [] ={cos(r4),sin(r4),0,0,0,0,-1,0,-sin(r4),cos(r4),0,0,158*cos(r4),158*sin(r4),0,1};
double AB5 [] ={cos(r5),sin(r5),0,0,0,0,1,0,sin(r5),-cos(r5),0,0,0,0,0,1};
double AB6 [] ={cos(r6),sin(r6),0,0,-sin(r6),cos(r6),0,0,0,0,1,0,0,0,0,1};
mwArray A1(4,4,AB1);
mwArray A2(4,4,AB2);
mwArray A3(4,4,AB3);
mwArray A4(4,4,AB4);
mwArray A5(4,4,AB5);
mwArray A6(4,4,AB6);
mwArray Re=A1*A2*A3*A4*A5*A6;
return Re;
}
void CRobotDlg::OnZhengGoButton()
{
// TODO: Add your control notification handler code here
UpdateData(true);
UCHAR ucRTRData;
UCHAR ucIdent;
if(!CanInitSuccess)
{
MessageBox("请先初始化!!", "初始化错误!",
MB_OK | MB_ICONERROR );
return;
}
ucRTRData = 0x03;/*00000011*/
char angel1,angel2;
float angel;
//第四关节
ucIdent=8;
angel=m_r4;
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 );
//第五关节
ucIdent=16;
angel=m_r5;
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 );
SetTimer(3,50,NULL);
}
void CRobotDlg::OnGraph()
{
int x_rot=520,y_rot=10;
int i;
CString str;
//double y;
// Creat a DC object
CClientDC dc(this);
CPen MyNewPen2;
MyNewPen2.CreatePen(PS_SOLID,
1,
RGB(0,0,0));
// Select the new pen
CPen* pOriginalPen;
pOriginalPen=dc.SelectObject(&MyNewPen2);
////////////////////////////////////////////////////绘制正弦曲线
for(i=start_time*10;i<start_time*10+10;i++)
{
str.Format("%d",i);
dc.TextOut(x_rot+(i*50)%500,y_rot+105,str);
}
//专门用于标示最后一位数据
str.Format("%d",i);
dc.TextOut(x_rot-10+500,y_rot+105,str);
//画图开始基准点
dc.MoveTo(x_rot,y_rot+100);
for(i=0;i<=Ttime;i++)
{
dc.LineTo(x_rot+i*5,(int)(-yResult[i]*1+y_rot+100));
}
///////////////////////////////////////////////////绘制曲线结束
//Return the original pen
dc.SelectObject(pOriginalPen);
}
void CRobotDlg::OnZhubengButton()
{
// TODO: Add your control notification handler code here
UCHAR ucRTRData;
UCHAR ucIdent;
int work,i;
ucIdent=64;
ucRTRData = 0x01;/*00000011*/
i=zhu%2;
zhu++;
if(i==0)
work=20;
if(i==1)
work=2;
cTxBuf[0] = ucIdent;
cTxBuf[1] = ucRTRData;
cTxBuf[2] = work;
if(CANSendMsg( m_Port, (UCHAR*)cTxBuf, FALSE)==0) ;
else MessageBox("数据发送错误!", "Transmit Error !",MB_OK | MB_ICONERROR );
////////////////////////////调用时钟中断 3
int iIstallResult;
iIstallResult=SetTimer(3,
50,
NULL);
if(iIstallResult==0)
{
MessageBox("Cannot install timer 3");
}
}
void CRobotDlg::OnFaButton()
{
// TODO: Add your control notification handler code here
UCHAR ucRTRData;
UCHAR ucIdent;
int work,i;
ucIdent=64;
ucRTRData = 0x01;/*00000011*/
i=fa%2;
fa++;
if(i==0)
work=30;
if(i==1)
work=3;
cTxBuf[0] = ucIdent;
cTxBuf[1] = ucRTRData;
cTxBuf[2] = work;
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::OnLengqueButton()
{
// TODO: Add your control notification handler code here
UCHAR ucRTRData;
UCHAR ucIdent;
int work,i;
ucIdent=64;
ucRTRData = 0x01;/*00000011*/
i=leng%2;
leng++;
if(i==0)
work=40;
if(i==1)
work=4;
cTxBuf[0] = ucIdent;
cTxBuf[1] = ucRTRData;
cTxBuf[2] = work;
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::OnPowerButton()
{
// TODO: Add your control notification handler code here
UCHAR ucRTRData;
UCHAR ucIdent;
int work,i;
ucIdent=64;
ucRTRData = 0x01;/*00000011*/
i=dian%2;
dian++;
if(i==0)
work=50;
if(i==1)
work=5;
cTxBuf[0] = ucIdent;
cTxBuf[1] = ucRTRData;
cTxBuf[2] = work;
if(CANSendMsg( m_Port, (UCHAR*)cTxBuf, FALSE)==0) ;
else MessageBox("数据发送错误!", "Transmit Error !",MB_OK | MB_ICONERROR );
////////////////////////////调用时钟中断 3
int iIstallResult;
iIstallResult=SetTimer(3,
50,
NULL);
if(iIstallResult==0)
{
MessageBox("Cannot install timer 3");
}
}
void CRobotDlg::OnErrorButton()
{
// TODO: Add your control notification handler code here
UCHAR ucRTRData;
UCHAR ucIdent;
int work,i;
ucIdent=64;
ucRTRData = 0x01;/*00000011*/
i=bao%2;
bao++;
if(i==0)
work=10;
if(i==1)
work=1;
cTxBuf[0] = ucIdent;
cTxBuf[1] = ucRTRData;
cTxBuf[2] = work;
if(CANSendMsg( m_Port, (UCHAR*)cTxBuf, FALSE)==0) ;
else MessageBox("数据发送错误!", "Transmit Error !",MB_OK | MB_ICONERROR );
////////////////////////////调用时钟中断 3
int iIstallResult;
iIstallResult=SetTimer(3,
50,
NULL);
if(iIstallResult==0)
{
MessageBox("Cannot install timer 3");
}
}
void CRobotDlg::OnButtonPlanrun()
{
// TODO: Add your control notification handler code here
UpdateData(true);
if(!CanInitSuccess)
{
MessageBox("CAN总线请先初始化!!", "初始化错误!",
MB_OK | MB_ICONERROR );
return;
}
if (m_plan == "1.平面直线")
{
CRobotDlg::GetParametersPlan();//获取数据
SetTimer(10,500,NULL);//启动定时器10
}
else if (m_plan =="2.平面圆形")
{
CRobotDlg::GetParametersPlan2();//获取数据
SetTimer(10,500,NULL);//启动定时器10
}
else if (m_plan =="3.空间曲线")
{ }
}
void CRobotDlg::GetParametersPlan()
{
int i;
for(i=0;i<=100;i++)
{
r1data[i]=0;
r2data[i]=c4r2data[i];
r3data[i]=c4r3data[i];
}
}
void CRobotDlg::GetParametersPlan2()
{
int i;
for(i=0;i<=40;i++)
{
//r1data[i]=0;
// r2data[i]=c2r2data[i];
// r3data[i]=c2r3data[i];
}
}
void CRobotDlg::OnButtonStop()
{
// TODO: Add your control notification handler code here
KillTimer(10);
iplantimes = 0;
UpdateData(FALSE);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -