MOTOCOM32运动控制器编程

// TestDlg.cpp : 僀儞僾儕儊儞僥乕僔儑儞 僼傽僀儖
//

#include "stdafx.h"
#include "Test.h"
#include "TestDlg.h"
#include <math.h>

#include <direct.h>     // <------ MOTOCOM32: 偙偺峴傪捛壛 
#include "motocom.h"    // <------ MOTOCOM32: 偙偺峴傪捛壛

// 幚峴娐嫬偵墳偠偰曄峏偟偰壓偝偄丅------>
//#error 埲壓傪幚峴娐嫬偵墳偠偰曄峏偟偰壓偝偄丅曄峏屻偼偙偺峴傪嶍彍偟偰壓偝偄丅
#define TEST_TRANS_MODE_IS_RS232C
#ifdef TEST_TRANS_MODE_IS_RS232C
#define TEST_TRANS_MODE 0   // RS-232C
#else
#define TEST_TRANS_MODE 1   // Ethernet
#endif
#define TEST_IP_ADDRESS "xxx.xxx.xxx.xx"
// 幚峴娐嫬偵墳偠偰曄峏偟偰壓偝偄丅<------

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

/**//////////////////////////////////////////////////////////////////////////////
// 傾僾儕働乕僔儑儞偺僶乕僕儑儞忣曬偱巊傢傟偰偄傞 CAboutDlg 僟僀傾儘僌
short g_nCid =0;// TestOpenComm ( TEST_TRANS_MODE );    // mode = 0:RS-232C or 1:Ethernet

class CAboutDlg : public CDialog
{
public:
    CAboutDlg();

// 僟僀傾儘僌 僨乕僞
    //{{AFX_DATA(CAboutDlg)
    enum { IDD = IDD_ABOUTBOX };
    //}}AFX_DATA

    // ClassWizard 偼壖憐娭悢偺僆乕僶乕儔僀僪傪惗惉偟傑偡
    //{{AFX_VIRTUAL(CAboutDlg)
    protected:
    virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV 偺僒億乕僩
    //}}AFX_VIRTUAL

// 僀儞僾儕儊儞僥乕僔儑儞
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)
        // 儊僢僙乕僕 僴儞僪儔偑偁傝傑偣傫丅
    //}}AFX_MSG_MAP
END_MESSAGE_MAP()

/**//////////////////////////////////////////////////////////////////////////////
// CTestDlg 僟僀傾儘僌

