📄 compasswp3300view.cpp
字号:
// CompassWP3300View.cpp : implementation of the CCompassWP3300View class
//
#include "stdafx.h"
#include "CompassWP3300.h"
#include "math.h"
#include "CompassWP3300Doc.h"
#include "CompassWP3300View.h"
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif
/////////////////////////////////////////////////////////////////////////////
// CCompassWP3300View
IMPLEMENT_DYNCREATE(CCompassWP3300View, CFormView)
BEGIN_MESSAGE_MAP(CCompassWP3300View, CFormView)
//{{AFX_MSG_MAP(CCompassWP3300View)
ON_WM_TIMER()
ON_BN_CLICKED(IDC_BUTTON_STOP, OnButtonStop)
ON_BN_CLICKED(IDC_BUTTON_START,OnButtonStart)
ON_BN_CLICKED(IDC_BUTTON_SETPARAM, OnButtonSetparam)
ON_BN_CLICKED(IDC_BUTTON_SERIALPORT, OnButtonSerialport)
ON_BN_CLICKED(IDC_BUTTON_SETPARAM2, OnButtonSetXYO)
ON_WM_CANCELMODE()
ON_WM_DESTROY()
//}}AFX_MSG_MAP
ON_MESSAGE(WM_FRAME,OnSonarFrame)
END_MESSAGE_MAP()
/////////////////////////////////////////////////////////////////////////////
// CCompassWP3300View construction/destruction
CCompassWP3300View::CCompassWP3300View()
: CFormView(CCompassWP3300View::IDD)
{
//{{AFX_DATA_INIT(CCompassWP3300View)
m_data5 = 0;
m_data6 = 0;
m_data7 = 0;
m_data8 = 0;
m_data1 = 0;
m_data2 = 0;
m_data3 = 0;
m_data4 = 0;
m_data9 = 0;
m_data10 = 0;
m_data11 = 0;
m_data12 = 0;
m_data13 = 0;
m_data14 = 0;
m_data15 = 0;
m_data16 = 0;
m_serialPort = 0;
m_originO = 0;
m_originx = 0;
m_originy = 0;
m_drawy = 0;
m_drawx = 0;
//}}AFX_DATA_INIT
// TODO: add construction code here
}
CCompassWP3300View::~CCompassWP3300View()
{
}
void CCompassWP3300View::DoDataExchange(CDataExchange* pDX)
{
CFormView::DoDataExchange(pDX);
//{{AFX_DATA_MAP(CCompassWP3300View)
DDX_Text(pDX, IDC_EDIT_5SONAR, m_data5);
DDX_Text(pDX, IDC_EDIT_6SONAR, m_data6);
DDX_Text(pDX, IDC_EDIT_7SONAR, m_data7);
DDX_Text(pDX, IDC_EDIT_8SONAR, m_data8);
DDX_Text(pDX, IDC_EDIT_1SONAR, m_data1);
DDX_Text(pDX, IDC_EDIT_2SONAR, m_data2);
DDX_Text(pDX, IDC_EDIT_3SONAR, m_data3);
DDX_Text(pDX, IDC_EDIT_4SONAR, m_data4);
DDX_Text(pDX, IDC_EDIT_9SONAR, m_data9);
DDX_Text(pDX, IDC_EDIT_10SONAR, m_data10);
DDX_Text(pDX, IDC_EDIT_11SONAR, m_data11);
DDX_Text(pDX, IDC_EDIT_12SONAR, m_data12);
DDX_Text(pDX, IDC_EDIT_13SONAR, m_data13);
DDX_Text(pDX, IDC_EDIT_14SONAR, m_data14);
DDX_Text(pDX, IDC_EDIT_15SONAR, m_data15);
DDX_Text(pDX, IDC_EDIT_16SONAR, m_data16);
DDX_Text(pDX, IDC_EDIT_SERIALPORT, m_serialPort);
DDV_MinMaxUInt(pDX, m_serialPort, 1, 254);
DDX_Text(pDX, IDC_EDIT_origino, m_originO);
DDX_Text(pDX, IDC_EDIT_originx, m_originx);
DDX_Text(pDX, IDC_EDIT_originy, m_originy);
DDX_Text(pDX, IDC_EDIT_y, m_drawy);
DDX_Text(pDX, IDC_EDIT_x, m_drawx);
//}}AFX_DATA_MAP
}
BOOL CCompassWP3300View::PreCreateWindow(CREATESTRUCT& cs)
{
// TODO: Modify the Window class or styles here by modifying
// the CREATESTRUCT cs
return CFormView::PreCreateWindow(cs);
}
void CCompassWP3300View::OnInitialUpdate()
{
CFormView::OnInitialUpdate();
GetParentFrame()->RecalcLayout();
ResizeParentToFit();
CheckRadioButton(IDC_RADIO1,IDC_RADIO4,IDC_RADIO2);
CheckDlgButton(IDC_CHECK5,1);
CheckDlgButton(IDC_CHECK6,1);
CheckDlgButton(IDC_CHECK7,1);
CheckDlgButton(IDC_CHECK8,1);
CheckDlgButton(IDC_CHECK13,1);
CheckDlgButton(IDC_CHECK14,1);
CheckDlgButton(IDC_CHECK15,1);
CheckDlgButton(IDC_CHECK16,1);
m_serialPort= 4;
m_drawx = 450;
m_drawy = 300;
m_dataFile.open("data.txt",ios::in|ios::out|ios::trunc);
m_posFile.open("pos.txt",ios::in|ios::out|ios::trunc);
//电机部分初始化操作，记录编码器初始值
m_acRobAction.SetBoardHandle( &m_sbRobSerBoard );
m_acRobAction.SetLeftMotorChannel(1);
m_acRobAction.SetRightMotorChannel(3);
m_board.SetMsgOnFrame(this->GetSafeHwnd(),WM_FRAME);
m_originRightDis = m_acRobAction.GetRightWheelRealDis();
m_originLeftDis = m_acRobAction.GetLeftWheelRealDis();
m_tread = m_acRobAction.GetTread();
originPoint.x = 0;
originPoint.y = 0;
angle = 0;
drawOriginPoint.x=m_drawx;
drawOriginPoint.y=m_drawy;
if(SetTimer(1,200,NULL) ==0)
MessageBox("set timer 1 failed!");
iDecNum=0;
dDeciRe =0;
dDeciIRe =0;
srand( (unsigned int)time( NULL));
UpdateData(false);
}
/////////////////////////////////////////////////////////////////////////////
// CCompassWP3300View diagnostics
#ifdef _DEBUG
void CCompassWP3300View::AssertValid() const
{
CFormView::AssertValid();
}
void CCompassWP3300View::Dump(CDumpContext& dc) const
{
CFormView::Dump(dc);
}
CCompassWP3300Doc* CCompassWP3300View::GetDocument() // non-debug version is inline
{
ASSERT(m_pDocument->IsKindOf(RUNTIME_CLASS(CCompassWP3300Doc)));
return (CCompassWP3300Doc*)m_pDocument;
}
#endif //_DEBUG
/////////////////////////////////////////////////////////////////////////////
// CCompassWP3300View message handlers
void CCompassWP3300View::OnTimer(UINT nIDEvent)
{
// TODO: Add your message handler code here and/or call default
//更新编码器数值，并计算本体坐标，和方向角 xyO;
if ( nIDEvent == 1 )
{
// CPoint temp;
double xl=0,xr=0,
x=0,y=0,
delta_sata=0;
xr = m_acRobAction.GetRightWheelRealDis() - m_originRightDis; //单位为厘米
xl = m_acRobAction.GetLeftWheelRealDis() - m_originLeftDis;
m_originRightDis = m_acRobAction.GetRightWheelRealDis();
m_originLeftDis = m_acRobAction.GetLeftWheelRealDis();
//在全局坐标系中划出机器人和探测到的障碍物
CDC *pDC=GetDC();
CPen *pOldPen,greenPen( PS_SOLID,1,RGB(0,255,0) ),redPen( PS_SOLID,1,RGB(255,00,0) );
pOldPen= pDC->SelectObject(&greenPen);
//画机器人本体
CPoint temp(drawOriginPoint);
temp.x += (int)originPoint.x;
temp.y -= (int)originPoint.y;
pDC->Ellipse(temp.x-5,temp.y-5,temp.x+5,temp.y +5);
pDC->SelectObject(&redPen);
pDC->MoveTo(temp);
temp.x += (int)( 5*cos(angle+3.1415926/2) );
temp.y -= (int)( 5*sin(angle+3.1415926/2) );
pDC->LineTo( temp );
pDC->SelectObject( pOldPen );
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//论文算法-----------------------------------------------
double delta_D=0,
delta_x=0,delta_y=0; //坐标增量
delta_D = (xr+xl)/2;
delta_sata = (xr-xl)/m_tread;
delta_x = delta_D * cos( angle + delta_sata/2 + 3.1415926/2);
delta_y = delta_D * sin( angle + delta_sata/2 + 3.1415926/2 );
x = originPoint.x + delta_x;
y = originPoint.y + delta_y;
angle = angle + delta_sata ;
originPoint.Offset( x, y ); // 自定义point类中offset为设定x y值
CString str,str1;
str.Format("x:%.2f\ty:%.2f\t%.3f ",x,y,angle*180/3.1415926 );
str1.Format("\tdel x:%.2f\ty:%.2f\t%.3f ",delta_x,delta_y,delta_sata*180/3.1415926 );
m_dataFile<<str<<str1<<endl;
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*
//我的算法--------------------------------------------
//相对前一时刻点的 相对坐标和偏角(单位为厘米)
delta_sata=(xr-xl)/m_tread;
if(delta_sata<0.0001 || delta_sata > -0.0001)
{
y=(xr+xl)/2;
x=0;
}
else
{
double dtemp=1.0/2*(xl+xr)/(xr-xl)*m_tread;
y = dtemp*sin(delta_sata);
x = (1-cos(delta_sata))*(-dtemp);
}
//更新机器人本体的绝对坐标（单位为毫米）
// originPoint.Offset(
// originPoint.x + x * 10 * cos( angle ) - y * 10 * sin( angle) ,
// originPoint.y + x * 10 * sin( angle ) + y * 10 * cos( angle)
// );
CString str,str2,str3,str4;
str4.Format(" origX: %.3f origY: %.3f ",originPoint.x, originPoint.y);
double delta_x = x * cos( angle ) - y * sin( angle) ,
delta_y= x * sin( angle ) + y * cos( angle) ;
originPoint.Offset(
originPoint.x + delta_x * 10,
originPoint.y + delta_y * 10
);
angle+=delta_sata;
str3.Format(" left: %.3f right: %.3f delta left : %.3f right %.3f delta_sata: %.3f",xl,xr,m_originLeftDis,m_originRightDis,delta_sata);;
str2.Format(" x: %f y:%f delta_x : %f y: %.3f",x,y, delta_x, delta_y);
str.Format("O x:%f y: %f o: %f ",originPoint.x, originPoint.y, angle*180.0/3.1415926);
m_dataFile<<str<<str2<<str3<<str4<<endl;
//~~~~我的算法--------------------------------------------
*/
ReleaseDC(pDC);
}
/*
WORD UltraSonarData[16];
m_board.GetFrame(UltraSonarData);
m_data1=UltraSonarData[0];
m_data2=UltraSonarData[1];
m_data3=UltraSonarData[2];
m_data4=UltraSonarData[3];
m_data5=UltraSonarData[4];
m_data6=UltraSonarData[5];
m_data7=UltraSonarData[6];
m_data8=UltraSonarData[7];
m_data9 =UltraSonarData[8];
m_data10=UltraSonarData[9];
m_data11=UltraSonarData[10];
m_data12=UltraSonarData[11];
m_data13=UltraSonarData[12];
m_data14=UltraSonarData[13];
m_data15=UltraSonarData[14];
m_data16=UltraSonarData[15];
UpdateData(FALSE);
*/
CFormView::OnTimer(nIDEvent);
}
void CCompassWP3300View::OnButtonStart()
{
OnButtonSetparam();
CButton *pButton=(CButton *)GetDlgItem(IDC_BUTTON_SERIALPORT);
pButton->EnableWindow(FALSE);
CEdit *pEdit=(CEdit *)GetDlgItem(IDC_EDIT_SERIALPORT);
pEdit->EnableWindow(FALSE);
//SetTimer(1,100,NULL);
m_board.StartCapture();
switch(tmpMaxDis)
{
case 0:
m_SonarCycle = 0.15;
break;
case 1: //量程为3米时,超声波两次数据返回时间间隔
m_SonarCycle = 0.25;
break;
case 2:
m_SonarCycle = 0.35;
break;
case 3:
m_SonarCycle = 0.45;
break;
default:
m_SonarCycle = 0.25;
break;
}
dMaxVelocity = (DEXSAFE - DSAFE) / (m_SonarCycle*10); //最大速度
}
void CCompassWP3300View::OnButtonStop()
{
//KillTimer(1);
m_board.StopCapture();
m_acRobAction.Stop(2);
}
void CCompassWP3300View::OnButtonSetparam()
{
OnButtonStop();
UpdateData(TRUE);
if( ! m_board.SetSerialPort(m_serialPort))
{
MessageBox("open com failed !");
return;
}
tmpMaxDis=0;
tmpMaxDis=GetCheckedRadioButton(IDC_RADIO1,IDC_RADIO4)-IDC_RADIO1;
WORD chEnable=0;
for(int i=0;i<16;i++)
{
if(IsDlgButtonChecked(IDC_CHECK1+i))
chEnable = chEnable + (1<<i);
}
m_board.SetParam(tmpMaxDis,chEnable);
}
void CCompassWP3300View::OnButtonSerialport()
{
UpdateData(TRUE);
m_board.SetSerialPort(m_serialPort);
}
void CCompassWP3300View::OnSonarFrame(WPARAM wParam,LPARAM lParam)
{
WORD UltraSonarData[16];
memcpy(UltraSonarData,(WORD *)wParam,sizeof(WORD)*16);
m_data1=UltraSonarData[0];
m_data2=UltraSonarData[1];
m_data3=UltraSonarData[2];
m_data4=UltraSonarData[3];
m_data5=UltraSonarData[4];
m_data6=UltraSonarData[5];
m_data7=UltraSonarData[6];
m_data8=UltraSonarData[7];
m_data9 =UltraSonarData[8];
m_data10=UltraSonarData[9];
m_data11=UltraSonarData[10];
m_data12=UltraSonarData[11];
m_data13=UltraSonarData[12];
m_data14=UltraSonarData[13];
m_data15=UltraSonarData[14];
m_data16=UltraSonarData[15];
UpdateData(FALSE);
//下面分析各数据，并换算单位为厘米
double sonar0 = UltraSonarData[12]/10, //前
sonar1 = UltraSonarData[7]/10, //前右
sonar2 = UltraSonarData[13]/10, //前左
sonar3 = UltraSonarData[6]/10, //右中
sonar4 = UltraSonarData[14]/10, //左中
sonar5 = UltraSonarData[5]/10, //后右
sonar6 = UltraSonarData[15]/10, //后左
sonar7 = UltraSonarData[4]/10; //后
//根据返回距离信息计算出障碍物所在相对坐标
double temp=sonar0+13.7;
sonarPoint[0].x = 0;
sonarPoint[0].y = temp;
temp=(sonar1+13.7)*0.707;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -