motiondemodlg.cpp

基于AS-R的避障小车 由南京航空航天大学编制

    if(m_pPsd)

     {

          m_pPsd->End();             //停止使用运动设备

          m_pPsd->Release();          //释放基本运动控制接口

          m_pPsd = NULL;

     }
    if(m_pSonar)

     {

          m_pSonar->End();             //停止使用运动设备

          m_pSonar->Release();          //释放基本运动控制接口

          m_pSonar = NULL;
	}

     if(m_pRobot)

     {

          m_pRobot->DestroyInstance();   //系统清除占用的资源

          m_pRobot = NULL;

     }

}

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

unsigned __stdcall  CMotionDemoDlg::SonarProc(LONG nOwner, void *lpParam)
{
	
    double* pValue = (double*)lpParam;
	CMotionDemoDlg* pThis = (CMotionDemoDlg*)nOwner;
    float dSonarData[MAX_Sonar]={0};
	//static  double s_LastS = 0;
    for(int i=0;i<MAX_Sonar;i++)
    {
		if(i==0)
		{
			pThis->m_Sonar1=pValue[i];
		}
		else if(i==1)
		{
			pThis->m_Sonar2=pValue[i];
		}
		else if(i==2)
		{
			pThis->m_Sonar3=pValue[i];
		}
		else if(i==3)
		{
			pThis->m_Sonar4=pValue[i];
		}
		else
		{
			pThis->m_Sonar5=pValue[i];
		}
		pThis->PostMessage(WM_MSG_PSD_UPDATEDATA);
        pThis->m_distance = pValue[i]<0 ? 0 : pValue[i];
     	if(pThis->m_distance>7.0)
		{
			pThis->m_distance=7.0;
		}

		if(pThis->m_distance<0)
		{
			pThis->m_distance=0;
		}
		dSonarData[i]= pThis->m_distance;
		//pThis->PostMessage(WM_MSG_SONAR_UPDATEDATA);
		double s =PI/6;
		//pThis->m_pMotion->GetS(WHEEL_LEFT);
	    s+=i*(PI/6); //s/ROBOT_RADIUS;
	    MAP_DATA data;
	    data.dArc = s;
	    data.dData = dSonarData[i];
	    pThis->m_graph.SampleData(data);
	    //s_LastS = s;
	}
	/*double s =PI/6;
		//pThis->m_pMotion->GetS(WHEEL_LEFT);
	    s+=i*(PI/6); //s/ROBOT_RADIUS;
	    MAP_DATA data;
	    data.dArc = s;
	    data.dData = dSonarData[i];
	    pThis->m_graph.SampleData(data);
	    //s_LastS = s;
	*/
        if(dSonarData[2]<=pThis->m_SonarSafeDis)
		{
           CMotionDemoDlg::BalkFlag=TRUE;
           if(dSonarData[1]<=pThis->m_SonarSafeDis)
		   {
           CMotionDemoDlg::BalkFlagL=TRUE;
		   }
           else
		   { 
           CMotionDemoDlg::BalkFlagL=FALSE;
		   }
           if(dSonarData[3]<=pThis->m_SonarSafeDis)
		   {
           CMotionDemoDlg::BalkFlagr=TRUE;
		   }
           else
		   { 
           CMotionDemoDlg::BalkFlagr=FALSE;
		   }
           if(dSonarData[1]<=pThis->m_SonarSafeDis && dSonarData[3]<=pThis->m_SonarSafeDis)
		   {
           CMotionDemoDlg::BalkFlagL=FALSE;
		   CMotionDemoDlg::BalkFlagr=FALSE;
		   }
		}
        else
		{ 
           CMotionDemoDlg::BalkFlag=FALSE;
		}
    	return 0;
	
}

