Diff: fuzzy/fuzzy.cpp

Revision:

0:dd5d4837292c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fuzzy/fuzzy.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,99 @@
+/************************************************************************************
+* @author：
+* @date : 2019/3/25
+* @fuction name：FUZZY_CONTROL
+* @fuction description： 模糊自适应控制算法
+*************************************************************************************/
+#include "fuzzy.h"
+
+fuzzy_control_t ankleFuzzy, kneeFuzzy;
+
+
+//模糊集合
+/*
+#define NL   -3
+#define NM   -2
+#define NS   -1
+#define ZE   0
+#define PS   1
+#define PM   2
+#define PL   3
+*/
+const float FuzzyRuleKp[7][7]={
+    /*                  EC                     */
+    /*  -3,       -2,     -1,     0,     1,      2,     3 */
+/*-3*/ 473.86, 473.86, 416.63, 416.63, 333.37, 250.00, 250.00,
+/*-2*/ 473.86, 473.86, 416.63, 333.37, 333.37, 250.00, 250.00,
+/*-1*/ 416.63, 416.63, 416.63, 416.63, 250.00, 166.64, 166.64,
+/* 0*/ 416.63, 416.63, 416.63, 250.00, 333.37, 83.38,  83.38,
+/* 1*/ 333.37, 333.37, 250.00, 166.64, 166.64, 83.38,  83.38,
+/* 2*/ 333.37, 250.00, 166.64, 83.38,  83.38,  83.38,  26.15,
+/* 3*/ 250.00, 250.00, 83.38,  83.38,  83.38,  26.15,  26.15,
+};
+
+const float FuzzyRuleKd[7][7]={
+    /*                  EC                     */
+    /*  -3,   -2,   -1,   0,     1,   2,    3 */
+/*-3*/ 3.33, 1.67, 0.26, 0.26, 0.26, 0.83, 3.33,
+/*-2*/ 3.33, 2.50, 0.26, 0.26, 0.26, 0.83, 3.33,
+/*-1*/ 2.50, 1.67, 1.67, 0.83, 1.67, 1.67, 2.50,
+/* 0*/ 2.50, 1.67, 4.17, 1.67, 1.67, 1.67, 2.50,
+/* 1*/ 2.50, 2.50, 2.50, 2.50, 2.50, 2.50, 2.50,
+/* 2*/ 4.74, 1.67, 3.33, 3.33, 3.33, 3.33, 4.74,
+/* 3*/ 4.74, 4.17, 4.17, 4.17, 3.33, 3.33, 0.26,
+};
+
+
+void fuzzy_init(fuzzy_control_t *fuzzy ,float max_e,float max_ec,float gain)
+{
+    fuzzy->quantied_e = 3.0f/max_e;
+    fuzzy->quantied_ec = 3.0f/max_ec;
+    fuzzy->quantied_resault = gain;
+    fuzzy->outKp = 0.0f;
+    fuzzy->outKd = 0.0f;
+}
+void fuzzy_control(float e,float ec,fuzzy_control_t *fuzzy,const float fuzzyRuleKp[7][7], const float fuzzyRuleKd[7][7])
+{
+
+     float etemp,ectemp;
+     float eLefttemp,ecLefttemp;
+     float eRighttemp ,ecRighttemp;
+
+     int eLeftIndex,ecLeftIndex;
+     int eRightIndex,ecRightIndex;
+     e = e * fuzzy->quantied_e;
+     ec = ec * fuzzy->quantied_ec;
+     e = range(e,-3.0f,3.0f);
+     ec = range(ec,-3.0f,3.0f);
+     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)))));
+     eLeftIndex = (int)((etemp-0.5f) + 3.0f);        //[-3,3] -> [0,6]
+     eRightIndex = (int)((etemp+0.5f) + 3.0f);
+     eLefttemp =etemp == 0.0f ? 0.0f:((etemp+0.5f)-e);             
+     eRighttemp=etemp == 0.0f ? 0.0f:( e-(etemp-0.5f));
+     
+     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) ))));
+     ecLeftIndex = (int)((ectemp-0.5f) + 3.0f);        //[-6,6] -> [0,12]
+     ecRightIndex = (int)((ectemp+0.5f) + 3.0f);
+     ecLefttemp =ectemp == 0.0f ? 0.0f:((ectemp+0.5f)-ec);
+     ecRighttemp=ectemp == 0.0f ? 0.0f:( ec-(ectemp-0.5f));
+
+
+/*************************************反模糊*************************************/
+
+
+
+
+    fuzzy->outKp = (eLefttemp * ecLefttemp * fuzzyRuleKp[eLeftIndex][ecLeftIndex]
+                    + eLefttemp * ecRighttemp * fuzzyRuleKp[eLeftIndex][ecRightIndex]
+                    + eRighttemp * ecLefttemp * fuzzyRuleKp[eRightIndex][ecLeftIndex]
+                    + eRighttemp * ecRighttemp * fuzzyRuleKp[eRightIndex][ecRightIndex]);
+                    
+    fuzzy->outKd = (eLefttemp * ecLefttemp * fuzzyRuleKd[eLeftIndex][ecLeftIndex]
+                    + eLefttemp * ecRighttemp * fuzzyRuleKd[eLeftIndex][ecRightIndex]
+                    + eRighttemp * ecLefttemp * fuzzyRuleKd[eRightIndex][ecLeftIndex]
+                    + eRighttemp * ecRighttemp * fuzzyRuleKd[eRightIndex][ecRightIndex]);
+
+    //对解算出的模糊结果进行量化
+    fuzzy->outKp = fuzzy->outKp * fuzzy->quantied_resault;
+    fuzzy->outKd = fuzzy->outKd * fuzzy->quantied_resault;
+}