?? balltracecommand.cpp
字號:
// File: BallTraceCommand.cpp
// Purpose:
// Created: 01-DEC-2005 by Grandar
//
// Comments:
// General comments go here, for example:
// - command line options
// - file formats
// - rules and conventions
// - descriptions of the main data structures
//
// Revisions:
// 01-DEC-2005 Jiang
// Added ...
//
//
#include "stdafx.h"
#include "BallTraceCommand.h"
#include "sduDemo.h"
#include "IASRImageProcess.h"
#include "sduDemoDlg.h"
HERROR __stdcall CBallTraceCommand::Execute(LPVOID pParam)
{
CSduDemoDlg* pWnd = (CSduDemoDlg*)::AfxGetMainWnd();
if(m_pImageProcess == NULL)
{
m_pImageProcess = pWnd->GetImageProcess();
}
if(m_pImage == NULL)
{
m_pImage = pWnd->GetImage();
}
if(m_pMotion == NULL)
{
m_pMotion = pWnd->GetMotion();
//m_pMotion->Init();
// m_pMotion->Start();
}
COLOR_INFO info;
COLOR_DESC desc = {COLOR_RED,60,250};
m_pImageProcess->GetColorInfo(desc,info);
TraceDecide(info.nArea,info.nX,info.nY);
m_pImageProcess->FilterKeyColor(desc);
CWnd* pWndpic;
pWndpic = (CWnd*)(pWnd->GetDlgItem(IDC_PROCESS));
m_pImage->PaintDib(pWndpic->GetSafeHwnd());
TRACE("%10d%10d%10d\n",info.nArea,info.nX,info.nY);
Sleep(0);
return TRUE;
}
CBallTraceCommand::CBallTraceCommand()
{
allCodeRight=0;
allCodeLeft=0;
m_pImageProcess = NULL;
m_pMotion = NULL;
m_pImage=NULL;
m_Step=0;
bMove=FALSE;
memset(&m_TraceParam,0,sizeof(m_TraceParam));
m_TraceParam.nBaseSpeed = 500;
m_TraceParam.nLastSpeed = 0;
m_TraceParam.nAddSpeed = 100;
m_TraceParam.nDiffSpeed = 100;
m_TraceParam.nState = UNKNOWN;
speedLeft=-600;
speedRight=-600;
}
CBallTraceCommand::~CBallTraceCommand()
{
}
void CBallTraceCommand::TraceDecide(int nArea,int x,int y)
{
if(NULL == m_pMotion||!(((CSduDemoDlg*)AfxGetMainWnd())->bStart))
{
return;
}
//static int nNum = 0;
CUR_STATE curState = UNKNOWN;
int nLeftPWM = m_TraceParam.nBaseSpeed;//+m_TraceParam.nDiffPWM;
int nRightPWM = m_TraceParam.nBaseSpeed;
if(nArea>500)
{
//m_pMotion->SetSpeed(WHEEL_LEFT,nLeftPWM);
//m_pMotion->SetSpeed(WHEEL_RIGHT,nRightPWM);
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)&&(m_Step==0))
{
/*if(curState&HORZ_MIDDLE)
{
//keep state
TRACE("Keep\n");
m_pMotion->Move(WHEEL_LEFT,0);
m_pMotion->Move(WHEEL_RIGHT,0);
}
else if(curState&HORZ_LEFT)
{
TRACE("MIDDLE AND LEFT\n");
m_pMotion->Move(WHEEL_LEFT,-0.8*nLeftSpeed);
m_pMotion->Move(WHEEL_RIGHT,0.8*nRightSpeed);
}
else if(curState&HORZ_RIGHT)
{
TRACE("MIDDLE AND RIGHT\n");
//nLeftPWM += m_TraceParam.nAddPWM;
m_pMotion->Move(WHEEL_LEFT,0.8*nLeftSpeed);
m_pMotion->Move(WHEEL_RIGHT,-0.8*nRightSpeed);
}*/
m_pMotion->SetSpeed(WHEEL_LEFT,nLeftPWM);
m_pMotion->SetSpeed(WHEEL_RIGHT,nRightPWM);
((CSduDemoDlg*)AfxGetMainWnd())->SetDlgItemText(IDC_EDITINFO,"發現目標");
}
else if((curState&VERT_FOR)&&(m_Step==0))
{
/*if(curState&HORZ_MIDDLE)
{
TRACE("FOR and MIDDLE\n");
m_pMotion->Move(WHEEL_LEFT,nLeftPWM);
m_pMotion->Move(WHEEL_RIGHT,nRightPWM);
}
else if(curState&HORZ_LEFT)
{
TRACE("FOR and LEFT\n");
nLeftPWM -= m_TraceParam.nAddPWM;
m_pMotion->Move(WHEEL_LEFT,nLeftSpeed);
m_pMotion->Move(WHEEL_RIGHT,nRightSpeed);
}
else if(curState&HORZ_RIGHT)
{
TRACE("FOR and RIGHT\n");
//nRightPWM -= m_TraceParam.nAddPWM;
m_pMotion->Move(WHEEL_LEFT,nLeftSpeed);
m_pMotion->Move(WHEEL_RIGHT,nRightSpeed);
}*/
m_pMotion->SetSpeed(WHEEL_LEFT,nLeftPWM);
m_pMotion->SetSpeed(WHEEL_RIGHT,nRightPWM);
((CSduDemoDlg*)AfxGetMainWnd())->SetDlgItemText(IDC_EDITINFO,"發現目標");
}
else if((curState&VERT_BACK)&&m_Step==0)
{
/* if(curState&HORZ_MIDDLE)
{
TRACE("BACK and MIDDLE\n");
m_pMotion->Move(WHEEL_LEFT,-nLeftSpeed);
m_pMotion->Move(WHEEL_RIGHT,-nRightSpeed);
}
else if(curState&HORZ_LEFT)
{
TRACE("BACK and LEFT\n");
nLeftPWM += m_TraceParam.nAddPWM;
m_pMotion->Move(WHEEL_LEFT,-nLeftSpeed);
m_pMotion->Move(WHEEL_RIGHT,-nRightSpeed);
}
else if(curState&HORZ_RIGHT)
{
TRACE("BACK and RIGHT\n");
nRightPWM += m_TraceParam.nAddPWM;
m_pMotion->Move(WHEEL_LEFT,-nLeftSpeed);
m_pMotion->Move(WHEEL_RIGHT,-nRightSpeed);
}
*/
//m_pMotion->SetSpeed(WHEEL_LEFT,0);
//m_pMotion->SetSpeed(WHEEL_RIGHT,0);
/*long tempL=0,tempR=0;
speedLeft=nLeftPWM;
for(;;)
{
if(0>speedLeft)
{
tempL=speedLeft+10;
if(tempL>0)
tempL=0;
}
else if(0<speedLeft)
{
tempL=speedLeft-10;
if(tempL<0)
tempL=0;
}
if(0>speedRight)
{
tempR=speedRight+10;
if(tempR>0)
tempR=0;
}
else if(0<speedRight)
{
tempR=speedRight-10;
if(tempR<0)
tempR=0;
}
speedLeft=tempL;
speedRight=tempR;
m_pMotion->SetSpeed(WHEEL_LEFT,tempL);
m_pMotion->SetSpeed(WHEEL_RIGHT,tempR);
if(tempL==0 && tempR==0)
break;
Sleep(10);
}*/
m_pMotion->End();
m_Step+=1;
((CSduDemoDlg*)AfxGetMainWnd())->SetDlgItemText(IDC_EDITINFO,"到達目標");
}
else
{
Work();
}
/* else if(curState == OUTSIGHT)
{
/*if(m_TraceParam.nState & HORZ_LEFT)
{
TRACE("OUT to clock\n");
m_pMotion->Move(WHEEL_LEFT,-nLeftSpeed+150);
m_pMotion->Move(WHEEL_RIGHT,nRightSpeed-150);
}
else if(m_TraceParam.nState & HORZ_RIGHT)
{
TRACE("OUT to unclock\n");
m_pMotion->Move(WHEEL_LEFT,nLeftSpeed-150);
m_pMotion->Move(WHEEL_RIGHT,-nRightSpeed+150);
}
else
{
TRACE("Still OUT\n");//停下來
}
//m_pMotion->SetSpeed(WHEEL_LEFT,nLeftPWM);
//m_pMotion->SetSpeed(WHEEL_RIGHT,nRightPWM);
}*/
m_TraceParam.nState = curState;
}
void CBallTraceCommand::Work()
{
static int icount=0;
int DeviceID=((CSduDemoDlg*)AfxGetMainWnd())->DeviceID;
int numOfModules=((CSduDemoDlg*)AfxGetMainWnd())->numOfModules;
for(int i=0;i<numOfModules;i++)
{
ID[i]=((CSduDemoDlg*)AfxGetMainWnd())->LogicIdMap[i];
}
if(m_Step==1)
{
m_Step+=1;
PCube_homeAll(DeviceID);
PCube_waitForHomeEndAll(DeviceID,30000);
}
else if(m_Step==2)
{
m_Step+=1;
PCube_setRampAcc( 0, ID[0], 1.0);
PCube_moveStep( 0, ID[0], 1.57, 10000);
PCube_setRampAcc( 0, ID[1], 1.0);
PCube_moveStep( 0, ID[1], 1.57, 10000);
PCube_setRampAcc( 0, ID[2], 1.0);
PCube_moveStep( 0, ID[2], 1.57, 10000);
PCube_setRampAcc( 0, ID[3], 1.0);
PCube_moveStep( 0, ID[3], 0.02, 10000);
PCube_waitForMotionEndAll(DeviceID,30000);
((CSduDemoDlg*)AfxGetMainWnd())->SetDlgItemText(IDC_EDITINFO,"抓取目標");
}
else if(m_Step==3)
{
m_Step+=1;
//PCube_setRampAcc( 0, ID[0], 1.0);
//PCube_moveStep( 0, ID[0], -1.57, 10000);
PCube_setRampAcc( 0, ID[1], 1.0);
PCube_moveStep( 0, ID[1], 0, 10000);
//PCube_setRampAcc( 0, ID[2], 1.0);
//PCube_moveStep( 0, ID[2], 1.57, 10000);
PCube_waitForMotionEndAll(DeviceID,30000);
((CSduDemoDlg*)AfxGetMainWnd())->SetDlgItemText(IDC_EDITINFO,"返回起點");
}
else if(m_Step==4)
{
if(!bMove)
{
m_pMotion->Init();
//m_pMotion->Start();
m_pMotion->SetSpeed(WHEEL_LEFT,speedLeft);
m_pMotion->SetSpeed(WHEEL_RIGHT,speedRight);
bMove=TRUE;
}
icount++;
if(icount>=50)
{
long tempL=speedLeft/30*(80-icount),tempR=speedRight/30*(80-icount);
m_pMotion->SetSpeed(WHEEL_LEFT,tempL);
m_pMotion->SetSpeed(WHEEL_RIGHT,tempR);
//m_pMotion->End();
//m_pMotion->SetSpeed(WHEEL_LEFT);
//m_pMotion->SetSpeed(WHEEL_RIGHT);
}
if(icount==80)
{
m_pMotion->SetSpeed(WHEEL_LEFT,0);
m_pMotion->SetSpeed(WHEEL_RIGHT,0);
m_Step+=1;
((CSduDemoDlg*)AfxGetMainWnd())->SetDlgItemText(IDC_EDITINFO,"放下物體");
}
}
else if(m_Step==5)
{
m_Step+=1;
PCube_setRampAcc( 0, ID[0], 1.0);
PCube_moveStep( 0, ID[0], 1.57, 10000);
PCube_setRampAcc( 0, ID[1], 1.0);
PCube_moveStep( 0, ID[1], 1.57, 10000);
PCube_setRampAcc( 0, ID[2], 1.0);
PCube_moveStep( 0, ID[2], 1.57, 10000);
PCube_setRampAcc( 0, ID[3], 1.0);
PCube_moveStep( 0, ID[3], 0.05, 10000);
PCube_waitForMotionEndAll(DeviceID,30000);
((CSduDemoDlg*)AfxGetMainWnd())->SetDlgItemText(IDC_EDITINFO,"手臂復位");
}
else if(m_Step==6)
{
PCube_homeAll(DeviceID);
PCube_waitForMotionEndAll(DeviceID,30000);
((CSduDemoDlg*)AfxGetMainWnd())->SetDlgItemText(IDC_EDITINFO,"完成動作序列");
m_Step++;
}
else
{
Sleep(0);
if(m_Step==7&&((CSduDemoDlg*)AfxGetMainWnd())->bStart)
{
m_Step=0;
bMove=false;
icount=0;
((CSduDemoDlg*)AfxGetMainWnd())->bStart=false;
(((CSduDemoDlg*)AfxGetMainWnd())->GetDlgItem(IDC_START))->EnableWindow(true);
}
}
}
/*unsigned __stdcall CBallTraceCommand::CounterProc(LONG nOwner,void* pResult)
{
COUNTER_DATA* pValue = (COUNTER_DATA*)pResult;
CBallTraceCommand* pThis = (CBallTraceCommand*)nOwner;
//pThis->ProcessCode(pValue->lCounter[WHEEL_LEFT],pValue->lCounter[WHEEL_RIGHT],pValue->lCounter[AXIS_EXTEND]);
//這三個參數為一個周期內編碼器差數數。
if(pThis->bMove)
{
pThis->allCodeLeft+=pValue->lCounter[WHEEL_LEFT];
pThis->allCodeRight+=pValue->lCounter[WHEEL_RIGHT];
}
return 0;
}*/
/*void CMotionPage::ProcessCode(long dcodeleft, long dcoderight, long dcodeextern)
{
m_CodeLeft+=dcodeleft;
m_CodeRight+=dcoderight;
m_CodeExtern+=dcodeextern;
m_CodeLeft=m_CodeLeft%66000;
m_CodeRight=m_CodeRight%66000;
m_CodeExtern=m_CodeExtern%66000;
m_CodeSecondLeft=(double)dcodeleft/(double)m_T*60000/66000;
m_CodeSecondRight=(double)dcoderight/(double)m_T*60000/66000;
m_CodeSecondExtern=(double)dcodeextern/(double)m_T*60000/66000;
static int iMotionTime=0;
iMotionTime++;
if(iMotionTime*m_T>=50)
{
if(fabs(m_CodeSecondLeft)>0 || fabs(m_CodeSecondRight)>0)
{
m_StaticLine.AddData(0,fabs((long)m_CodeSecondLeft));
m_StaticLine.AddData(1,fabs((long)m_CodeSecondRight));
m_StaticLine.DrawLine();
iMotionTime=0;
}
}
m_SpeedLeft=GetSpeedFromDCode(dcodeleft);
m_SpeedRight=GetSpeedFromDCode(dcoderight);
m_SpeedExtern=GetSpeedFromDCode(dcodeextern);
PostMessage(WM_MSG_MOTION_UPDATEDATA);
}*/
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -