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);
}*/