finding.cpp

足球机器人功能demo

// Finding.cpp: implementation of the CFinding class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Finding.h"
#include "VoyPID.h"

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

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

CFinding::CFinding()
{
	m_BehShow = NULL;

	m_para.ReSet();
	m_para.Ven = TRUE;
	m_para.Vmin = 200;
	m_para.Vmax = 1270;
	m_CV.SetParam(&m_para);
}

CFinding::~CFinding()
{

}

struct SSPID vPID1,wPID1;
void CFinding::AfterUpdateVideoSample(BYTE *pBuffer, long lWidth, long lHeight, double dbTime, UINT nState)
{

	//摄像头图像祯更新
	m_CV.ImageBuf(lWidth,lHeight,pBuffer);
	m_CV.FindSplash(TRUE);

	

	if (m_para.Ysum == 0 && m_para.Usum == 0 && m_para.Vsum == 0)
	{	//针对某些线程阻塞的情况
		m_para.bBusy = FALSE;
	}

	static unsigned char StartFlag = false;

	struct SSPID *v_PID = &vPID1;
	struct SSPID *w_PID = &wPID1;

	if(StartFlag == false)
	{
///
		v_PID->si_Ref = 35;		//位置PID，位移设定值
		v_PID->si_FeedBack = 0;	//位置PID，位移反馈值,当前位置

		v_PID->si_PreError = 0;	//位置PID，前一次，位移误差,ui_Ref - FeedBack
		v_PID->si_PreIntegral = 0;	//位置PID，前一次，位移积分项，ui_PreIntegral+ui

		v_PID->si_Kp = 5000;		//位置PID，比例系数	
		v_PID->si_Ki = 500;		//位置PID，积分系数	
		v_PID->si_Kd = 0;		//位置PID，微分系数

		v_PID->SS_MAX = 3000<<10;		//输出上限，使控制信号扩大,以使过渡平顺	
		v_PID->SS_MIN = -3000<<10;		//输出下限，
		v_PID->SS_Imax = 20000;		//积分饱和上限
		v_PID ->SS_Imin = -20000;		//积分饱和下限
		v_PID->PLUS_DEADLINE = 5;	//偏差死限的上限
		v_PID->MINUS_DEADLINE = -5; 	//偏差死限的下限

		v_PID->si_Accel = 0;		//位置PID，加速度
		v_PID->si_Speed = 0;		//位置PID，最大速度	

		v_PID->ul_PreU = 0;		//电机控制输出值
///
		w_PID->si_Ref = 0;		//位置PID，位移设定值
		w_PID->si_FeedBack = 0;	//位置PID，位移反馈值,当前位置

		w_PID->si_PreError = 0;	//位置PID，前一次，位移误差,ui_Ref - FeedBack
		w_PID->si_PreIntegral = 0;	//位置PID，前一次，位移积分项，ui_PreIntegral+ui

		w_PID->si_Kp = 6000;		//位置PID，比例系数	
		w_PID->si_Ki = 400;		//位置PID，积分系数	
		w_PID->si_Kd = 0;		//位置PID，微分系数

		w_PID->SS_MAX = 1500<<10;		//输出上限，使控制信号扩大,以使过渡平顺	
		w_PID->SS_MIN = -1500<<10;		//输出下限，
		w_PID->SS_Imax = 10000;		//积分饱和上限
		w_PID ->SS_Imin = -10000;		//积分饱和下限
		w_PID->PLUS_DEADLINE = 3;	//偏差死限的上限
		w_PID->MINUS_DEADLINE = -3; 	//偏差死限的下限

		w_PID->si_Accel = 0;		//位置PID，加速度
		w_PID->si_Speed = 0;		//位置PID，最大速度	

		w_PID->ul_PreU = 0;		//电机控制输出值

		StartFlag = true;
	}

	//像素数判断视频情况
	if (m_para.Vsum < 1000)
	{
		static unsigned char StartFlag =0;
		
		m_strShow.Format("丢失目标……");
		m_BehShow->AddString(m_strShow);
		m_BehShow->SetCurSel(m_BehShow->GetCount()-1);
		
		if(m_pCmd->m_iLspeed == 0 && m_pCmd->m_iRspeed == 0);
		else if(m_pCmd->m_iLspeed > m_pCmd->m_iRspeed)
		{
			m_pCmd->SetBothMotorsSpeed(800,-800);
		}
		else if(m_pCmd->m_iLspeed < m_pCmd->m_iRspeed)
		{
			m_pCmd->SetBothMotorsSpeed(-800,800);
		}
		return;

	}

	v_PID->si_FeedBack = m_para.Vy;

	if(m_para.Vy < 50)
	{
		w_PID->si_FeedBack = m_para.Vx;
		w_PID->si_FeedBack = 160 - w_PID->si_FeedBack;
	}
	else
	{
		w_PID->si_FeedBack = m_para.Vx;
		w_PID->si_FeedBack = 160 - w_PID->si_FeedBack;
		w_PID->si_FeedBack *= 50;
		w_PID->si_FeedBack /= m_para.Vy;
		
	}
	
	signed int TempSpeedV=0,TempSpeedW=0;

	TempSpeedV = SSPIDCalc( v_PID );
	TempSpeedV = 0 - TempSpeedV;
	TempSpeedW = SSPIDCalc( w_PID );

	m_pCmd->SetBothMotorsSpeed(TempSpeedV+TempSpeedW,TempSpeedV-TempSpeedW);

	m_strShow.Format("(%d,%d):%d:  %d,%d",m_para.Vx,m_para.Vy,m_para.Vsum,TempSpeedV,TempSpeedW);
	m_BehShow->AddString(m_strShow);
	m_BehShow->SetCurSel(m_BehShow->GetCount()-1);


	

}