voycmd.cpp

足球机器人功能demo

// VoyCmd.cpp: implementation of the CVoyCmd class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "VoyCmd.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

//自动查询超声传感器线程
UINT QueryUSonicThread(LPVOID pParam)
{
	CVoyCmd * pcmd = (CVoyCmd *)pParam;
	const UINT Time = pcmd->QueryUSonicTime;
	while (Time == pcmd->QueryUSonicTime && FALSE == pcmd->bToEndThreads)
	{
		pcmd->QueryUltrasonicSensor();		
		Sleep(Time);
	}
	return 0L;
}

//自动查询红外传感器线程
UINT QueryInfraRedThread(LPVOID pParam)
{
	CVoyCmd * pcmd = (CVoyCmd *)pParam;
	const UINT Time = pcmd->QueryInfraRedTime;
	while (Time == pcmd->QueryInfraRedTime && FALSE == pcmd->bToEndThreads)
	{
		pcmd->QueryInfrared();
		Sleep(Time);
	}
	return 0L;
}

//自动查询罗盘信息线程
UINT QueryCompassThread(LPVOID pParam)
{
	CVoyCmd * pcmd = (CVoyCmd *)pParam;
	const UINT Time = pcmd->QueryCompassTime;
	while (Time == pcmd->QueryCompassTime && FALSE == pcmd->bToEndThreads)
	{
		pcmd->QueryCompass();
		Sleep(Time);
	}
	return 0L;
}

CVoyCmd::CVoyCmd()
{
	m_charUltrasonic=new UCHAR[ULTRASONICAMOUNT];
	EnableUSonic= new BOOL[ULTRASONICAMOUNT];
	ValUSonic = new DOUBLE[ULTRASONICAMOUNT];

	m_charInfrared= new UCHAR[INFRAREDCHAR];
	EnableInfrared = new BOOL[INFRAREDMOUNT];
	ValInfrared = new BOOL[INFRAREDMOUNT];

	for (int i=0;i<ULTRASONICAMOUNT;i++)
	{
		ValUSonic[i] = 1;
	}

	for (i=0;i<ULTRASONICAMOUNT;i++)
	{
		EnableUSonic[i] = TRUE;
		EnableInfrared[i] = TRUE;
	}

	for (i=0;i<INFRAREDMOUNT;i++)
	{
		ValInfrared[i] = FALSE;
	}
	
	event=CreateEvent(NULL,TRUE,FALSE,NULL);

	nState =0;
	m_iLspeed = m_iRspeed = 0;

	m_pRcvBuf=new UCHAR[MAX_BUF];
	m_pSendBuf=new UCHAR[MAX_BUF];
	m_nRcvIndex = 0;

	m_pBeh = NULL;
	m_pPhy = NULL;
	m_pCap = NULL;

	QueryUSonicTime = 0;
	QueryInfraRedTime = 0;
	QueryCompassTime = 0;

	bToEndThreads = FALSE;
	bToRemoveBeh = FALSE;

	CmdShow_Send = NULL;
	CmdShow_Rec = NULL;

	USDataShow = NULL;
	IRDataShow = NULL;
	CmpsDataShow = NULL;
}

CVoyCmd::~CVoyCmd()
{
	QueryUSonicTime = 0;
	QueryInfraRedTime = 0;
	
	bToEndThreads = TRUE;

	CloseHandle(event);
	
	delete[] m_charUltrasonic;
	delete[] EnableUSonic;
	delete[] ValUSonic;
	delete[] m_charInfrared;
	delete[] ValInfrared;
	delete[] EnableInfrared;
	m_charInfrared = m_charUltrasonic = NULL;

	delete []m_pRcvBuf;
	delete []m_pSendBuf;

	//延时，给查询线程足够的时间退出
	if (QueryUSonicTime > QueryInfraRedTime)
	{
		Sleep(QueryUSonicTime+10);
	}
	else
	{
		Sleep(QueryInfraRedTime+10);
	}
	CoUninitialize();
}

