fstarcmd.cpp

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

CPP
2,455 行
字号
1234
	pLb->SetIcon(hIcon[3]);
	pIO4->SetIcon(hIcon[4]);
	pIO5->SetIcon(hIcon[5]);
	pIO6->SetIcon(hIcon[6]);
	pIO7->SetIcon(hIcon[7]);

	pIO8->SetIcon(hIcon[8]);
	pIO9->SetIcon(hIcon[9]);
	pIO10->SetIcon(hIcon[10]);
	pIO11->SetIcon(hIcon[11]);
	pIO12->SetIcon(hIcon[12]);
	pIO13->SetIcon(hIcon[13]);
	pIO14->SetIcon(hIcon[14]);
	pIO15->SetIcon(hIcon[15]);	
}

void CFstarCmd::TestMotion(UCHAR motionSelect)
{
	m_GenerateSendBuffer((UCHAR)0x01,0,1,(UCHAR)0x41,&motionSelect);
}

void CFstarCmd::QueryUS()
{
	m_GenerateSendBuffer((UCHAR)0x53,0,0,(UCHAR)0x01,NULL);
}
void CFstarCmd::AutoQueryUS(UINT TimeCap)
{
	if (QueryUSonicTime==TimeCap)
	{
		return;
	}
	QueryUSonicTime=TimeCap;
	if (QueryUSonicTime!=0)
	{
		AfxBeginThread(QueryUSonicThread,(LPVOID)this,THREAD_PRIORITY_TIME_CRITICAL);
	}
}

void CFstarCmd::m_USprocess()
{	
	switch(demo_us_flag)
	{
	case 0:
		SetBothMotorsSpeed(0,0);
		break;
	case USAWAYOBSTACLE:
		m_UsAwayObstacle();
		break;
	case USONICFOLLOW:
		m_UsFollow();
		break;
	case USONLYSEARCH:
		m_SetUSData();
		break;
	case USREMOTESEARCH:
		break;
	/*case USDETECTION:
		//PlaySound("us_ok.wav",GetModuleHandle(NULL),SND_ASYNC|SND_FILENAME);
		demo_us_flag=0;
		detect_flag=0;
		break;*/
	default:
		break;
	}
	
}
void CFstarCmd::LockDirection(int speed)
{
	WORD lrspeed = m_CalculateSpeed(speed);
	UCHAR bothspeed[4];	
	
	bothspeed[0]=0;
	bothspeed[1]=0;
	
	bothspeed[2]=(UCHAR)((lrspeed >> 8) & 0x00ff);
	bothspeed[3]=(UCHAR)(lrspeed & 0x00ff);	
	
	m_GenerateSendBuffer((UCHAR)0x01,0,4,(UCHAR)0x2C,bothspeed);
}

//DEL void CFstarCmd::MoveSquare()
//DEL {
//DEL 	m_GenerateSendBuffer((UCHAR)0x01,0,0,(UCHAR)0x2D,NULL);
//DEL }

