//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// ±¸µ¿ °ü·Ã ÆĶó¸ÞÅÍ
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// ÃßÈÄ¿¡ ¿¬»ê ½Ã°£À» ÁÙÀ̱â À§ÇØ °ö¼À, ³ª´°¼À ¸ÅÅ©·Î ´ë½Å »ó¼ö·Î ´ëüÇÒ °Í.

// ¸ðÅÍ 1ȸÀü´ç ÆÞ½º ¼ö
#define MOTOR_PPR		(12.0f)

// ¸ðÅÍ 1ȸÀü´ç 4ä¹è ÆÞ½º ¼ö
#define MOTOR_PPR_4X		((MOTOR_PPR) * 4.0f)

// ±â¾î °¨¼Óºñ
#define GEAR_RATIO		(49.89f)

// ¹ÙÄû 1ȸÀü´ç 4ä¹è ÆÞ½º ¼ö : 48.0f * 49.89f = 2394.72f
#define WHEEL_PPR_4X		((MOTOR_PPR_4X) * (GEAR_RATIO))
//#define WHEEL_PPR_4X		(2394.72f)

// ¹ÙÄû ¿Ü°æ ¹ÝÁö¸§: °ªÀ» ÁÙÀÌ¸é ´õ °¡°í, °ªÀ» ´Ã¸®¸é ´ú °£´Ù.
#define WHEEL_RADIUS		(35.0f)

// ¹ÙÄû ¿Ü°æ Áö¸§ : 68.0f
#define WHEEL_DIAMETER		((WHEEL_RADIUS) * 2.0f)

// ¹ÙÄû ¿øÁÖ : 213.62830044410594021545975006301
#define WHEEL_CIRCLE_LEN	((WHEEL_DIAMETER) * (M_PI))

// ÇÑ ÆÞ½º´ç ÁøÇà °Å¸® : 0.10628273653935618916192027366319 (mm/pulse)
#define MM_PER_PULSE		((WHEEL_CIRCLE_LEN) / (WHEEL_PPR_4X))

// 1mm ÁøÇà¿¡ ÇØ´çÇÏ´Â ÆÞ½º ¼ö : 9.4088657533738124969251871876104 (pulse/mm)
#define PULSE_PER_MM		((WHEEL_PPR_4X) / (WHEEL_CIRCLE_LEN))

// ÆÞ½º -> °Å¸® º¯È¯
#define PULSE2MM(p)		((p) * (MM_PER_PULSE))

// °Å¸® -> ÆÞ½º º¯È¯
#define MM2PULSE(d)		((d) * (PULSE_PER_MM))

// Á¦¾î ÁÖ±â (sec)
//#define WHEEL_CTRL_PERIOD	(0.050f)
#define WHEEL_CTRL_PERIOD	(1.0f/TICKS_PER_SEC)


// Á¦¾î Áֱ⿡ ´ëÇÑ ÁÖÆļö (Hz) - PWM ÁÖÆļö°¡ ¾Æ´Ï¶ó ÃÊ´ç ¹ÙÄû ¼Óµµ Á¦¾î Ƚ¼öÀÓ.
#define WHEEL_CTRL_FREQ		(1.0f / (WHEEL_CTRL_PERIOD))

// ÆÞ½º -> ¼Óµµ º¯È¯
#define PULSE2VEL(p)		(PULSE2MM(p) / WHEEL_CTRL_PERIOD)

// ¼Óµµ -> ÆÞ½º º¯È¯
#define VEL2PULSE(v)		(MM2PULSE(v) * WHEEL_CTRL_PERIOD)

// ¹ÙÄûÀÇ ÃÖÀú ¼Óµµ (mm/sec)
#define MIN_WHEEL_VEL		(3.0f)

// ¹ÙÄûÀÇ ÃÖ°í ¼Óµµ (mm/sec)
#define MAX_WHEEL_VEL		(450.0f)

#define MAX_PWM_DUTY		(99)

// ¹ÙÄû°£ °Å¸®: °ªÀ» ÁÙÀÌ¸é ´ú ȸÀüÇÏ°í, Å°¿ì¸é ´õ ȸÀüÇÑ´Ù. (odometry °¢µµ »ç¿ë½Ã)
#define WHEEL_BASE		(237.5f)


//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// Math
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=

#ifndef M_PI
#define M_PI			(3.1415926535897932384626433832795f)
#endif // M_PI

#define DEG2RAD(deg)		(((M_PI)/180.0f)*(FLOAT)(deg))
#define RAD2DEG(rad)		((180.0f/(M_PI))*(FLOAT)(rad))

#define SQR(x)			((x)*(x))

#define MAX(a,b)		(((a)>(b))?(a):(b))
#define MIN(a,b)		(((a)<(b))?(a):(b))
#define SAT(x,l,h)		MIN(MAX((x),(l)),(h))


/**
 * @fn		PUBLIC inline S16 Math2CalEncOffset(U16 u16PrevEnc, U16 u16CurrEnc)
 * @brief	¿£ÄÚ´õ À̵¿·® °è»ê
 * @param	u16PrevEnc  [in] ÀÌÀü ¿£ÄÚ´õ Ä«¿îÆ® °ª
 * @param	u16CurrEnc  [in] ÇöÀç ¿£ÄÚ´õ Ä«¿îÆ® °ª
 */