//将接受到的数据分割组合成完整指令
void CVoyCmd::m_ParseBuffer(const UCHAR buf)
{
	//search for frame start tag
		if (buf==(UCHAR)0xaa && m_cLast==(UCHAR)0x55 && !m_bFrameStart)
		{
			m_pRcvBuf[0] = 0x55;
			m_pRcvBuf[1] = 0xAA;
			m_bFrameStart=true;
			return;
		}
			
		if (m_bFrameStart)
		{
			//put received data into buffer
			m_cLast=0x00;
			m_pRcvBuf[m_nRcvIndex+2]=buf;
			if (m_nRcvIndex==1)
			{
				m_nFrameLength=(buf & 0x3f)+4;
			}
			m_nRcvIndex++;
			//receive one frame, invoke ParseFrame to parse
			if (m_nRcvIndex==m_nFrameLength)
			{ 
				m_ParseFrame(m_pRcvBuf+2, m_nRcvIndex); 
				m_ResetRcvBuf(); 
			}

			//receive buffer overflow
				if (m_nRcvIndex>=MAX_BUF) 
				{
					m_ResetRcvBuf();
				}
		}
		else
			m_cLast=buf;
}

void CVoyCmd::m_ParseFrame(UCHAR *buf, int length)
{
	CString str,strTemp;

	if (!m_ValidFrame(buf,length))
	{
		m_ResponseError();
		return;
	}

	//通过局域网转发指令
	if (m_netsend.isConnecting())
	{
		m_netsend.Send(m_pRcvBuf,length+2);
	}

	bool ctrlcodevalid=true;
	switch (buf[2])
	{
		//there are two section of response
		//first is for query response
	case 0x30:
		{
			//query ultrasonic sensor response
			for (int i=0;i<ULTRASONICAMOUNT;i++)
			{
				m_charUltrasonic[i]=buf[i+3];
				ValUSonic[i] = m_CalDistance(m_charUltrasonic[i]);	//换算成距离
			}

			ShowSensor();

			if (m_pBeh != NULL)
			{
				if (bToRemoveBeh == TRUE)
				break;

				m_pBeh->AfterUpdateUSonic(ValUSonic,EnableUSonic,nState);
			}
			break;
		}
	case 0x34:
		{	//罗盘陀螺仪返回数据
			/*if (compass != NULL)
			{
				compass->Update(&buf[5],2);
				compass->Invalidate();
			}*/
			if (CmpsDataShow != NULL)
			{
				if (buf[3] < 0x80)
				{
					str.Format("X轴滚转角度：%d°\n\n",buf[3]);
				}
				else
				{
					str.Format("X轴滚转角度：%d°\n\n",-(0xff-buf[3]));
				}

				if (buf[4] < 0x80)
				{
					strTemp.Format("Y轴滚转角度：%d°",buf[4]);
				}
				else
				{
					strTemp.Format("Y轴滚转角度：%d°",-(0xff-buf[4]));
				}
				str += strTemp;
				CmpsDataShow->SetWindowText(str);
			}
			break;
		}
	case 0x36:
		{
			//query infrared sensor response
			for (int i=0;i<INFRAREDCHAR;i++)
			{
				m_charInfrared[i]=buf[i+3];
				for (int j=7;j>=0;j--)
				{
				
					if ((m_charInfrared[i] & 0x01)==0x01)
					{
						ValInfrared[j+8*i]=false;
					}
					else
					{
						ValInfrared[j+8*i]=true;
					}

					m_charInfrared[i]=m_charInfrared[i] >> 1;
				}
				m_charInfrared[i]=m_charInfrared[i] >> 1;	//使m_charInfrared恢复到原来的位顺序
			}

			ShowSensor();
			
			if (m_pBeh != NULL)
			{
				if(bToRemoveBeh == TRUE)
					break;

				m_pBeh->AfterUpdateInfrared(m_charInfrared,EnableInfrared,nState);
			}
			break;
		}
		//second is for setting response
	case 0x21: break;
	case 0x24: break;
	case 0x25: break;
	case 0x26: break;
	case 0x2c: break;	//罗盘闭环直行回执
	case 0x2d: break;	//罗盘闭环旋转回执
	default:
		{
			ctrlcodevalid=false;
			break;
		}
	}
	
	SetEvent(event);

	if (ctrlcodevalid)
		m_Response(buf,length);
	else
		m_ResponseError();
}

void CVoyCmd::Parse(void *buf, int length)
{
	if (length==0 || buf==NULL) return;
			
		for (int i=0;i<length;i++)
			m_ParseBuffer(((UCHAR*)buf)[i]);
}

void CVoyCmd::m_ResetRcvBuf()
{
	memset(m_pRcvBuf, 0, MAX_BUF);
	m_nRcvIndex=0;
	m_bFrameStart=false;
	m_cLast=0x00;
	m_nFrameLength=0;
}

void CVoyCmd::m_ResetSendBuf()
{
	memset(m_pSendBuf, 0, MAX_BUF); 
	m_nSendlength=0;
}

BOOL CVoyCmd::m_ValidFrame(UCHAR *buf, int length)
{
	if ((buf[1] & 0x3f)!=length-4)
		return false;

	int sum=0x000000ff;

	for (int i=0;i<length-1;i++)
		sum+=buf[i];

	if (buf[length-1]!=(UCHAR)(sum & 0x000000ff))
		return false;

	return true;
}

void CVoyCmd::m_ResponseError()
{	
	//解析不合协议的指令回调

}

void CVoyCmd::m_Response(UCHAR *recbuf, int length)
{	//解析合法指令回调
	m_pPhy->bSending = FALSE;

	//显示接收指令
	if (CmdShow_Rec != NULL)
	{
		CString cmd,str;
		CTime tm;
		tm = CTime::GetCurrentTime();
		cmd = tm.Format("%X:");
		for (int i=0;i<length+2;i++)
		{
			str.Format("%.2X ",m_pRcvBuf[i]);
			cmd += str;
		}
		CmdShow_Rec->AddString(cmd);
		CmdShow_Rec->SetCurSel(CmdShow_Rec->GetCount()-1);
	}
}

UCHAR CVoyCmd::m_CalSum(int length)
{
	int temp=0;
	for (int i=0;i<length;i++)
	temp+=m_pSendBuf[i];

	UCHAR ret;
	ret=(UCHAR)(temp & 0x000000ff);
	return ret;
}

DOUBLE CVoyCmd::m_CalDistance(UCHAR inUSChar)
{
	DOUBLE Distance;
	Distance = (DOUBLE) inUSChar*0.02174;
	return Distance;
}

void CVoyCmd::m_GenerateSendBuffer(UCHAR addr, UCHAR status, UCHAR length, UCHAR ctrlcode, UCHAR *data)
{
	m_pSendBuf[0]=(UCHAR)0x55;
	m_pSendBuf[1]=(UCHAR)0xaa;
	m_pSendBuf[2]=addr;
	m_pSendBuf[3]=((status << 6) & 0xc0) | (length & 0x3f);
	m_pSendBuf[4]=ctrlcode;
	if (length>0 && data!=NULL)
	{
		memcpy(&m_pSendBuf[5],data,length);
	}
	m_pSendBuf[length+5]=m_CalSum(length+5);

	m_nSendlength=length+6;

	//发送指令
	if (m_pPhy != NULL )
	{
		m_pPhy->Send(m_pSendBuf,m_nSendlength);

		//显示发送指令
		if (CmdShow_Send != NULL)
		{
			CString cmd,str;
			CTime tm;
			tm = CTime::GetCurrentTime();
			cmd = tm.Format("%X: ");
			for (int i=0;i<m_nSendlength;i++)
			{
				str.Format("%.2X ",m_pSendBuf[i]);
				cmd += str;
			}
			CmdShow_Send->AddString(cmd);
			CmdShow_Send->SetCurSel(CmdShow_Send->GetCount()-1);
		}
	}
	
	if (m_pBeh!= NULL)
	{
		m_pBeh->AfterSendCommand(m_pSendBuf,m_nSendlength,nState);
	}
	
}


void CVoyCmd::Brake(UCHAR breakmode)
{
	m_GenerateSendBuffer((UCHAR)0x01,0,1,(UCHAR)0x21,&breakmode);
	m_iLspeed = 0;
	m_iRspeed = 0;
	nState = STOP;
}

void CVoyCmd::SetBothMotorsSpeed(int inleftspeed, int inrightspeed)
{
	if (inleftspeed == m_iLspeed && inrightspeed == m_iRspeed)
	{
		return;
	}
	m_iLspeed = inleftspeed;
	m_iRspeed = inrightspeed;
	
    WORD leftspeed = m_CalculateSpeed(inleftspeed);
	WORD rightspeed = m_CalculateSpeed(inrightspeed);

	UCHAR bothspeed[4];	
	
	bothspeed[0]=(UCHAR)((leftspeed >> 8) & 0x00ff);
	bothspeed[1]=(UCHAR)(leftspeed & 0x00ff);

	bothspeed[2]=(UCHAR)((rightspeed >> 8) & 0x00ff);
	bothspeed[3]=(UCHAR)(rightspeed & 0x00ff);	

	m_GenerateSendBuffer((UCHAR)0x01,0,4,(UCHAR)0x26,bothspeed);
	m_UpdateState();
}

