fstarcmd.cpp

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

CPP
2,455 行
字号
1234
	{
		if (0 == m_iRspeed)
		{
			::PostMessage(m_hWnd,UM_TEXT,STOP,0);
			return;
		}
		if (m_iLspeed > 0)
		{
			::PostMessage(m_hWnd,UM_TEXT,RIGHT,0);
		}
		else
		{
			::PostMessage(m_hWnd,UM_TEXT,LEFT,0);
		}
		return;
	}
	
	//左右电机大小相等，方向相反
	if (m_iLspeed == -m_iRspeed)
	{
		if (m_iLspeed > 0)
		{
			::PostMessage(m_hWnd,UM_TEXT,FORWARD,0);
		}
		else
		{
			::PostMessage(m_hWnd,UM_TEXT,BACKWARD,0);
		}
		return;
	}
	
	//右电机比左电机大（正方向上）
	if (m_iLspeed < m_iRspeed)
	{
		if (m_iLspeed + m_iRspeed > 0)
		{	//右前进速度较大
			::PostMessage(m_hWnd,UM_TEXT,LEFTBACK,0);
		} 
		else
		{	//左后退速度较大
			::PostMessage(m_hWnd,UM_TEXT,RIGHTBACK,0);
		}
	}
	else
	{	//m_iLspeed > m_iRspeed
		if (m_iLspeed + m_iRspeed > 0)
		{	//左前进速度较大
			::PostMessage(m_hWnd,UM_TEXT,RIGHTFRONT,0);
		} 
		else
		{	//右后退速度较大
			::PostMessage(m_hWnd,UM_TEXT,LEFTFRONT,0);
		}		
	}
}

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

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

// void CFstarCmd::SpeedByGyro(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 CFstarCmd::TurnByGyro(int inAngle, int inSpeed)
{
	UINT Angle = 32768+inAngle*2;
// 	if (inAngle <-4076 || inAngle > 4076)
// 	{
// 		return;
// 	}
	//角度值计算
	WORD wAngle;
// 	BOOL forward;
// 	if (inAngle >= 0)
// 	{
// 		forward = TRUE;
// 	}
// 	else
// 	{
// 		forward = FALSE;
// 		inAngle = -inAngle;
// 	}	
// 	if (inAngle >4076)
// 	{
// 		inAngle = 4076;
// 	}	
// 	inAngle &=0x7fff;	
// 	if (forward)
// 		inAngle &= 0x7fff;
// 	else
// 		inAngle |= 0x8000;
	
	wAngle = Angle;
	//速度值计算
	WORD lrspeed = m_CalculateSpeed(inSpeed);	
	UCHAR bothspeed[4];	
	
	bothspeed[0]=(UCHAR)((wAngle >> 8) & 0x00ff);
	bothspeed[1]=(UCHAR)(wAngle & 0x00ff);
	
	bothspeed[2]=(UCHAR)((lrspeed >> 8) & 0x00ff);
	bothspeed[3]=(UCHAR)(lrspeed & 0x00ff);	
	
	m_GenerateSendBuffer((UCHAR)0x01,0,4,0x2D,bothspeed);
}





void CFstarCmd::Demarcate()
{
	m_GenerateSendBuffer(0x01,0,0,0x36,NULL);
}

// void CFstarCmd::SetLMotorPos(int inPos, int inSpeed)
// {
// 	UCHAR buf[6];
// 	WORD  speed;
// 	UINT  Pos;
// 	if (inSpeed < 0)
// 	{
// 		speed = (WORD)(-inSpeed*m_fSpReduc);
// 	}
// 	else
// 	{
// 		speed = (WORD)(inSpeed*m_fSpReduc);
// 	}
// 	buf[0] = speed<<8;
// 	buf[1] = speed;
// 	
// 	if (inPos < 0 )
// 	{	//反向位置
// 		Pos = (-inPos/360)*m_fSpReduc*4096;
// 		buf[2] = Pos <<24;
// 		buf[3] = Pos <<16;
// 		buf[4] = Pos <<8;
// 		buf[5] = Pos;
// 		buf[2] |= 0x80;
// 	}
// 	else
// 	{	//正向位置
// 		Pos = (inPos/360)*m_fSpReduc*4096;
// 		buf[2] = Pos <<24;
// 		buf[3] = Pos <<16;
// 		buf[4] = Pos <<8;
// 		buf[5] = Pos;
// 		buf[2] &= 0x7f;
// 	}
// 	
// 	m_GenerateSendBuffer((UCHAR)0x01,0,6,(UCHAR)0x22,buf);
// }