void CFstarCmd::TurnOnLgt(UCHAR lrlight)
{
	if (lrlight==1)
	{
		motorData[0]=0xFE;
		motorData[1]=0xFF;
		motorData[2]=0;
		motorData[3]=0;
	} 
	else if (lrlight==2)
	{
		motorData[0]=0;
		motorData[1]=0;
		motorData[2]=0xFE;
		motorData[3]=0xFF;
	}
	else
	{
		motorData[0]=0xFE;
		motorData[1]=0xFF;
		motorData[2]=0xFE;
		motorData[3]=0xFF;		
	}
	
	m_GenerateSendBuffer((UCHAR)0x71,0,8,(UCHAR)0x02,motorData);
}
void CFstarCmd::TurnOffLgt(UCHAR lrlight)
{
	if (lrlight==1)
	{
		motorData[0]=0;
		motorData[1]=0;
	} 
	else if (lrlight==2)
	{
		motorData[2]=0;
		motorData[3]=0;
	}
	else
	{
		motorData[0]=0;
		motorData[1]=0;
		motorData[2]=0;
		motorData[3]=0;
	}
	
	m_GenerateSendBuffer((UCHAR)0x71,0,8,(UCHAR)0x02,motorData);
	
}
void CFstarCmd::WaveHead(UCHAR angle,UCHAR speed)
{
	serveData[0]=angle;
	serveData[1]=speed;
	
	m_GenerateSendBuffer((UCHAR)0x71,0,24,(UCHAR)0x01,serveData);
	
}
void CFstarCmd::LcdShow(CString cs)
{
	UCHAR toSend[16];
	toSend[0]=0xB2;
	toSend[1]=0xA9;
	toSend[2]=0xB4;
	toSend[3]=0xB4;
	toSend[4]=0xBF;
	toSend[5]=0xC6;
	toSend[6]=0xBC;
	toSend[7]=0xBC;
	toSend[8]=0xCE;
	toSend[9]=0xB4;
	toSend[10]=0xC0;
	toSend[11]=0xB4;
	toSend[12]=0xD6;
	toSend[13]=0xAE;
	toSend[14]=0xD0;
	toSend[15]=0xC7;
	m_GenerateSendBuffer((UCHAR)0x52,0,16,0,toSend);
}
void CFstarCmd::LasorOn(UCHAR sel)
{
	switch(sel)
	{
	case 1:
		motorData[4] = 0xFE;
		motorData[5] = 0xFF;
		motorData[6] = 0;
		motorData[7] = 0;
		break;
	case 2:
		motorData[4] = 0;
		motorData[5] = 0;
		motorData[6] = 0xFE;
		motorData[7] = 0xFF;
		break;
	case 3:
		motorData[4] = 0xFE;
		motorData[5] = 0xFF;
		motorData[6] = 0xFE;
		motorData[7] = 0xFF;
		break;
	default:
		break;
	}	
	m_GenerateSendBuffer((UCHAR)0x71,0,8,(UCHAR)0x02,motorData);
}
void CFstarCmd::m_UsAwayObstacle()
{
	int aveDis;
	int lspeed;
	int rspeed;
	CString cs;
	int i=rand()%10;
	aveDis=m_USdata[0]*256+m_USdata[1];
	if (aveDis>30)
	{
		lspeed=2000+aveDis*4;
		rspeed=2000+aveDis*4;
		SetBothMotorsSpeed(lspeed,rspeed);
	} 
	else
	{
		SetBothMotorsSpeed(-2000,2000);
	}
	cs.Format("%d",aveDis);
	pUsdis->SetWindowText(cs);
	
}

void CFstarCmd::m_UsFollow()
{
	int aveDis;
	int lspeed;
	int rspeed;
	CString cs;
	int i=rand()%10;
	aveDis=m_USdata[0]*256+m_USdata[1];
	if (aveDis>30)
	{
		lspeed=2000+aveDis*4;
		rspeed=2000+aveDis*4;
		SetBothMotorsSpeed(lspeed,rspeed);
	}
	else if (aveDis<20)
	{
		SetBothMotorsSpeed(-(2000+i),-(2000+i));
	}
	else
	{
		SetBothMotorsSpeed(0,0);
	}
	cs.Format("%d",aveDis);
	pUsdis->SetWindowText(cs);
}

void CFstarCmd::Dance()
{
	//dz1:0x5a,0x04,0x9D,0x5a,0x5a,0x94,0xa8,0x15,0x5a,0x5a,0x9d,0x5a 
	//dz2:0x5a,0x76,0x9d,0x5a,0x5a,0x8f,0x39,0x14,0x5a,0x5a,0x9b,0x5a
	//dz3:0x5a,0x4d,0x9d,0x5a,0x5a,0x96,0x66,0x14,0x5a,0x5a,0x99,0x5a
	//dz4:0x52,0x6d,0x92,0x92,0x5a,0x97,0x72,0x1b,0x0b,0x5a,0x9b,0x5a
	//dz5:0x8d,0x29,0x7f,0x77,0x94,0x97,0x52,0x1b,0x20,0x5a,0x9b,0x5a
	//dz6:0x24,0xb4,0x70,0x57,0x94,0x97,0x9d,0x19,0x3d,0x5a,0x9b,0x5a 
	//dz7:120,40,157,106,146,150,69,26,37,91,146(jiguang)
	//dz8:80,98,159,136,146,150,120,56,56,67,148
	//dz9:90,122,137,142,122,140,62,33,29,27,150
	//dz10:90,30,137,142,122,140,144,33,29,27,150
	AfxBeginThread(DanceThread,(LPVOID)this,THREAD_PRIORITY_TIME_CRITICAL);
}
void CFstarCmd::LasorOff(UCHAR sel)
{
	switch(sel)
	{
	case 1:
		motorData[4]=0;
		motorData[5]=0;
		break;
	case 2:
		motorData[6]=0;
		motorData[7]=0;
		break;
	case 3:
		motorData[4]=0;
		motorData[5]=0;
		motorData[6]=0;
		motorData[7]=0;
		break;
	default:
		break;
	}
	m_GenerateSendBuffer((UCHAR)0x71,0,8,(UCHAR)0x02,motorData);
}