void CVoyCmd::SetLMotorSpeed(int inleftspeed)
{
	if (inleftspeed == m_iLspeed)
	{
		return;
	}

	m_iLspeed = inleftspeed;

	WORD leftspeed = m_CalculateSpeed(inleftspeed);

	UCHAR lspeed[2];

	lspeed[1]=(UCHAR)(leftspeed & 0x00ff);
	lspeed[0]=(UCHAR)((leftspeed >> 8) & 0x00ff);

	m_GenerateSendBuffer((UCHAR)0x01,0,2,(UCHAR)0x24,lspeed);
	m_UpdateState();
}

void CVoyCmd::SetRMotorSpeed(int inrightspeed)
{
	if (inrightspeed == m_iRspeed)
	{
		return;
	}
	m_iRspeed = inrightspeed;

	WORD rightspeed = m_CalculateSpeed(inrightspeed);

	UCHAR rspeed[2];

	rspeed[1]=(UCHAR)(rightspeed & 0x00ff);
	rspeed[0]=(UCHAR)((rightspeed >> 8) & 0x00ff);

	m_GenerateSendBuffer((UCHAR)0x01,0,2,(UCHAR)0x25,rspeed);
	m_UpdateState();
}

void CVoyCmd::QueryInfrared()
{
	m_GenerateSendBuffer((UCHAR)0x02,0,0,(UCHAR)0x36,NULL);
}

void CVoyCmd::QueryUltrasonicSensor()
{	
	UCHAR ultrasonicchar[3];

	for (int i=0;i<=3;i++)
	{
		ultrasonicchar[i]=0xff;
	}
	

	m_GenerateSendBuffer((UCHAR)0x02,0,3,(UCHAR)0x30,ultrasonicchar);
}

void CVoyCmd::SetBehavior(IBehavior *pBeh)
{
	if (pBeh != NULL)
	{
		m_pBeh = pBeh;
		m_pBeh->SetCmd(this);
	}
	else
	{	//去除行为
		bToRemoveBeh = TRUE;

		/*if (m_pBeh != NULL)
		{
			//等待行为调用完毕
			while(m_pBeh->m_Ref >0)
			{
				;
			}
		}*/
		
		m_pBeh = NULL;
		bToRemoveBeh = FALSE;
	}
}

WORD CVoyCmd::m_CalculateSpeed(int speed)
{
	BOOL forward;
	if (speed >= 0)
	{
		forward = TRUE;
	}
	else
	{
		forward = FALSE;
		speed = -speed;
	}

	if (speed > 30000)
	{
		speed = 30000;
	}

	WORD ret=0;
	speed &=0x7fff;

	if (forward)
		speed &= 0x7fff;
	else
		speed |= 0x8000;

	ret=speed;
	return ret;
}

void CVoyCmd::m_UpdateState()
{
	//左右速度相等
	if (m_iLspeed == m_iRspeed)
	{
		if (0 == m_iRspeed)
		{
			nState = STOP;
			return;
		}
		if (m_iLspeed > 0)
		{
			nState = FORWARD;
		}
		else
		{
			nState = BACKWARD;
		}
		return;
	}

	//左右电机大小相等，方向相反
	if (m_iLspeed == -m_iRspeed)
	{
		if (m_iLspeed > 0)
		{
			nState = RIGHT;
		}
		else
		{
			nState = LEFT;
		}
		return;
	}

	//右电机比左电机大（正方向上）
	if (m_iLspeed < m_iRspeed)
	{
		if (m_iLspeed + m_iRspeed > 0)
		{	//右前进速度较大
			nState = LEFTFRONT;
		} 
		else
		{	//左后退速度较大
			nState = RIGHTBACK;
		}
	}
	else
	{	//m_iLspeed > m_iRspeed
		if (m_iLspeed + m_iRspeed > 0)
		{	//左前进速度较大
			nState = RIGHTFRONT;
		} 
		else
		{	//右后退速度较大
			nState = LEFTBACK;
		}		
	}
}

UINT CVoyCmd::GetState()
{
	return nState;
}