CTestDlg::CTestDlg(CWnd* pParent /**//*=NULL*/)
    : CDialog(CTestDlg::IDD, pParent)
{
    //{{AFX_DATA_INIT(CTestDlg)
    m_distance = 5.0;
    m_vel = 0.0;
    m_PosB = 0.0;
    m_PosL = 0.0;
    m_PosR = 0.0;
    m_PosS = 0.0;
    m_PosT = 0.0;
    m_PosU = 0.0;
    //}}AFX_DATA_INIT
    // 儊儌: LoadIcon 偼 Win32 偺 DestroyIcon 偺僒僽僔乕働儞僗傪梫媮偟傑偣傫丅
    m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CTestDlg::DoDataExchange(CDataExchange* pDX)
{
    CDialog::DoDataExchange(pDX);
    //{{AFX_DATA_MAP(CTestDlg)
    DDX_Control(pDX, IDC_JOBNAME, m_jobname);
    DDX_Text(pDX, IDC_EDIT_DIST, m_distance);
    DDX_Text(pDX, IDC_EDIT_VEL, m_vel);
    DDX_Text(pDX, IDC_POS_B, m_PosB);
    DDX_Text(pDX, IDC_POS_L, m_PosL);
    DDX_Text(pDX, IDC_POS_R, m_PosR);
    DDX_Text(pDX, IDC_POS_S, m_PosS);
    DDX_Text(pDX, IDC_POS_T, m_PosT);
    DDX_Text(pDX, IDC_POS_U, m_PosU);
    //}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CTestDlg, CDialog)
    //{{AFX_MSG_MAP(CTestDlg)
    ON_WM_SYSCOMMAND()
    ON_WM_PAINT()
    ON_WM_QUERYDRAGICON()
    ON_BN_CLICKED(IDC_UPLOAD, OnUpload)
    ON_BN_CLICKED(IDC_DOWNLOAD, OnDownload)
    ON_BN_CLICKED(IDC_BUTTON_STATUS, OnButtonStatus)
    ON_BN_CLICKED(IDC_BUTTON_BscImov, OnBUTTONBscImov)
    ON_EN_KILLFOCUS(IDC_EDIT_DIST, OnKillfocusEditDist)
    ON_EN_KILLFOCUS(IDC_EDIT_VEL, OnKillfocusEditVel)
    ON_BN_CLICKED(IDC_RADIO_TYPEV, OnRadioTypev)
    ON_BN_CLICKED(IDC_RADIO_TYPEVR, OnRadioTypevr)
    ON_BN_CLICKED(IDC_BUTTON_BscMovj, OnBUTTONBscMovj)
    ON_BN_CLICKED(IDC_BUTTON_BscPMovj, OnBUTTONBscPMovj)
    ON_BN_CLICKED(IDC_BUTTON_ZERO, OnButtonZero)
    ON_BN_CLICKED(IDC_BUTTON_ServOff, OnBUTTONServOff)
    ON_BN_CLICKED(IDC_BUTTON_ServOn, OnBUTTONServOn)
    ON_WM_DESTROY()
    ON_BN_CLICKED(IDC_BUTTON_ZHIJIAO, OnButtonZhijiao)
    ON_BN_CLICKED(IDC_BUTTON_JOINT, OnButtonJoint)
    //}}AFX_MSG_MAP
END_MESSAGE_MAP()

/**//////////////////////////////////////////////////////////////////////////////
// CTestDlg 儊僢僙乕僕 僴儞僪儔

BOOL CTestDlg::OnInitDialog()
{
    CDialog::OnInitDialog();

    // "僶乕僕儑儞忣曬" 儊僯儏乕崁栚傪僔僗僥儉 儊僯儏乕傊捛壛偟傑偡丅

    // IDM_ABOUTBOX 偼僐儅儞僪 儊僯儏乕偺斖埻偱側偗傟偽側傝傑偣傫丅
    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);
        }
    }

    // 偙偺僟僀傾儘僌梡偺傾僀僐儞傪愝掕偟傑偡丅僼儗乕儉儚乕僋偼傾僾儕働乕僔儑儞偺儊僀儞
    // 僂傿儞僪僂偑僟僀傾儘僌偱側偄帪偼帺摦揑偵愝掕偟傑偣傫丅
    SetIcon(m_hIcon, TRUE);            // 戝偒偄傾僀僐儞傪愝掕
    SetIcon(m_hIcon, FALSE);        // 彫偝偄傾僀僐儞傪愝掕
    
    // TODO: 摿暿側弶婜壔傪峴偆帪偼偙偺応強偵捛壛偟偰偔偩偝偄丅
//    g_nCid = TestOpenComm ( TEST_TRANS_MODE );
    if(g_nCid<0)
    {
        AfxMessageBox("Error open com");
        return FALSE;
    }
    else
        AfxMessageBox("Open OK");
    return TRUE;  // TRUE 傪曉偡偲僐儞僩儘乕儖偵愝掕偟偨僼僅乕僇僗偼幐傢傟傑偣傫丅
}

void CTestDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
    if ((nID & 0xFFF0) == IDM_ABOUTBOX)
    {
        CAboutDlg dlgAbout;
        dlgAbout.DoModal();
    }
    else
    {
        CDialog::OnSysCommand(nID, lParam);
    }
}

// 傕偟僟僀傾儘僌儃僢僋僗偵嵟彫壔儃僞儞傪捛壛偡傞側傜偽丄傾僀僐儞傪昤夋偡傞
// 僐乕僪傪埲壓偵婰弎偡傞昁梫偑偁傝傑偡丅MFC 傾僾儕働乕僔儑儞偼 document/view
// 儌僨儖傪巊偭偰偄傞偺偱丄偙偺張棟偼僼儗乕儉儚乕僋偵傛傝帺摦揑偵張棟偝傟傑偡丅

void CTestDlg::OnPaint() 
{
    if (IsIconic())
    {
        CPaintDC dc(this); // 昤夋梡偺僨僶僀僗 僐儞僥僉僗僩

        SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);

        // 僋儔僀傾儞僩偺嬮宍椞堟撪偺拞墰
        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;

        // 傾僀僐儞傪昤夋偟傑偡丅
        dc.DrawIcon(x, y, m_hIcon);
    }
    else
    {
        CDialog::OnPaint();
    }
}

// 僔僗僥儉偼丄儐乕僓乕偑嵟彫壔僂傿儞僪僂傪僪儔僢僌偟偰偄傞娫丄
// 僇乕僜儖傪昞帵偡傞偨傔偵偙偙傪屇傃弌偟傑偡丅
HCURSOR CTestDlg::OnQueryDragIcon()
{
    return (HCURSOR) m_hIcon;
}

