📄 serverdlg.cpp
字号:
// ServerDlg.cpp : implementation file
//
#include "stdafx.h"
#include "Server.h"
#include "ServerDlg.h"
#include "m5.h"
#include <math.h>
/*
extern "c"
{
#include "m5.h"
}
*/
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
/////////////////////////////////////////////////////////////////////////////
// CAboutDlg dialog used for App About
class CAboutDlg : public CDialog
{
public:
CAboutDlg();
// Dialog Data
//{{AFX_DATA(CAboutDlg)
enum { IDD = IDD_ABOUTBOX };
//}}AFX_DATA
// ClassWizard generated virtual function overrides
//{{AFX_VIRTUAL(CAboutDlg)
protected:
virtual void DoDataExchange(CDataExchange* pDX); // DDX/DDV support
//}}AFX_VIRTUAL
// Implementation
protected:
//{{AFX_MSG(CAboutDlg)
//}}AFX_MSG
DECLARE_MESSAGE_MAP()
};
CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
//{{AFX_DATA_INIT(CAboutDlg)
//}}AFX_DATA_INIT
}
void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
CDialog::DoDataExchange(pDX);
//{{AFX_DATA_MAP(CAboutDlg)
//}}AFX_DATA_MAP
}
BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
//{{AFX_MSG_MAP(CAboutDlg)
// No message handlers
//}}AFX_MSG_MAP
END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
// CServerDlg dialog
CServerDlg::CServerDlg(CWnd* pParent /*=NULL*/)
: CDialog(CServerDlg::IDD, pParent)
{
//{{AFX_DATA_INIT(CServerDlg)
m_eJoint1 = 0.0;
m_eJoint2 = 0.0;
m_eJoint3 = 0.0;
m_eJoint4 = 90.0;
m_eJoint5 = 0.0;
m_eJoint6 = -90.0;
m_eJoint7 = 90.0;
m_eJoint8 = 0.0;
//}}AFX_DATA_INIT
// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
UserPort=3346;
DataPort=3348;
BackDataPort=3350;
row=0;
// m_nIndex=0;
m_bOpened=FALSE;
m_bArray=FALSE;
}
void CServerDlg::DoDataExchange(CDataExchange* pDX)
{
CDialog::DoDataExchange(pDX);
//{{AFX_DATA_MAP(CServerDlg)
DDX_Control(pDX, IDC_SLIDER_JOINT8, m_sldJoint8);
DDX_Control(pDX, IDC_SLIDER_JOINT7, m_sldJoint7);
DDX_Control(pDX, IDC_SLIDER_JOINT6, m_sldJoint6);
DDX_Control(pDX, IDC_SLIDER_JOINT5, m_sldJoint5);
DDX_Control(pDX, IDC_SLIDER_JOINT4, m_sldJoint4);
DDX_Control(pDX, IDC_SLIDER_JOINT3, m_sldJoint3);
DDX_Control(pDX, IDC_SLIDER_JOINT2, m_sldJoint2);
DDX_Control(pDX, IDC_SLIDER_JOINT1, m_sldJoint1);
DDX_Text(pDX, IDC_EDIT_JOINT1, m_eJoint1);
DDX_Text(pDX, IDC_EDIT_JOINT2, m_eJoint2);
DDX_Text(pDX, IDC_EDIT_JOINT3, m_eJoint3);
DDX_Text(pDX, IDC_EDIT_JOINT4, m_eJoint4);
DDX_Text(pDX, IDC_EDIT_JOINT5, m_eJoint5);
DDX_Text(pDX, IDC_EDIT_JOINT6, m_eJoint6);
DDX_Text(pDX, IDC_EDIT_JOINT7, m_eJoint7);
DDX_Text(pDX, IDC_EDIT_JOINT8, m_eJoint8);
//}}AFX_DATA_MAP
}
BEGIN_MESSAGE_MAP(CServerDlg, CDialog)
//{{AFX_MSG_MAP(CServerDlg)
ON_WM_SYSCOMMAND()
ON_WM_PAINT()
ON_WM_QUERYDRAGICON()
ON_BN_CLICKED(IDC_CONNECT, OnConnect)
ON_BN_CLICKED(IDC_EXIT, OnExit)
ON_BN_CLICKED(IDC_OPEN, OnOpen)
ON_BN_CLICKED(IDC_STOP, OnStop)
ON_BN_CLICKED(IDC_GOHOME, OnGohome)
ON_BN_CLICKED(IDC_MOVE, OnMove)
ON_WM_HSCROLL()
//}}AFX_MSG_MAP
END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
// CServerDlg message handlers
BOOL CServerDlg::OnInitDialog()
{
CDialog::OnInitDialog();
// Add "About..." menu item to system menu.
// IDM_ABOUTBOX must be in the system command range.
ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
ASSERT(IDM_ABOUTBOX < 0xF000);
CMenu* pSysMenu = GetSystemMenu(FALSE);
if (pSysMenu != NULL)
{
CString strAboutMenu;
strAboutMenu.LoadString(IDS_ABOUTBOX);
if (!strAboutMenu.IsEmpty())
{
pSysMenu->AppendMenu(MF_SEPARATOR);
pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
}
}
// Set the icon for this dialog. The framework does this automatically
// when the application's main window is not a dialog
SetIcon(m_hIcon, TRUE); // Set big icon
SetIcon(m_hIcon, FALSE); // Set small icon
// TODO: Add extra initialization here
m_sldJoint1.SetRange(0,3000);
m_sldJoint1.SetPos(1500);
m_sldJoint2.SetRange(0,3000);
m_sldJoint2.SetPos(1500);
m_sldJoint3.SetRange(0,3000);
m_sldJoint3.SetPos(1500);
m_sldJoint4.SetRange(0,3000);
m_sldJoint4.SetPos(1653);
m_sldJoint5.SetRange(0,3000);
m_sldJoint5.SetPos(1347);
m_sldJoint6.SetRange(0,3000);
m_sldJoint6.SetPos(1421);
m_sldJoint7.SetRange(0,3000);
m_sldJoint7.SetPos(1500);
m_sldJoint8.SetRange(0,3000);
m_sldJoint8.SetPos(1500);
return TRUE; // return TRUE unless you set the focus to a control
}
void CServerDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
if ((nID & 0xFFF0) == IDM_ABOUTBOX)
{
CAboutDlg dlgAbout;
dlgAbout.DoModal();
}
else
{
CDialog::OnSysCommand(nID, lParam);
}
}
// If you add a minimize button to your dialog, you will need the code below
// to draw the icon. For MFC applications using the document/view model,
// this is automatically done for you by the framework.
void CServerDlg::OnPaint()
{
if (IsIconic())
{
CPaintDC dc(this); // device context for painting
SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);
// Center icon in client rectangle
int cxIcon = GetSystemMetrics(SM_CXICON);
int cyIcon = GetSystemMetrics(SM_CYICON);
CRect rect;
GetClientRect(&rect);
int x = (rect.Width() - cxIcon + 1) / 2;
int y = (rect.Height() - cyIcon + 1) / 2;
// Draw the icon
dc.DrawIcon(x, y, m_hIcon);
}
else
{
CDialog::OnPaint();
}
}
// The system calls this to obtain the cursor to display while the user drags
// the minimized window.
HCURSOR CServerDlg::OnQueryDragIcon()
{
return (HCURSOR) m_hIcon;
}
void CServerDlg::OnConnect()
{
// TODO: Add your control notification handler code here
if (m_srvrSocket.m_hSocket==INVALID_SOCKET) {
BOOL bFlag=m_srvrSocket.Create(UserPort,SOCK_STREAM,FD_ACCEPT);
if (!bFlag) {
AfxMessageBox("Socket Error!");
m_srvrSocket.Close();
PostQuitMessage(0);
return;
}
}
if (!m_srvrSocket.Listen(1)) {
int nErrorCode=m_srvrSocket.GetLastError();
if (nErrorCode!=WSAEWOULDBLOCK) {
AfxMessageBox("Socket Error!");
m_srvrSocket.Close();
PostQuitMessage(0);
return;
}
}
//数据传输socket
if (m_srvrDataSocket.m_hSocket==INVALID_SOCKET) {
if (!(m_srvrDataSocket.Create(DataPort,SOCK_STREAM,FD_ACCEPT))) {
AfxMessageBox("Data Socket Error!");
m_srvrDataSocket.Close();
return;
}
}
if (!m_srvrDataSocket.Listen(1)) {
int nErrorCode=m_srvrDataSocket.GetLastError();
if (nErrorCode!=WSAEWOULDBLOCK) {
AfxMessageBox("Data Socket Error!");
m_srvrDataSocket.Close();
return;
}
}
//当前关节值回传Socket
if (m_srvrBackDataSocket.m_hSocket==INVALID_SOCKET) {
if (!(m_srvrBackDataSocket.Create(BackDataPort,SOCK_STREAM,FD_ACCEPT))) {
AfxMessageBox("Back Data Socket Error!");
m_srvrBackDataSocket.Close();
return;
}
}
if (!m_srvrBackDataSocket.Listen(1)) {
int nErrorCode=m_srvrBackDataSocket.GetLastError();
if (nErrorCode!=WSAEWOULDBLOCK) {
AfxMessageBox("Back Data Socket Error!");
m_srvrBackDataSocket.Close();
return;
}
}
}
void CServerDlg::OnExit()
{
// TODO: Add your control notification handler code here
if (m_bOpened) {
int retVal2=PCube_closeDevice(Dev);
}
m_srvrSocket.ShutDown(2);
m_srvrDataSocket.ShutDown(2);
m_srvrBackDataSocket.ShutDown(2);
EndDialog(0);
}
void CServerDlg::OnOpen()
{
// TODO: Add your control notification handler code here
/*
m_srvrDataSocket.m_pDataSocket->m_obArray.Add(lastJointVal);
AfxMessageBox("aaa");
*/
if (m_bOpened) {
AfxMessageBox("Already opened!");
}
else{
char* pInitString="VCI:7,0,1";
int retVal=PCube_openDevice(&Dev,pInitString);
if (retVal!=CLD_OK) {
AfxMessageBox("Fail to open MODULE!");
return;
}
Num=PCube_getModulesIdMap(Dev,idMap);
for(int i=0; i<Num; i++)
{
int retVal1=PCube_syncModule(Dev,idMap[i]);
if(retVal1==CLD_OK)
{
Bool sync=FALSE;
while((retVal1=PCube_querySyncEnd(Dev,idMap[i],&sync))==CLD_OK && !sync)
Sleep(100);
}
}
lastJointVal[0]=0.0;
lastJointVal[1]=1.5707963;
lastJointVal[2]=-1.5707963;
lastJointVal[3]=0.0;
lastJointVal[4]=1.5707963;
lastJointVal[5]=0.0;
lastJointVal[6]=0.0;
lastJointVal[7]=0.0;
if (m_bArray) {
// m_srvrDataSocket.m_pDataSocket->m_obArray.Add(lastJointVal);
for(int i=0;i<8;i++)
m_srvrDataSocket.m_pDataSocket->angle[row][i]=lastJointVal[i];
row++;
}
m_bOpened=TRUE;
}
}
void CServerDlg::OnStop()
{
// TODO: Add your control notification handler code here
if (m_bOpened) {
int retVal2=PCube_closeDevice(Dev);
if (retVal2==CLD_OK) {
m_bOpened=FALSE;
/*
if (m_bArray) {
m_srvrDataSocket.m_pDataSocket->m_obArray.RemoveAll();
m_nIndex=0;
}
*/
row=0;
}
else{
AfxMessageBox("Fail to stop MODULE!");
}
}
else{
AfxMessageBox("Not opened!");
}
}
void CServerDlg::OnGohome()
{
// TODO: Add your control notification handler code here
if (m_bOpened) {
for(int i=0;i<Num;i++){
float pos=(float)0.0,vel=(float)0.3,acc=(float)1.2;
int retVal9=PCube_moveRamp(Dev,idMap[i],pos,vel,acc);
Bool ready=FALSE;
if (retVal9==CLD_OK) {
while((retVal9=PCube_queryEndPos(Dev,idMap[i],&ready))==CLD_OK&&!ready)
Sleep(100);
}
}
/*
if (m_bArray) {
m_srvrDataSocket.m_pDataSocket->m_obArray.RemoveAll();
m_nIndex=0;
}
*/
row=0;
lastJointVal[0]=0.0;
lastJointVal[1]=1.5707963;
lastJointVal[2]=-1.5707963;
lastJointVal[3]=0.0;
lastJointVal[4]=1.5707963;
lastJointVal[5]=0.0;
lastJointVal[6]=0.0;
lastJointVal[7]=0.0;
if(m_bArray){
for(int i=0;i<8;i++)
m_srvrDataSocket.m_pDataSocket->angle[row][i]=lastJointVal[i];
row++;
}
// m_srvrDataSocket.m_pDataSocket->m_obArray.Add(lastJointVal);
// double temp[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
Display(lastJointVal);
}
else{
AfxMessageBox("Not opened!");
}
}
void CServerDlg::OnMove()
{
// TODO: Add your control notification handler code here
UpdateData(TRUE);
if (m_bOpened) {
double temp[8];
double oneStep[7];
float r[7];
float vel=float(0.01);//设置关节最大速度
temp[0]=0.0;
temp[1]=m_eJoint7/180.0*3.1415926;
temp[2]=m_eJoint6/180.0*3.1415926;
temp[3]=m_eJoint5/180.0*3.1415926;
temp[4]=m_eJoint4/180.0*3.1415926;
temp[5]=m_eJoint3/180.0*3.1415926;
temp[6]=m_eJoint2/180.0*3.1415926;
temp[7]=m_eJoint1/180.0*3.1415926;
int num=StepNum(vel,lastJointVal,temp,oneStep);
for(int i=1;i<=num;i++){
r[0]=-(lastJointVal[1]+i*oneStep[0]-1.5707963);
r[1]=-(lastJointVal[2]+i*oneStep[1]+1.5707963);
r[2]=-(lastJointVal[3]+i*oneStep[2]);
r[3]=-(lastJointVal[4]+i*oneStep[3]-1.5707963);
r[4]=-(lastJointVal[5]+i*oneStep[4]);
r[5]=-(lastJointVal[6]+i*oneStep[5]);
r[6]=-(lastJointVal[7]+i*oneStep[6]);
for(int j=0;j<7;j++){
int retVal2 = PCube_moveStep(Dev,idMap[j],r[j],100);
}
}
for(i=0;i<8;i++)
lastJointVal[i]=temp[i];
}
else{
AfxMessageBox("Not opened!");
}
}
void CServerDlg::Move()
{
double show[8];
show[0]=0.0;
double* tempLast=m_srvrDataSocket.m_pDataSocket->angle[(row-1)];
double* tempNext=m_srvrDataSocket.m_pDataSocket->angle[row];
double oneStep[7];
float r[7];
float vel=float(0.01);//设置关节最大速度
int num=StepNum(vel,tempLast,tempNext,oneStep);
row++;
for(int i=1;i<=num;i++){
r[0]=-(lastJointVal[1]+i*oneStep[0]-1.5707963);
r[1]=-(lastJointVal[2]+i*oneStep[1]+1.5707963);
r[2]=-(lastJointVal[3]+i*oneStep[2]);
r[3]=-(lastJointVal[4]+i*oneStep[3]-1.5707963);
r[4]=-(lastJointVal[5]+i*oneStep[4]);
r[5]=-(lastJointVal[6]+i*oneStep[5]);
r[6]=-(lastJointVal[7]+i*oneStep[6]);
for(int j=0;j<7;j++){
int retVal2 = PCube_moveStep(Dev,idMap[j],r[j],100);
show[j+1]=(double)r[j];
}
Display(show);
}
}
void CServerDlg::Display(double* Joint)
{
m_eJoint1=Joint[7]/3.1415926*180.0;
m_eJoint2=Joint[6]/3.1415926*180.0;
m_eJoint3=Joint[5]/3.1415926*180.0;
m_eJoint4=Joint[4]/3.1415926*180.0;
m_eJoint5=Joint[3]/3.1415926*180.0;
m_eJoint6=Joint[2]/3.1415926*180.0;
m_eJoint7=Joint[1]/3.1415926*180.0;
// m_eJoint8=Joint[0]/3.1415926*180.0;
m_sldJoint1.SetPos(int((m_eJoint1+1080.0)/2160.0*3000.0));
m_sldJoint2.SetPos(int((m_eJoint2+95.0)/190.0*3000.0));
m_sldJoint3.SetPos(int((m_eJoint3+160.0)/320.0*3000.0));
m_sldJoint4.SetPos(int((m_eJoint4-22.5)/122.5*3000.0));
m_sldJoint5.SetPos(int((m_eJoint5+55.0)/122.5*3000.0));
m_sldJoint6.SetPos(int((m_eJoint6+180.0)/190.0*3000.0));
m_sldJoint7.SetPos(int((m_eJoint7+70.0)/320.0*3000.0));
m_sldJoint8.SetPos(int((m_eJoint8+1.0)/2.0*3000.0));
UpdateData(FALSE);
}
void CServerDlg::OnHScroll(UINT nSBCode, UINT nPos, CScrollBar* pScrollBar)
{
// TODO: Add your message handler code here and/or call default
int ID=((CSliderCtrl*)pScrollBar)->GetDlgCtrlID();
switch(ID) {
case IDC_SLIDER_JOINT1:
m_eJoint1=m_sldJoint1.GetPos()/3000.0*2160.0-1080.0;
UpdateData(FALSE);
break;
case IDC_SLIDER_JOINT2:
m_eJoint2=m_sldJoint2.GetPos()/3000.0*190.0-95.0;
UpdateData(FALSE);
break;
case IDC_SLIDER_JOINT3:
m_eJoint3=m_sldJoint3.GetPos()/3000.0*320.0-160.0;
UpdateData(FALSE);
break;
case IDC_SLIDER_JOINT4:
m_eJoint4=m_sldJoint4.GetPos()/3000.0*122.5+22.5;
UpdateData(FALSE);
break;
case IDC_SLIDER_JOINT5:
m_eJoint5=m_sldJoint5.GetPos()/3000.0*122.5-55.0;
UpdateData(FALSE);
break;
case IDC_SLIDER_JOINT6:
m_eJoint6=m_sldJoint6.GetPos()/3000.0*190.0-180.0;
UpdateData(FALSE);
break;
case IDC_SLIDER_JOINT7:
m_eJoint7=m_sldJoint7.GetPos()/3000.0*320.0-70.0;
UpdateData(FALSE);
break;
case IDC_SLIDER_JOINT8:
m_eJoint8=m_sldJoint8.GetPos()/3000.0*2.0-1.0;
UpdateData(FALSE);
break;
default:
break;
}
CDialog::OnHScroll(nSBCode, nPos, pScrollBar);
}
int CServerDlg::StepNum(float maxVel, double* currentPos, double* nextPos, double* stepVal)
{
double temp[8];
double maxDis=0.0;
for(int i=1;i<8;i++){
temp[i]=fabs(nextPos[i]-currentPos[i]);
if (temp[i]>maxDis) {
maxDis=temp[i];
}
}
int stepnum=int(maxDis/maxVel*10);
for(i=0;i<7;i++){
stepVal[i]=temp[i+1]/stepnum;
// currentPos[i+1]=nextPos[i+1];
}
return stepnum;
}
void CServerDlg::GetJointValue()
{
int nErrorCode;
float angle;
for(int i=0;i<7;i++){
nErrorCode=PCube_getActPos(Dev,idMap[i],&angle);
if (nErrorCode==CLD_OK) {
m_srvrBackDataSocket.m_backDataSocket->backAngle[i]=angle;
}
else{
AfxMessageBox("Get Joint Value Error!");
return;
}
}
m_srvrBackDataSocket.m_backDataSocket->AsyncSelect(FD_WRITE);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -