compasswp3300view.cpp

机器人超声波避障系统

	sonarPoint[1].x=temp;
	sonarPoint[1].y=temp;
	
	temp = (sonar2+13.7)*0.707;
	sonarPoint[2].x=-temp;
	sonarPoint[2].y=temp;
	
	
	sonarPoint[3].x=sonar3+15;
	sonarPoint[3].y=-2.0;
	
	sonarPoint[4].x=-sonar4+15;
	sonarPoint[4].y=-2.0;
	
	temp = (sonar5+13.7)*0.707;
	sonarPoint[5].x=temp;
	sonarPoint[5].y=-temp-16.0;
	
	temp = (sonar6+13.7)*0.707;
	sonarPoint[6].x=-temp;
	sonarPoint[6].y=-temp-16.0;
	
	sonarPoint[7].x=0;
	sonarPoint[7].y=-15.0-sonar7;

//开始决策：-------------------------------------------------
	double dDeci=0,               //左右方向度 [-1，1]
		   dDeciI=1,              //前后距离度[-1，1]
		   dSide=0;               //左右最小距离
//	double dL0=0,dL1=0,dL2=0;
	
	//正前方距离处理
	if ( sonar0 > DEXSAFE)
	{
		dDeciI += 0;
	}
	else if ( sonar0 < DSAFE)
	{
		dDeciI += -2;
	}
	else
	{		
		dDeciI	+= -(sonar0 - DSAFE)/(DEXSAFE-DSAFE);
	}
	//右方距离处理
	if ( sonar1 > DEXSIDESAFE)
	{
		dDeci += 0;
	}
	else if ( sonar1 < DSIDESAFE)
	{
		dDeci += -1;
	}
	else
	{
		dSide = sonar1 - DSIDESAFE;
		dDeci	-= (sonar1 - DSIDESAFE)/(DEXSIDESAFE-DSIDESAFE);
		dDeciI	-= ( (sonar1 - DSIDESAFE)/(DEXSIDESAFE-DSIDESAFE) )/2;
	}
	//左方距离处理
	if ( sonar2 > DEXSIDESAFE)
	{
		dDeci += 0;
	}
	else if ( sonar2 < DSIDESAFE)
	{
		dDeci += 1;
	}
	else
	{

		dDeci	+= (sonar2 - DSIDESAFE)/(DEXSIDESAFE-DSIDESAFE);
		dDeciI	-= ( (sonar2 - DSIDESAFE)/(DEXSIDESAFE-DSIDESAFE) )/2;
		if ( dSide> sonar2 - DSIDESAFE )
		{
			dSide = sonar2 - DSIDESAFE;
		}
	}
	if ( dDeciI <-1 ) 
	{
		dDeciI = -1;
	}

	//根据方向向量，决策运动方式
	//相似命令执行次数
	if( fabs(dDeci - dDeciRe)<0.0001 && fabs(dDeciI - dDeciIRe)<0.0001 )
	{
		iDecNum++;
	}
	else
	{
		iDecNum=0;
	}
	//决策运动方式
	if (iDecNum > 50 )
	{
		m_acRobAction.Stop(2);
		//其他解决方案
	}
	else 
	{
		bool leftright,foreback;
		double velocity=0,radius=0,radlength=0;
		CString info;

		if ( fabs(dDeciI)< 0.00001)	//前进方向力为零，只能左右拐
		{
			if ( fabs(dDeci)<0.00001 )	//也没有左右方向，则随机左右拐
			{
				
				if ( rand()%2 ==1 )
				{
					leftright = true;
				}
				else
				{
					leftright = false;
				}
			}
			else
			{
				if ( dDeci < 0.00001 )
				{
					leftright = true;
				}
				else
				{
					leftright = false;
				}
			}
			radius = (PI/4)/m_SonarCycle ;
			m_acRobAction.Turn(leftright,radius);
//			m_acRobAction.Turn(leftright,20,5);
			info.Format("turn %d",leftright);
		}
		else		//前进方向力不为零，在前进中左右拐
		{
			if ( fabs(dDeci)<0.00001 )	//没有左右方向，则前进
			{
				if( dDeciI<-1 )	//完全后退
				{
					foreback =false;
					velocity = dMaxVelocity ;// * fabs(dDeciI);
					m_acRobAction.GoStraight(foreback,velocity,15);
					Sleep(80);
					m_acRobAction.Turn(true,15,25);
					Sleep(200);
//					velocity = 20 + fabs(dDeciI) *15;
//					radlength = 8+  fabs(dDeciI) * 5;
//					m_acRobAction.GoStraight( foreback,velocity,radlength);
//					Sleep(500);
				}
				else
				{
					if ( dDeciI < 0.00001)
					{
						foreback = false;
					}
					else
					{
						foreback = true;
					}
					velocity = dMaxVelocity * fabs(dDeciI);
					m_acRobAction.GoStraight(foreback,velocity);
					if (dSide > 5) 
					{
						Sleep(80);
						m_acRobAction.Turn(true,15,15);
					}
//					velocity = 20 + fabs(dDeciI) *15;
//					radlength = 2+  fabs(dDeciI) * 5;
//					m_acRobAction.GoStraight( foreback,velocity,radlength);
					info.Format("gostr ");
				}
					
			}
			else						//有左右方向
			{
				if ( dDeciI < 0.00001)
				{
					foreback=false;
				}
				else
				{
					foreback=true;
				}
				if ( dDeci < 0.000001 ) 
				{
					leftright=true;
				}
				else
				{
					leftright=false;
				}
				velocity = dMaxVelocity *fabs(dDeciI );

//				velocity = fabs(dDeciI ) * 20 + 15;
				radius	= m_tread/2+10*dDeciI * dDeciI / (dDeciI*dDeciI +dDeci *dDeci);
				radlength = 7;//PI/4 *radius;
//				m_acRobAction.Curve(leftright,foreback,velocity,radius,radlength);
				m_acRobAction.Curve(leftright,foreback,velocity,radius);
				info.Format("curve LR:%d FB:%d ",leftright,foreback);
			}

		}
/*
		if ( dDeciI < 0.00001)
		{
			foreback=false;
		}
		else
		{
			foreback=true;
		}
		if ( dDeci < 0.000001 ) 
		{
			leftright=true;
		}
		else
		{
			leftright=false;
		}
		velocity = fabs(dDeciI ) * 20 + 15;
		radius	= 70*dDeciI * dDeciI / (dDeciI*dDeciI +dDeci *dDeci);
		radlength = PI/4 *radius;

		m_acRobAction.Curve(leftright,foreback,velocity,radius,radlength);
*/
//debug-------------------------
		CString strpos;
		strpos.Format(" x:%.1f y:%.1f \t v:%.2f r:%.2f rl:%.2f\n",dDeci,dDeciI,velocity,radius,radlength);		
		m_posFile<<info<<strpos;
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~debug
	}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~决策完	


