postion_reg.c

无刷电机资料

#include "Postion_reg.h"

int	PostionPid(POSTIONPID *postion)
{
	long	PostionE;
	int		k0,k1,k2;
	int		PostionE0;
	k0=postion->PostionKp*10+postion->PostionKi+postion->PostionKd;
	k1=postion->PostionKp*10+2*postion->PostionKd;
	k2=postion->PostionKd;
	PostionE=postion->PostionR-postion->PostionF;
	if (PostionE>32767)	PostionE0=32767;
	
	else if (PostionE<-32768)PostionE0=-32768;
	else PostionE0=PostionE;
	postion->PostionU+=k0*PostionE0-k1*postion->PostionE1+k2*postion->PostionE2;
	postion->PostionE2=postion->PostionE1;
	postion->PostionE1=PostionE0;
		
	postion->PostionU/=1000;
	postion->speed_reg->Speed=postion->PostionU;
	if (postion->speed_reg->Speed>postion->Speed)	
		postion->speed_reg->Speed=postion->Speed;
	else if (postion->speed_reg->Speed<(0-postion->Speed))
		postion->speed_reg->Speed=0-postion->Speed;
	postion->speed_reg->updata(postion->speed_reg);


}