PUBLIC inline S16 Math2CalEncOffset(U16 u16PrevEnc, U16 u16CurrEnc)
{
	U16 u16IntAbsOffset = 0, u16ExtAbsOffset = 0;
	S16 s16EncOffset = 0;

	// combination of Prev and Curr:
	//
	// (1)						Prev   Curr
	// ------------------------------+------+---- forward (Prev <= Curr)
	//
	// (2) Curr							Prev
	// -----+-------------------------------+---- forward (Prev > Curr, overflow)
	//
	// (3)						Curr   Prev
	// ------------------------------+------+---- backward (Prev > Curr)
	//
	// (4) Prev							Curr
	// -----+-------------------------------+---- backward (Prev <= Curr, underflow)

	// case of (1) and (4)
	if (u16PrevEnc <= u16CurrEnc )
	{
		// internal offset
		u16IntAbsOffset = u16CurrEnc - u16PrevEnc;

		// external offset
		u16ExtAbsOffset = (0xffff - u16CurrEnc) + u16PrevEnc + 1;

		// encoder count offset
		s16EncOffset = ( u16IntAbsOffset <= u16ExtAbsOffset ) ?
				 u16IntAbsOffset:		// case of (1) forward
				-u16ExtAbsOffset;		// case of (4) backward (underflow)
	}
	// case of (2) and (3)
	else
	{
		// internal offset
		u16IntAbsOffset = u16PrevEnc - u16CurrEnc;

		// external offset
		u16ExtAbsOffset = (0xffff - u16PrevEnc) + u16CurrEnc + 1;

		// encoder count offset
		s16EncOffset = ( u16IntAbsOffset > u16ExtAbsOffset ) ?
				 u16ExtAbsOffset:		// case of (2) forward (overflow)
				-u16IntAbsOffset;		// case of (3) backward
	}

	return s16EncOffset;

}

/*******************************************************************************
** @fn		PUBLIC inline void Math2WheelVel2LinAngVel(const float fLVel, const float fRVel, float* fLinVel, float* fAngVel)
** @brief	convert linear/angular velocity into left/right wheel velocity
** @param	fLVel		[in] left wheel velocity
** @param	fRVel		[in] right wheel velocity
** @param	fLinVel		[out] linear velocity
** @param	fAngVel		[out] angular velocity
** @remark	This function is called periodically every MOTION_DELTA_T by Prd.
*******************************************************************************/
PUBLIC inline void Math2WheelVel2LinAngVel(float fLVel,float fRVel, float* fLinVel, float* fAngVel)
{
	*fLinVel = (fLVel + fRVel) * 0.5f;
	*fAngVel = (fRVel - fLVel) / WHEEL_BASE;
}

/*******************************************************************************
** @fn		PUBLIC inline void Math2LinAngVel2WheelVel(const float fLinVel, const float fAngVel, float* fLVel, float* fRVel)
** @brief	convert left/right wheel velocity into linear/angular velocity
** @param	fLinVel		[out] linear velocity
** @param	fAngVel		[out] angular velocity
** @param	fLVel		[in] left wheel velocity
** @param	fRVel		[in] right wheel velocity
** @remark	This function is called periodically every DELTA_T by Prd.
*******************************************************************************/
PUBLIC inline void Math2LinAngVel2WheelVel(const float fLinVel, const float fAngVel, float* fLVel, float* fRVel)
{
	*fLVel = fLinVel - 0.5f * WHEEL_BASE * fAngVel;
	*fRVel = fLinVel + 0.5f * WHEEL_BASE * fAngVel;
}

/*******************************************************************************
** PUBLIC inline void Math2NormalizeOri(FLOAT* fOri)
** This function normalize fOri with between -M_PI and +M_PI.
** @param   fOri	[in/out] orientation angle (radian)
** @return  normalized orientation angle
*******************************************************************************/
PUBLIC inline void Math2NormalizeOri(FLOAT* fOri)
{
	while ( *fOri <= -M_PI )	*fOri += (M_PI + M_PI);
	while ( *fOri >  +M_PI )	*fOri -= (M_PI + M_PI);
}


//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// Quadrature Encoder
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// Quadrature Encoder Truth Table
// ÀüÁ¦Á¶°Ç: A»óÀÌ B»óº¸´Ù 90µµ ¾Õ¼­´Â °ÍÀÌ ÀüÁø(CW,Áõ°¡)ÀÌ´Ù.
// ------+-------------------------------------------------
// (A:B) |				Curr.
// Prev. | (0:0)	(0:1)	(1:0)	(1:1)
// ------+-------------------------------------------------
// (0:0) | No_Change  Backward   Forward	Error
// (0:1) | Forward	No_Change  Error	Backward
// (1:0) | Backward   Error	No_Change  Forward
// (1:1) | Error	Forward	Backward   No_Change
// ------+--------------------------------------------------

// Quadrature Encoder
PRIVATE U08	bPrevEncLeftPhaseA = FALSE;
PRIVATE U08	bPrevEncLeftPhaseB = FALSE;

PRIVATE U08	bPrevEncRightPhaseA = FALSE;
PRIVATE U08	bPrevEncRightPhaseB = FALSE;

PUBLIC	U16	u16EncoderLeft  = 0;
PUBLIC	U16	u16EncoderRight = 0;

void __ISR(_INPUT_CAPTURE_1_VECTOR, ipl5) IC1_Interrupt_ISR(void)
{
	mIC1ClearIntFlag();

	if ((bPrevEncLeftPhaseA == bPrevEncLeftPhaseB) && (IN_ENC_LEFT_A != IN_ENC_LEFT_B))
	{
		if (bPrevEncLeftPhaseA == IN_ENC_LEFT_A)
			u16EncoderLeft++;
		else
			u16EncoderLeft--;
	}
	else if ((bPrevEncLeftPhaseA != bPrevEncLeftPhaseB) && (IN_ENC_LEFT_A == IN_ENC_LEFT_B))
	{
		if (bPrevEncLeftPhaseA == IN_ENC_LEFT_A)
			u16EncoderLeft--;
		else
			u16EncoderLeft++;
	}

	// update previous phase
	bPrevEncLeftPhaseA = IN_ENC_LEFT_A;
	bPrevEncLeftPhaseB = IN_ENC_LEFT_B;
}