// void CFstarCmd::SetRMotorPos(int inPos, int inSpeed)
// {
// 	UCHAR buf[6];
// 	WORD  speed;
// 	UINT  Pos;
// 	if (inSpeed < 0)
// 	{
// 		speed = (WORD)(-inSpeed*m_fSpReduc);
// 	}
// 	else
// 	{
// 		speed = (WORD)(inSpeed*m_fSpReduc);
// 	}
// 	buf[0] = speed<<8;
// 	buf[1] = speed;
// 	
// 	if (inPos < 0 )
// 	{	//反向位置
// 		Pos = (-inPos/360)*m_fSpReduc*4096;
// 		buf[2] = Pos <<24;
// 		buf[3] = Pos <<16;
// 		buf[4] = Pos <<8;
// 		buf[5] = Pos;
// 		buf[2] |= 0x80;
// 	}
// 	else
// 	{	//正向位置
// 		Pos = (inPos/360)*m_fSpReduc*4096;
// 		buf[2] = Pos <<24;
// 		buf[3] = Pos <<16;
// 		buf[4] = Pos <<8;
// 		buf[5] = Pos;
// 		buf[2] &= 0x7f;
// 	}
// 	
// 	m_GenerateSendBuffer((UCHAR)0x01,0,6,(UCHAR)0x23,buf);
// }
void CFstarCmd::SetWnd(HWND hWnd)
{
	m_hWnd=hWnd;
}
void CFstarCmd::SearchMPara()
{
	m_GenerateSendBuffer((UCHAR)0x01,0,0,(UCHAR)0x28,0);
}
void CFstarCmd::SetServMotor(int inSpeed)
{
	UCHAR buf[9];
	if (inSpeed > 0xff)
	{
		buf[8] = 0xff;
	}
	else if (inSpeed < 0)
	{
		buf[8] = 0;
	}
	else
	{
		buf[8] = (BYTE) inSpeed;
	}
	m_GenerateSendBuffer(0x06,0,10,0x01,buf);
}
void CFstarCmd::stop()
{
	SetBothMotorsSpeed(0,0);
}
void CFstarCmd::m_Forlight()
{
	UCHAR tl_light,tr_light;
	UCHAR ave,sub,en_light;
	char lr_light;
	CString strrlight;
	CString strave;
	CString strsub;
	CString strllight;	
	
	tl_light=m_Addata[0];
	tr_light=m_Addata[1];
	
	lr_light=abs(tl_light-tr_light);
	en_light=(tl_light+tr_light)/2;
	sub=lr_light;
	ave=en_light;
	
	if (light_stop==1)
	{
		SetBothMotorsSpeed(0,0);
		light_stop=0;
		return;
	}
	if (en_light<=minLgt)
	{
		SetBothMotorsSpeed(-2004,2004);
	} 
	else if (en_light<maxLgt)
	{
		if (lr_light>20)
		{
			lspeed=1500+(tr_light-tl_light)*12;
			rspeed=1500+(tl_light-tr_light)*12;
			SetBothMotorsSpeed(lspeed,rspeed);
		}
		else
		{
			SetBothMotorsSpeed(2000,2000);
		}
	}
	else
	{
		if (lr_light>20)
		{
			lspeed=1000+(tr_light-tl_light)*10;
			rspeed=1000+(tl_light-tr_light)*10;
			SetBothMotorsSpeed(lspeed,rspeed);
		} 
		else
		{
			SetBothMotorsSpeed(0,0);
			light_stop=1;
		}
	}
	
	strllight.Format("%d",tl_light);
	strrlight.Format("%d",tr_light);
	strave.Format("%d",ave);
	strsub.Format("%d",sub);
	pLlight->SetWindowText(strllight);
	pRlight->SetWindowText(strrlight);
	pLAve->SetWindowText(strave);
	pLSub->SetWindowText(strsub);
	
}
void CFstarCmd::m_AwayObstacle(UCHAR temp)
{
	temp&=0x0F;
	if (hitStatus==temp)
	{
		return;
	}
	hitStatus=temp;	
	
	switch(hitStatus)
	{
	case 0x00:
		pHitstatus->SetWindowText("无障碍");
		AfxBeginThread(Hit_Move,(LPVOID)this,THREAD_PRIORITY_NORMAL);
		break;
	case 0x01:
		{
			pHitstatus->SetWindowText("左前");
			if (jumpflag==4)//不断左转右转试加一个扰动
			{
				SetBothMotorsSpeed(-3000,-4500);
				jumpflag=0;
			}
			else
			{
				SetBothMotorsSpeed(-2510,-5520);
				jumpflag+=1;
			}
			
		}
		break;
	case 0x02:
		pHitstatus->SetWindowText("右前");
		SetBothMotorsSpeed(-5506,-2504);
		jumpflag+=1;
		break;
	case 0x03:
		pHitstatus->SetWindowText("前");
		SetBothMotorsSpeed(-2508,-5508);
		break;
	case 0x04:
		pHitstatus->SetWindowText("右后");
		SetBothMotorsSpeed(2505,5507);
		break;
	case 0x05: 
		pHitstatus->SetWindowText("左前.右后");
		SetBothMotorsSpeed(3010,-3010);
		break;
	case 0x06:
		pHitstatus->SetWindowText("右侧");
		SetBothMotorsSpeed(0,0);
		break;
	case 0x07:
		pHitstatus->SetWindowText("前.右后");
		SetBothMotorsSpeed(0,0);
		break;
	case 0x08:
		pHitstatus->SetWindowText("左后");
		SetBothMotorsSpeed(5502,2505);
		break;
	case 0x09:
		pHitstatus->SetWindowText("左侧");
		SetBothMotorsSpeed(0,0);
		break;
	case 0x0a:
		pHitstatus->SetWindowText("右前.左后");
		SetBothMotorsSpeed(-3020,3020);
		break;
	case 0x0b:
		pHitstatus->SetWindowText("前.左后");
		SetBothMotorsSpeed(0,0);
		break;
	case 0x0c:
		pHitstatus->SetWindowText("后");
		SetBothMotorsSpeed(5510,2500);
		break;
	case 0x0d:
		pHitstatus->SetWindowText("左前.后");
		SetBothMotorsSpeed(0,0);
		break;
	case 0x0e:
		pHitstatus->SetWindowText("右前.后");
		SetBothMotorsSpeed(0,0);
		break;
	case 0x0f:	
		pHitstatus->SetWindowText("四方");
		SetBothMotorsSpeed(0,0);
		break;
	default:
		break;
	}
	
}
void CFstarCmd::AutoQueryIO(UINT TimeCap)
{	
	if (QueryIOTime==TimeCap)
	{
		return;
	}
	QueryIOTime=TimeCap;
	
	
	if (QueryIOTime!=0)
	{
		AfxBeginThread(QueryIOThread,(LPVOID)this,THREAD_PRIORITY_TIME_CRITICAL);
	}
}
void CFstarCmd::QueryIO()
{
	m_GenerateSendBuffer((UCHAR)0x71,0,0,(UCHAR)0x05,NULL);
}
void CFstarCmd::QueryAD()
{
	m_GenerateSendBuffer((UCHAR)0x71,0,0,(UCHAR)0x06,NULL);
}
void CFstarCmd::AutoQueryAD(UINT TimeCap)
{
	if (QueryADTime==TimeCap)
	{
		return;
	}
	QueryADTime=TimeCap;
	
	if (QueryADTime!=0)
	{
		AfxBeginThread(QueryADThread,(LPVOID)this,THREAD_PRIORITY_TIME_CRITICAL);
	}
}
void CFstarCmd::m_SetHitIcon(UCHAR temp)
{
	HICON hIcon[4];
	
	((temp&0x01)==0) ?(hIcon[0]=hIoff):(hIcon[0]=hIon);
	((temp&0x02)==0) ?(hIcon[1]=hIoff):(hIcon[1]=hIon);
	((temp&0x04)==0) ?(hIcon[2]=hIoff):(hIcon[2]=hIon);
	((temp&0x08)==0) ?(hIcon[3]=hIoff):(hIcon[3]=hIon);
	
	pLf->SetIcon(hIcon[0]);
	pRf->SetIcon(hIcon[1]);
	pRb->SetIcon(hIcon[2]);
	pLb->SetIcon(hIcon[3]);
	
}
void CFstarCmd::m_ForEdge()
{
	UCHAR temp = m_IOStatus[1];
	temp&=0x30;
	switch(temp)
	{
	case 0x00:
		SetBothMotorsSpeed(1510,1520);
		pFl->SetIcon(hIoff);
		pFr->SetIcon(hIoff);		
		break;	
	case 0x30:
		SetBothMotorsSpeed(-3010,-1510);
		pFl->SetIcon(hIon);
		pFr->SetIcon(hIon);
		break;
	case 0x10:
		SetBothMotorsSpeed(-3100,-1500);
		pFl->SetIcon(hIon);
		pFr->SetIcon(hIoff);		
		break;
	case 0x20:
		SetBothMotorsSpeed(-4010,-2510);
		pFl->SetIcon(hIoff);
		pFr->SetIcon(hIon);	
		break;

	default:
		break;
	}
/*
	UCHAR lEdge,rEdge;
	lEdge=m_Addata[2];
	rEdge=m_Addata[3];
	int i=rand()%10;
	if ((lEdge>3))
	{
		if (rEdge>3)
		{
			SetBothMotorsSpeed(-3010,-1510);
			pFl->SetIcon(hIon);
			pFr->SetIcon(hIon);
		}
		else
		{
			SetBothMotorsSpeed(-3100,-1500);
			pFl->SetIcon(hIon);
			pFr->SetIcon(hIoff);
		}
	}
	else
	{
		if (rEdge>3)
		{
			SetBothMotorsSpeed(-4010,-2510);
			pFl->SetIcon(hIoff);
			pFr->SetIcon(hIon);
		}
		else
		{
			SetBothMotorsSpeed(1510,1520);
			pFl->SetIcon(hIoff);
			pFr->SetIcon(hIoff);
		}
	}
*/
}


