#include "ClearDog.h"

#define q 3.14159265/180
#define FREQ 100

extern int alpha,theta;
extern int init_alpha,init_theta;

float fAlpha[2]={0,0},fTheta[2]={0,0};
float DiffAlpha_Buf[2]={0},DiffTheta_Buf[2]={0};
extern float FeedbackCoeff[4];

float Control()  /* Control algorithm	*/
{    
   	float a,t;
	float fCtrl,fValpha,fVtheta;
	
    /*	Convert to degree */
	a = ((alpha-init_alpha)/1023.0)*345.0;
	t = ((theta-init_theta)/1023.0)*345.0;
	t+=a;              /*************************/
	
    /*計算角速度，并濾波*/	

	fAlpha[0]=fAlpha[1];   
	fAlpha[1]=a*q;
	fTheta[0]=fTheta[1];
	fTheta[1]=t*q;
	
	DiffAlpha_Buf[0] = DiffAlpha_Buf[1];
	DiffAlpha_Buf[1] = (fAlpha[1]-fAlpha[0])*FREQ; 
	
	DiffTheta_Buf[0] = DiffTheta_Buf[1];
	DiffTheta_Buf[1] = (fTheta[1]-fTheta[0])*FREQ;
	
	fValpha = 0.6*DiffAlpha_Buf[1] + 0.4*DiffAlpha_Buf[0];
	fVtheta = 0.6*DiffTheta_Buf[1] + 0.4*DiffTheta_Buf[0]; 
	
	/*********/
    fCtrl=-(FeedbackCoeff[0]*fAlpha[1]+FeedbackCoeff[1]*fTheta[1]+FeedbackCoeff[2]*fValpha+FeedbackCoeff[3]*fVtheta);
		         
	ClearDog();
	return fCtrl;
}