avoiddlg.cpp

上海广茂达as-r机器人避障程序

         ReleaseInterface(m_pImageCap);
    }

	if( m_pMotion )
	{
	    m_pMotion->Abort(WHEEL_LEFT);
		m_pMotion->Abort(WHEEL_RIGHT);

		m_pMotion->End();
		ReleaseInterface(m_pMotion);
	}

	if( m_pRobot )
	{
		m_pRobot->DestroyInstance();
		m_pRobot = NULL;
	}
}


unsigned __stdcall CAVOIDDlg::PsdProc(LONG nOwner,void* lpParam)
{
	float* pValue = (float*)lpParam;
	CAVOIDDlg* pThis = (CAVOIDDlg*)nOwner;
   
  if(pThis->bRun)
  { 
	for( int i = 0;i<MAX_PSD;i++ )
	{	if(pValue[i]>80)
			{pThis->m_getPsd[i]=80;}
	    else if(pValue[i]<0)
			{pThis->m_getPsd[i] =0;}
	    else 
			{pThis->m_getPsd[i] = pValue[i];}
      
	}
  pThis->MOVE(pThis->m_getPsd,pThis->m_getSonar);

  }

  else 
  {
	for( int i = 0;i<MAX_PSD;i++ )
	{
    pThis->m_getPsd[i] =0;
    pThis->MOVE(pThis->m_getPsd,pThis->m_getSonar);
	}
  }

    return 0;
}

unsigned __stdcall CAVOIDDlg::SonarProc(LONG nOwner,void* lpParam)
{
	double* pValue = (double*)lpParam;
	CAVOIDDlg* pThis = (CAVOIDDlg*)nOwner;
    
	
if(pThis->bRun)
	{	
	for( int i = 0;i<MAX_Sonar;i++ )
		{
    	if(pValue[i]>3)
			{pThis->m_getSonar[i] =3;}
	    else if(pValue[i]<0)
			{pThis->m_getSonar[i] =0;}
	    else 
			{pThis->m_getSonar[i] = pValue[i];}
      
		}
      
	}
 else 
	{
	 for( int i = 0;i<MAX_Sonar;i++ )
		{
         pThis->m_getSonar[i] =0;
		}
	}	
	return 0;
}

void CAVOIDDlg::OnDestroy() 
{   
	EndEnv();
	CDialog::OnDestroy();
	
	// TODO: Add your message handler code here
	
}

void CAVOIDDlg::OnRun() 
{
	
   bRun =TRUE;
   
}



