avoidlistview.cpp

智能移动机器人是一种在复杂的环境下工作的具有自规划、自组织、自适应能力的机器人。导航算法的研究是智能机器人研究领域的一个热点话题。智能导航的目的就是在没有人干预下使机器人有目的地移动并完成特定任务

// avoidlistView.cpp : implementation of the CAvoidlistView class
//

#include "stdafx.h"
#include "avoidlist.h"
#include <math.h>
#include "avoidlistDoc.h"
#include "avoidlistView.h"

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

/////////////////////////////////////////////////////////////////////////////
// CAvoidlistView

IMPLEMENT_DYNCREATE(CAvoidlistView, CView)

BEGIN_MESSAGE_MAP(CAvoidlistView, CView)
	//{{AFX_MSG_MAP(CAvoidlistView)
	ON_COMMAND(ID_OBSTACLE, OnObstacle)
	ON_COMMAND(ID_OBSTACLE_AXIS, OnObstacleAxis)
	ON_WM_LBUTTONDOWN()
	ON_COMMAND(ID_KILL_OBSTACLE, OnKillObstacle)
	ON_WM_TIMER()
	ON_COMMAND(ID_PATHPLAN, OnPathplan)
	//}}AFX_MSG_MAP
	// Standard printing commands
	ON_COMMAND(ID_FILE_PRINT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_DIRECT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_PREVIEW, CView::OnFilePrintPreview)
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CAvoidlistView construction/destruction

CAvoidlistView::CAvoidlistView()
{
head=NULL;
obstacle=NULL;	// TODO: add construction code here
idd=0;
startpoint=CPoint(10,10);
endpoint=CPoint(700,400);//(400,400);
rmax=10;
ks=0.6;//取0 .6效果好些
j=0;
for(int i=0;i<100;i++)
degcommand[i]=0;
Isexisting=false;// sin
angle=atan(double(endpoint.y-startpoint.y)/double(endpoint.x-startpoint.x));
//nextpoint[0].x=startpoint;
//nextpoint[0].r=15;
//nextpoint[0].degree=angle;
xr=0;
yr=0;
constant=1;
constant1=5;
xt=0;
yt=0;
lastpoint.x=startpoint.x;
lastpoint.y=startpoint.y;
nextpoint.r=rmax;
nextpoint.degree=angle;
}

CAvoidlistView::~CAvoidlistView()
{
}

BOOL CAvoidlistView::PreCreateWindow(CREATESTRUCT& cs)
{
	// TODO: Modify the Window class or styles here by modifying
	//  the CREATESTRUCT cs

	return CView::PreCreateWindow(cs);
}

/////////////////////////////////////////////////////////////////////////////
// CAvoidlistView drawing

void CAvoidlistView::OnDraw(CDC* pDC)
{
	CAvoidlistDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);
	// TODO: add draw code for native data here
}

/////////////////////////////////////////////////////////////////////////////
// CAvoidlistView printing

BOOL CAvoidlistView::OnPreparePrinting(CPrintInfo* pInfo)
{
	// default preparation
	return DoPreparePrinting(pInfo);
}

void CAvoidlistView::OnBeginPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add extra initialization before printing
}

void CAvoidlistView::OnEndPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add cleanup after printing
}

/////////////////////////////////////////////////////////////////////////////
// CAvoidlistView diagnostics

#ifdef _DEBUG
void CAvoidlistView::AssertValid() const
{
	CView::AssertValid();
}

void CAvoidlistView::Dump(CDumpContext& dc) const
{
	CView::Dump(dc);
}

CAvoidlistDoc* CAvoidlistView::GetDocument() // non-debug version is inline
{
	ASSERT(m_pDocument->IsKindOf(RUNTIME_CLASS(CAvoidlistDoc)));
	return (CAvoidlistDoc*)m_pDocument;
}
#endif //_DEBUG

/////////////////////////////////////////////////////////////////////////////
// CAvoidlistView message handlers

void CAvoidlistView::OnObstacle() 
{
idd=1;	// TODO: Add your command handler code here
	
}

void CAvoidlistView::OnObstacleAxis() //显示障碍物的坐标
{
	CClientDC dc(this);
	char str[20];//,str1[20];
	int i=1;
	obstaclelist * OB;
	
	if(head==NULL)
		return;
	OB=head;
	for( ;OB!=NULL;OB=OB->next)
	{
		wsprintf(str,"%d",OB->point.x);
		dc.TextOut(10,20*i,str);
        wsprintf(str,"%d",OB->point.y);
		dc.TextOut(40,20*i,str);
		i+=1;
	}

		

	// TODO: Add your command handler code here
	
}