void __ISR(_INPUT_CAPTURE_2_VECTOR, ipl5) IC2_Interrupt_ISR(void)
{
	mIC2ClearIntFlag();

	if ((bPrevEncLeftPhaseA == bPrevEncLeftPhaseB) && (IN_ENC_LEFT_A != IN_ENC_LEFT_B))
	{
		if (bPrevEncLeftPhaseA == IN_ENC_LEFT_A)
			u16EncoderLeft++;
		else
			u16EncoderLeft--;
	}
	else if ((bPrevEncLeftPhaseA != bPrevEncLeftPhaseB) && (IN_ENC_LEFT_A == IN_ENC_LEFT_B))
	{
		if (bPrevEncLeftPhaseA == IN_ENC_LEFT_A)
			u16EncoderLeft--;
		else
			u16EncoderLeft++;
	}

	// update previous phase
	bPrevEncLeftPhaseA = IN_ENC_LEFT_A;
	bPrevEncLeftPhaseB = IN_ENC_LEFT_B;
}

void __ISR(_INPUT_CAPTURE_3_VECTOR, ipl5) IC3_Interrupt_ISR(void)
{
	mIC3ClearIntFlag();

	if ((bPrevEncRightPhaseA == bPrevEncRightPhaseB) && (IN_ENC_RIGHT_A != IN_ENC_RIGHT_B))
	{
		if (bPrevEncRightPhaseA == IN_ENC_RIGHT_A)
			u16EncoderRight--;
		else
			u16EncoderRight++;
	}
	else if ((bPrevEncRightPhaseA != bPrevEncRightPhaseB) && (IN_ENC_RIGHT_A == IN_ENC_RIGHT_B))
	{
		if (bPrevEncRightPhaseA == IN_ENC_RIGHT_A)
			u16EncoderRight++;
		else
			u16EncoderRight--;
	}

	// update previous phase
	bPrevEncRightPhaseA = IN_ENC_RIGHT_A;
	bPrevEncRightPhaseB = IN_ENC_RIGHT_B;
}

void __ISR(_INPUT_CAPTURE_4_VECTOR, ipl5) IC4_Interrupt_ISR(void)
{
	mIC4ClearIntFlag();

	if ((bPrevEncRightPhaseA == bPrevEncRightPhaseB) && (IN_ENC_RIGHT_A != IN_ENC_RIGHT_B))
	{
		if (bPrevEncRightPhaseA == IN_ENC_RIGHT_A)
			u16EncoderRight--;
		else
			u16EncoderRight++;
	}
	else if ((bPrevEncRightPhaseA != bPrevEncRightPhaseB) && (IN_ENC_RIGHT_A == IN_ENC_RIGHT_B))
	{
		if (bPrevEncRightPhaseA == IN_ENC_RIGHT_A)
			u16EncoderRight++;
		else
			u16EncoderRight--;
	}

	// update previous phase
	bPrevEncRightPhaseA = IN_ENC_RIGHT_A;
	bPrevEncRightPhaseB = IN_ENC_RIGHT_B;
}


PUBLIC U16 EncGetCountLeftWheel(void)
{
	return u16EncoderLeft;
}

PUBLIC U16 EncGetCountRightWheel(void)
{
	return u16EncoderRight;
}


//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// PID °ü·Ã
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// ¹ÙÄû Á¤¼Ó Á¦¾î¸¦ À§ÇÑ P, I °ÔÀΰª
#define K_P			(0.1f)
#define K_I			(0.1f)

// °¡º¯ P °ÔÀÎÀ» Àû¿ëÇÒ ¼Óµµ ±¸°£
#define V_LSL			(80.0f)
#define V_USL			(150.0f)

// I °ÔÀο¡ ÀÇÇÑ ¿£ÄÚ´õ Ä«¿îÆ®ÀÇ ÃÖ´ë ´©Àû ¿ÀÂ÷
#define MAX_ERRSUM		(3000)

// PWM ºñ±³°ª °¡°¨ ¿É¼Â
#define PWM_OFFSET		(700)

// »õ·Î¿î Áö·É ¼Óµµ ·Îµù ¿©ºÎ
PRIVATE BOOL	m_bNewCmd = FALSE;

// Áö·É ¼Óµµ
PRIVATE FLOAT	m_fDesiredVl = 0.0f;
PRIVATE FLOAT	m_fDesiredVr = 0.0f;

// ¸ñÇ¥ ¼Óµµ
PRIVATE FLOAT	m_fTargVl = 0.0f;
PRIVATE FLOAT	m_fTargVr = 0.0f;

// ¸ñÇ¥ ¼Óµµ¿¡ ÁØÇÏ´Â ¿£ÄÚ´õ ¿É¼Â·®
PRIVATE FLOAT	m_fTargEncL = 0;
PRIVATE FLOAT	m_fTargEncR = 0;

// ´©Àû ¿£ÄÚ´õ ¿É¼Â·®
PRIVATE FLOAT	m_fEncErrSumL = 0;
PRIVATE FLOAT	m_fEncErrSumR = 0;

// PWM
PUBLIC  USHORT	tmpPWMLeft  = 0;
PUBLIC  USHORT	tmpPWMRight = 0;
PUBLIC  UCHAR	tmpPWMCount = 0;