void CAVOIDDlg::MOVE(double m_getPsd[],double m_getSonar[]) 
{
	// TODO: Add your control notification handler code here
	if( !m_pMotion)
	{
		return;
	}

	if( !bRun)
	{
		return;
	}
        
    if (m_getPsd[0] ==80)
	{
		dData[0]=m_getSonar[0]<m_getSonar [1]?m_getSonar[0]:m_getSonar [1];
        dData[0]=dData[0]*100;
	}
    else
	{
	  dData[0]=m_getPsd [0];
	}

    if (m_getPsd[3] ==80)
	{
	  dData[2]=m_getSonar[3]<m_getSonar [4]?m_getSonar[3]:m_getSonar [4];
      dData[2]=dData[2]*100;
	}
    else
	{
	  dData[2]=m_getPsd [3];
	}

     if (m_getPsd[1] ==80 && m_getPsd [2] ==80)
	{
      dData[1]=m_getSonar [2]*100;
	}
     else if (m_getPsd[1] ==80 || m_getPsd [2] ==80)
	 {
	  dData[1]=m_getPsd[2] ==80 ?m_getPsd[1]:m_getPsd [2];
	 }
   
	 else if (m_getPsd[1] <80&& m_getPsd [2] <80)
   {
	dData[1]=m_getPsd[1]<m_getPsd [2]?m_getPsd[1]:m_getPsd [2];
   }

 
  m_graph.SetData(dData,sizeof(dData));

  if(dData[1]<m_dSafeDis) 
{
	if(dData[0]<m_dSafeDis && dData[2]<m_dSafeDis) 
  { 
     if (dData[0]<dData[2])
       {m_pMotion->Move(WHEEL_LEFT,-m_dTurnSpeed);      
        m_pMotion->Move(WHEEL_RIGHT,-m_dTurnSpeed-0.1);}
     else 
		{m_pMotion->Move(WHEEL_LEFT,-m_dTurnSpeed-0.1);                                            
         m_pMotion->Move(WHEEL_RIGHT,-m_dTurnSpeed);}
		}
  else if(dData[0]<m_dSafeDis && dData[2]>m_dSafeDis && dData[2]-m_dSafeDis>100 ) 
	 {
		m_pMotion->Move(WHEEL_LEFT,m_dTurnSpeed+0.1);                          
        m_pMotion->Move(WHEEL_RIGHT,m_dTurnSpeed);
	 }
  else if ( dData[0]>m_dSafeDis&&dData[0]-m_dSafeDis >100 && dData[2]<m_dSafeDis) 
	 {
		m_pMotion->Move(WHEEL_LEFT,m_dTurnSpeed);                     
		m_pMotion->Move(WHEEL_RIGHT, m_dTurnSpeed+0.1);
	 }
  else if ( dData[0]>m_dSafeDis && dData[2]>m_dSafeDis && dData[0]-m_dSafeDis >100 && dData[2]-m_dSafeDis >100) 
	 {  
		m_pMotion->Move(WHEEL_LEFT,m_dTurnSpeed+0.1);                          
        m_pMotion->Move(WHEEL_RIGHT, m_dTurnSpeed);
	 }

  else 
	 {
        m_pMotion->Move(WHEEL_LEFT,m_dTurnSpeed);      
        m_pMotion->Move(WHEEL_RIGHT,-m_dTurnSpeed);
	 }

}

else if( dData[1]>m_dSafeDis) 
{ 
  if (dData[0]<m_dSafeDis && dData[2]>m_dSafeDis && dData[2]-m_dSafeDis >100)
  {
	 m_pMotion->Move(WHEEL_LEFT,m_dTurnSpeed+0.1);                                    
	 m_pMotion->Move(WHEEL_RIGHT, m_dTurnSpeed);
  }
  else if (dData[0]>m_dSafeDis && dData[0]-m_dSafeDis >100 && dData[2]<m_dSafeDis)
  { 
	 m_pMotion->Move(WHEEL_LEFT,m_dTurnSpeed);                            
     m_pMotion->Move(WHEEL_RIGHT,m_dTurnSpeed+0.1);
  }	
 
  else if (dData[0]>m_dSafeDis && dData[2]>m_dSafeDis && dData[0]-m_dSafeDis >100 && dData[1]-m_dSafeDis >100 && dData[2]-m_dSafeDis >100)
  {	 
	 bRun =FALSE;
	 bGOGO=TRUE;

  }
  else 
  {
     m_pMotion->SetDCC(WHEEL_LEFT,m_dDcc);                              
     m_pMotion->SetDCC(WHEEL_RIGHT,m_dDcc);
  }
}  
     
}


