模糊pid c语言实现

#include "stdio.h"

#include "stdlib.h"

typedef unsigned char uint8;

enum MembershipDegree

{

    NB=-3,NM,NS,ZO,PS,PM,PB

};

struct fuzzyparams

{

    float error;

    float error_mapped;

    float max;

    float min;

    float gradmembership[2];

    uint8 grad_index[2];

};

enum MembershipDegree membership_valus[7]={NB,NM,NS,ZO,PS,PM,PB};

struct fuzzyparams fuzzy_e;

struct fuzzyparams fuzzy_ec;

enum MembershipDegree  Kp_rule_list[7][7] = {

                            {PB,PB,PM,PM,PS,ZO,ZO},     //kp规则表

                            {PB,PB,PM,PS,PS,ZO,NS},

                            {PM,PM,PM,PS,ZO,NS,NS},

                            {PM,PM,PS,ZO,NS,NM,NM},

                            {PS,PS,ZO,NS,NS,NM,NM},

                            {PS,ZO,NS,NM,NM,NM,NB},

                            {ZO,ZO,NM,NM,NM,NB,NB} };

enum MembershipDegree   Ki_rule_list[7][7] = {

                            {NB,NB,NM,NM,NS,ZO,ZO},     //ki规则表

                            {NB,NB,NM,NS,NS,ZO,ZO},

                            {NB,NM,NS,NS,ZO,PS,PS},

                            {NM,NM,NS,ZO,PS,PM,PM},

                            {NM,NS,ZO,PS,PS,PM,PB},

                            {ZO,ZO,PS,PS,PM,PB,PB},

                            {ZO,ZO,PS,PM,PM,PB,PB} };    

enum MembershipDegree   Kd_rule_list[7][7] = {

                            {PS,NS,NB,NB,NB,NM,PS},    //kd规则表

                            {PS,NS,NB,NM,NM,NS,ZO},

                            {ZO,NS,NM,NM,NS,NS,ZO},

                            {ZO,NS,NS,NS,NS,NS,ZO},

                            {ZO,ZO,ZO,ZO,ZO,ZO,ZO},

                            {PB,NS,PS,PS,PS,PS,PB},

                            {PB,PM,PM,PM,PS,PS,PB} };

       

void get_e_membership(struct fuzzyparams* fuzzy)

{

    int i;

    fuzzy->error_mapped=membership_valus[0]+(fuzzy->error-fuzzy->min)*(membership_valus[6]-membership_valus[0])/(fuzzy->max-fuzzy->min);

    if(fuzzy->error_mapped>membership_valus[0]&&fuzzy->error_mapped<membership_valus[6])

    {

        for(i=0;i<6;i++)

        {

            if(fuzzy->error_mapped <= membership_valus[i+1])

            {

                fuzzy->gradmembership[0]=membership_valus[i+1]-fuzzy->error_mapped;

                fuzzy->gradmembership[1]=fuzzy->error_mapped-membership_valus[i];

                fuzzy->grad_index[0]=i;

                fuzzy->grad_index[1]=i+1;

                break;

            }

        }    

    }

    else if(fuzzy->error_mapped <= membership_valus[0])

    {

        fuzzy->gradmembership[0]=1;

        fuzzy->gradmembership[1]=0;

        fuzzy->grad_index[0]=0;

        fuzzy->grad_index[1]=-1;

    }

    else

    {

        fuzzy->gradmembership[0]=1;

        fuzzy->gradmembership[1]=0;

        fuzzy->grad_index[0]=6;

        fuzzy->grad_index[1]=-1;        

    }

}

float get_output_mapped(enum MembershipDegree list[7][7],struct fuzzyparams fuzzy_e,struct fuzzyparams fuzzy_ec,float min,float max)

{

    float output=0;

    float output_mapped=0;

    output =fuzzy_e.gradmembership[0]*fuzzy_ec.gradmembership[0]*list[fuzzy_e.grad_index[0]][fuzzy_ec.grad_index[0]]+

            fuzzy_e.gradmembership[0]*fuzzy_ec.gradmembership[1]*list[fuzzy_e.grad_index[0]][fuzzy_ec.grad_index[1]]+

            fuzzy_e.gradmembership[1]*fuzzy_ec.gradmembership[0]*list[fuzzy_e.grad_index[1]][fuzzy_ec.grad_index[0]]+

            fuzzy_e.gradmembership[1]*fuzzy_ec.gradmembership[1]*list[fuzzy_e.grad_index[1]][fuzzy_ec.grad_index[1]];

    output_mapped=(output-NB)*(max-min)/(PB-NB)+min;

    return output_mapped;

}

struct PID_para

{

    float target;

    float actual;

    float actual_max;

    float actual_min;

    float error;

    float error_pre;

    float error_sum;

    float kp;

    float kp_max;

    float kp_min;  

    float ki;

    float ki_max;

    float ki_min;

    float kd;

    float kd_max;

    float kd_min;

}PID_turn;

void main()

{

    PID_turn.target=30;

    PID_turn.actual=50;

    PID_turn.actual_max=80;

    PID_turn.actual_min=20;

    PID_turn.kp_max=0.18;

    PID_turn.kp_min=0;

    PID_turn.kd_max=0;

    PID_turn.kd_min=0;

    PID_turn.ki_max=0;

    PID_turn.ki_min=0;

    int i;

    for(i=0;i<40;i++)

    {

        PID_turn.error=PID_turn.target-PID_turn.actual;//20

        fuzzy_e.error=PID_turn.error;

        PID_turn.error_sum+=PID_turn.error;

        fuzzy_e.max=PID_turn.target-PID_turn.actual_min;//50

        fuzzy_e.min=PID_turn.target-PID_turn.actual_max;//-10

        fuzzy_ec.error=PID_turn.error-PID_turn.error_pre;

        fuzzy_ec.max=60;

        fuzzy_ec.min=0;

        PID_turn.error_pre=fuzzy_e.error;

        get_e_membership(&fuzzy_e);

        get_e_membership(&fuzzy_ec);

        PID_turn.kp=get_output_mapped(Kp_rule_list,fuzzy_e,fuzzy_ec,PID_turn.kp_min,PID_turn.kp_max);    

        PID_turn.ki=get_output_mapped(Ki_rule_list,fuzzy_e,fuzzy_ec,PID_turn.ki_min,PID_turn.ki_max);

        PID_turn.kd=get_output_mapped(Kd_rule_list,fuzzy_e,fuzzy_ec,PID_turn.kd_min,PID_turn.kd_max);  

        PID_turn.actual=PID_turn.kp*PID_turn.error+PID_turn.kd*fuzzy_ec.error+PID_turn.ki*PID_turn.error_sum;

        printf("%f\n",PID_turn.actual);

 

    }

    system("pause");

}

简单调试了一下结果如下：

不知道为什么，不加id效果反而更好