// MOTOCOM32: 偙偺峴埲壓傪捛壛 -------->
short CTestDlg::TestOpenComm( int mode )    // mode = 0:RS-232C or 1:Ethernet
{
    // MOTOCOM32: Motohelp.hlp Visual C++ 偱偺嶌惉曽朄 傛傝 -------->
    short ncid;
    short rc;
    char cur_dir[_MAX_DIR];             // 幚峴娐嫬偵墳偠偰曄峏偟偰壓偝偄丅
    char *IPAddress= TEST_IP_ADDRESS;   // 擟堄偺IP蹦挹綍稁殫駚@幚峴娐嫬偵墳偠偰曄峏偟偰壓偝偄丅

    _getcwd( cur_dir, sizeof(cur_dir) );
    if( mode==0 )
    {
        // COM億乕僩傪僆乕僾儞偟傑偡丅
        ncid = BscOpen( cur_dir, PACKETCOM );
        if( ncid < 0 )
        {
            return( ncid );
        }

        // 僔儕傾儖捠怣偺僷儔儊乕僞傪愝掕偟傑偡丅
        rc = BscSetCom( ncid, 1, 9600, 2, 8, 0 );
        if( rc != 1 )
        {
            rc = BscClose( ncid );
            return( -1 );
        }

    } else
    {
        // Ethernet夞慄傪僆乕僾儞偟傑偡丅
        ncid = BscOpen( cur_dir, PACKETETHERNET );
        if( ncid < 0 )
        {
            return( ncid );
        }

        // Ethernet捠怣偺僷儔儊乕僞傪愝掕偟傑偡丅
        rc = BscSetEther( ncid, IPAddress, 0, GetSafeHwnd() );
        if( rc != 1 )
        {
            rc = BscClose( ncid );
            return( -1 );
        }
    }
    // 捠怣夞慄傪愙懕偟傑偡丅
    rc = BscConnect( ncid );
    if( rc != 1 )
    {
        rc = BscClose( ncid );
        return( -1 );
    }

    return( ncid ); // <-------- MOTOCOM32: Motohelp.hlp Visual C++ 偱偺嶌惉曽朄 傛傝
}

short CTestDlg::TestCloseComm( short ncid )
{
    // MOTOCOM32: Motohelp.hlp Visual C++ 偱偺嶌惉曽朄 傛傝 -------->
    short rc;
  
    // 捠怣夞慄傪愗抐偟傑偡丅
    rc = BscDisConnect( ncid );

    // 億乕僩傪僋儘乕僘偟傑偡丅
    rc = BscClose( ncid );

    return( rc );   // <-------- MOTOCOM32: Motohelp.hlp Visual C++ 偱偺嶌惉曽朄 傛傝
}
// <------ MOTOCOM32: 捛壛 

// MOTOCOM32: 偙偺峴埲壓偼丄ClassWizard 偵偰捛壛 --------> 
void CTestDlg::OnDownload() 
{
    // MOTOCOM32: Motohelp.hlp Visual C++ 偱偺嶌惉曽朄 傛傝捛壛 -------->
    short  g_nCid;
    short  rc;
    CString  JobName;

    // 僄僨傿僢僩僐儞僩儘乕儖偐傜僕儑僽柤傪妉摼偟傑偡丏
    m_jobname.GetWindowText(JobName);
    JobName.MakeUpper();

    // 僕儑僽柤偑巜掕偝傟偰偄傞偐僠僃僢僋偟傑偡丏
    if(!JobName.IsEmpty() )
    {
        // 捠怣僴儞僪儔傪妉摼偟傑偡丏   // 伀幚峴娐嫬偵墳偠偰曄峏偟偰壓偝偄丅
//        g_nCid = TestOpenComm ( TEST_TRANS_MODE );    // mode = 0:RS-232C or 1:Ethernet
        // 捠怣僴儞僪儔傪妉摼偱偒偨偐僠僃僢僋偟傑偡丏

        if( g_nCid >= 0 )
        {
            // 僕儑僽傪憲怣偟傑偡丏
            rc = BscDownLoad( g_nCid, (char*)(LPCSTR)JobName);
            // 栠傝抣傪妋擣偟傑偡丏
            if( rc == 0 )
                AfxMessageBox ( "ok" );
            else
                AfxMessageBox ( "BscDownLoad fail" );

            // 捠怣僴儞僪儔傪夝曻偟傑偡丏
//            rc = TestCloseComm( g_nCid );
        }
        else
            AfxMessageBox ("TestOpenComm fail" );
    }
    else
    {
        AfxMessageBox ( "JobName.IsEmpty" );
    }
 // <-------- MOTOCOM32: Motohelp.hlp Visual C++ 偱偺嶌惉曽朄 傛傝捛壛
}