void CAvoidlistView::OnLButtonDown(UINT nFlags, CPoint point) //设置障碍物
{
	CClientDC dc(this);
    CBrush brush;
    brush.CreateSolidBrush(RGB(255,255,0));
    dc.SelectObject(brush);
	CPoint y,point1;

	if(idd==0)
	{
		CView::OnLButtonDown(nFlags,point);
		return;
	}
	y.x=point.x;
	y.y=point.y;
	point1.x=int(y.x/10)*10+5;
	point1.y=int(y.y/10)*10+5;
	if(head==NULL)
	{
		head=new(obstaclelist);
		head->point=point1;
		head->next=NULL;		
        dc.Rectangle(point1.x-5,point1.y-5,point1.x+5,point1.y+5);
	    CView::OnLButtonDown(nFlags,point);
		return;
	}
	obstacle=head;
/*	if(obstacle->next==NULL)
	{
		//x.x=obstacle->point.x;
	//	x.y=obstacle->point.y;
		if(obstacle->point==point1)//(sqrt((y.x-x.x)*(y.x-x.x)+(y.y-x.y)*(y.y-x.y))<=5)
		{
        CView::OnLButtonDown(nFlags,point);
		 return;
		}
	}*/
    obstaclelist *OO;
	for(;obstacle!=NULL;OO=obstacle,obstacle=obstacle->next)//&&obstacle->point!=point1
	{
		
		if(obstacle->point==point1)//(sqrt((y.x-x.x)*(y.x-x.x)+(y.y-x.y)*(y.y-x.y))<=5)
		{
         CView::OnLButtonDown(nFlags,point);
		 return;
		}
	}	
	obstaclelist *O;
	O=new(obstaclelist);
    O->point=point1;
	O->next=NULL;
	OO->next=O;

    dc.Rectangle(point1.x-5,point1.y-5,point1.x+5,point1.y+5);
	// TODO: Add your message handler code here and/or call default
	
	CView::OnLButtonDown(nFlags, point);
//	return head;
}

BOOL CAvoidlistView::Searchd()//搜索障碍物与机器人当前位置之间的最小距离,若小于10，则认为存在障碍物
{
	obstaclelist * OA;
		double distance,dmin;
	int x2,x1,y2,y1;
	x1=lastpoint.x;
	y1=lastpoint.y;
	if(head==NULL)
		return false;

	x2=head->point.x;
	y2=head->point.y;
	OA=head;
	dmin=sqrt(double((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)));
	for(;OA!=NULL;OA=OA->next)
	{x2=OA->point.x;
	y2=OA->point.y;
	distance=sqrt(double((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)));
	if(distance<dmin)
		dmin=distance;
	}
	if(dmin<20)
		return true;
	return false;
}


void CAvoidlistView::OnKillObstacle() //清除障碍物
{
	obstaclelist *OKill,*OO;
	idd=0;
	if(head==NULL)
	{
		return;
	}
	OKill=head;
	for( ;OKill!=NULL;)
	{
		OO=OKill;
		OKill=OKill->next;
		free(OO);
	}
	head=NULL;
	Invalidate(TRUE);

	// TODO: Add your command handler code here
	
}
void CAvoidlistView::OnStop() 
{
	// TODO: Add your command handler code here
KillTimer(ID_CLOCK_TIMER);	
}
void CAvoidlistView::OnTimer(UINT nIDEvent) 
{
	// CDC
	// TODO: Add your message handler code here and/or call default
	CClientDC dc(this);
	findnextpoint();//找到下一个点
	CPen pen,*op;
	pen.CreatePen(PS_SOLID,2,RGB(0,0,0));
	op=dc.SelectObject(&pen);
//若下一个点超过了目标点，则下一个点即为目标点
   if((nextpoint.x>=endpoint.x&&nextpoint.y>=endpoint.y)||sqrt(double((lastpoint.x-endpoint.x)*(lastpoint.x-endpoint.x)+(lastpoint.y-endpoint.y)*(lastpoint.y-endpoint.y)))<15)
	{
		
    dc.MoveTo(lastpoint.x,lastpoint.y);
	dc.LineTo(endpoint.x,endpoint.y);
	OnStop();//停止规划
//CView::OnTimer(nIDEvent);
	}
    dc.MoveTo(lastpoint.x,lastpoint.y);
	dc.LineTo(nextpoint.x,nextpoint.y);
	lastpoint=nextpoint;
	dc.SelectObject(op);
	CView::OnTimer(nIDEvent);
}

void CAvoidlistView::OnPathplan() 
{
	// TODO: Add your command handler code here
SetTimer(ID_CLOCK_TIMER,200,NULL);	
}

