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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers fuzzylogic.cpp Source File

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 }