void CFstarCmd::m_OffHeat()
{	
	UCHAR ttl_tmp,ttr_tmp;
	UCHAR ave,sub,en_Tmp;
	char lr_tmp;
	CString strave;
	CString strsub;
	CString strltmp;	
	CString strrtmp;
	ttl_tmp=m_Addata[4];
	ttr_tmp=m_Addata[5];
	
	lr_tmp=abs(ttl_tmp-ttr_tmp);
	en_Tmp=(ttl_tmp+ttr_tmp)/2;
	sub=lr_tmp;
	ave=en_Tmp;
	if (en_Tmp<=minTmp)
	{
		SetBothMotorsSpeed(0,0);
	} 
	else 
	{
		if (lr_tmp<4)
		{
			SetBothMotorsSpeed(-2000,-2000);
		}
		else
		{
			lspeed=2000+(ttl_tmp-ttr_tmp)*4;
			rspeed=-2000+(ttl_tmp-ttr_tmp)*4;
			SetBothMotorsSpeed(lspeed,-rspeed);
		}
	}
	strltmp.Format("%d",ttl_tmp);
	strrtmp.Format("%d",ttr_tmp);
	strave.Format("%d",ave);
	strsub.Format("%d",sub);
	pLtmp->SetWindowText(strltmp);
	pRtmp->SetWindowText(strrtmp);
	pTAve->SetWindowText(strave);
	pTSub->SetWindowText(strsub);
}