void CTestDlg::OnUpload() 
{
    // MOTOCOM32: Motohelp.hlp Visual C++ 偱偺嶌惉曽朄 傛傝捛壛 -------->
    short   g_nCid;
    short   rc;
    CString JobName;

    // 僄僨傿僢僩僐儞僩儘乕儖偐傜僕儑僽柤傪妉摼偟傑偡丏
    m_jobname.GetWindowText(JobName);
    JobName.MakeUpper();
    
    // 僕儑僽柤偑巜掕偝傟偰偄傞偐僠僃僢僋偟傑偡丏
    if(!JobName.IsEmpty() )
    {
        // 捠怣僴儞僪儔傪妉摼偟傑偡丏   // 伀幚峴娐嫬偵墳偠偰曄峏偟偰壓偝偄丅
//        g_nCid = TestOpenComm ( TEST_TRANS_MODE );    // mode = 0:RS-232C or 1:Ethernet
        // 捠怣僴儞僪儔傪妉摼偱偒偨偐僠僃僢僋偟傑偡丏

        if( g_nCid >= 0 )
        {
            // 僕儑僽傪庴怣偟傑偡丏
            rc = BscUpLoad(g_nCid, (char*)(LPCSTR)JobName);
            // 栠傝抣傪妋擣偟傑偡丏
            if( rc == 0 )
                AfxMessageBox ( "僕儑僽庴怣姰椆" );
            else
                AfxMessageBox("僕儑僽庴怣幐攕" );

            // 捠怣僴儞僪儔傪夝曻偟傑偡丏
//            rc = TestCloseComm( g_nCid );

        }
        else
            AfxMessageBox ( "億乕僩傪僆乕僾儞偱偒傑偣傫丏" );
    }
    else
    {
        AfxMessageBox ( "僕儑僽柤傪巜掕偟偰壓偝偄丏" );
    }
    // <-------- MOTOCOM32: Motohelp.hlp Visual C++ 偱偺嶌惉曽朄 傛傝捛壛
}
// <------ MOTOCOM32: 捛壛 

#define CHECK_VOID(x)  {if(!(x)){char s[1000];sprintf(s,"Error %s",#x);AfxMessageBox(s);return;}}
void CTestDlg::OnButtonStatus() 
{
    // TODO: Add your control notification handler code here
    DWORD tm1=GetTickCount();

    short g_nCid = TestOpenComm ( TEST_TRANS_MODE );    // mode = 0:RS-232C or 1:Ethernet
    
    CString s;

    if( g_nCid < 0 )
    {
        AfxMessageBox("open error");return;
    }
    
    {
        char szPos[2000];    WORD toolno;
                              
        WORD rconf;double p[12];double p1[12];
        CHECK_VOID(BscIsLoc(g_nCid,1,&rconf,p)==0);
    //    CHECK_VOID(BscIsLoc(g_nCid,0,&rconf,p1)==0);
            CHECK_VOID(BscIsRobotPos(g_nCid,"BASE",0,&rconf,&toolno,p1)==0);
    
        sprintf(szPos,"----PulseNO.S %g,L %g,U %g,R %g,B %g,T %g\r\n"
            "----CartPos.S %g,L %g,U %g,R %g,B %g,T %g\r\n",
            p[0],p[1],p[2],p[3],p[4],p[5],
            p1[0],p1[1],p1[2],p1[3],p1[4],p1[5]
            );
        s=szPos;
    }
    
    {
        WORD d1;WORD d2;
        char szStatus[2000];
        CHECK_VOID(BscGetStatus(g_nCid,&d1,&d2)==0);
        sprintf(szStatus,"---Status:%s %s %s %s %s %s %s %s "
            "%s %s %s %s %s %s %s \r\n ",
            d1&(1<<0)?"Step\r\n":"",
            d1&(1<<1)?"1-cycle\r\n":"",
            d1&(1<<2)?"Auto operation\r\n":"",
            
            d1&(1<<3)?"Operating\r\n":"",                //??
            d1&(1<<4)?"Operation at safe speed\r\n":"", //???
            
            d1&(1<<5)?"Teach\r\n":"",
            d1&(1<<6)?"Play\r\n":"",
            d1&(1<<7)?"Command remote \r\n":"",
            
            d2&(1<<0)?"Playback box hold\r\n":"",
            d2&(1<<1)?"Programming pendant hold\r\n":"",
            d2&(1<<2)?"External hold\r\n":"",
            d2&(1<<3)?"Command hold\r\n":"",
            
            d2&(1<<4)?"Alarm occurred\r\n":"",
            d2&(1<<5)?"Error occurred\r\n":"",
            
            d2&(1<<6)?"Servo ON\r\n":"Servo OFF\r\n"
            );
        s+=szStatus;
    }
    
    {
        char jobname[250];
        char szJob[2000];
        CHECK_VOID(BscIsJobName(g_nCid,jobname,250)==0);
        short jobLine=0;
        jobLine= BscIsJobLine(g_nCid);
        CHECK_VOID(jobLine!=-1);
        
        short jobStep=0;
        jobStep= BscIsJobStep(g_nCid);
        CHECK_VOID(jobStep!=-1);
        
        sprintf(szJob,"---Job:%s line %d jobStep:%d\r\n",
            jobname,
            jobLine,
            jobStep
            );
        s+=szJob;
    }

    
#define N_COR 2
    {
        char Pos[N_COR][250];
        char framename[10][250]={"BASE","ROBOT","UF1"};
        WORD toolno;
        WORD rconf;double p[12];
        for(int i=0;i<N_COR;i++)
        {
            (BscIsRobotPos(g_nCid,framename[i],0,&rconf,&toolno,p));
            sprintf(Pos[i],"---Frame(%s): S %g,L %g,U %g,R %g,B %g,T %g\r\n",
                framename[i],
                p[0],p[1],p[2],p[3],p[4],p[5]
                );
            s=s+Pos[i];
        }    
    }

    {
        CTime tm=CTime::GetCurrentTime();
        char szTm[200] ;
        sprintf(szTm,"Time:%d/%d %d:%d:%d\r\n",
            tm.GetMonth(),tm.GetDay(),tm.GetHour(),tm.GetMinute(),tm.GetSecond());
        s+=szTm;
    }
_EXIT:
    //TestCloseComm( g_nCid );
    {
        char szTm[200];
        sprintf(szTm,"UsingTm %d ms\r\n",GetTickCount()-tm1);
        s+=szTm;
    }
    SetDlgItemText(IDC_EDIT1,s);
    

}

