首先，就增量式PID的函數進行編寫：

頭文件，全局變量與宏定義如下：

#include "stdio.h"
#include "math.h"
void pid_init();//PID參數初始化
float pid_realise(float speed);//實現PID算法

#define value 1400.0

首先定義結構體pid的相關內容，后面將根據PID里面的相關參數進行修改調整，觀察波形情況。

struct {
   float set_speed;//設定速度 
   float actual_speed;//實際速度
   float error;//偏差  
   float error_next;//上一個偏差  
   float error_last;//上上一個偏差 
   float kp,ki,kd;//定義比例，積分，微分參數  
}pid;

將PID結構體中的參數進行初始化設置，其中的參數是我經過調整后的參數

void pid_init()
{
    pid.set_speed = 0;
    pid.actual_speed = 10000.0;//原始值0.0
    pid.error = 0.0;
    pid.error_next = 0.0;
    pid.error_last = 0.0;
    //可調節PID 參數。使跟蹤曲線慢慢接近階躍函數200.0 // 
    pid.kp = 0.33333;//原始值0.2
    pid.ki = 0.2111;//原始值0.1
    pid.kd = 0.1;//原始值0.3
}

接下來實現PID實現的過程函數：

float pid_realise(float speed)//實現pid  
{   float increment_speed;//增量
    pid.set_speed = speed;//設置目標速度  
    pid.error = pid.set_speed - pid.actual_speed;
 
     
    increment_speed = pid.kp*(pid.error-pid.error_next)+pid.ki*pid.error+\
    pid.kd*(pid.error-2*pid.error_next+pid.error_last);//增量計算公式  
    
    pid.actual_speed+= increment_speed;
    pid.error_last = pid.error_next;//下一次迭代  
    pid.error_next = pid.error;
    return pid.actual_speed; 
}

接著就是主函數，主函數這里將進行設置，邏輯如下：初始化參數，進行運算，運算次數設置為400次，可以提高，因為調整后的參數，后面又有break可以跳出while循環因此不需要在意這里，接下來就是不斷判斷期望值與實際值誤差書否在5以內

int main()
{ int count = 1;
 int num = 0;
 int type;
   pid_init();
   while(count<400)//進行400次 PID 運算可以提高，使初始值從0開始接近200.0   
   {
       float speed = pid_realise(value);//設定值設定為200.0
 type=abs(speed-value);
 printf("%f\n",speed);//
 if( type <= 5)    ///判斷每次的誤差是否在5以內
 {
       num++;
  if(num==6)
  {
    printf("run number is %d\r\n",count);
    break;
  }
  else count++;
 }
 else count++;
   }
}

運行函數之后得到如下圖所示的曲線，可以看到，假設單位階躍相應的過程中期望值發生改變，假設初始值為7000，要調整到理想值按照當前參數大約需要47次變換。