page2.cpp

机器人运动学动力学及控制的一个小程序,供有兴趣的人参考

// Page2.cpp : implementation file
//

#include "stdafx.h"
#include "MySDOpenGL.h"
#include "Page2.h"
#include "MySDOpenGLDoc.h"


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

//BOOL gravity;
/////////////////////////////////////////////////////////////////////////////
// CPage2 dialog


CPage2::CPage2(CWnd* pParent /*=NULL*/)
	: CDialog(CPage2::IDD, pParent)
{
	//{{AFX_DATA_INIT(CPage2)
	m_gravity = TRUE;
	m_GRA = _T("有重力");
	m_g = 9.8;
	m_beata = 1000000.0;
	m_friction = FALSE;
	m_theta1 = 0.0;
	m_Kp = 1.0;
	m_Kv = 1.0;
	m_theta2 = 0.0;
	m_theta3 = 0.0;
	m_theta4 = 0.0;
	m_control = FALSE;
	m_tip = _T(" 按[开始仿真]  可进行动力学仿真");
	//}}AFX_DATA_INIT


}


void CPage2::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CPage2)
	DDX_Control(pDX, IDC_BUTTON1, m_fz);
	DDX_Check(pDX, IDC_CHECK1, m_gravity);
	DDX_Text(pDX, IDC_GRAVITY, m_GRA);
	DDX_Text(pDX, IDC_EDIT1, m_g);
	DDX_Text(pDX, IDC_EDIT2, m_beata);
	DDX_Check(pDX, IDC_CHECK2, m_friction);
	DDX_Text(pDX, IDC_EDIT5, m_theta1);
	DDX_Text(pDX, IDC_EDIT3, m_Kp);
	DDX_Text(pDX, IDC_EDIT4, m_Kv);
	DDX_Text(pDX, IDC_EDIT6, m_theta2);
	DDX_Text(pDX, IDC_EDIT7, m_theta3);
	DDX_Text(pDX, IDC_EDIT8, m_theta4);
	DDX_Check(pDX, IDC_CHECK3, m_control);
	DDX_Text(pDX, IDC_TIP, m_tip);
	//}}AFX_DATA_MAP
}


BEGIN_MESSAGE_MAP(CPage2, CDialog)
	//{{AFX_MSG_MAP(CPage2)
	ON_BN_CLICKED(IDC_CHECK1, OnGravity)
	ON_BN_CLICKED(IDC_BUTTON1, OnButton1)
	ON_BN_CLICKED(IDC_BUTTON2, OnButton2)
	ON_BN_CLICKED(IDC_CHECK2, OnFriction)
	ON_BN_CLICKED(IDC_CHECK3, OnControl)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CPage2 message handlers

void CPage2::OnGravity() 
{
	// TODO: Add your control notification handler code here
//	CMySDOpenGLDoc *temp=(CMySDOpenGLDoc *)pDoc;
//	m_GRA.
	if(m_gravity)
	{(GetDlgItem(IDC_GRAVITY))->EnableWindow(FALSE);
      (GetDlgItem(IDC_EDIT1))->EnableWindow(FALSE);
     (GetDlgItem(IDC_g))->EnableWindow(FALSE);
	  m_gravity=FALSE;
	  m_tip=" 无重力环境 ";
	}
	else
    {(GetDlgItem(IDC_GRAVITY))->EnableWindow(TRUE);
	 (GetDlgItem(IDC_EDIT1))->EnableWindow(TRUE);
     (GetDlgItem(IDC_g))->EnableWindow(TRUE);
	  m_gravity=TRUE; 
     m_tip=" 有重力环境 ";
	}
  UpdateData(FALSE);
}