/*	
	double sonar0 = UltraSonarData[7]/10,	//前
		sonar1 = UltraSonarData[7]/10,	//前右
		sonar2 = UltraSonarData[13]/10,	//前左
		
		sonar3 = UltraSonarData[6]/10,	//右中		
		sonar4 = UltraSonarData[14]/10,	//左中
		
		sonar5 = UltraSonarData[6]/10,	//后右
		sonar6 = UltraSonarData[6]/10,	//后左
		sonar7 = UltraSonarData[14]/10;	//后
	
	

	//根据返回距离信息计算出障碍物所在相对坐标
	double temp=sonar0+13.7;
	temp=(sonar1+13.7)*0.707;
	
	sonarPoint[0].x = temp;
	sonarPoint[0].y	= temp;
	
	temp=(sonar1+13.7)*0.707;
	sonarPoint[1].x=temp;
	sonarPoint[1].y=temp;
	
	temp = (sonar2+13.7)*0.707;
	sonarPoint[2].x=-temp;
	sonarPoint[2].y=temp;
	
	
	sonarPoint[3].x=sonar3+15;
	sonarPoint[3].y=-2.0;
	
	sonarPoint[4].x=-sonar4+15;
	sonarPoint[4].y=-2.0;
	
	sonarPoint[5].x=temp;
	sonarPoint[5].y=temp;
	
	sonarPoint[6].x=temp;
	sonarPoint[6].y=temp;
	
	sonarPoint[7].x=temp;
	sonarPoint[7].y=temp;
*/	

	//将相对坐标转换为绝对坐标（全局坐标系）
	for(int i=0;i<8;i++ )
	{
		absoluteSonarPoint[i].x=
			originPoint.x+sonarPoint[i].x * cos( angle ) - sonarPoint[i].y * sin( angle) ;
		absoluteSonarPoint[i].y=
			originPoint.y+sonarPoint[i].x * sin( angle ) + sonarPoint[i].y * cos( angle) ;
	}
	
	

//------------------------------------------------------------
//在全局坐标系中划出机器人和探测到的障碍物

	CDC *pDC=GetDC();	
	CPen pen(PS_SOLID,1,RGB(0,0,255) ),*pOldPen,greenPen( PS_SOLID,1,RGB(0,255,0) );
	pOldPen= pDC->SelectObject(&greenPen);


