fstarcmd.cpp

来自「手柄驱动DEMO程序源码」· C++ 代码 · 共 2,455 行 · 第 1/4 页

CPP
2,455 行
字号
1234
// VoyCmd.cpp: implementation of the CFstarCmd class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "FstarCmd.h"

#include "IBehavior.h"
#include "Enjoystick.h"

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

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


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

//罗盘标定线程
UINT DemarcateForCmps(LPVOID pParam)
{
	CFstarCmd * pcmd = (CFstarCmd *)pParam;
	Sleep(500);
	pcmd->SetBothMotorsSpeed(-700,-700);
	Sleep(20000);
	pcmd->Brake(1);
	return 0L;
}
// 查询IO状态线程
UINT QueryIOThread(LPVOID pParam)
{
	//	DWORD exitcode;
	CFstarCmd *pCmd=(CFstarCmd * )pParam;
	UINT Time=pCmd->QueryIOTime;
	while (Time==pCmd->QueryIOTime&&FALSE==pCmd->bToEndThreads)
	{
		pCmd->QueryIO();
		Sleep(Time);
	}
	
	return 0L;
}

//查询AD线程
UINT QueryADThread(LPVOID pParam)
{
	CFstarCmd *pCmd=(CFstarCmd * )pParam;
	UINT Time=pCmd->QueryADTime;
	while (Time==pCmd->QueryADTime&&FALSE==pCmd->bToEndThreads)
	{
		pCmd->QueryAD();
		Sleep(Time);
	}
	return 0L;
}
//同时查询ADIO线程
UINT QueryADIOThread(LPVOID pParam)
{
	CFstarCmd *pCmd=(CFstarCmd * )pParam;
	UINT Time=pCmd->QueryADIOTime;
	while (Time==pCmd->QueryADIOTime&&FALSE==pCmd->bToEndThreads)
	{
		pCmd->QueryADIO();
		Sleep(Time);
	}
	return 0L;
}
//碰撞后，前进前延迟线程
UINT Hit_Move(LPVOID pParam)
{
	CFstarCmd *pCmd=(CFstarCmd * )pParam;
	Sleep(1000);
	pCmd->SetBothMotorsSpeed(4500,4500);
	return 0L;
}
//碰撞后延迟线程
UINT Hit_Delay(LPVOID pParam)
{
	CFstarCmd *pCmd=(CFstarCmd * )pParam;
	Sleep(200);
	return 0L;
}

extern CEnjoyStickApp theApp;
//构造函数

CFstarCmd::CFstarCmd()
{	
	for(int i=0;i<23;i+=2)
	{
		serveData[i]=0x5A;
		serveData[i+1]=0x96;
	}
	for (int j=0;j<8;j++)
	{
		motorData[j]=0;
	}
	nState =0;
	m_iLspeed = m_iRspeed = 0;
	m_fSpReduc = (float)14.6;
	
	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;
	QueryCompassTime = 0;
	
	hitStatus = 0xFF;
	//寻光时的速度值和系数设置
	lspeed = SPEED;
	rspeed = SPEED;
	
	maxLgt = 210;
	minLgt = 20;
	maxTmp = 30;
	minLgt = 19;
	
	//	minTmp=0x10;
	light_stop = 0;
	jumpflag = 0;
	
	demo_ad_flag = 0;
	demo_io_flag = 0;
	demo_us_flag = 0;
	detect_flag = 0;
	
	bToEndThreads = FALSE;
	bToEndDance	= FALSE;
	bToEndDetect = FALSE;
	bAct1End = FALSE;
	stopCounter=0;
	
	demoIndex = 0;
}
//析构函数
CFstarCmd::~CFstarCmd()
{
	//	QueryUSonicTime = 0;
	QueryADTime=0;
	QueryIOTime=0;
	bToEndThreads = TRUE;
	
	
	delete []m_pRcvBuf;
	delete []m_pSendBuf;
	
	//延时，给查询线程足够的时间退出
	if (QueryADTime > QueryIOTime)
	{
		Sleep(QueryADTime+10);
	}
	else
	{
		Sleep(QueryIOTime+10);
	}
	//	Sleep(QueryADTime+100);
	CoUninitialize();
}

