位置式PID函數（可重複使用）

//pid_fuction.h

#ifndef __PID_H
#define __PID_H	 
#include "sys.h"	
#include "stdbool.h"
//PID模塊
typedef struct PID_FIXCYCLE
{	
//INTPUT
	char  RESET;		 //重新設置控制器
	char  MANUAL;     //如果MANUAL是1，調節器暫停，切換到手工操作	
	
	float ACTUAL;  	//當前採樣值
	float SET_POINT;    //設定值或期望值		
	
	float Y_OFFSET;	//Y 輸出補償值
	
	float Y_MAX;      	//最大值限制
	float Y_MIN;      	//最小值限制
	
	float CYCLE;		//設置功能週期時間
//OUTPUT	
	
	float Y;        	//本次輸出量
	
//SYSTEM
	float KP;           //定義比例常數	  
	float KI;           //定義積分常數
	float KD;          //定義微分常數
		
	float Ui;           //定義積分
	float Ud;          //定義微分

	float E_2;         //存儲前前次誤差
	float E_1;         //存諸前次誤差
	float E;           //存儲本次誤差
	char  LIMITS_ACTVE;	//超出Y_MIN和Y_MAX範圍，LIMITS_ACTVE輸出變爲1
	char  OVERFLOW;		//積分溢出標誌
	char  Init;         //單次賦值
				
 }PID_FIXCYCLE;
extern PID_FIXCYCLE  motorA_PID;         //定義A電機pid參數
extern PID_FIXCYCLE  motorB_PID;         //定義B電機pid參數
 
void PidFixWork(PID_FIXCYCLE* Control,u8 *Time_a) ;
void runA_PIDInit(void);
void runB_PIDInit(void);

#endif

pid_fuction.c
#include <string.h>
#include <stdio.h>

#include "pid_fuction.h"	 //pid控制
/*增量式PID
            
1 實現給電機速度閉環控制；
2 主要放在電機PWM輸入之前，梯形加速之後；											 													 
*/
//===========================增量式PID=====================
void PidFixWork(PID_FIXCYCLE* Control,u8 *Time_a) 
{
  if(Control->RESET==0)                   //PID 復位
  {
    if(*Time_a >= Control->CYCLE)         //運行週期
    {    
      *Time_a = 0;
			Control->E  =Control->SET_POINT-Control->ACTUAL;     //得到本次誤差
      if(!Control->OVERFLOW)  //積分未溢出
      {
        Control->Ui =Control->Ui+Control->KI*Control->E;   //得到積分項	      
      }
      Control->Ud =Control->KD*(Control->E-2*Control->E_1+Control->E_2); //得到微分項   
      Control->E_2=Control->E_1;                //歷史存儲
      Control->E_1=Control->E;	                //計算增量和
      Control->Y  =Control->KP*(Control->E+Control->Ud+Control->Ui)+Control->Y_OFFSET;      
      
	 if((Control->E<=100)&&(Control->E>=-100))
     {
	   Control->Y = 0;//偏差爲0，自動清零,增量式
	   Control->Ui= 0;
	 }
      if((Control->Y<Control->Y_MIN))
      {
        Control->Y=Control->Y_MIN;      //值域限制 
        Control->LIMITS_ACTVE=1;        //超出輸出      
        if(Control->E<=0)								//積分溢出
        {Control->OVERFLOW=1;}
        else{Control->OVERFLOW=0;}        
      }
      else if((Control->Y>Control->Y_MAX))
      {	
        Control->Y=Control->Y_MAX;
        Control->LIMITS_ACTVE=1;  
        if(Control->E>=0)								//積分溢出
        {Control->OVERFLOW=1;}
        else{Control->OVERFLOW=0;}         
      }else
      {
        Control->LIMITS_ACTVE=0;
        Control->OVERFLOW=0;
      }   
    }
//
   }    
    else{//復位(刷一遍參數)
      Control->Y=Control->Y_MIN;     
	  *Time_a = 0;			
   }
}

//PID初始化

PID_FIXCYCLE  motorA_PID;         //定義A電機pid參數
PID_FIXCYCLE  motorB_PID;         //定義B電機pid參數

void runA_PIDInit(void)
{
	if(motorA_PID.Init==0)           //單次進入
	{
		motorA_PID.RESET    = 0;         //PID 函數復位
		motorA_PID.KP       = 3.5;	     //比例值 P
		motorA_PID.KI         = 0.035;	 //積分值 I
		motorA_PID.KD         = 1;		 //微分值 D  
		motorA_PID.Y_MAX     =  5000;	 //限副 輸出 最大值
		motorA_PID.Y_MIN      = - 5000;	 //限副 輸出 最小值
		motorA_PID.Y_OFFSET = 0;	     //輸出補償值
		motorA_PID.CYCLE    = 5;         //PID運行週期 * 10ms
		
	  motorA_PID.Init = 1;
	}
}

void runB_PIDInit(void)
{
	if(motorB_PID.Init==0)           //單次進入
	{
		motorB_PID.RESET     = 0;       //PID 函數復位
		motorB_PID.KP         = 3.5;	//比例值 P
		motorB_PID.KI         = 0.035;	//積分值 I
		motorB_PID.KD        = 1;		//微分值 D  
		motorB_PID.Y_MAX    =  5000;	//限副 輸出 最大值
		motorB_PID.Y_MIN     = - 5000;	//限副 輸出 最小值
		motorB_PID.Y_OFFSET  = 0;		//輸出補償值
		motorB_PID.CYCLE     = 5;       //PID運行週期 * 10ms
		
	  motorB_PID.Init = 1;
	}
}

/*---------------------應用樣例---------------

#include "pid_fuction.h"	 //pid控制

void main(void)
{
......
  while(1)
  {

 
	runA_PIDInit();   //A 電機PID初始化
	runB_PIDInit();   //B 電機PID初始化


	motorA_PID.SET_POINT  =  MotorA.Vout ;    //a行走 電機 輸出給PID函數
	motorB_PID.SET_POINT  =  MotorB.Vout ;    //b行走 電機 輸出給PID函數
			
	motorA_PID.ACTUAL = SensorOne.ActualA 	  //a行走 電機 實際速度
	motorB_PID.ACTUAL = SensorOne.ActualB; 	  //b行走 電機 實際速度

	PidFixWork(&motorA_PID,&TP10ms.task04);      //a行走 電機 PID
	PidFixWork(&motorB_PID,&TP10ms.task05);	      //b行走 電機 PID	

	CmdCar.motorAPWM  =  motorA_PID.Y;        //a行走 電機 PID 結果輸出
	CmdCar.motorBPWM  =  motorB_PID.Y;        //b行走 電機 PID 結果輸出	

  }

}