//debug:~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*
	strpos.Format("%.2f,%.3f",originPoint.x,originPoint.y);
	m_posFile<<endl<<endl<<strpos<<endl;
	
	for(int ipos=0;ipos<8;ipos++)
	{
		strpos.Format("x:%.2f y: %.2f \t",sonarPoint[ipos].x,sonarPoint[ipos].y);
		m_posFile<<strpos;
	}

	m_posFile<<endl<<"point:";
	
	CPen redPen(PS_SOLID,1,RGB(255,0,0));
	pDC->SelectObject(&redPen);
	for(int jj=0;jj<8;jj++ )
	{
		int a=(int)sonarPoint[jj].x+drawOriginPoint.x,
			b=-(int)sonarPoint[jj].y+drawOriginPoint.y;
		pDC->Ellipse(a-1,b-1,a+1,b+1);
	}
*/
///~~~~~~~~~~~~~~~~~~~~~~~~~~~~debug	

	
	//将相对坐标转换为绝对坐标（全局坐标系）
	pDC->SelectObject(&pen);
	for(int j=0;j<8;j++ )
	{
		int a=(int)absoluteSonarPoint[j].x+drawOriginPoint.x,
			b=-(int)absoluteSonarPoint[j].y+drawOriginPoint.y;
		pDC->Ellipse(a-1,b-1,a+1,b+1);


//debug:~~~~~~~~~~~~~~~~~~~~~~~~~~~~		
/*		strpos.Format("%.1f %.1f",sonarPoint[j].x,sonarPoint[j].y);
		pDC->TextOut(a,b,strpos);
		
		strpos.Format("x:%d y: %d \t",a,b);		
		m_posFile<<strpos;
*/
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~debug	
		
	}
	pDC->SelectObject( pOldPen );
	ReleaseDC(pDC);
//------------------------------------------------------------	

}

int GetNearGrade(int v,int nearpoint , int farpoint)
{
	if( nearpoint > farpoint  || ( v < 0) )
		return -1;


	if( v<nearpoint )
		return 0;
	else
	{
		if( v<farpoint )
			return 1;
		else
			return (v-nearpoint)*10/(farpoint-nearpoint);
	}

}

void CCompassWP3300View::OnDraw(CDC* pDC) 
{
	// TODO: Add your specialized code here and/or call the base class
/*	for(int i=0;i<8;i++ )
	{
		int a=(int)absoluteSonarPoint[i].x,b=(int)absoluteSonarPoint[i].y;
		pDC->Ellipse(a,b,a+2,b+2);		
	}
*/
	//画机器人本体
	CMyPoint temp;
	temp.x=drawOriginPoint.x;
	temp.y=drawOriginPoint.y;
	temp.x += (int)originPoint.x;
	temp.y -= (int)originPoint.y;
	pDC->Ellipse(temp.x-1,temp.y-1,temp.x+1,temp.y+1 );
	pDC->MoveTo((int)temp.x,(int)temp.y);
	temp.x += (int)( 10*cos(angle+3.1415926/2) );
	temp.y -= (int)( 10*sin(angle+3.1415926/2) );
	pDC->LineTo( (int)temp.x,(int)temp.y );
}

void CCompassWP3300View::OnButtonSetXYO() 
{
	// TODO: Add your control notification handler code here
	UpdateData(true);

	drawOriginPoint.x = m_drawx;
	drawOriginPoint.y = m_drawy;

	originPoint.x	  = m_originx;
	originPoint.y	  = m_originy;
	angle		  = m_originO/180.0;

	m_originLeftDis  = m_acRobAction.GetLeftWheelRealDis();
	m_originRightDis = m_acRobAction.GetRightWheelRealDis();
	m_tread			 = m_acRobAction.GetTread();

	m_dataFile<<"set  "<<originPoint.x<<"  "<<originPoint.y<<"  "<<angle*180<<endl;
}



BOOL CCompassWP3300View::PreTranslateMessage(MSG* pMsg) 
{
	// TODO: Add your specialized code here and/or call the base class
	 if(pMsg->message == WM_KEYDOWN)
	 {
		 switch(pMsg->wParam)
		 {
			case VK_UP:
				m_acRobAction.GoStraight( FORE,28,3);
				break;
			case VK_DOWN:
				m_acRobAction.GoStraight( BACK,25,2);
				break;
			case VK_LEFT:
				m_acRobAction.Turn(true,38,9);
				break;
			case VK_RIGHT:
				m_acRobAction.Turn(false,38,9);
				break;
			default:
				m_acRobAction.Stop(2);
				break;
		 }
	 }

	return CFormView::PreTranslateMessage(pMsg);
}

void CCompassWP3300View::OnDestroy() 
{
	CFormView::OnDestroy();
	
	// TODO: Add your message handler code here
	m_board.StopCapture();
	KillTimer(1);

}