// Catching.cpp: implementation of the CCatching class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Catching.h"
#include "VoyPID.h"

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

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

CCatching::CCatching()
{
	m_BehShow = NULL;
	m_ClrShow = NULL;
	m_poverlookfind = NULL;

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

CCatching::~CCatching()
{

}

struct SSPID vPID,wPID;

void CCatching::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 = &vPID;
	struct SSPID *w_PID = &wPID;

	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)
	{
		
		m_strShow.Format("前方視距范圍內丟失目標……");
		output(&m_strShow,m_BehShow);
		if (m_poverlookfind == NULL)
		{
			m_pCmd->Brake(1);
		} 
		else
		{
			m_pCmd->SetBehavior(m_poverlookfind);
		}
		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);
	output(&m_strShow,m_ClrShow);
/*
	//對準球后靠近
	if (m_para.Vx < 200 && m_para.Vx >120)
	{
		if (m_para.Vy < 10)
		{
			m_pCmd->Brake(1);
		} 
		else
		{
			m_pCmd->SetBothMotorsSpeed(m_para.Vy*5,m_para.Vy*5);
		}
		return;
	}

	//沒對準球則先對準球先對準球
	if (m_para.Vx<160)
	{
		m_pCmd->SetBothMotorsSpeed(-150,150);
	}
	else
	{
		m_pCmd->SetBothMotorsSpeed(150,-150);
	}
*/
}

void CCatching::output(CString *strShow, CListBox *inList)
{
	if (inList != NULL)
	{
		inList->AddString(*strShow);
		inList->SetCurSel(m_BehShow->GetCount()-1);
	}
}