arith.c

实现PID算法的C语言程序

BOOLEAN fPIDParamRef = TRUE;

static float alfa;
static float Udk_1[TCTOT] ;
static float Ek_1[TCTOT] ;
static float SumE[TCTOT];

void PIDInit(INT8U n)
	{
	Udk_1[n] = 0;
	Ek_1[n]  = 0;
	SumE[n] = 0;
	}
	
void PIDParamCacu(USERPARAM * up)
	{
	INT32U n;
	
	for (n=0;n<2;n++)
		{
		UserPIDInfo[n].pidKp = (up->pidKp[n] != 0)?(up->pidKp[n]):(up->pidKp[0]);
		UserPIDInfo[n].pidTi = (up->pidTi[n] != 0)?(up->pidTi[n]):(up->pidTi[0]);
		UserPIDInfo[n].pidTd = (up->pidTd[n] != 0)?(up->pidTd[n]):(up->pidTd[0]);
		}

	for (n=2;n<TCTOT;n++)
		{
		UserPIDInfo[n].pidKp = (up->pidKp[n] != 0)?(up->pidKp[n]):(up->pidKp[2+n%2]);
		UserPIDInfo[n].pidTi = (up->pidTi[n] != 0)?(up->pidTi[n]):(up->pidTi[2+n%2]);
		UserPIDInfo[n].pidTd = (up->pidTd[n] != 0)?(up->pidTd[n]):(up->pidTd[2+n%2]);
		}

	for (n=0;n<TCTOT;n++)
		{
		UserPIDInfo[n].Kp = ((float)UserPIDInfo[n].pidKp)/10;	//单位：秒
		UserPIDInfo[n].Ki = UserPIDInfo[n].Kp * ((float)up->PID_Ts) / ((float)UserPIDInfo[n].pidTi);
		UserPIDInfo[n].Kd = UserPIDInfo[n].Kp * ((float)UserPIDInfo[n].pidTd) / ((float)up->PID_Ts);
		}

	alfa = (float)(up->PID_Tf) / ((float)(1 + up->PID_Tf));
	}


INT32U iabs(INT32S d)
	{
	if (d>=0)
		return (INT32U)d;
	else
		return (INT32U)(-d);
	}