#include?“pid.h“

PID?pid;?//存放PID算法所需要的數(shù)據(jù)
PID1?pid1;
void?PID_Init（）
{
	pid.Sv=03000;?????//用戶設(shè)定值
	pid.Pv=00.0;??????//檢測(cè)值
	pid.OUT0?=00;??????//PID常數(shù)
	pid.Ek=0.0;???????//當(dāng)前誤差值
	pid.Ek_1=0.0;?????//上一個(gè)誤差值
	pid.Ek_2=0.0;?????//上上一個(gè)誤差值
	pid.Kp=36;?????//比例常數(shù)
	pid.Ki=0.25;???????//積分常數(shù)
	pid.Kd=135;???????//微分常數(shù)
	pid.uplimit=6000;?//上限值
	pid.lowlimit=-6000;//下限值
}
void?PID1_Init（）
{
	pid1.Sv=1400.525;?????//用戶設(shè)定值
	pid1.Pv=00.0;??????//檢測(cè)值
	pid1.OUT0?=0;??????//PID常數(shù)
	pid1.Ek=0.0;???????//當(dāng)前誤差值
	pid1.Ek_1=0.0;?????//上一個(gè)誤差值
	pid1.Ek_2=0.0;?????//上上一個(gè)誤差值
	pid1.Kp=6.5;?????//比例常數(shù)
	pid1.Ki=0.36;???????//積分常數(shù)
	pid1.Kd=0.00005;???????//微分常數(shù)
	pid1.uplimit=6000;?//上限值
	pid1.lowlimit=-6000;//下限值
	pid1.PIDOUT=-6000;
}
float?Set_Pwm（float?moto1）
{
	float?OUT;
	if（moto1>0）			AIN2=0			AIN1=1;
	else?	??????????AIN2=1			AIN1=0;
	OUT=myabs（moto1）;
	return?OUT;
}
//絕對(duì)值函數(shù)
float?myabs（float?a）
{?		???
	??int?temp;
		if（a<0）??temp=-a;??
	??else?temp=a;
	??return?temp;
}

int?PID1_output?（）
{
	float?PIDOUT;
	pid1.Ek=pid1.Sv-pid1.Pv;
	PIDOUT=pid1.Kp*（pid1.Ek-pid1.Ek_1）+pid1.Ki*pid1.Ek+pid1.Kd*（pid1.Ek_2*pid1.Ek_1+pid1.Ek_2）+pid1.OUT0;
	pid1.Ek_2=pid1.Ek_1;
	pid1.Ek_1=pid1.Ek;
	pid1.PIDOUT+=PIDOUT;
	pid1.PIDOUT=（pid1.PIDOUTpid1.uplimit）???pid1.uplimit?:?pid1.PIDOUT;
	return?pid1.PIDOUT;
}
int?PID_output?（）
{
	float?PIDOUT;
	pid.Ek=pid.Sv-pid.Pv;
	PIDOUT=pid.Kp*pid.Ek+pid.Ki*（pid.Ek+pid.Ek_1+pid.Ek_2）+pid.Kd*（pid.Ek-pid.Ek_1）+pid.OUT0;
	pid.Ek_2=pid.Ek_1;
	pid.Ek_1=pid.Ek;
	pid.PIDOUT=PIDOUT;
	pid.PIDOUT=（pid.PIDOUTpid.uplimit）???pid.uplimit?:?pid.PIDOUT;
	return?pid.PIDOUT;
}