void CTestDlg::OnBUTTONBscImov() 
{
    // TODO: Add your control notification handler code here
    UpdateData(TRUE);
    //short g_nCid = TestOpenComm ( TEST_TRANS_MODE );    // mode = 0:RS-232C or 1:Ethernet
    CString s; char PosPreImov [12000];
   char PosPostImov[120000];
    if( g_nCid < 0 )
    {
        AfxMessageBox("open error");return;
    }
    
    double spd=2.0;
 toolno=0;
    double p[12]={0,0,0,0,0,0,0,0,0,0,0,0};
    double p2[12]={0};
    double p1[12]={0,0,0,0,0,0,0,0,0,0,0,0};
    CHECK_VOID(BscIsLoc(g_nCid,1,&rconf,p2)==0);
    CHECK_VOID(BscIsRobotPos(g_nCid,"BASE",0,&rconf,&toolno,p1)==0);
    
        sprintf(PosPreImov,"----PosPreImov PulseNO.S %g,L %g,U %g,R %g,B %g,T %g\r\n"
            "----CartPos.S %g,L %g,U %g,R %g,B %g,T %g\r\n",
            p2[0],p2[1],p2[2],p2[3],p2[4],p2[5],
            p1[0],p1[1],p1[2],p1[3],p1[4],p1[5]
            );
        s=PosPreImov;
    int nIdAxis=GetCheckedRadioButton(IDC_RADIO_AXIS1,IDC_RADIO_AXIS6)-IDC_RADIO_AXIS1;
    int nIdType=GetCheckedRadioButton(IDC_RADIO_TYPEV,IDC_RADIO_TYPEVR)-IDC_RADIO_TYPEV;
    CHECK_VOID(nIdAxis>=0 && nIdAxis<6);
    CHECK_VOID(nIdType>=0 && nIdType<2);

    {
#if 0
        char szDist[100];
        double dwDist;
        GetDlgItemText(IDC_EDIT_DIST,szDist,100);
        dwDist=atof(szDist);
        if(fabs(dwDist)>360)
        {
            AfxMessageBox("too large");
            GetDlgItem(IDC_EDIT_DIST)->SetFocus();
        }
        
        char szVel[100];
        GetDlgItemText(IDC_EDIT_VEL,szVel,100);
        double dwVel=atof(szVel);
        if(fabs(dwVel)>360)
        {
            AfxMessageBox("too large");
            GetDlgItem(IDC_EDIT_VEL)->SetFocus();
        }
        
        p[nIdAxis]=dwDist;
#else
        p[nIdAxis]=m_distance;
        double dwVel=m_vel;
#endif
        CHECK_VOID(BscImov(g_nCid,nIdType==0?"V":"VR",dwVel,"BASE",toolno,p)==0);
        
    }
_EXIT:;
     //TestCloseComm( g_nCid );
     UpdateData(FALSE);
         CHECK_VOID(BscIsLoc(g_nCid,1,&rconf,p)==0);
    CHECK_VOID(BscIsRobotPos(g_nCid,"BASE",0,&rconf,&toolno,p1)==0);

        sprintf(PosPostImov,"----PosPostImov PulseNO.S %g,L %g,U %g,R %g,B %g,T %g\r\n"
            "----CartPos.S %g,L %g,U %g,R %g,B %g,T %g\r\n",
            p[0],p[1],p[2],p[3],p[4],p[5],
            p1[0],p1[1],p1[2],p1[3],p1[4],p1[5]
            );
        s+=PosPostImov;
     SetDlgItemText(IDC_EDIT1,s);
     
     
}