#define _PI		3.14159265
#define enc2cm		0.0313
#define cm2enc		31.889
#define angle2enc	8.0324

#define NORMAL_SPEED	90	// 90 => 30cm/s
#define WF_SPEED	65
#define MAX_SPEED	98
#define MIN_SPEED	50
#define MAXDOCK_SPEED	85
#define DOCKING_SPEED	65	//65
#define HOMING_SPEED	65	//70
#define TURNING_SPEED	80
#define BACK_SPEED	80
#define SPIRAL_SPEED	65
#define STOP_SPEED	0

PUBLIC void DevWheelForwardDifferential(U08 lSpeed, U08 rSpeed)
{
	U08 paramLeftWheel;
	U08 paramRightWheel;

	paramLeftWheel  = lSpeed;
	paramRightWheel = rSpeed;

	OUT_LWHEEL_CONTROL_A1 = 0;
	OUT_RWHEEL_CONTROL_A0 = 0;

	OUT_LWHEEL_CONTROL_A0 = 1;
	OUT_RWHEEL_CONTROL_A1 = 1;

	HalOcEnable(OC_3,FALSE);
	HalOcEnable(OC_1,TRUE);
	HalOcEnable(OC_4,TRUE);

	OC1RS = (UINT16)(((PR2/100) * paramLeftWheel) + 1);
	OC4RS = (UINT16)(((PR2/100) * paramRightWheel ) + 1);

	HalTimerOn(TMR_2,TRUE);
}

PUBLIC void DevWheelTurnRightDifferential(U08 lSpeed, U08 rSpeed)
{
	U08 paramLeftWheel;
	U08 paramRightWheel;

	paramLeftWheel  = lSpeed;
	paramRightWheel = rSpeed;

	OUT_LWHEEL_CONTROL_A1 = 0;
	OUT_RWHEEL_CONTROL_A1 = 0;

	OUT_LWHEEL_CONTROL_A0 = 1;
	OUT_RWHEEL_CONTROL_A0 = 1;

	HalOcEnable(OC_3,FALSE);
	HalOcEnable(OC_1,TRUE);
	HalOcEnable(OC_4,TRUE);

	OC1RS = (UINT16)(((PR2/100) * paramLeftWheel) + 1);
	OC4RS = (UINT16)(((PR2/100) * paramRightWheel ) + 1);

	HalTimerOn(TMR_2,TRUE);
}

PUBLIC void DevWheelTurnRightRound(U08 lSpeed)
{
	U08 paramLeftWheel;

	paramLeftWheel  = lSpeed;

	HalOcEnable(OC_3,FALSE);
	HalOcEnable(OC_4,FALSE);

	OUT_RWHEEL_CONTROL_A0 = 0;
	OUT_LWHEEL_CONTROL_A1 = 0;
	OUT_RWHEEL_CONTROL_A1 = 0;

	OUT_LWHEEL_CONTROL_A0 = 1;

	HalOcEnable(OC_1,TRUE);

	OC1RS = (UINT16)(((PR2/100) * paramLeftWheel) + 1);

	HalTimerOn(TMR_2,TRUE);
}

PUBLIC void DevWheelTurnLeftDifferential(U08 lSpeed, U08 rSpeed)
{
	U08 paramLeftWheel;
	U08 paramRightWheel;

	paramLeftWheel  = lSpeed;
	paramRightWheel = rSpeed;

	OUT_LWHEEL_CONTROL_A0 = 0;
	OUT_RWHEEL_CONTROL_A0 = 0;

	OUT_LWHEEL_CONTROL_A1 = 1;
	OUT_RWHEEL_CONTROL_A1 = 1;

	HalOcEnable(OC_3,FALSE);
	HalOcEnable(OC_1,TRUE);
	HalOcEnable(OC_4,TRUE);

	OC1RS = (UINT16)(((PR2/100) * paramLeftWheel) + 1);
	OC4RS = (UINT16)(((PR2/100) * paramRightWheel ) + 1);

	HalTimerOn(TMR_2,TRUE);
}

PUBLIC void DevWheelBackwardDifferential(U08 lSpeed, U08 rSpeed)
{
	U08 paramLeftWheel;
	U08 paramRightWheel;

	paramLeftWheel  = lSpeed;
	paramRightWheel = rSpeed;

	OUT_LWHEEL_CONTROL_A0 = 0;
	OUT_RWHEEL_CONTROL_A1 = 0;

	OUT_LWHEEL_CONTROL_A1 = 1;
	OUT_RWHEEL_CONTROL_A0 = 1;

	HalOcEnable(OC_3,FALSE);
	HalOcEnable(OC_1,TRUE);
	HalOcEnable(OC_4,TRUE);

	OC1RS = (UINT16)(((PR2/100) * paramLeftWheel) + 1);
	OC4RS = (UINT16)(((PR2/100) * paramRightWheel ) + 1);

	HalTimerOn(TMR_2,TRUE);
}

PUBLIC void DevWheelTurnLeftBackRound(U08 lSpeed)
{
	U08 paramLeftWheel;

	paramLeftWheel  = lSpeed;

	HalOcEnable(OC_3,FALSE);
	HalOcEnable(OC_4,FALSE);

	OUT_LWHEEL_CONTROL_A0 = 0;
	OUT_RWHEEL_CONTROL_A0 = 0;
	OUT_RWHEEL_CONTROL_A1 = 0;

	OUT_LWHEEL_CONTROL_A1 = 1;

	HalOcEnable(OC_1,TRUE);

	OC1RS = (UINT16)(((PR2/100) * paramLeftWheel) + 1);

	HalTimerOn(TMR_2,TRUE);
}

