PID算法

1、增量型PID 

 1 /*
 2     增量型pid
 3 */
 4 #include<stdio.h>
 5 #include<stdlib.h>
 6 struct _pid{
 7     float SetSpeed; //定义设定值
 8     float ActualSpeed; //定义实际值
 9     float err; //定义偏差值
10     float err_next; //定义上一个偏差值
11     float err_last; //定义最上前的偏差值
12     float Kp,Ki,Kd; //定义比例、积分、微分系数
13 }pid;
14 
15 void PID_init(){
16     pid.SetSpeed=0.0;
17     pid.ActualSpeed=0.0;
18     pid.err=0.0;
19     pid.err_last=0.0;
20     pid.err_next=0.0;
21     pid.Kp=0.2;
22     pid.Ki=0.132;
23     pid.Kd=0.3;
24 }
25 float PID_realize(float speed){
26     pid.SetSpeed=speed;
27     pid.err=pid.SetSpeed-pid.ActualSpeed;
28     float
29     incrementSpeed=pid.Kp*(pid.err-pid.err_next)+pid.Ki*pid.err+pid.Kd*(pid.err-2*pid.err_next+pid.err_last);
30     pid.ActualSpeed+=incrementSpeed;
31     pid.err_last=pid.err_next;
32     pid.err_next=pid.err;
33     return pid.ActualSpeed;
34 }
35 int main(){
36     PID_init();
37     int count=0;
38     while(count<100)
39     {
40     float speed=PID_realize(200.0);
41     printf("%f\n",speed);
42     count++;
43     }
44     return 0;
45 }

 2、位置型PID

 1 /*
 2     位置型pid
 3 */
 4 #include <stdio.h>
 5 //#include<ioctl.h>
 6 
 7 struct _pid{
 8 float SetSpeed;         //定义设定值
 9 float ActualSpeed;         //定义实际值
10 float err;                 //定义偏差值
11 float err_last;         //定义上一个偏差值
12 float Kp,Ki,Kd;         //定义比例、积分、微分系数
13 float voltage;             //定义电压值（控制执行器的变量）
14 float integral;            //定义积分值
15 }pid;
16 
17 //项目中获取到的参数
18 void PID_init(){
19     printf("PID_init begin \n");
20     pid.SetSpeed=0.0;
21     pid.ActualSpeed=0.0;
22     pid.err=0.0;
23     pid.err_last=0.0;
24     pid.voltage=0.0;
25     pid.integral=0.0;
26     pid.Kp=0.01;                //自己设定
27     pid.Ki=0.115;            //自己设定
28     pid.Kd=0.35;                //自己设定
29     printf("PID_init end \n");
30 }
31 
32 float PID_realize(float speed){
33     pid.SetSpeed=speed;                        //设定值
34     pid.err=pid.SetSpeed-pid.ActualSpeed;    //设定值-实际值
35     pid.integral+=pid.err;                    //积分值，偏差累加
36     pid.voltage=pid.Kp*pid.err+pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
37     pid.err_last=pid.err;                    //上一个偏差值
38     pid.ActualSpeed=pid.voltage*1.0;        //算出实际值
39     return pid.ActualSpeed;                    //返回
40 }
41 
42 void delay(int i)
43 {
44      while(i--);
45 }
46 int main(){
47     printf("System begin \n");
48     PID_init();
49     int count=0;
50     while(count<100)
51     {
52         float  speed=PID_realize(3289);
53         printf("%f\n",speed);
54         count++;
55     }
56     return 0;
57 }

 

posted @ 2020-08-15 15:52  0-1-0  阅读(71)  评论(0)    收藏  举报