fuzzyPID代码参考模糊

95 篇文章 5 订阅

pid.h

/*
 * FuzzyPID.h
 *
 *  Created on: 2022��1��21��
 *  Author: 30516
 *  Effect: ����ģ��PID
 */

#ifndef CODE_FUZZYPID_H_
#define CODE_FUZZYPID_H_

#include "common.h" //������������

//������ģ�����ĺ궨��{NB,NM,NS,ZO,PS,PM,PB},������������Ϊ������������ı�д
//******************ģ���Ӽ�******************************
#define NB -3
#define NM -2
#define NS -1
#define ZO  0
#define PS  1
#define PM  2
#define PB  3
//*******************************************************

//����ֵ�ĵ�λֵ��FuzzyArray[]*���=����ֵ����ֵ��Ҫ����ʹ�ã�(MAX-MIN)/8�õ�
/*******************��������궨��****************************/
//*******************************************************
#define BiasMembership 60 //ƫ��
#define BiasCMembership 10//ƫ������һ��ƫ��IJ�
#define SteerKP 1
#define SteerKd 1
#define MotorKP 1
#define MotorKI 1
//********************************************************

/*************************ģ������***********************/
uint8 KPFuzzyRule[7][7]={{PB,PB,PM,PM,PS,ZO,ZO},
                         {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}};//KP������ģ�������
uint8 KIFuzzyRule[7][7]={{NB,NB,NM,NM,NS,ZO,ZO},
                         {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}};//KI������ģ�������
uint8 KDFuzzyRule[7][7]={{PS,NS,NB,NB,NB,NM,PS},
                         {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}};//KD������ģ�������
/**********************************************************/

void ClacMembership(float E,float Membership[2],int Index[2]);//���ƫ�������ֵ��������
int  SolutionFuzzy(int IndexE[2],float MSE[2],int IndexEC[2],float MSEC[2],int type);//��ģ��

#endif /* CODE_FUZZYPID_H_ */

pid.c

/*
 * FuzzyPID.c
 *
 *  Created on: 2022��1��21��
 *  Author: 30516
 *  Effect: ����ģ��PID
 */

#include "FuzzyPID.h"

/********************************************************************************************
 ** ��������: ���ƫ�������ֵ��������
 ** ��    ��: float E: ƫ��/ƫ������궨�壨������������-3~3֮�䣩
 **           float Membership[2]: ���ڴ洢�������������������ȵ����飬M[0]�DZȽ�С��ģ���Ӽ���������
 **           int Index[2]: �������飬�����������������Membershipͬ��M[0]�DZȽ�С��ģ���Ӽ�������ֵ
 ** �� �� ֵ: ��
 ** ��    ��: LJF
 *********************************************************************************************/
void ClacMembership(float E,float Membership[2],int Index[2])
{
    /*���������Ⱥ���*/
    if(E>=NB && E<=NM)
    {
        Index[0]=0;//NB
        Index[1]=1;//NM
        Membership[0]=NM-E;//������NB��������(NM-E)/(NM-NB)����Ϊ��ĸ����1
        Membership[1]=E-NB;//������NM��������(E-NB)/(NM-NB)
    }
    else if(E>=NM && E<=NS)
    {
        Index[0]=1;
        Index[1]=2;
        Membership[0]=NS-E;//������NM��������
        Membership[1]=E-NM;//������NS��������
    }
    else if(E>=NS && E<=ZO)
    {
        Index[0]=2;
        Index[1]=3;
        Membership[0]=ZO-E;//������NS��������
        Membership[1]=E-NS;//������ZO��������
    }
    else if(E>=ZO && E<=PS)
    {
        Index[0]=3;
        Index[1]=4;
        Membership[0]=PS-E;//������ZO��������
        Membership[1]=E-ZO;//������PS��������
    }
    else if(E>=PS && E<=PM)
    {
        Index[0]=4;
        Index[1]=5;
        Membership[0]=PM-E;//������PS��������
        Membership[1]=E-PS;//������PM��������
    }
    else if(E>=PM && E<=PB)
    {
        Index[0]=5;
        Index[1]=6;
        Membership[0]=PB-E;//������PS��������
        Membership[1]=E-PM;//������PM��������
    }
}

/********************************************************************************************
 ** ��������: ����ģ��������Լ����ķ�������
 ** ��    ��: int IndexE[2]: ƫ�����������
 **           float MSE[2]: ƫ�������������ֵ������������
 **           int IndexEC[2]: ƫ��仯�ʵ���������
 **           float MSEC[2]: ƫ��仯�ʶ�����������ֵ������������
 **           int type: 1��KP 2��KI 3��KD
 ** �� �� ֵ: K������K=K0+detaK*����ϵ��
 ** ��    ��: LJF
 *********************************************************************************************/
int  SolutionFuzzy(int IndexE[2],float MSE[2],int IndexEC[2],float MSEC[2],int type)
{
    int temp=0;
    switch(type)
    {
        case 1:
            //���ķ����
            temp=KPFuzzyRule[IndexE[0]][IndexEC[0]]*MSE[0]*MSEC[0]+
                 KPFuzzyRule[IndexE[0]][IndexEC[1]]*MSE[0]*MSEC[1]+
                 KPFuzzyRule[IndexE[1]][IndexEC[0]]*MSE[1]*MSEC[0]+
                 KPFuzzyRule[IndexE[1]][IndexEC[1]]*MSE[1]*MSEC[1];
            break;
        case 2:
            temp=KIFuzzyRule[IndexE[0]][IndexEC[0]]*MSE[0]*MSEC[0]+
                 KIFuzzyRule[IndexE[0]][IndexEC[1]]*MSE[0]*MSEC[1]+
                 KIFuzzyRule[IndexE[1]][IndexEC[0]]*MSE[1]*MSEC[0]+
                 KIFuzzyRule[IndexE[1]][IndexEC[1]]*MSE[1]*MSEC[1];
            break;
        case 3:
            temp=KDFuzzyRule[IndexE[0]][IndexEC[0]]*MSE[0]*MSEC[0]+
                 KDFuzzyRule[IndexE[0]][IndexEC[1]]*MSE[0]*MSEC[1]+
                 KDFuzzyRule[IndexE[1]][IndexEC[0]]*MSE[1]*MSEC[0]+
                 KDFuzzyRule[IndexE[1]][IndexEC[1]]*MSE[1]*MSEC[1];
            break;
        default:
            break;
    }
    return temp;
}


  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值