void CAVOIDDlg::TraceDecide(int nArea,int x,int y)//跟踪程序段开始
{
    //TRACER_PARAM nState;
	CUR_STATE	curState = UNKNOWN;//设置状态变量初值

	if(nArea>0)//目标在成像平面中的情况
	{
		if( x<X1 )
		{
			curState |= HORZ_LEFT;//在左边
		}
		else if(x>X2) 
		{
			curState |= HORZ_RIGHT;//在右边
		}
		else
		{
			curState |= HORZ_MIDDLE;//在中间
		}
		
		if(y<Y1)
		{
			curState |= VERT_FOR;//在前面
		}
		else if(y>Y2)
		{
			curState |= VERT_BACK;//在后面
		}
		else
		{
			curState |= VERT_MIDDLE;//在中间
		}
	}
	else
	{   
		curState = OUTSIGHT;//不在成像平面	
	}

	if(curState&VERT_MIDDLE)//按位取与判断状态值
	{
		 if(curState&HORZ_MIDDLE)
		 {	TRACE("Keep\n");
			m_pMotion->Stop(WHEEL_LEFT);
			m_pMotion->Stop(WHEEL_RIGHT);
		 }
		 else if(curState&HORZ_LEFT)
		 {
			TRACE("MIDDLE AND LEFT\n");
			m_pMotion->Move(WHEEL_LEFT,-m_dTurnSpeed);
			m_pMotion->Move(WHEEL_RIGHT,m_dTurnSpeed+0.1);
		 }
		 else if(curState&HORZ_RIGHT)
		 {
			TRACE("MIDDLE AND RIGHT\n");
		
			m_pMotion->Move(WHEEL_LEFT,m_dTurnSpeed+0.1);
			m_pMotion->Move(WHEEL_RIGHT,-m_dTurnSpeed);
		 }

	}
	
	else if(curState&VERT_FOR)
	{
		if(curState&HORZ_MIDDLE)
		{
			TRACE("FOR and MIDDLE\n");
			m_pMotion->Move(WHEEL_LEFT,m_dTurnSpeed);
			m_pMotion->Move(WHEEL_RIGHT,m_dTurnSpeed);
		}
		else if(curState&HORZ_LEFT)
		{
			TRACE("FOR and LEFT\n");
			m_pMotion->Move(WHEEL_LEFT,m_dTurnSpeed-0.05);
			m_pMotion->Move(WHEEL_RIGHT,m_dTurnSpeed);
		}
		else if(curState&HORZ_RIGHT)
		{
			TRACE("FOR and RIGHT\n");
			m_pMotion->Move(WHEEL_LEFT,m_dTurnSpeed);
			m_pMotion->Move(WHEEL_RIGHT,m_dTurnSpeed-0.05);
		}
	
	}
	
	else if(curState&VERT_BACK)
	{
		if(curState&HORZ_MIDDLE)
		{
			TRACE("BACK and MIDDLE\n");
			m_pMotion->Move(WHEEL_LEFT,-m_dTurnSpeed);
			m_pMotion->Move(WHEEL_RIGHT,-m_dTurnSpeed);
		}
		else if(curState&HORZ_LEFT)
		{
			TRACE("BACK and LEFT\n");
			m_pMotion->Move(WHEEL_LEFT,-m_dTurnSpeed-0.1);
			m_pMotion->Move(WHEEL_RIGHT,-m_dTurnSpeed);
		}
		else if(curState&HORZ_RIGHT)
		{
			TRACE("BACK and RIGHT\n");
			m_pMotion->Move(WHEEL_LEFT,-m_dTurnSpeed);
			m_pMotion->Move(WHEEL_RIGHT,-m_dTurnSpeed-0.1);
		}
	
	}
  
	else if(curState == OUTSIGHT) 
	{	
		if(nState & HORZ_LEFT)
		{
			TRACE("OUT to unclock\n");
			m_pMotion->Move(WHEEL_LEFT,-m_dTurnSpeed+0.1);
			m_pMotion->Move(WHEEL_RIGHT,m_dTurnSpeed);
		}
		else if(nState & HORZ_RIGHT)
		{
			TRACE("OUT to clock\n");
			m_pMotion->Move(WHEEL_LEFT,m_dTurnSpeed);
			m_pMotion->Move(WHEEL_RIGHT,-m_dTurnSpeed+0.1);
		}
		else
		{
		   TRACE("Still OUT\n");
		   
		}
		
	  
	}
     nState = curState;
	 bRun=TRUE;
	 bGOGO=FALSE;
}


void CAVOIDDlg::OnStop() 
{  
	bRun=FALSE;
    m_pMotion->Move(WHEEL_LEFT,0); 
	m_pMotion->Move(WHEEL_RIGHT,0);
}

IASRFootball* CAVOIDDlg::GetImageProcess()
{
	return m_pImageProcess;
}