#include"math.h"
#include"..\inc\menu.h"
#include"..\inc\MotorCtrl.h"

extern int ADRESULT[];
extern int Spcnt;
extern int adc_js;
extern int Ctrldata[];
extern unsigned int qepCnt;

extern unsigned int iopcread();

int volt=0;
int current=0;
int spdgive=0;
int speedgive=0;
int hardspeedgive=0;

void filter();
//-------------------------------------------------------
//	函數名稱：filter
//	輸入參數：無
//	輸出參數：無
//	功能描述：誤差正負及均值濾波
//-------------------------------------------------------
void filter()
{
	int i,j;

	Ctrldata[HARDSPDSET]=ADRESULT[HARDSPDSET];		//硬速度給定
	Ctrldata[CHA]=(iopcread())? -ADRESULT[CHA]: ADRESULT[CHA];		//PC口為0，則差量為正，非0則差量為負
	if(read_menudata(RFWAY) == 0)
	{
		Ctrldata[SUDU]=ADRESULT[SUDU];		//速度反饋
	}
	else if(read_menudata(RFWAY) == 1)
	{
		Ctrldata[SUDU]=qepCnt*3/2;	//編碼器反饋
	}
	
	if(Ctrldata[LINGSU]<(1000*12))
	{
		Ctrldata[LINGSU]+=Ctrldata[SUDU];
	}
	if(Ctrldata[WUCHAJIFEN]<(1000*12))
	{
		Ctrldata[WUCHAJIFEN]+=ADRESULT[GEIDING];
	}
	current+=ADRESULT[DIANLIU];		//反饋電流累加16次
	volt+=ADRESULT[DIANYA];		//反饋電壓累加16次
	speedgive+=ADRESULT[GEIDING];
	hardspeedgive+=ADRESULT[HARDSPDSET];
	return;
}