void CFstarCmd::m_SetADData()
{
	UCHAR temp;
	CString  cs[8];
	for (int i=0;i<7;i++)
	{
		temp=m_Addata[i];
		cs[i].Format("%d",temp);
	} 
	pLlight->SetWindowText(cs[0]);
	pRlight->SetWindowText(cs[1]);
	pFl->SetWindowText(cs[2]);
	pFr->SetWindowText(cs[3]);
	pLtmp->SetWindowText(cs[4]);
	pRtmp->SetWindowText(cs[5]);
	pAD7->SetWindowText(cs[6]);
}

void CFstarCmd::m_SetIOData()
{
	UCHAR temp1,temp2;
	HICON hIcon[16];
	temp1 = m_IOStatus[1];
	temp2 = m_IOStatus[0];
	((temp1&0x01)==0) ?(hIcon[0]=hIoff):(hIcon[0]=hIon);
	((temp1&0x02)==0) ?(hIcon[1]=hIoff):(hIcon[1]=hIon);
	((temp1&0x04)==0) ?(hIcon[2]=hIoff):(hIcon[2]=hIon);
	((temp1&0x08)==0) ?(hIcon[3]=hIoff):(hIcon[3]=hIon);
	((temp1&0x10)==0) ?(hIcon[4]=hIoff):(hIcon[4]=hIon);
	((temp1&0x20)==0) ?(hIcon[5]=hIoff):(hIcon[5]=hIon);
	((temp1&0x40)==0) ?(hIcon[6]=hIoff):(hIcon[6]=hIon);
	((temp1&0x80)==0) ?(hIcon[7]=hIoff):(hIcon[7]=hIon);

	((temp2&0x01)==0) ?(hIcon[8]=hIoff):(hIcon[8]=hIon);
	((temp2&0x02)==0) ?(hIcon[9]=hIoff):(hIcon[9]=hIon);
	((temp2&0x04)==0) ?(hIcon[10]=hIoff):(hIcon[10]=hIon);
	((temp2&0x08)==0) ?(hIcon[11]=hIoff):(hIcon[11]=hIon);
	((temp2&0x10)==0) ?(hIcon[12]=hIoff):(hIcon[12]=hIon);
	((temp2&0x20)==0) ?(hIcon[13]=hIoff):(hIcon[13]=hIon);
	((temp2&0x40)==0) ?(hIcon[14]=hIoff):(hIcon[14]=hIon);
	((temp2&0x80)==0) ?(hIcon[15]=hIoff):(hIcon[15]=hIon);
	
	pLf->SetIcon(hIcon[0]);
	pRf->SetIcon(hIcon[1]);
	pRb->SetIcon(hIcon[2]);
1234

fstarcmd.cpp - 源码说明

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

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

⌨️ 快捷键说明

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