void CTestDlg::OnKillfocusEditDist() 
{
    // TODO: Add your control notification handler code here
//    char szDist[100];
//    GetDlgItemText(IDC_EDIT_DIST,szDist,100);
//    double dwDist=atof(szDist);
//    if(fabs(dwDist)>360)
//    {
//        AfxMessageBox("too large");
//        GetDlgItem(IDC_EDIT_DIST)->SetFocus();
//    }
}

void CTestDlg::OnKillfocusEditVel() 
{
    // TODO: Add your control notification handler code here
//    char szVel[100];
//    GetDlgItemText(IDC_EDIT_VEL,szVel,100);
//    double dwDist=atof(szVel);
//    if(fabs(dwDist)>360)
//    {
//        AfxMessageBox("too large");
//        GetDlgItem(IDC_EDIT_VEL)->SetFocus();
//    }
}

void CTestDlg::OnRadioTypev() 
{
    SetDlgItemText(IDC_EDIT_VEL,"10");
    SetDlgItemText(IDC_EDIT_DIST,"10");
}

//angle should be small
void CTestDlg::OnRadioTypevr() 
{
    SetDlgItemText(IDC_EDIT_VEL,"0.1");
    SetDlgItemText(IDC_EDIT_DIST,"5");
}




void CTestDlg::OnBUTTONBscMovj() 
{
    // TODO: Add your control notification handler code here
    UpdateData(TRUE);
    CString s;
    DWORD t=GetTickCount();
    char szText[200]="\0";
    //short g_nCid = TestOpenComm ( TEST_TRANS_MODE );    // mode = 0:RS-232C or 1:Ethernet
    
    if( g_nCid < 0 )
    {
        AfxMessageBox("open error");return;
    }
    
    double spd=2.0;
    short toolno=0;
    double p[12]={0,0,0,0,0,0,0,0,0,0,0,0};
    WORD rconf;
    
    
    int nIdAxis=GetCheckedRadioButton(IDC_RADIO_AXIS1,IDC_RADIO_AXIS6)-IDC_RADIO_AXIS1;
    int nIdType=GetCheckedRadioButton(IDC_RADIO_TYPEV,IDC_RADIO_TYPEVR)-IDC_RADIO_TYPEV;
    CHECK_VOID(nIdAxis>=0 && nIdAxis<6);
    CHECK_VOID(nIdType>=0 && nIdType<2);
    CHECK_VOID(BscIsLoc(g_nCid,0,&rconf,p)==0);
    sprintf(szText,
        "----CartPos.S %g,L %g,U %g,R %g,B %g,T %g\r\n",
        p[0],p[1],p[2],p[3],p[4],p[5]
        );
    s+=szText;
    
    {
#if 0
        char szDist[100];
        double dwDist;
        GetDlgItemText(IDC_EDIT_DIST,szDist,100);
        dwDist=atof(szDist);
        if(fabs(dwDist)>360)
        {
            AfxMessageBox("too large");
            GetDlgItem(IDC_EDIT_DIST)->SetFocus();
        }
        
        char szVel[100];
        GetDlgItemText(IDC_EDIT_VEL,szVel,100);
        double dwVel=atof(szVel);
        if(fabs(dwVel)>360)
        {
            AfxMessageBox("too large");
            GetDlgItem(IDC_EDIT_VEL)->SetFocus();
        }
        
        p[nIdAxis]=dwDist;
#else
        p[nIdAxis]+=m_distance;
        double dwVel=m_vel;
#endif
        //CHECK_VOID(BscImov(g_nCid,nIdType==0?"V":"VR",dwVel,"BASE",toolno,p)==0);
        CHECK_VOID(BscMovj(g_nCid,dwVel,"BASE",rconf,toolno, p)==0);
    }
_EXIT:;
      sprintf(szText,"Using Time:%dms\r\n",GetTickCount()-t);
      s+=szText;
      SetDlgItemText(IDC_EDIT1,s);
     //TestCloseComm( g_nCid );
     UpdateData(FALSE);
}