void CAvoidlistView::findnextpoint()//寻求机器人运动的下一个点
{
	double Tdegree,Wdegree,x1,y1;
    Isexisting=Searchd();//搜索是否存在障碍物
   if(!Isexisting)//若在一定距离内 不存在障碍物
   {//求取下一个点的各个分量
	nextpoint.r=rmax;
	nextpoint.degree=atan(double(endpoint.y-lastpoint.y)/double(endpoint.x-lastpoint.x));

    nextpoint.x=(nextpoint.r*cos(nextpoint.degree)+lastpoint.x);
	nextpoint.y=(nextpoint.r*sin(nextpoint.degree)+lastpoint.y);
    return;
   }
   else 
   {//OnStop();
	double deg;
    repulforce();//斥力
    attractforce();//引力
    deg=(xr*(lastpoint.r*cos(lastpoint.degree))+yr*(lastpoint.r*sin(nextpoint.degree)))/(lastpoint.r*sqrt(xr*xr+yr*yr));
    xr=0.25*xr+0.75*(-deg)*xr;//调整斥力
    yr=0.25*yr+0.75*(-deg)*yr;
    resultx=xt+xr;//求总合力的x、y分量
    resulty=yt+yr;
    Rdegree=computedeg(resultx,resulty);//求取合力的角度，并利用其求取下一个点的各个分量
   //j++;
//	degcommand[j]=ks*(lastpoint.degree-Rdegree);
	//if(j<2)
//	degcommand[j]=(0.1*degcommand[j-1]+0.3*degcommand[j])/0.4;//(0.1*degcommand[j-1]+0.1*degcommand[j])/0.2
    nextpoint.degree=lastpoint.degree+ks*(lastpoint.degree-Rdegree);//degcommand[j];
	
    nextpoint.r=rmax*(1-abs(deg));
    nextpoint.x=(nextpoint.r*cos(nextpoint.degree)+lastpoint.x);
    nextpoint.y=(nextpoint.r*sin(nextpoint.degree)+lastpoint.y);
    Tdegree=atan(double(endpoint.y-lastpoint.y)/double(endpoint.x-lastpoint.x));
    if((abs(Tdegree-nextpoint.degree)>3.1415926/2))//判断当前点的运动方向与目标目标方向之间的偏离角，若该角度超过90度，则进入wall-following method
	{
	  xr=xr;
	  yr=yr;
	  Wdegree=computedeg(xr,yr);//斥力的方向
	  Wdegree+=3.1415926*93/180;//斥力的角度加上一个角度，该角度应该大于90度，并且越靠近90度效果越好，取93度效果好一些
      x1=sqrt(xt*xt+yt*yt)*cos(Wdegree);//在该相应的方向上加一个虚拟引力
	  y1=sqrt(xt*xt+yt*yt)*sin(Wdegree);
	  resultx=xr+x1;//求出现在的合力方向
	  resulty=yr+y1;
	  Rdegree=computedeg(resultx,resulty);
	  nextpoint.degree=Rdegree;
      nextpoint.x=(nextpoint.r*cos(nextpoint.degree)+lastpoint.x);
      nextpoint.y=(nextpoint.r*sin(nextpoint.degree)+lastpoint.y);
	}
 // nextpoint.r=rmax*cos(nextpoint.degree);	
   }
  return;
}

double CAvoidlistView::computedeg(double x0, double y0)//根据x、y坐标求取向量角度
{
  double degree;
  if(x0>0&&y0>0)
     degree=atan(y0/x0);
  if(x0>0&&y0<0)
     degree=-atan(-y0/x0);
  if(x0<0&&y0>0)
     degree=3.1415926-atan(-y0/x0);
  else
     degree=-3.1415926+atan(y0/x0);
  return degree;
}

void CAvoidlistView::repulforce()//计算所有障碍物对机器人施加的虚拟斥力
{
 double xi,yi,di;
 obstaclelist * OB;
 if(head==NULL)
 return;
 OB=head;
 for(;OB!=NULL;OB=OB->next)
 {
	 di=sqrt(double((OB->point.x-lastpoint.x)*(OB->point.x-lastpoint.x)+(OB->point.y-lastpoint.y)*(OB->point.y-lastpoint.y)));
     xi=constant*(OB->point.x-lastpoint.x)/(di*di*di);
     yi=constant*(OB->point.y-lastpoint.y)/(di*di*di);
	 xr+=xi;
	 yr+=yi;
 }
}

void CAvoidlistView::attractforce()//计算目标点对机器人施加的虚拟引力
{
double dt;
dt=sqrt(double((endpoint.x-lastpoint.x)*(endpoint.x-lastpoint.x)+(endpoint.y-lastpoint.y)*(endpoint.y-lastpoint.y)));
xt=constant1*(endpoint.x-lastpoint.x)/dt;
yt=constant1*(endpoint.y-lastpoint.y)/dt;
}