void CMotionDemoDlg::UpdateMyData(WPARAM wParam, LPARAM lParam)
{
    UpdateData(FALSE);
}
//unsigned __stdcall CMotionDemoDlg::PsdProc(LONG nOwner,void* lpParam)
unsigned __stdcall  CMotionDemoDlg::PsdProc(LONG nOwner, void *lpParam)
{
    float* pValue = (float*)lpParam;
	CMotionDemoDlg* pThis = (CMotionDemoDlg*)nOwner;
    float dPsdData[MAX_PSD]={0};
    for(int i=0;i<MAX_PSD;i++)
    {   
		if(i==0)
		{
			pThis->m_Psd1 = pValue[i];
		}
		else if(i==1)
		{
			pThis->m_Psd2=pValue[i];
		}
		else if(i==2)
		{
			pThis->m_Psd3=pValue[i];
		}
		else
		{
			pThis->m_Psd4=pValue[i];
		}
		pThis->PostMessage(WM_MSG_PSD_UPDATEDATA);
		pThis->m_distance1 = pValue[i]>80 ? 80 : pValue[i];
        if(pThis->m_distance1>80)
		{
			pThis->m_distance1=80;
		}

		if(pThis->m_distance1<10)
		{
			pThis->m_distance1=10;
		}
        dPsdData[i]= pThis->m_distance1;
		//pThis->PostMessage(WM_MSG_PSD_UPDATEDATA);
	}
       /*for(int i=0;i<MAX_PSD;i++)
    {    
	    double s = PI/4;
		//pThis->m_pMotion->GetS(WHEEL_LEFT);
	    s+=i*(PI/6);//s/ROBOT_RADIUS;
	    MAP_DATA data;
	    data.dArc = s;
	    data.dData = dPsdData[i];
	    pThis->m_graph.SampleData(data);
	}*/
	   /*double s = PI/4;
		//pThis->m_pMotion->GetS(WHEEL_LEFT);
	    s+=i*(PI/6);//s/ROBOT_RADIUS;
	    MAP_DATA data;
	    data.dArc = s;
	    data.dData = dPsdData[i];
	    pThis->m_graph.SampleData(data);*/
  
	    if((dPsdData[1]<=pThis->m_PsdSafeDis) || (dPsdData[2]<=pThis->m_PsdSafeDis))
		{
           CMotionDemoDlg::BalkFlag1=TRUE;
		}
        else
		{ 
           CMotionDemoDlg::BalkFlag1=FALSE;
		}
		return 0;
}

void CMotionDemoDlg::OnStart() 
{
	// TODO: Add your control notification handler code here
	UpdateData(TRUE);
	m_graph.Clear();
	m_pPsd->Start();
    m_pSonar->Start();
    m_pMotion->Start();
}

void CMotionDemoDlg::OnPause() 
{
	// TODO: Add your control notification handler code here
	UpdateData(TRUE);
	m_pPsd->Pause();
	m_pSonar->Pause();
    m_pMotion->Pause();	
}

void CMotionDemoDlg::OnEnd() 
{
	// TODO: Add your control notification handler code here
	m_pPsd->End();
	m_pSonar->End();
    m_pMotion->End();
}

void CMotionDemoDlg::OnFront() 
{
	// TODO: Add your control notification handler code here
	if(m_pMotion)

   {
		  UpdateData(TRUE);

          // 设定左电机的运动速度。

          m_pMotion->SetSpeed(WHEEL_LEFT,m_SpeedL);

          m_pMotion->SetSpeed(WHEEL_RIGHT,m_SpeedR);
		  if(CMotionDemoDlg::BalkFlag==TRUE)
		  {	
	     	m_pMotion->SetSpeed(WHEEL_LEFT,0.6*m_SpeedL);
	        m_pMotion->SetSpeed(WHEEL_RIGHT,0.6*m_SpeedR);
		  
		  }

          if(CMotionDemoDlg::BalkFlagL==TRUE || CMotionDemoDlg::BalkFlag1==TRUE)
		  {	
		    m_pMotion->SetSpeed(WHEEL_LEFT,0.8*m_SpeedL);
	        m_pMotion->SetSpeed(WHEEL_RIGHT,-0.8*m_SpeedR);
		  }
          if(CMotionDemoDlg::BalkFlagr==TRUE)
		  {	
	     	m_pMotion->SetSpeed(WHEEL_LEFT,-0.8*m_SpeedL);
	        m_pMotion->SetSpeed(WHEEL_RIGHT,0.8*m_SpeedR);
		  }

          m_pMotion->SetSpeed(WHEEL_LEFT,m_SpeedL);

          m_pMotion->SetSpeed(WHEEL_RIGHT,m_SpeedR);
   }
}

void CMotionDemoDlg::OnBack() 
{
	// TODO: Add your control notification handler code here

   if(m_pMotion)

   {

          m_pMotion->SetSpeed(WHEEL_LEFT,-m_SpeedL);

          m_pMotion->SetSpeed(WHEEL_RIGHT,-m_SpeedR);

   }



}

void CMotionDemoDlg::OnClockwise() 
{
	// TODO: Add your control notification handler code here
	if(m_pMotion)

   {

           m_pMotion->SetSpeed(WHEEL_LEFT,m_SpeedL);

          m_pMotion->SetSpeed(WHEEL_RIGHT,-m_SpeedR);

   }
}

void CMotionDemoDlg::OnRevension() 
{
	// TODO: Add your control notification handler code here
	if(m_pMotion)

   {

        m_pMotion->SetSpeed(WHEEL_LEFT,-m_SpeedL);                             
        m_pMotion->SetSpeed(WHEEL_RIGHT,m_SpeedR);

   }


}

void CMotionDemoDlg::OnStop() 
{
	// TODO: Add your control notification handler code here
	if(m_pMotion)

   { 
        m_pMotion->SetSpeed(WHEEL_LEFT,0);                             
        m_pMotion->SetSpeed(WHEEL_RIGHT,0);

   }


}