標題: PID增量算法 c語言 [打印本頁]

---

作者: lgyhbu    時間: 2016-12-16 13:55
標題: PID增量算法 c語言
#include<stdio.h>
struct _pid{
float SetSpeed;            //
    float ActualSpeed;        //
    float err;                //
    float err_next;            //
    float err_last;            //
    float Kp,Ki,Kd;
        float last_speed;
}pid;   
void PID_init(void){                                         //?????????
    pid.SetSpeed=3.0;
        pid.ActualSpeed=0.0;
    //pid.last_Speed=0.0;
    pid.err=0.0;
    pid.err_next=0.0;
    pid.err_last=0.0;
    pid.Kp=0.2;
    pid.Ki=0.15;
    pid.Kd=0.2;
        pid.last_speed=0.0;
}
float absu(float err)
         {
                 if(err<0)
                err=0-err;
                 return err;
         
         }
      void PID_realize(float speed)                                                
{
                              
    float inSpeed=0;int index;
        int i=0;
    pid.SetSpeed=speed;  
  pid.err=pid.SetSpeed-pid.ActualSpeed;
        while(pid.err>0.0001)
        {
                i++;
        
                pid.err=pid.SetSpeed-pid.ActualSpeed;
          if(absu(pid.err)>pid.SetSpeed-0.01)      
          {
            index=0;
        }
        else
        {
            index=1;}
        inSpeed = pid.Kp*(pid.err-pid.err_next)+index*pid.Ki*pid.err+pid.Kd*(pid.err-2*pid.err_next+pid.err_last);
    pid.ActualSpeed+=inSpeed;
    pid.err_last=pid.err_next;
    pid.err_next=pid.err;
        printf("no:%d,%f,%f\n",i,pid.ActualSpeed,inSpeed);
    }

        }
   main()
   {
                           PID_init();
                    PID_realize(200);

   }

---

歡迎光臨 (http://www.torrancerestoration.com/bbs/)

Powered by Discuz! X3.1