void CFstarCmd::PWMoff()
{
	UCHAR sendData[8];
	for (int i=0;i<8;i++)
	{
		sendData[i]=0;
	}
	m_GenerateSendBuffer((UCHAR)0x71,0,8,(UCHAR)0x02,sendData);
}

void CFstarCmd::DetectBoard()
{
	AfxBeginThread(DetectSysThread,(LPVOID)this,THREAD_PRIORITY_TIME_CRITICAL);
}

void CFstarCmd::m_SetUSData()
{
	int aveDis;
	CString cs;
	aveDis=m_USdata[0]*256+m_USdata[1];
	cs.Format("%d",aveDis);
	pUsdis->SetWindowText(cs);
}

// void CFstarCmd::TurnRByGyro(int inAngle, UCHAR inSpeed)
// {
// 	if (inAngle <-3600 || inAngle > 3600)
// 	{
// 		return;
// 	}
// 	WORD wAngle,wSpeed;
// 	UCHAR toSend[4];
// 	
// 	toSend[1] = (UCHAR) inAngle&0x00ff;
// 	toSend[0] = (UCHAR) (inAngle>>8)&0x00ff;
// 	
// 	toSend[3] = inSpeed;
// 	toSend[2] = 0x02;
// 	
// 	m_GenerateSendBuffer((UCHAR) 0x01,0,4,0x2E,toSend);
// }

void CFstarCmd::SendArmData(UCHAR angle[])
{
	UCHAR speed[10];
	memset(speed,0x80,10);
	
	serveData[2]=angle[0];
	serveData[3]=speed[0];
	serveData[4]=angle[1];
	serveData[5]=speed[1];
	serveData[6]=angle[2];
	serveData[7]=speed[2];
	serveData[8]=angle[3];
	serveData[9]=speed[3];
	serveData[10]=angle[4];
	serveData[11]=speed[4];
	serveData[12]=angle[5];
	serveData[13]=speed[5];
	serveData[14]=angle[6];
	serveData[15]=speed[6];
	serveData[16]=angle[7];
	serveData[17]=speed[7];
	serveData[18]=angle[8];
	serveData[19]=speed[8];
	serveData[20]=angle[9];
	serveData[21]=speed[9];
	serveData[22]=0;
	serveData[23]=0;
	
	m_GenerateSendBuffer((UCHAR)0x71,0,24,(UCHAR)0x01,serveData);
}
void CFstarCmd::ResetServe()
{
	// 	serveData[0]=0x5A;
	// 	serveData[1]=0x56;
	// 	serveData[2]=0x5A;
	// 	serveData[3]=0x56;
	// 	serveData[4]=0x5A;
	// 	serveData[5]=0x56;
	// 	serveData[6]=0x5A;
	// 	serveData[7]=0x56;
	// 	serveData[8]=0x5A;
	// 	serveData[9]=0x56;
	// 	serveData[10]=0x5A;
	// 	serveData[11]=0x56;
	// 	serveData[12]=0x5A;
	// 	serveData[13]=0x56;
	// 	serveData[14]=0x5A;
	// 	serveData[15]=0x56;
	// 	serveData[16]=0x5A;
	// 	serveData[17]=0x56;
	// 	serveData[18]=0x5A;
	// 	serveData[19]=0x56;
	// 	serveData[20]=0x5A;
	// 	serveData[21]=0x56;
	// 	serveData[22]=0x5A;
	// 	serveData[23]=0x56;
	
	for (int i=0;i<23;i+=2)
	{
		serveData[i]=0x5A;
		serveData[i+1]=0x56;
	}
	m_GenerateSendBuffer((UCHAR)0x71,0,24,(UCHAR)0x01,serveData);
}

void CFstarCmd::CopyArray(UCHAR * pSource)
{
	UCHAR speed[12];
	UCHAR data[24];
	memset(speed,0x80,12);
	for (int i=0;i<24;i++)
	{
		if (i%2==0)
		{
			data[i]=pSource[i/2];
		} 
		else
		{
			data[i]=speed[i/2];
		}
	}
	memcpy(serveData,data,24);
}

void CFstarCmd::AutoQueryADIO(UINT TimeCap)
{
	if (QueryADIOTime==TimeCap)
	{
		return;
	}
	QueryADIOTime=TimeCap;
	
	if (QueryADIOTime!=0)
	{
		AfxBeginThread(QueryADIOThread,(LPVOID)this,THREAD_PRIORITY_TIME_CRITICAL);
	}
}

void CFstarCmd::QueryADIO()
{
	m_GenerateSendBuffer((UCHAR)0x71,0,0,(UCHAR)0x05,NULL);
	m_GenerateSendBuffer((UCHAR)0x71,0,0,(UCHAR)0x06,NULL);
}