//将接受到的数据分割组合成完整指令
void CFstarCmd::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 CFstarCmd::m_IOProcess()
{
	switch(demo_io_flag)
	{
	case 0:
		SetBothMotorsSpeed(0,0);
		break;
	case IO_ONLYSEARCH:
		m_SetIOData();
		break;
	case IO_AUTOMOVE:
		m_SetHitIcon(m_IOStatus[1]);
		m_AwayObstacle(m_IOStatus[1]);
		break;
	case IO_REMOTESEARCH:
		break;
	case SELFDETECT:
		//PlaySound("multi_ok.wav",GetModuleHandle(NULL),SND_ASYNC|SND_FILENAME);
		demo_io_flag=0;
		detect_flag=0;
		break;
	case IO_FOREDGE:
		m_ForEdge();
		break;
	case IO_FOLLOW:
	case IO_FORLIGHT:
		break;
	default:
		break;
	}
}
void CFstarCmd::m_ADProcess()
{
	switch(demo_ad_flag)
	{
	case 0:
		SetBothMotorsSpeed(0,0);
		break;
	case AD_ONLYSEARCH:
		m_SetADData();
		break;
// 	case AD_FOREDGE:
// 		m_ForEdge();
// 		break;
	case AD_FORLIGHT:
		m_Forlight();
		break;
	case AD_OFFHEAT:
		m_OffHeat();
		break;
	case AD_REMOTESEARCH:
		break;
	case AD_AHFORLIGHT:
		m_AdForlight();
	default:
		break;
	}
}
void CFstarCmd::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[0])
	{
	case 0x01:
		{
			switch(buf[2])
			{
			case 0x01:
				//PlaySound("gpro_ok.wav",GetModuleHandle(NULL),SND_ASYNC|SND_FILENAME);
				detect_flag=0;
				break;
			case 0x02:
				//PlaySound("advb_ok.wav",GetModuleHandle(NULL),SND_ASYNC|SND_FILENAME);
				detect_flag=0;
				break;
			case 0x24:
				break;
			case 0x25:
				break;
			case 0x26:
				break;
			case 0x27:
				break;
			default:
				ctrlcodevalid=false;
				break;
			}
		}	
		break;
	case 0x52:
		{
			//PlaySound("LCD_ok.wav",GetModuleHandle(NULL),SND_ASYNC|SND_FILENAME);
			detect_flag=0;
		}
		break;
	case 0x53:
		{
			switch(buf[2])
			{
			case 0x01:
				{
					m_USdata[0]=buf[3];
					m_USdata[1]=buf[4];
					m_USprocess();
				}
				break;
			default:
				ctrlcodevalid=false;				
				break;
			}
		}
		break;
	case 0x71:
		{
			switch(buf[2])
			{
			case 0x05:
				{
					m_IOStatus[0]=buf[3];
					m_IOStatus[1]=buf[4];
					m_IOProcess();				
				}
				break;
			case 0x06:
				{
					for (int i=0;i<8;i++)
					{
						m_Addata[i]=buf[3+i];
					}
					m_ADProcess();
				}
				break;
			default:
				ctrlcodevalid=false;		
				break;
			}
		}
		break;
	default:
		break;
	}
	
	if (ctrlcodevalid)
		m_Response(buf,length);
	else
		m_ResponseError();
}

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

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

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

BOOL CFstarCmd::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 CFstarCmd::m_ResponseError()
{	
	//解析不合协议的指令回调
	
}

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

UCHAR CFstarCmd::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 CFstarCmd::m_CalDistance(UCHAR inUSChar)
{
	DOUBLE Distance;
	Distance = (DOUBLE) inUSChar*0.02174;
	return Distance;
}

void CFstarCmd::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);
	}		
}


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

void CFstarCmd::SetBothMotorsSpeed(int inleftspeed, int inrightspeed)
{
	//if (inleftspeed == m_iLspeed && inrightspeed == m_iRspeed)
	//{
	//	if (stopCounter>3)
	//	{
	//		return;
	//	}
	//	if ((inleftspeed!=0)&(inrightspeed!=0))
	//	{
	//		stopCounter++;
	//		return;
	//	} 
	//}
	m_iLspeed = inleftspeed;
	m_iRspeed = inrightspeed;
	
	WORD leftspeed = m_CalculateSpeed(m_iLspeed);
	WORD rightspeed = m_CalculateSpeed(m_iRspeed);
	
	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);//26为设置两个电机的速度
	m_UpdateState();
}

void CFstarCmd::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 CFstarCmd::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 CFstarCmd::QueryInfrared()
// {
// 	m_GenerateSendBuffer((UCHAR)0x02,0,0,(UCHAR)0x36,NULL);
// }

void CFstarCmd::SetBehavior(IBehavior *pBeh)
{	
	m_pBeh = pBeh;
	
	if (m_pBeh != NULL)
	{
		m_pBeh->SetCmd(this);
	}
}

WORD CFstarCmd::m_CalculateSpeed(int speed)
{
	BOOL forward;
	if (speed >= 0)
	{
		forward = TRUE;
	}
	else
	{
		forward = FALSE;
		speed = -speed;
	}
	
	if (speed >6000)
	{
		speed = 6000;
	}
	
	WORD ret=0;
	speed &=0x7fff;
	
	if (forward)
		speed &= 0x7fff;
	else
		speed |= 0x8000;
	
	ret=speed;
	return ret;
}

void CFstarCmd::m_UpdateState()
{
	//左右速度相等
	if (m_iLspeed == m_iRspeed)
1234

fstarcmd.cpp - 源码说明

本页面展示了「手柄驱动DEMO程序源码」中的 fstarcmd.cpp 源码文件，采用 C++ 编程语言编写，共 2,455 行代码。您可以在线阅读完整代码内容，也可以返回资源详情页下载完整源码包进行本地学习和开发。

虫虫开发者社区收录了大量与手柄驱动相关的技术资源，包括源代码、技术文档、电路图等，是电子工程师和嵌入式开发者的专业学习平台。

⌨️ 快捷键说明

复制代码Ctrl + C
搜索代码Ctrl + F
全屏模式F11
增大字号Ctrl + =
减小字号Ctrl + -
显示快捷键?