PUBLIC void DevWheelTurnLeftRound(U08 rSpeed)
{
	U08 paramRightWheel;

	paramRightWheel = rSpeed;

	HalOcEnable(OC_3,FALSE);
	HalOcEnable(OC_1,FALSE);

	OUT_LWHEEL_CONTROL_A1 = 0;
	OUT_LWHEEL_CONTROL_A0 = 0;
	OUT_RWHEEL_CONTROL_A0 = 0;

	OUT_RWHEEL_CONTROL_A1 = 1;

	HalOcEnable(OC_4,TRUE);

	OC4RS = (UINT16)(((PR2/100) * paramRightWheel ) + 1);

	HalTimerOn(TMR_2,TRUE);
}

PUBLIC void DevWheelTurnRightBackRound(U08 rSpeed)
{
	U08 paramRightWheel;

	paramRightWheel = rSpeed;

	HalOcEnable(OC_3,FALSE);
	HalOcEnable(OC_1,FALSE);

	OUT_LWHEEL_CONTROL_A1 = 0;
	OUT_RWHEEL_CONTROL_A1 = 0;
	OUT_LWHEEL_CONTROL_A0 = 0;

	OUT_RWHEEL_CONTROL_A0 = 1;

	HalOcEnable(OC_4,TRUE);

	OC4RS = (UINT16)(((PR2/100) * paramRightWheel ) + 1);

	HalTimerOn(TMR_2,TRUE);
}

PUBLIC void DevWheelStop(void)
{
	HalOcEnable(OC_1,FALSE);
	HalOcEnable(OC_4,FALSE);

	OC1RS = (UINT16)(STOP_SPEED);
	OC4RS = (UINT16)(STOP_SPEED);

	m_u8DevWheelState = WHEEL_STATE_STOP_STOP;

	OUT_LWHEEL_CONTROL_A0 = 0;
	OUT_LWHEEL_CONTROL_A1 = 0;

	OUT_RWHEEL_CONTROL_A0 = 0;
	OUT_RWHEEL_CONTROL_A1 = 0;
}

/**
 * @fn		PUBLIC inline void WhlResetEncErrSum(void)
 * @brief	ÁÂ¿ì ¹ÙÄûÀÇ ¿£ÄÚ´õ ´©Àû ¿ÀÂ÷ ¸®¼Â
 */
PUBLIC inline void WhlResetEncErrSum(void)
{
	m_fEncErrSumL = 0.0f;
	m_fEncErrSumR = 0.0f;

	// 2007.09.02
	m_u16StsPrevEncL = EncGetCountLeftWheel();
	m_u16StsPrevEncR = EncGetCountRightWheel();

}

/*
 * @fn		PUBLIC inline void WhlSetDirPwm(S16 s16CmpValL, S16 s16CmpValR)
 * @brief	Á¿ìÃø L298ÀÇ È¸Àü ¹æÇâ ¹× PWM Á¦¾î
 * @param   u16CmpValL	[in] ÁÂÃø ¹ÙÄûÀÇ PWM ºñ±³°ª
 * @param   u16CmpValR	[in] ¿ìÃø ¹ÙÄûÀÇ PWM ºñ±³°ª
 * @remark  ºÎÈ£´Â ȸÀü ¹æÇâÀ» ÀÇ¹Ì (+: DIR_FORWARD, -: DIR_BACKWARD)
 */
PUBLIC inline void WhlSetDirPwm(S16 s16CmpValL, S16 s16CmpValR)
{
	S08 s8CurrDirL = DIR_NON, s8CurrDirR = DIR_NON;
	// ---------------------------------------------------------------
	// ÁÂÃø ¹ÙÄûÀÇ È¸Àü ¹æÇâ
	// ¼Óµµ ¸í·ÉÀÌ ¾ç¼öÀÎ °æ¿ì (ÀüÁø)
	if (s16CmpValL > 0 )
	{
		s8CurrDirL = DIR_FORWARD;
	}
	// ¼Óµµ ¸í·ÉÀÌ À½¼öÀÎ °æ¿ì (ÈÄÁø)
	else if ( s16CmpValL < 0 )
	{
		s8CurrDirL = DIR_BACKWARD;
	}
	else //Á¤Áö
	{
		s8CurrDirL = DIR_NON;
	}

	// ¿ìÃø ¹ÙÄûÀÇ È¸Àü ¹æÇâ
	// ¼Óµµ ¸í·ÉÀÌ ¾ç¼öÀÎ °æ¿ì (ÀüÁø)
	if (s16CmpValR > 0 )
	{
		s8CurrDirR = DIR_FORWARD;
	}
	// ¼Óµµ ¸í·ÉÀÌ À½¼öÀÎ °æ¿ì (ÈÄÁø)
	else if ( s16CmpValR < 0 )
	{
		s8CurrDirR = DIR_BACKWARD;
	}
	else //Á¤Áö
	{
		s8CurrDirR = DIR_NON;
	}

	// ---------------------------------------------------------------
	// PWM ºñ±³°ª ¼³Á¤
	// ¾ç¼öÈ­ÇÏ¿© ¿É¼Â ´õÇϱâ
	s16CmpValL = abs(s16CmpValL);// + PWM_OFFSET;
	s16CmpValR = abs(s16CmpValR);// + PWM_OFFSET;

	// PWM ºñ±³°ªÀÇ »óÇÑ°ª Á¦ÇÑ
	s16CmpValL = SAT(s16CmpValL, 0, MAX_PWM_DUTY);
	s16CmpValR = SAT(s16CmpValR, 0, MAX_PWM_DUTY);


	if (s8CurrDirL == DIR_FORWARD)
	{
		if (s8CurrDirR == DIR_FORWARD)
		{
			DevWheelForwardDifferential(s16CmpValL, s16CmpValR);
		}
		else if (s8CurrDirR == DIR_BACKWARD)
		{
			DevWheelTurnRightDifferential(s16CmpValL, s16CmpValR);
		}
		else
		{
			DevWheelTurnRightRound(s16CmpValL);
		}
	}
	else if (s8CurrDirL == DIR_BACKWARD)
	{
		if (s8CurrDirR == DIR_FORWARD)
		{
			DevWheelTurnLeftDifferential(s16CmpValL, s16CmpValR);
		}
		else if (s8CurrDirR == DIR_BACKWARD)
		{
			DevWheelBackwardDifferential(s16CmpValL, s16CmpValR);
		}
		else
		{
			DevWheelTurnLeftBackRound(s16CmpValL);
		}
	}
	else
	{
		if (s8CurrDirR == DIR_FORWARD)
		{
			DevWheelTurnLeftRound(s16CmpValR);
		}
		else if (s8CurrDirR == DIR_BACKWARD)
		{
			DevWheelTurnRightBackRound(s16CmpValR);
		}
		else
		{
			DevWheelStop();
			WhlResetEncErrSum();
		}
	}
}