void CFstarCmd::m_AdForlight()
{
	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;
	UCHAR temp=m_IOStatus[1]&0x0F;
	if (temp!=0)
	{
		switch(temp)
		{
		case 0x01:
			SetBothMotorsSpeed(-2010,-4520);
			break;
		case 0x02:
			SetBothMotorsSpeed(-4506,-2004);
			break;
		case 0x03:
			SetBothMotorsSpeed(-2008,-4508);
			break;			
		case 0x04:
			SetBothMotorsSpeed(2005,4507);
			break;
		case 0x05: 
			SetBothMotorsSpeed(3010,-3010);
			break;
		case 0x06:
		case 0x07:
			SetBothMotorsSpeed(0,0);
			break;
		case 0x08:
			SetBothMotorsSpeed(4502,2005);
			break;
		case 0x09:
			SetBothMotorsSpeed(0,0);
			break;
		case 0x0a:
			SetBothMotorsSpeed(-3020,3020);
			break;
		case 0x0b:
			SetBothMotorsSpeed(0,0);
			break;
		case 0x0c:
			SetBothMotorsSpeed(4510,2000);
			break;
		case 0x0d:
		case 0x0e:
		case 0x0f:	
			SetBothMotorsSpeed(0,0);
			break;			
		default:
			break;
		}
		AfxBeginThread(Hit_Delay,(LPVOID)this,THREAD_PRIORITY_NORMAL);
	} 
	else
	{
		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::SetPIDpara(UCHAR pid1,UCHAR pid2,UCHAR pid3)
{
	UCHAR toSend[3];
	toSend[0]=pid1;
	toSend[1]=pid2;
	toSend[2]=pid3;
	m_GenerateSendBuffer((UCHAR)0x01,0,3,(UCHAR)0x35,toSend);
}
void CFstarCmd::QueryGyro()
{
	m_GenerateSendBuffer(0x01,0,0,0x34,NULL);
}
void CFstarCmd::AutoQueryGyro(UINT TimeCap)
{
	if (QueryGyroTime==TimeCap)
	{
		return;
	}
	QueryGyroTime=TimeCap;
	
	if (QueryGyroTime!=0)
	{
		AfxBeginThread(QueryGyroThread,(LPVOID)this,THREAD_PRIORITY_TIME_CRITICAL);
	}
}
UINT CFstarCmd::DetectSysThread(LPVOID pParam)
{
	CFstarCmd * pCmd=(CFstarCmd * )pParam;
	//PlaySound("PC_ok.wav",GetModuleHandle(NULL),SND_ASYNC|SND_FILENAME);
	if (pCmd->bToEndDetect)
	{
		//PlaySound(NULL,GetModuleHandle(NULL),SND_ASYNC|SND_FILENAME);
		return 1L;
	}
	Sleep(3000);
	
	
	pCmd->detect_flag=0x01;
	if (pCmd->bToEndDetect)
	{
		return 1L;
	}
	pCmd->m_GenerateSendBuffer((UCHAR)0x01,0,0,(UCHAR)0x01,NULL);			//高级卡自检
	pCmd->m_GenerateSendBuffer((UCHAR)0x01,0,0,(UCHAR)0x01,NULL);			//高级卡自检
	Sleep(5000);
	if (pCmd->detect_flag==0x01)
	{
		//PlaySound("advb_error.wav",GetModuleHandle(NULL),SND_ASYNC|SND_FILENAME);
		Sleep(2000);
	}
	
	pCmd->detect_flag=0x02;
	if (pCmd->bToEndDetect)
	{
		return 1L;
	}
	pCmd->m_GenerateSendBuffer((UCHAR)0x01,0,0,(UCHAR)0x02,NULL);			//IIC检查
	pCmd->m_GenerateSendBuffer((UCHAR)0x01,0,0,(UCHAR)0x02,NULL);			//IIC检查
	Sleep(5000);
	if (pCmd->detect_flag==0x02)
	{
		//PlaySound("gyro_error.wav",GetModuleHandle(NULL),SND_ASYNC|SND_FILENAME);//VIA自检
		Sleep(2000);
	}
	
	
	pCmd->detect_flag=0x52;
	if (pCmd->bToEndDetect)
	{
		return 1L;
	}
	pCmd->m_GenerateSendBuffer((UCHAR)0x52,0,0,(UCHAR)0xff,NULL);			//LCD检查
	Sleep(5000);
	if (pCmd->detect_flag==0x52)
1234

fstarcmd.cpp - 源码说明

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

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

⌨️ 快捷键说明

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