void CVoyCmd::AutoQueryUSonic(UINT TimeGap)
{
	if (TimeGap == QueryUSonicTime)
	{
		return;
	}
	QueryUSonicTime = TimeGap;

	if (QueryUSonicTime != 0)
	{
		UCHAR sw = 0x01;
		m_GenerateSendBuffer(2,0,1,0x31,&sw);
		AfxBeginThread(QueryUSonicThread,(LPVOID)this,THREAD_PRIORITY_TIME_CRITICAL);
	}
	else
	{	//参数为0 ，停止超声硬件工作
		UCHAR sw = 0x00;
		m_GenerateSendBuffer(2,0,1,0x31,&sw);
	}
}

void CVoyCmd::AutoQueryInfraRed(UINT TimeGap)
{
	if (TimeGap == QueryInfraRedTime)
	{
		return;
	}
	QueryInfraRedTime = TimeGap;
	if (QueryInfraRedTime != 0)
	{
		AfxBeginThread(QueryInfraRedThread,(LPVOID)this,THREAD_PRIORITY_NORMAL);
	}
}

void CVoyCmd::AutoQueryCompass(UINT TimeGap)
{
	if (TimeGap == QueryCompassTime)
	{
		return;
	}
	QueryCompassTime = TimeGap;
	if (QueryCompassTime != 0)
	{
		AfxBeginThread(QueryCompassThread,(LPVOID)this,THREAD_PRIORITY_NORMAL);
	}
}

void CVoyCmd::ShowSensor()
{
	CString strUS,strIR;
	CString temp;
	int i;

	if (USDataShow != NULL)
	{
		for (i=0;i<ULTRASONICAMOUNT;i++)
		{
			if (EnableUSonic[i] == FALSE)
			{
				continue;
			}
			
			temp.Format("%2d号:%2.2f   ",i+1,ValUSonic[i]);
			strUS += temp;
			/*if ((i+1)%3 == 0)
			{
				*inUSstr += "\r";
			}*/
		}
		USDataShow->SetWindowText(strUS);
	}

	if (IRDataShow != NULL)
	{
		for (i=0;i<INFRAREDMOUNT;i++)
		{
			if (EnableInfrared[i] == FALSE)
			{
				continue;
			}
			
			temp.Format("%2d号:",i+1);
			if (ValInfrared[i] == TRUE)
			{
				temp+="有  ";
			} 
			else
			{
				temp+="无  ";
			}
			strIR += temp;
			/*if (i%2 == 1)
			{
				*inIRstr += "\r";
			}*/
		}
		IRDataShow->SetWindowText(strIR);
	}

	/*if (pieSensor != NULL)
	{
		pieSensor->UpdateUntrasonic(ValUSonic,EnableUSonic);
		pieSensor->UpdateInfrared(ValInfrared);
		pieSensor->Invalidate();
	}*/
}

void CVoyCmd::SpeedByCmps(int inSpeed)
{
	WORD wspeed = m_CalculateSpeed(inSpeed);
	UCHAR speed[4];
	speed[0] = 0x00;
	speed[1] = 0x00;
	speed[3] = (UCHAR)(wspeed & 0x00ff);
	speed[2] = (UCHAR)((wspeed>>8) & 0x00ff);

	m_GenerateSendBuffer((UCHAR) 0x01,0,4,0x2C,speed);
}

void CVoyCmd::CircleByCmps(int inAngle, int inSpeed)
{
	if (inAngle <-360 || inAngle > 360)
	{
		return;
	}

	WORD wAngle,wSpeed;
	UCHAR toSend[4];

	wAngle = m_CalculateSpeed(inAngle*39000/3445);
	toSend[1] = (UCHAR) wAngle&0x00ff;
	toSend[0] = (UCHAR) (wAngle>>8)&0x00ff;

	wSpeed = m_CalculateSpeed(inSpeed);
	toSend[3] = (UCHAR)(wSpeed & 0x00ff);
	toSend[2] = (UCHAR)((wSpeed>>8) & 0x00ff);

	m_GenerateSendBuffer((UCHAR) 0x01,0,4,0x2D,toSend);
}

void CVoyCmd::QueryCompass()
{
	m_GenerateSendBuffer(0x03,0,0,0x34,NULL);
}

void CVoyCmd::Kick()
{
	m_GenerateSendBuffer(0x04,0,0,0x2f,NULL);
}

void CVoyCmd::Demarcate()
{
	m_GenerateSendBuffer(0x01,0,0,0x2f,NULL);
}