PUBLIC inline void WhlProc(void)
{
	FLOAT	fPwmCmpValL = 0, fPwmCmpValR = 0;	// PWM ºñ±³°ª
	FLOAT   fEncErrL = 0.0f, fEncErrR = 0.0f;	// ¿£ÄÚ´õ Ä«¿îÆ® ¿ÀÂ÷
	FLOAT   fPwmDutyL = 0.0f, fPwmDutyR = 0.0f;	// PWM duty (percentage)
	FLOAT   fKp = 0.0f;				// P °ÔÀÎ

	// »õ·Î¿î Áö·É ¼Óµµ ¼³Á¤½Ã, ¸ñÇ¥ ¼Óµµ °»½Å
	if (m_bNewCmd == TRUE )
	{
		m_bNewCmd = FALSE;

		// ¸ñÇ¥ ¼Óµµ °»½Å
		m_fTargVl = m_fDesiredVl;
		m_fTargVr = m_fDesiredVr;

		// ¸ñÇ¥ ¼Óµµ¿¡ ÁØÇÏ´Â ¿£ÄÚ´õ ¿É¼Â·®
		m_fTargEncL = VEL2PULSE(m_fTargVl);
		m_fTargEncR = VEL2PULSE(m_fTargVr);
	}

	// ¿£ÄÚ´õ Ä«¿îÆ® ¿ÀÂ÷
	fEncErrL = m_fTargEncL - (FLOAT)m_s16StsEncOffsetL;
	fEncErrR = m_fTargEncR - (FLOAT)m_s16StsEncOffsetR;

	// ¿£ÄÚ´õ Ä«¿îÆ®ÀÇ ´©Àû ¿ÀÂ÷
	m_fEncErrSumL += fEncErrL;
	m_fEncErrSumR += fEncErrR;

	// ¿£ÄÚ´õ Ä«¿îÆ®ÀÇ ´©Àû ¿ÀÂ÷ »óÇÏÇÑ Á¦ÇÑ
	if (m_fEncErrSumL < -MAX_ERRSUM )	m_fEncErrSumL = -MAX_ERRSUM;
	if (m_fEncErrSumL > +MAX_ERRSUM )	m_fEncErrSumL = +MAX_ERRSUM;
	if (m_fEncErrSumR < -MAX_ERRSUM )	m_fEncErrSumR = -MAX_ERRSUM;
	if (m_fEncErrSumR > +MAX_ERRSUM )	m_fEncErrSumR = +MAX_ERRSUM;

	if (m_bMtnStop == TRUE)
	{
//		tmpPWMCount = 0;

		DevWheelStop();
		WhlResetEncErrSum();
		m_bMtnCtrl = FALSE;
		m_bMtnStop = FALSE;
	}
	// ¹ÙÄû Á¤¼Ó Á¦¾î¸¦ ÇÒ °æ¿ì
	else if ( m_bMtnCtrl == TRUE )
	{
		// ÁÂÃø ¹ÙÄû - Àú¼ÓÀÎ °æ¿ì I °ÔÀÎ Á¦¾î
		if (fabs(m_fTargVl) <= V_LSL )
		{
//			tmpPWMCount = 1;
			fPwmCmpValL = (K_I * m_fEncErrSumL);
		}
		// °í¼ÓÀÎ °æ¿ì PI °ÔÀÎ Á¦¾î
		else if ( fabs(m_fTargVl) > V_USL )
		{
//			tmpPWMCount = 3;
			fPwmCmpValL = (K_P * fEncErrL + K_I * m_fEncErrSumL);
		}
		// Áß°£ ¼ÓµµÀÎ °æ¿ì °¡º¯ P, °íÁ¤ I Á¦¾î
		else
		{
//			tmpPWMCount = 2;
			// V_LSL¿¡¼­ fKp = 0, V_USL¿¡¼­ fKp = K_P°¡ µÇµµ·Ï ¼±ÇüÈ­
			fKp = (K_P) * ( fabs(m_fTargVl) - V_LSL );
			fKp /= (V_USL - V_LSL);
			fPwmCmpValL = (fKp * fEncErrL + K_I * m_fEncErrSumL);
		}

		// ¿ìÃø ¹ÙÄû - Àú¼ÓÀÎ °æ¿ì I °ÔÀÎ Á¦¾î
		if (fabs(m_fTargVr) <= V_LSL )
		{
			fPwmCmpValR = (K_I * m_fEncErrSumR);
		}
		// °í¼ÓÀÎ °æ¿ì PI °ÔÀÎ Á¦¾î
		else if ( fabs(m_fTargVr) > V_USL )
		{
			fPwmCmpValR = (K_P * fEncErrR + K_I * m_fEncErrSumR);
		}
		// Áß°£ ¼ÓµµÀÎ °æ¿ì °¡º¯ P, °íÁ¤ I Á¦¾î
		else
		{
			// V_LSL¿¡¼­ fKp = 0, V_USL¿¡¼­ fKp = K_P°¡ µÇµµ·Ï ¼±ÇüÈ­
			fKp = (K_P) * ( fabs(m_fTargVr) - V_LSL );
			fKp /= (V_USL - V_LSL);
			fPwmCmpValR = (fKp * fEncErrR + K_I * m_fEncErrSumR);
		}
		//2007.09.02
		fPwmCmpValL = SAT(fPwmCmpValL, -MAX_PWM_DUTY, +MAX_PWM_DUTY);
		fPwmCmpValR = SAT(fPwmCmpValR, -MAX_PWM_DUTY, +MAX_PWM_DUTY);

//		tmpPWMLeft  = fPwmCmpValL;
//		tmpPWMRight = fPwmCmpValR;

		// ȸÀü ¹æÇâ ¹× PWM ºñ±³°ª ¼³Á¤
		WhlSetDirPwm((S16)floor(fPwmCmpValL+0.5f), (S16)floor(fPwmCmpValR+0.5f));

		ErrChkWheelLeft(fPwmCmpValL, m_s16StsEncOffsetL);
		ErrChkWheelRight(fPwmCmpValR, m_s16StsEncOffsetR);
	}
	// ¹ÙÄû Á¤¼Ó Á¦¾î¸¦ ÇÏÁö ¾ÊÀ» °æ¿ì
	else
	{
//		tmpPWMCount = 4;

		WhlResetEncErrSum();
		ErrChkWheelLeft(0, 0);
		ErrChkWheelRight(0, 0);
	}

}


