?? motiondemodlg.cpp
字號:
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);
}
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -