serverdlg.cpp

控制机器人完成螺丝刀插孔实验

// ServerDlg.cpp : implementation file
//

#include "stdafx.h"
#include "Server.h"
#include "ServerDlg.h"
#include "Pa.h"
#include "Paerr.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

/*
#ifdef INT32
#define long  INT32;
#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 = 0.0;
	m_eJoint5 = 0.0;
	m_eJoint6 = 0.0;
	m_eJoint7 = 0.0;
	m_eJoint8 = 0.0;
	m_show = _T("");
	m_receive = _T("hh");
	m_position = 0.0f;
	//}}AFX_DATA_INIT
	// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
	UserPort=3336;
	DataPort=3338;
	BackDataPort=3340;
//	m_nIndex=0;
	row=0;

	m_bOpened=FALSE;
	m_bArray=FALSE;
}

void CServerDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CServerDlg)
	DDX_Control(pDX, IDC_GRASPOBJECT_COMBO, m_grasp_comb);
	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);
	DDX_Control(pDX, IDC_MSCOMM1, m_mscom);
	DDX_Text(pDX, IDC_SHOWCHAR_EDIT, m_show);
	DDX_Text(pDX, IDC_RECEIVE_EDIT, m_receive);
	DDX_Text(pDX, IDC_POSITION_EDIT, m_position);
	//}}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()
	ON_BN_CLICKED(IDC_BUTTON1, OnButton1)
	ON_BN_CLICKED(IDC_BUTTON2, OnButton2)
	ON_BN_CLICKED(IDC_SEND_BUTTON, OnSendButton)
	ON_BN_CLICKED(IDC_MOTION_BUTTON, OnMotionButton)
	ON_BN_CLICKED(IDC_LEFT_BUTTON, OnLeftButton)
	ON_BN_CLICKED(IDC_RIGHT_BUTTON4, OnRightButton4)
	ON_BN_CLICKED(IDC_UP_BUTTON, OnUpButton)
	ON_BN_CLICKED(IDC_DOWN_BUTTON, OnDownButton)
	ON_BN_CLICKED(IDC_FRONT_BUTTON, OnFrontButton)
	ON_BN_CLICKED(IDC_BACK_BUTTON8, OnBackButton8)
	ON_BN_CLICKED(IDC_SENDLEGGER_BUTTON, OnSendleggerButton)
	ON_BN_CLICKED(IDC_SENDC_BUTTON, OnSendcButton)
	//}}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,1000);
	m_sldJoint1.SetPos(500);
	m_sldJoint2.SetRange(0,1000);
	m_sldJoint2.SetPos(500);
	m_sldJoint3.SetRange(0,1000);
	m_sldJoint3.SetPos(500);
	m_sldJoint4.SetRange(0,1000);
	m_sldJoint4.SetPos(500);
	m_sldJoint5.SetRange(0,1000);
	m_sldJoint5.SetPos(500);
	m_sldJoint6.SetRange(0,1000);
	m_sldJoint6.SetPos(500);
	m_sldJoint7.SetRange(0,1000);
	m_sldJoint7.SetPos(500);
	m_sldJoint8.SetRange(0,1000);
	m_sldJoint8.SetPos(500);	
	m_grasp_comb.SetCurSel(0);

	m_show="";

	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
	pa_ext_arm(ARM0);
	pa_cls_arm(ARM0);
	pa_ter_sys();

	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
	if (m_bOpened) {
		AfxMessageBox("Already opened!");
	}
	else{
		if (pa_ini_sys()==ERR_OK) {
			if (pa_opn_arm(ARM0)==ERR_OK) {
				if (pa_sta_arm(ARM0)==ERR_OK) {
					m_bOpened=TRUE;							
				}
				else{
					AfxMessageBox("Fail to open PA10!");
					return;
				}
			}
			else{
				AfxMessageBox("Fail to open PA10!");
				return;
			}
		}
		else{
			AfxMessageBox("Fail to open PA10!");
			return;
		}
	}	
}

void CServerDlg::OnStop() 
{
	// TODO: Add your control notification handler code here
	if (m_bOpened) {
		pa_ext_arm(ARM0);
		pa_cls_arm(ARM0);
		pa_ter_sys();
		m_bOpened=FALSE;
/*		if (m_bArray) {
			m_srvrDataSocket.m_pDataSocket->m_obArray.RemoveAll();
			m_nIndex=0;			
		}
*/
		row=0;
	}
	else{
		AfxMessageBox("Not opened!");
	}
}

void CServerDlg::OnGohome() 
{
	// TODO: Add your control notification handler code here
	if (m_bOpened) {
		pa_exe_hom(ARM0,WM_WAIT);
/*		if (m_bArray) {
			m_srvrDataSocket.m_pDataSocket->m_obArray.RemoveAll();
			m_nIndex=0;				
		}
*/
		row=0;
		double temp[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
		Display(temp);
	}
	else{
		AfxMessageBox("Not opened!");
	}
	
	AfxMessageBox("Already home!");
}

void CServerDlg::OnMove() 
{
	// TODO: Add your control notification handler code here
	UpdateData(TRUE);

	if (m_bOpened) {
		double temp[8];
		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;
		
		ANGLE angle;
		int succeed;
		REAL32 vel[7];
		for(int i=0;i<7;i++)
			vel[i]=0.02;
		pa_set_vel(ARM0,VT_ONEVEL,vel);
		angle.s1=temp[1];
		angle.s2=temp[2];
		angle.s3=temp[3];
		angle.e1=temp[4];
		angle.e2=temp[5];
		angle.w1=temp[6];
		angle.w2=temp[7];
		succeed=pa_exe_axs(ARM0,S1|S2|S3|E1|E2|W1|W2,&angle,WM_WAIT);

		
		if(succeed==0)
		{
			senddata="a";
	        m_mscom.SetOutput(COleVariant(senddata));
			
		}
		if(receive[0]=='b')
		{
			pa_exe_axs(ARM0,S1|S2|S3|E1|E2|W1|W2,&angle,WM_WAIT);
		}

	}
	else{
		AfxMessageBox("Not opened!");
	}

//	Display(temp);
}

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;