PUBLIC void	ErrChkWheelLeft(S16 s16PwmCmpValL, S16 s16EncOffsetL)
{
	if (bIsNearlyDie || bOn.TestMode || gErrorMode != ERR_ID_NONE)	return;
#ifdef _ERROR_CHECK_WHEEL_
	static S16 s16Count = 0;
	static S16 s16PwmCmpValMaxL = 0;
	static S16 s16EncOffset1sL = 0;
	static S16 s16ErrCntL = 0;

	// 1ÃÊ µ¿¾ÈÀÇ ÃÖ´ë PWM ºñ±³°ª
	s16PwmCmpValMaxL = MAX(abs(s16PwmCmpValMaxL), abs(s16PwmCmpValL));

	// 1ÃÊ µ¿¾ÈÀÇ ¿£ÄÚ´õ À̵¿·®
	s16EncOffset1sL += s16EncOffsetL;

	// 1Ãʸ¶´Ù °Ë»ç
	if (++s16Count < 20 ) return;

	// Áö·É ¼Óµµ°¡ 0ÀÎ °æ¿ì, ¿¡·¯ Ä«¿îÆ® ¸®¼Â
	if (s16PwmCmpValL < 20 )
	{
		s16PwmCmpValMaxL = 0;
		s16EncOffset1sL = 0;
		s16ErrCntL = 0;
	}

	// ÁÂÃø ¹ÙÄû ±¸¼Ó °¨Áö
	if (abs(s16PwmCmpValMaxL) > 95 )	// 95%
	{
		//if ( abs(s16EncOffset1sL) < 60 )
		if (abs(s16EncOffset1sL) < 300 )
		{
			// ÀÏÁ¤ ½Ã°£ ÀÌ»ó Áö¼Ó½Ã ¿¡·¯ ó¸®
			if (++s16ErrCntL >= 5 )
			{
				s16ErrCntL = 0;
				SetErrMode(ERR_ID_LWHEEL);
			}
		}
		else
		{
			s16ErrCntL = 0;
		}
	}
	else
	{
		s16ErrCntL = 0;
	}

	// reset variables for the next time
	s16Count = 0;
	s16PwmCmpValMaxL = 0;
	s16EncOffset1sL = 0;
#endif
}

PUBLIC void	ErrChkWheelRight(S16 s16PwmCmpValR, S16 s16EncOffsetR)
{
	if (bIsNearlyDie || bOn.TestMode || gErrorMode != ERR_ID_NONE)	return;
#ifdef _ERROR_CHECK_WHEEL_
	static S16 s16Count = 0;
	static S16 s16PwmCmpValMaxR = 0;
	static S16 s16EncOffset1sR = 0;
	static S16 s16ErrCntR = 0;

	// 1ÃÊ µ¿¾ÈÀÇ ÃÖ´ë PWM ºñ±³°ª
	s16PwmCmpValMaxR = MAX(abs(s16PwmCmpValMaxR), abs(s16PwmCmpValR));

	// 1ÃÊ µ¿¾ÈÀÇ ¿£ÄÚ´õ À̵¿·®
	s16EncOffset1sR += s16EncOffsetR;

	// 1Ãʸ¶´Ù °Ë»ç
	if (++s16Count < 20 ) return;

	// Áö·É ¼Óµµ°¡ 0ÀÎ °æ¿ì, ¿¡·¯ Ä«¿îÆ® ¸®¼Â
	if (s16PwmCmpValR < 20 )
	{
		s16PwmCmpValMaxR = 0;
		s16EncOffset1sR = 0;
		s16ErrCntR = 0;
	}

	// ¿ìÃø ¹ÙÄû ±¸¼Ó °¨Áö
	if (abs(s16PwmCmpValMaxR) > 95 )
	{
		//if ( abs(s16EncOffset1sR) < 60 )
		if (abs(s16EncOffset1sR) < 300 )
		{
			if (++s16ErrCntR >= 5 )
			{
				s16ErrCntR = 0;
				SetErrMode(ERR_ID_RWHEEL);
			}
		}
		else
		{
			s16ErrCntR = 0;
		}
	}
	else
	{
		s16ErrCntR = 0;
	}

	// reset variables for the next time
	s16Count = 0;
	s16PwmCmpValMaxR = 0;
	s16EncOffset1sR = 0;
#endif
}