void CPage2::OnButton1() 
{
	// TODO: Add your control notification handler code here
	CMySDOpenGLDoc *temp=(CMySDOpenGLDoc *)pDoc;
	UpdateData(TRUE);
	if(m_gravity)
       temp->lpTransport->gravity=m_g;
	else
       temp->lpTransport->gravity=0;

    temp->lpTransport->beata.MakeUnitMatrix(4);
	if(!m_friction)  m_beata=0;      
	if(m_beata>1000000) m_beata=1000000;
	else if (m_beata<0) m_beata=0;
	temp->lpTransport->beata=temp->lpTransport->beata*m_beata;

	if(m_control){
       temp->lpTransport->Kp.MakeUnitMatrix(4);
       temp->lpTransport->Kv.MakeUnitMatrix(4);
       temp->lpTransport->Kp=temp->lpTransport->Kp*m_Kp;
       temp->lpTransport->Kv=temp->lpTransport->Kv*m_Kv;

       temp->lpTransport->thetad.Init(4,1);
       temp->lpTransport->thetad.SetElement(0,0,m_theta1);
       temp->lpTransport->thetad.SetElement(1,0,m_theta2);
       temp->lpTransport->thetad.SetElement(2,0,m_theta3);
       temp->lpTransport->thetad.SetElement(3,0,m_theta4);
       temp->lpTransport->control=true;
	}
	else
        temp->lpTransport->control=false;

    temp->Dynamics=true;

	m_tip="可进行设置看不同仿真效果并进行控制 ";
    UpdateData(FALSE);
}

void CPage2::OnButton2() 
{
	// TODO: Add your control notification handler code here
		CMySDOpenGLDoc *temp=(CMySDOpenGLDoc *)pDoc;
	UpdateData(TRUE);
//    temp->lpTransport->gravity=0;
//    temp->lpTransport->beata.MakeUnitMatrix(4);
//	temp->lpTransport->beata=temp->lpTransport->beata*beata;
    temp->lpTransport->theta.Init(4,1);
    temp->lpTransport->dtheta.Init(4,1);
    temp->Reset=true;
   
    temp->Dynamics=false;

   m_tip=" 在右边窗口拖动鼠标可浏览场景 ";
   UpdateData(FALSE);
}

void CPage2::OnFriction() 
{
	// TODO: Add your control notification handler code here
		if(m_friction)
	{(GetDlgItem(IDC_FRICTION))->EnableWindow(FALSE);
	 (GetDlgItem(IDC_EDIT2))->EnableWindow(FALSE);
     (GetDlgItem(IDC_f))->EnableWindow(FALSE);
		m_friction=FALSE;
    m_tip=" 无摩擦环境 ";
	}
	else
    {(GetDlgItem(IDC_FRICTION))->EnableWindow(TRUE);
	 (GetDlgItem(IDC_EDIT2))->EnableWindow(TRUE);
     (GetDlgItem(IDC_f))->EnableWindow(TRUE);
	  m_friction=TRUE; 
    m_tip=" 在环境中加入摩擦 ";
	}
UpdateData(FALSE);
}

void CPage2::OnControl() 
{
	// TODO: Add your control notification handler code here
    m_tip=" 调整Kp Kv可使机器人尽快稳定 ";


			if(m_control)
	{(GetDlgItem(IDC_CONTROL))->EnableWindow(FALSE);
      (GetDlgItem(IDC_EDIT3))->EnableWindow(FALSE);
      (GetDlgItem(IDC_EDIT4))->EnableWindow(FALSE);
      (GetDlgItem(IDC_EDIT5))->EnableWindow(FALSE);
      (GetDlgItem(IDC_EDIT6))->EnableWindow(FALSE);
      (GetDlgItem(IDC_EDIT7))->EnableWindow(FALSE);
      (GetDlgItem(IDC_EDIT8))->EnableWindow(FALSE);
      (GetDlgItem(IDC_KP))->EnableWindow(FALSE);
      (GetDlgItem(IDC_KV))->EnableWindow(FALSE);
      (GetDlgItem(IDC_ANGLE))->EnableWindow(FALSE);	  
	  m_control=FALSE;
	}
	else
    {(GetDlgItem(IDC_CONTROL))->EnableWindow(TRUE);
      (GetDlgItem(IDC_EDIT3))->EnableWindow(TRUE);
      (GetDlgItem(IDC_EDIT4))->EnableWindow(TRUE);
      (GetDlgItem(IDC_EDIT5))->EnableWindow(TRUE);
      (GetDlgItem(IDC_EDIT6))->EnableWindow(TRUE);
      (GetDlgItem(IDC_EDIT7))->EnableWindow(TRUE);
      (GetDlgItem(IDC_EDIT8))->EnableWindow(TRUE);
      (GetDlgItem(IDC_KP))->EnableWindow(TRUE);
      (GetDlgItem(IDC_KV))->EnableWindow(TRUE);
      (GetDlgItem(IDC_ANGLE))->EnableWindow(TRUE);	
	  m_control=TRUE; 
	}
    UpdateData(FALSE);
}