/**//************************************************************************/
/**//*移动关节，关节坐标系，输入为绝对的各个关节脉冲数目
/************************************************************************/
void CTestDlg::OnBUTTONBscPMovj() 
{
    // TODO: Add your control notification handler code here
    UpdateData(TRUE);

    DWORD t=GetTickCount();
    char szText[200]="\0";
    //short g_nCid = TestOpenComm ( TEST_TRANS_MODE );    // mode = 0:RS-232C or 1:Ethernet
    
    if( g_nCid < 0 )
    {
        AfxMessageBox("open error");return;
    }
    
    double spd=2.0;
    double p[12]={0,0,0,0,0,0,0,0,0,0,0,0};
    double p2[12]={0};
    double p1[12]={0,0,0,0,0,0,0,0,0,0,0,0};
    CString s; char PosPreBscPMovj [12000];
    char PosPostBscPMovj[120000];
     toolno=0;

    CHECK_VOID(BscIsLoc(g_nCid,1,&rconf,p2)==0);
    CHECK_VOID(BscIsRobotPos(g_nCid,"BASE",0,&rconf,&toolno,p1)==0);
    
        sprintf(PosPreBscPMovj,"----PosPreBscPMovj PulseNO.S %g,L %g,U %g,R %g,B %g,T %g\r\n"
            "----CartPos.S %g,L %g,U %g,R %g,B %g,T %g\r\n",
            p2[0],p2[1],p2[2],p2[3],p2[4],p2[5],
            p1[0],p1[1],p1[2],p1[3],p1[4],p1[5]
            );
        s=PosPreBscPMovj;
    
    int nIdAxis=GetCheckedRadioButton(IDC_RADIO_AXIS1,IDC_RADIO_AXIS6)-IDC_RADIO_AXIS1;
    int nIdType=GetCheckedRadioButton(IDC_RADIO_TYPEV,IDC_RADIO_TYPEVR)-IDC_RADIO_TYPEV;
    CHECK_VOID(nIdAxis>=0 && nIdAxis<6);
    CHECK_VOID(nIdType>=0 && nIdType<2);
    CHECK_VOID(BscIsLoc(g_nCid,1,&rconf,p)==0);
    sprintf(szText,
        "----PulsePos.S %g,L %g,U %g,R %g,B %g,T %g\r\n",
        p[0],p[1],p[2],p[3],p[4],p[5]
        );
    s+=szText;
    
    {
        p[nIdAxis]+=m_distance;
        double dwVel=m_vel;
        //CHECK_VOID(BscImov(g_nCid,nIdType==0?"V":"VR",dwVel,"BASE",toolno,p)==0);
        CHECK_VOID(BscPMovj(g_nCid,dwVel,toolno, p)==0);
    }
_EXIT:;
          CHECK_VOID(BscIsLoc(g_nCid,1,&rconf,p)==0);
    CHECK_VOID(BscIsRobotPos(g_nCid,"BASE",0,&rconf,&toolno,p1)==0);

        sprintf(PosPostBscPMovj,"----PosPostBscPMovj PulseNO.S %g,L %g,U %g,R %g,B %g,T %g\r\n"
            "----CartPos.S %g,L %g,U %g,R %g,B %g,T %g\r\n",
            p[0],p[1],p[2],p[3],p[4],p[5],
            p1[0],p1[1],p1[2],p1[3],p1[4],p1[5]
            );
        s+=PosPostBscPMovj;
      sprintf(szText,"Using Time:%dms\r\n",GetTickCount()-t);
      s+=szText;
      SetDlgItemText(IDC_EDIT1,s);
     //TestCloseComm( g_nCid );
     UpdateData(FALSE);
}