// ÈÙ Á¦¾î ¿©ºÎ
PUBLIC BOOL	m_bMtnCtrl = FALSE;

// ÈÙ Á¤Áö ¿©ºÎ
PUBLIC BOOL	m_bMtnStop = FALSE;

//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// ÁÖÇà °ü·Ã
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
// ¿£ÄÚ´õ offset
PUBLIC S16	m_s16StsEncOffsetL = 0;
PUBLIC S16	m_s16StsEncOffsetR = 0;			// ¿£ÄÚ´õ Ä«¿îÆ® ¿É¼Â

// ÀÌÀü ¿£ÄÚ´õ ÆÞ½º Ä«¿îÆ®
PUBLIC U16	m_u16StsPrevEncL = 0;
PUBLIC U16	m_u16StsPrevEncR = 0;

// current wheel velocity
PUBLIC FLOAT	m_fStsCurrLVel = 0.0f;			/* current left wheel velocity */
PUBLIC FLOAT	m_fStsCurrRVel = 0.0f;			/* current right wheel velocity */

// current linear/angular velocity
PUBLIC FLOAT	m_fStsCurrLinVel = 0.0f;		/* current linear velocity */
PUBLIC FLOAT	m_fStsCurrAngVel = 0.0f;		/* current angular velocity */

// Odometry, Gyro
PUBLIC FLOAT	m_fStsRobotX = 0.0f;
PUBLIC FLOAT	m_fStsRobotY = 0.0f;
PUBLIC FLOAT	m_fStsRobotQ = 0.0f;			// This can be either odometry angle or gyro angle.
PUBLIC FLOAT	m_fStsDiffQ = 0.0f;
PUBLIC FLOAT	m_fStsDiffA = 0.0f;

// Odometer
PUBLIC FLOAT	m_fStsOdometer = 0.0f;
PUBLIC FLOAT	m_fStsDiffOdo = 0.0f;


PUBLIC void StsProc()
{
	U16	u16CurrEncL = 0, u16CurrEncR = 0;			// ÇöÀç ¿£ÄÚ´õ Ä«¿îÆ®
	FLOAT	fDistWL, fDistWR;
	FLOAT	fAngVelGyro = 0.0f;
	static	FLOAT	fOldQ = 0.0f;

	// ÇöÀç ¿£ÄÚ´õ Ä«¿îÆ® Àбâ
	u16CurrEncL = EncGetCountLeftWheel();
	u16CurrEncR = EncGetCountRightWheel();

	// ¿£ÄÚ´õ Ä«¿îÆ® °ª ¿É¼Â °è»ê
	m_s16StsEncOffsetL = Math2CalEncOffset(m_u16StsPrevEncL, u16CurrEncL);
	m_s16StsEncOffsetR = Math2CalEncOffset(m_u16StsPrevEncR, u16CurrEncR);

	// ÀÌÀü ¿£ÄÚ´õ °ª °»½Å
	m_u16StsPrevEncL = u16CurrEncL;
	m_u16StsPrevEncR = u16CurrEncR;

	fDistWL = (float)(PULSE2MM(m_s16StsEncOffsetL));
	fDistWR = (float)(PULSE2MM(m_s16StsEncOffsetR));
	m_fStsDiffOdo = (fDistWL + fDistWR) / 2.0f;		// ·Îº¿ Áß½ÉÁ¡ÀÇ À̵¿ °Å¸® fDistOffsetCP

	// Sts¿¡ ÇöÀç ¼Óµµ Á¤º¸ ÀúÀå
	m_fStsCurrLVel = fDistWL/ WHEEL_CTRL_PERIOD;
	m_fStsCurrRVel = fDistWR/ WHEEL_CTRL_PERIOD;
	Math2WheelVel2LinAngVel(m_fStsCurrLVel, m_fStsCurrRVel, &m_fStsCurrLinVel, &m_fStsCurrAngVel);

	// ·Îº¿ À§Ä¡ °è»ê
	m_fStsOdometer = m_fStsOdometer + m_fStsDiffOdo;
	m_fStsRobotQ = -GyroGetRadAngle();
	m_fStsRobotX += m_fStsDiffOdo * cosf(m_fStsRobotQ);
	m_fStsRobotY += m_fStsDiffOdo * sinf(m_fStsRobotQ);

	// ȸÀüGoniometer
	m_fStsDiffA = (fDistWR - fDistWL) / WHEEL_BASE;
	m_fStsDiffQ = m_fStsRobotQ - fOldQ;
	Math2NormalizeOri(&m_fStsDiffQ);
	fAngVelGyro = m_fStsDiffQ/WHEEL_CTRL_PERIOD;
	fOldQ = m_fStsRobotQ;
}