Implementation of the fuzzy control algorithm for obstacle avoidance. This algorithm is for a two-wheel differential drive robot. It uses distance information from 3 analogue IR range sensors and controls DC motors using ESCON servo controllers. This C++ library is written for mbed LPC1768. For any questions and/or reporting bugs contact author: vans.edw@gmail.com
fuzzylogic.cpp
00001 #include <stdlib.h> 00002 #include "fuzzylogic.h" 00003 00004 00005 fuzzylogic::fuzzylogic(int nI1, int nI2, int nI3, int nO) : m_nI1(nI1), m_nI2(nI2), m_nI3(nI3), m_nO(nO) 00006 { 00007 //ctor 00008 00009 //srand(time(NULL)); //randomize the random number generator 00010 00011 m_inParam1 = new double[m_nI1]; 00012 m_inParam2 = new double[m_nI2]; 00013 m_inParam3 = new double[m_nI3]; 00014 m_outParam = new double[m_nO]; 00015 mem_in1 = new double[m_nI1]; 00016 mem_in2 = new double[m_nI2]; 00017 mem_in3 = new double[m_nI3]; 00018 00019 //Default SF values 00020 SF_in1 = 1.0; 00021 SF_in2 = 1.0; 00022 SF_in3 = 1.0; 00023 SF_out = 1.0; 00024 00025 00026 00027 I = 3; 00028 R2 = 0; 00029 R3 = 0; 00030 00031 for(int i = 0; i < m_nI1; i++) 00032 { 00033 mem_in1[i] = 0.0; 00034 } 00035 for(int i = 0; i < m_nI2; i++) 00036 { 00037 mem_in2[i] = 0.0; 00038 } 00039 for(int i = 0; i < m_nI3; i++) 00040 { 00041 mem_in3[i] = 0.0; 00042 } 00043 00044 //Activation Degrees 00045 actv = new double**[m_nI1]; 00046 FRB = new double**[m_nI1]; 00047 for (int i = 0; i < m_nI1; i++) 00048 { 00049 actv[i] = new double*[m_nI2]; 00050 FRB[i] = new double*[m_nI2]; 00051 for(int j = 0; j < m_nI2; j++) 00052 { 00053 actv[i][j] = new double[m_nI3]; 00054 FRB[i][j] = new double[m_nI3]; 00055 for(int k = 0; k < m_nI3; k++) 00056 { 00057 actv[i][j][k] = 0.0; 00058 FRB[i][j][k] = 0.0; 00059 } 00060 } 00061 } 00062 } 00063 00064 fuzzylogic::~fuzzylogic() 00065 { 00066 //dtor 00067 delete[] m_inParam1; 00068 delete[] m_inParam2; 00069 delete[] m_inParam3; 00070 delete[] m_outParam; 00071 delete[] mem_in1; 00072 delete[] mem_in2; 00073 delete[] mem_in3; 00074 00075 for (int i = 0; i < m_nI1; i++) 00076 { 00077 for(int j = 0; j < m_nI2; j++) 00078 { 00079 delete[] actv[i][j]; 00080 delete[] FRB[i][j]; 00081 } 00082 delete[] actv[i]; 00083 delete[] FRB[i]; 00084 } 00085 delete[] actv; 00086 delete[]FRB; 00087 } 00088 00089 void fuzzylogic::setInParam(double *inParam1, double *inParam2, double *inParam3) 00090 { 00091 for(int i = 0; i < m_nI1; i++) 00092 { 00093 m_inParam1[i] = inParam1[i]; 00094 } 00095 00096 for(int i = 0; i < m_nI2; i++) 00097 { 00098 m_inParam2[i] = inParam2[i]; 00099 } 00100 00101 for(int i = 0; i < m_nI3; i++) 00102 { 00103 m_inParam3[i] = inParam3[i]; 00104 } 00105 } 00106 00107 void fuzzylogic::setOutParam(double *outParam) 00108 { 00109 for(int i = 0; i < m_nO; i++) 00110 { 00111 m_outParam[i] = outParam[i]; 00112 } 00113 } 00114 00115 void fuzzylogic::setScalingFactor(double SFI1, double SFI2, double SFI3, double SFO) 00116 { 00117 SF_in1 = SFI1; 00118 SF_in2 = SFI2; 00119 SF_in3 = SFI3; 00120 SF_out = SFO; 00121 } 00122 00123 void fuzzylogic::fuzzification(double *mem_in, double *inParam, double in, int m_nI) 00124 { 00125 //fuzzification 00126 double F = 0.0; 00127 for(int i = 0; i < m_nI; i++) 00128 { 00129 mem_in[i] = 0.0; 00130 } 00131 00132 if(in <= inParam[0]) 00133 { 00134 mem_in[0] = 1.0; //Leftmost Case 00135 } 00136 else if(in >= inParam[m_nI - 1]) 00137 { 00138 mem_in[m_nI - 1] = 1.0; //Rightmost Case 00139 } 00140 else 00141 { 00142 for(int i = 0; i < (m_nI - 1); i++) 00143 { 00144 if(in > inParam[i] && in <= inParam[i + 1]) 00145 { 00146 F = (in - inParam[i])/(inParam[i + 1] - inParam[i]); 00147 mem_in[i] = 1.0 - F; 00148 mem_in[i + 1] = F; 00149 } 00150 } 00151 } 00152 00153 } 00154 00155 double fuzzylogic::getMembershipIn1(int m1) 00156 { 00157 return mem_in1[m1]; 00158 } 00159 00160 double fuzzylogic::getMembershipIn2(int m2) 00161 { 00162 return mem_in2[m2]; 00163 } 00164 00165 double fuzzylogic::getMembershipIn3(int m3) 00166 { 00167 return mem_in3[m3]; 00168 } 00169 00170 void fuzzylogic::fi() 00171 { 00172 //Fuzzy Inference 00173 for(int i = 0; i < m_nI1; i++) 00174 { 00175 for(int j = 0; j < m_nI2; j++) 00176 { 00177 for(int k = 0; k < m_nI3; k++) 00178 { 00179 actv[i][j][k] = mem_in1[i]*mem_in2[j]*mem_in3[k]; 00180 //std::cout<<actv[i][j][k]<<"\n"; 00181 } 00182 } 00183 //cout<<endl; 00184 } 00185 } 00186 00187 double fuzzylogic::getRuleBase(int i, int j, int k) //Function for FUzzy Rule Base 00188 { 00189 int NBPB[2] = {0, 6}; 00190 int NMPM[2] = {1, 5}; 00191 int NSZPS[3] = {2, 3, 4}; 00192 00193 if(i == 0 && j == 0 && k == 0){R2 = rand() % 2; I = NBPB[R2];} //Rule 27 00194 if(i == 0 && j == 0 && k == 1){I = 6;} //Rule 26 00195 if(i == 0 && j == 0 && k == 2){I = 6;} //Rule 25 00196 if(i == 0 && j == 1 && k == 0){R2 = rand() % 2;I = NBPB[R2];} //Rule 24 00197 if(i == 0 && j == 1 && k == 1){I = 6;} //Rule 23 00198 if(i == 0 && j == 1 && k == 2){I = 6;} //Rule 22 00199 if(i == 0 && j == 2 && k == 0){I = 3;}//R2 = rand() % 2;I = NBPB[R2];} //Rule 21 00200 if(i == 0 && j == 2 && k == 1){I = 6;} //Rule 20 00201 if(i == 0 && j == 2 && k == 2){I = 6;} //Rule 19 00202 00203 if(i == 1 && j == 0 && k == 0){I = 0;} //Rule 18 00204 if(i == 1 && j == 0 && k == 1){R2 = rand() % 2;I = NBPB[R2];} //Rule 17 00205 if(i == 1 && j == 0 && k == 2){I = 6;} //Rule 16 00206 if(i == 1 && j == 1 && k == 0){I = 0;} //Rule 15 00207 if(i == 1 && j == 1 && k == 1){R2 = rand() % 2;I = NMPM[R2];} //Rule 14 00208 if(i == 1 && j == 1 && k == 2){I = 5;} //Rule 13 00209 if(i == 1 && j == 2 && k == 0){I = 0;} //Rule 12 00210 if(i == 1 && j == 2 && k == 1){I = 3;}//R2 = rand() % 2;I = NMPM[R2];} //Rule 11 00211 if(i == 1 && j == 2 && k == 2){I = 5;} //Rule 10 00212 00213 if(i == 2 && j == 0 && k == 0){I = 0;} //Rule 9 00214 if(i == 2 && j == 0 && k == 1){I = 0;} //Rule 8 00215 if(i == 2 && j == 0 && k == 2){R2 = rand() % 2;I = NBPB[R2];} //Rule 7 00216 if(i == 2 && j == 1 && k == 0){I = 0;} //Rule 6 00217 if(i == 2 && j == 1 && k == 1){I = 1;} //Rule 5 00218 if(i == 2 && j == 1 && k == 2){R2 = rand() % 2;I = NMPM[R2];} //Rule 4 00219 if(i == 2 && j == 2 && k == 0){I = 0;} //Rule 3 00220 if(i == 2 && j == 2 && k == 1){I = 1;} //Rule 2 00221 if(i == 2 && j == 2 && k == 2){R3 = rand() % 3;I = NSZPS[R3];} //Rule 1 00222 00223 return m_outParam[I]; 00224 } 00225 00226 void fuzzylogic::setRuleBase() 00227 { 00228 for(int i = 0; i < m_nI1; i++) 00229 { 00230 for(int j = 0; j < m_nI2; j++) 00231 { 00232 for(int k = 0; k < m_nI3; k++) 00233 { 00234 FRB[i][j][k] = getRuleBase(i, j, k); 00235 } 00236 00237 } 00238 } 00239 } 00240 00241 double fuzzylogic::defuzzification() 00242 { 00243 //Defuzzification Method - Weighted Average 00244 double sum1 = 0.0; 00245 double sum2 = 0.0; 00246 for(int i = 0; i < m_nI1; i++) 00247 { 00248 for(int j = 0; j < m_nI2; j++) 00249 { 00250 for(int k = 0; k < m_nI3; k++) 00251 { 00252 sum1 = sum1 + actv[i][j][k]; 00253 sum2 = sum2 + actv[i][j][k]*FRB[i][j][k]; 00254 } 00255 00256 } 00257 } 00258 return (sum2/sum1); 00259 } 00260 00261 double fuzzylogic::evalfis(double in1, double in2, double in3) 00262 { 00263 //evaluate the FIS 00264 s_in1 = in1 * SF_in1; //scaled inputs 00265 s_in2 = in2 * SF_in2; 00266 s_in3 = in3 * SF_in3; 00267 00268 fuzzification(mem_in1, m_inParam1, s_in1, m_nI1); 00269 fuzzification(mem_in2, m_inParam2, s_in2, m_nI2); 00270 fuzzification(mem_in3, m_inParam3, s_in3, m_nI3); 00271 fi(); 00272 out = defuzzification() * SF_out; 00273 return out; 00274 }
Generated on Fri Jul 22 2022 11:40:57 by
1.7.2