pan zhan / panzhan_main_controller_continuous

12

Dependencies:   mbed-dev_spine

fuzzy/fuzzy.cpp@0:dd5d4837292c, 2021-06-09 (annotated)

Committer:
panzhan
Date:
Wed Jun 09 01:41:45 2021 +0000
Revision:
0:dd5d4837292c
continuous motion

Who changed what in which revision?

UserRevisionLine numberNew contents of line
panzhan 0:dd5d4837292c 1 /************************************************************************************
panzhan 0:dd5d4837292c 2 * @author：
panzhan 0:dd5d4837292c 3 * @date : 2019/3/25
panzhan 0:dd5d4837292c 4 * @fuction name：FUZZY_CONTROL
panzhan 0:dd5d4837292c 5 * @fuction description： 模糊自适应控制算法
panzhan 0:dd5d4837292c 6 *************************************************************************************/
panzhan 0:dd5d4837292c 7 #include "fuzzy.h"
panzhan 0:dd5d4837292c 8
panzhan 0:dd5d4837292c 9 fuzzy_control_t ankleFuzzy, kneeFuzzy;
panzhan 0:dd5d4837292c 10
panzhan 0:dd5d4837292c 11
panzhan 0:dd5d4837292c 12 //模糊集合
panzhan 0:dd5d4837292c 13 /*
panzhan 0:dd5d4837292c 14 #define NL -3
panzhan 0:dd5d4837292c 15 #define NM -2
panzhan 0:dd5d4837292c 16 #define NS -1
panzhan 0:dd5d4837292c 17 #define ZE 0
panzhan 0:dd5d4837292c 18 #define PS 1
panzhan 0:dd5d4837292c 19 #define PM 2
panzhan 0:dd5d4837292c 20 #define PL 3
panzhan 0:dd5d4837292c 21 */
panzhan 0:dd5d4837292c 22 const float FuzzyRuleKp[7][7]={
panzhan 0:dd5d4837292c 23 /* EC */
panzhan 0:dd5d4837292c 24 /* -3, -2, -1, 0, 1, 2, 3 */
panzhan 0:dd5d4837292c 25 /*-3*/ 473.86, 473.86, 416.63, 416.63, 333.37, 250.00, 250.00,
panzhan 0:dd5d4837292c 26 /*-2*/ 473.86, 473.86, 416.63, 333.37, 333.37, 250.00, 250.00,
panzhan 0:dd5d4837292c 27 /*-1*/ 416.63, 416.63, 416.63, 416.63, 250.00, 166.64, 166.64,
panzhan 0:dd5d4837292c 28 /* 0*/ 416.63, 416.63, 416.63, 250.00, 333.37, 83.38, 83.38,
panzhan 0:dd5d4837292c 29 /* 1*/ 333.37, 333.37, 250.00, 166.64, 166.64, 83.38, 83.38,
panzhan 0:dd5d4837292c 30 /* 2*/ 333.37, 250.00, 166.64, 83.38, 83.38, 83.38, 26.15,
panzhan 0:dd5d4837292c 31 /* 3*/ 250.00, 250.00, 83.38, 83.38, 83.38, 26.15, 26.15,
panzhan 0:dd5d4837292c 32 };
panzhan 0:dd5d4837292c 33
panzhan 0:dd5d4837292c 34 const float FuzzyRuleKd[7][7]={
panzhan 0:dd5d4837292c 35 /* EC */
panzhan 0:dd5d4837292c 36 /* -3, -2, -1, 0, 1, 2, 3 */
panzhan 0:dd5d4837292c 37 /*-3*/ 3.33, 1.67, 0.26, 0.26, 0.26, 0.83, 3.33,
panzhan 0:dd5d4837292c 38 /*-2*/ 3.33, 2.50, 0.26, 0.26, 0.26, 0.83, 3.33,
panzhan 0:dd5d4837292c 39 /*-1*/ 2.50, 1.67, 1.67, 0.83, 1.67, 1.67, 2.50,
panzhan 0:dd5d4837292c 40 /* 0*/ 2.50, 1.67, 4.17, 1.67, 1.67, 1.67, 2.50,
panzhan 0:dd5d4837292c 41 /* 1*/ 2.50, 2.50, 2.50, 2.50, 2.50, 2.50, 2.50,
panzhan 0:dd5d4837292c 42 /* 2*/ 4.74, 1.67, 3.33, 3.33, 3.33, 3.33, 4.74,
panzhan 0:dd5d4837292c 43 /* 3*/ 4.74, 4.17, 4.17, 4.17, 3.33, 3.33, 0.26,
panzhan 0:dd5d4837292c 44 };
panzhan 0:dd5d4837292c 45
panzhan 0:dd5d4837292c 46
panzhan 0:dd5d4837292c 47 void fuzzy_init(fuzzy_control_t *fuzzy ,float max_e,float max_ec,float gain)
panzhan 0:dd5d4837292c 48 {
panzhan 0:dd5d4837292c 49 fuzzy->quantied_e = 3.0f/max_e;
panzhan 0:dd5d4837292c 50 fuzzy->quantied_ec = 3.0f/max_ec;
panzhan 0:dd5d4837292c 51 fuzzy->quantied_resault = gain;
panzhan 0:dd5d4837292c 52 fuzzy->outKp = 0.0f;
panzhan 0:dd5d4837292c 53 fuzzy->outKd = 0.0f;
panzhan 0:dd5d4837292c 54 }
panzhan 0:dd5d4837292c 55 void fuzzy_control(float e,float ec,fuzzy_control_t *fuzzy,const float fuzzyRuleKp[7][7], const float fuzzyRuleKd[7][7])
panzhan 0:dd5d4837292c 56 {
panzhan 0:dd5d4837292c 57
panzhan 0:dd5d4837292c 58 float etemp,ectemp;
panzhan 0:dd5d4837292c 59 float eLefttemp,ecLefttemp;
panzhan 0:dd5d4837292c 60 float eRighttemp ,ecRighttemp;
panzhan 0:dd5d4837292c 61
panzhan 0:dd5d4837292c 62 int eLeftIndex,ecLeftIndex;
panzhan 0:dd5d4837292c 63 int eRightIndex,ecRightIndex;
panzhan 0:dd5d4837292c 64 e = e * fuzzy->quantied_e;
panzhan 0:dd5d4837292c 65 ec = ec * fuzzy->quantied_ec;
panzhan 0:dd5d4837292c 66 e = range(e,-3.0f,3.0f);
panzhan 0:dd5d4837292c 67 ec = range(ec,-3.0f,3.0f);
panzhan 0:dd5d4837292c 68 etemp = e > 3.0f ? 0.0f : (e < - 3.0f ? 0.0f : (e >= 0.0f ? (e >= 2.0f ? 2.5f: (e >= 1.0f ? 1.5f : 0.5f)) : (e >= -1.0f ? -0.5f : (e >= -2.0f ? -1.5f : (e >= -3.0f ? -2.5f : 0.0f)))));
panzhan 0:dd5d4837292c 69 eLeftIndex = (int)((etemp-0.5f) + 3.0f); //[-3,3] -> [0,6]
panzhan 0:dd5d4837292c 70 eRightIndex = (int)((etemp+0.5f) + 3.0f);
panzhan 0:dd5d4837292c 71 eLefttemp =etemp == 0.0f ? 0.0f:((etemp+0.5f)-e);
panzhan 0:dd5d4837292c 72 eRighttemp=etemp == 0.0f ? 0.0f:( e-(etemp-0.5f));
panzhan 0:dd5d4837292c 73
panzhan 0:dd5d4837292c 74 ectemp = ec > 3.0f ? 0.0f : (ec < - 3.0f ? 0.0f : (ec >= 0.0f ? (ec >= 2.0f ? 2.5f: (ec >= 1.0f ? 1.5f : 0.5f)) : (ec >= -1.0f ? -0.5f : (ec >= -2.0f ? -1.5f : (ec >= -3.0f ? -2.5f : 0.0f) ))));
panzhan 0:dd5d4837292c 75 ecLeftIndex = (int)((ectemp-0.5f) + 3.0f); //[-6,6] -> [0,12]
panzhan 0:dd5d4837292c 76 ecRightIndex = (int)((ectemp+0.5f) + 3.0f);
panzhan 0:dd5d4837292c 77 ecLefttemp =ectemp == 0.0f ? 0.0f:((ectemp+0.5f)-ec);
panzhan 0:dd5d4837292c 78 ecRighttemp=ectemp == 0.0f ? 0.0f:( ec-(ectemp-0.5f));
panzhan 0:dd5d4837292c 79
panzhan 0:dd5d4837292c 80
panzhan 0:dd5d4837292c 81 /*************************************反模糊*************************************/
panzhan 0:dd5d4837292c 82
panzhan 0:dd5d4837292c 83
panzhan 0:dd5d4837292c 84
panzhan 0:dd5d4837292c 85
panzhan 0:dd5d4837292c 86 fuzzy->outKp = (eLefttemp * ecLefttemp * fuzzyRuleKp[eLeftIndex][ecLeftIndex]
panzhan 0:dd5d4837292c 87 + eLefttemp * ecRighttemp * fuzzyRuleKp[eLeftIndex][ecRightIndex]
panzhan 0:dd5d4837292c 88 + eRighttemp * ecLefttemp * fuzzyRuleKp[eRightIndex][ecLeftIndex]
panzhan 0:dd5d4837292c 89 + eRighttemp * ecRighttemp * fuzzyRuleKp[eRightIndex][ecRightIndex]);
panzhan 0:dd5d4837292c 90
panzhan 0:dd5d4837292c 91 fuzzy->outKd = (eLefttemp * ecLefttemp * fuzzyRuleKd[eLeftIndex][ecLeftIndex]
panzhan 0:dd5d4837292c 92 + eLefttemp * ecRighttemp * fuzzyRuleKd[eLeftIndex][ecRightIndex]
panzhan 0:dd5d4837292c 93 + eRighttemp * ecLefttemp * fuzzyRuleKd[eRightIndex][ecLeftIndex]
panzhan 0:dd5d4837292c 94 + eRighttemp * ecRighttemp * fuzzyRuleKd[eRightIndex][ecRightIndex]);
panzhan 0:dd5d4837292c 95
panzhan 0:dd5d4837292c 96 //对解算出的模糊结果进行量化
panzhan 0:dd5d4837292c 97 fuzzy->outKp = fuzzy->outKp * fuzzy->quantied_resault;
panzhan 0:dd5d4837292c 98 fuzzy->outKd = fuzzy->outKd * fuzzy->quantied_resault;
panzhan 0:dd5d4837292c 99 }