PID电机控制简单应用

float Pid_realize(float speed)
{
	 
	u8 output;
	mPid.SetSpeed=speed;

	mPid.err=mPid.SetSpeed-mPid.ActualSpeed;
	//mPid.integral+=mPid.err;
	#if 1
	adjustment=mPid.err*mPid.Kp;   //  p  

	if(mPid.output>=LIFT_DUTY_MAX)
	{
		adjustment=adjustment+mPid.integral/100;
	}
	else 
	{

		mPid.integral+=mPid.err*mPid.Ki/10;
		if(mPid.integral<0)
		{
			mPid.integral=0;
		}
		
		adjustment=adjustment+mPid.integral/100;

	}

	    mPid.err*=10;
		adjustment+=((mPid.err-mPid.err_last)/10)*(mPid.Kd);
		mPid.err_last=(mPid.err_last * 9 + mPid.err) / 10;
        adjustment /=  20;
		
		mPid.output=adjustment;

		if(mPid.output>=LIFT_DUTY_MAX)
		{
			mPid.output=LIFT_DUTY_MAX;
		}
		else if(mPid.output<=LIFT_DUTY_MIN)
		{
			mPid.output=LIFT_DUTY_MIN;
		}

	return  mPid.output;
	#else 

	
	mPid.voltage=mPid.Kp*mPid.err+mPid.Ki*mPid.integral+mPid.Kd*(mPid.err-mPid.err_last);
	mPid.err_last=mPid.err;
	output=mPid.voltage/100.0;
	if(output>=LIFT_DUTY_MAX)
	{
		output=LIFT_DUTY_MAX;
	}
	else if(output<=LIFT_DUTY_MIN)
	{
		output=LIFT_DUTY_MIN;
	}

	
	return output;  //	pid.ActualSpeed;
	
	#endif
}


void pid_ini(float p,float i,float d)
{
	float j,k,l;
	j=p;k=i;l=d;
	mPid.SetSpeed=0.0;
	mPid.ActualSpeed=0.0;
	mPid.err=0.0;
	mPid.err_last=0.0;
	mPid.voltage=0.0;
	mPid.integral=0.0;
	mPid.Kp=j;
	mPid.Ki=k;
	mPid.Kd=l;
}