void CTestDlg::OnButtonZero() 
{
    // TODO: Add your control notification handler code here
    UpdateData(TRUE);
    CString s;
    DWORD t=GetTickCount();
    char szText[200]="\0";
    //short g_nCid = TestOpenComm ( TEST_TRANS_MODE );    // mode = 0:RS-232C or 1:Ethernet
    
    if( g_nCid < 0 )
    {
        AfxMessageBox("open error");return;
    }
    
    double spd=2.0;
    short toolno=0;
    double p[12]={0,0,0,0,0,0,0,0,0,0,0,0};
    WORD rconf;
    
    {
        double dwVel=m_vel;
        //CHECK_VOID(BscImov(g_nCid,nIdType==0?"V":"VR",dwVel,"BASE",toolno,p)==0);
        CHECK_VOID(BscPMovj(g_nCid,dwVel,toolno, p)==0);
        //CHECK_VOID(0==BscPMovj(g_nCid,dwVel,toolno, p));
    }

    CHECK_VOID(BscIsLoc(g_nCid,1,&rconf,p)==0);
    sprintf(szText,
        "----PulsePos.S %g,L %g,U %g,R %g,B %g,T %g\r\n",
        p[0],p[1],p[2],p[3],p[4],p[5]
        );
    s+=szText;
_EXIT:;

      sprintf(szText,"Using Time:%dms\r\n",GetTickCount()-t);
      s+=szText;
      SetDlgItemText(IDC_EDIT1,s);
     //TestCloseComm( g_nCid );
     UpdateData(FALSE);
}

void CTestDlg::OnBUTTONServOff() 
{
    // TODO: Add your control notification handler code here
    
    CString s;
    DWORD t=GetTickCount();
    char szText[200]="\0";
    //short g_nCid = TestOpenComm ( TEST_TRANS_MODE );    // mode = 0:RS-232C or 1:Ethernet
    
    if( g_nCid < 0 )
    {
        AfxMessageBox("open error");return;
    }
    
    

    CHECK_VOID(BscServoOff(g_nCid)==0);
    
_EXIT:;
      //TestCloseComm( g_nCid );
}

void CTestDlg::OnBUTTONServOn() 
{
    // TODO: Add your control notification handler code here
    CString s;
    DWORD t=GetTickCount();
    char szText[200]="\0";
//short g_nCid = TestOpenComm ( TEST_TRANS_MODE );    // mode = 0:RS-232C or 1:Ethernet
    
    if( g_nCid < 0 )
    {
        AfxMessageBox("open error");return;
    }
    
    

    CHECK_VOID(BscServoOn(g_nCid)==0);
    
_EXIT:;
     // TestCloseComm( g_nCid );
}

BOOL CTestDlg::DestroyWindow() 
{
    // TODO: Add your specialized code here and/or call the base class
    
    return CDialog::DestroyWindow();
}

void CTestDlg::OnDestroy() 
{
    CDialog::OnDestroy();
    
    // TODO: Add your message handler code here
    TestCloseComm( g_nCid );
}


void CTestDlg::OnButtonZhijiao() 
{
    // TODO: Add your control notification handler code here
    UpdateData(true);
        if( g_nCid < 0 )
    {
        AfxMessageBox("open error");return;
    }
    
 toolno=0;
    double p[12]={0,0,0,0,0,0,0,0,0,0,0,0};
 //    WORD rconf;
    double s,l,u,r,b,t=0.0;
    p[0]=m_PosS;
    p[1]=m_PosL;
    p[2]=m_PosU;
    p[3]=m_PosR;
    p[4]=m_PosB;
    p[5]=m_PosT;
    double dwVel=m_vel;                        
//    CHECK_VOID(BscMov(g_nCid,dwVel,"BASE",rconf,toolno, p)==0);
//    CHECK_VOID(BscImov(g_nCid,"V",dwVel,"ROBOT",toolno,p)==0);
        CHECK_VOID(BscMovj(g_nCid,dwVel,"BASE",rconf,toolno, p)==0);

_EXIT:;
      
          //TestCloseComm( g_nCid );
     UpdateData(FALSE);
    
}

void CTestDlg::OnButtonJoint() 
{
    // TODO: Add your control notification handler code here
    UpdateData(true);
        if( g_nCid < 0 )
    {
        AfxMessageBox("open error");return;
    }
    
//    short toolno=0;
    double p[12]={0,0,0,0,0,0,0,0,0,0,0,0};
     WORD rconf;
    double s,l,u,r,b,t=0.0;
    p[0]=m_PosS;
    p[1]=m_PosL;
    p[2]=m_PosU;
    p[3]=m_PosR;
    p[4]=m_PosB;
    p[5]=m_PosT;
    double dwVel=m_vel;
    CHECK_VOID(BscPMovj(g_nCid,dwVel,toolno, p)==0);
    _EXIT:;
      
          //TestCloseComm( g_nCid );
     UpdateData(FALSE);
}