Revision 1:b00ebb75099f, committed 2017-11-15

Comitter:

efrain95

Date:

Wed Nov 15 01:20:21 2017 +0000

Parent:

0:49465eeab179

Commit message:

Control de pendulo invertido;

Changed in this revision

diff -r 49465eeab179 -r b00ebb75099f main.cpp
--- a/main.cpp	Mon Nov 06 22:37:56 2017 +0000
+++ b/main.cpp	Wed Nov 15 01:20:21 2017 +0000
@@ -22,10 +22,13 @@
 const int fis_gcO = 1;
 // Number of rules to the fuzzy inference system
 const int fis_gcR = 5;
-
+int velocidad;
+int vel_ant;
+Serial speed1(PTE0,PTE1);
+Serial speed2(PTE22,PTE23);
 FIS_TYPE g_fisInput[fis_gcI];
 FIS_TYPE g_fisOutput[fis_gcO];
-
+ 
 //***********************************************************************
 // Support functions for Fuzzy Inference System                          
 //***********************************************************************
@@ -41,35 +44,35 @@
     t1 = min(t1, t2);
     return (FIS_TYPE) max(t1, 0);
 }
-
+ 
 FIS_TYPE fis_min(FIS_TYPE a, FIS_TYPE b)
 {
     return min(a, b);
 }
-
+ 
 FIS_TYPE fis_max(FIS_TYPE a, FIS_TYPE b)
 {
     return max(a, b);
 }
-
+ 
 FIS_TYPE fis_array_operation(FIS_TYPE *array, int size, _FIS_ARR_OP pfnOp)
 {
     int i;
     FIS_TYPE ret = 0;
-
+ 
     if (size == 0) return ret;
     if (size == 1) return array[0];
-
+ 
     ret = array[0];
     for (i = 1; i < size; i++)
     {
         ret = (*pfnOp)(ret, array[i]);
     }
-
+ 
     return ret;
 }
-
-
+ 
+ 
 //***********************************************************************
 // Data for Fuzzy Inference System                                       
 //***********************************************************************
@@ -78,13 +81,13 @@
 {
     fis_trimf
 };
-
+ 
 // Count of member function for each Input
 int fis_gIMFCount[] = { 5 };
-
+ 
 // Count of member function for each Output 
 int fis_gOMFCount[] = { 5 };
-
+ 
 // Coefficients for the Input Member Functions
 FIS_TYPE fis_gMFI0Coeff1[] = { -60, -40, -20 };
 FIS_TYPE fis_gMFI0Coeff2[] = { -40, -20, 0 };
@@ -93,7 +96,7 @@
 FIS_TYPE fis_gMFI0Coeff5[] = { 20, 40, 60 };
 FIS_TYPE* fis_gMFI0Coeff[] = { fis_gMFI0Coeff1, fis_gMFI0Coeff2, fis_gMFI0Coeff3, fis_gMFI0Coeff4, fis_gMFI0Coeff5 };
 FIS_TYPE** fis_gMFICoeff[] = { fis_gMFI0Coeff };
-
+ 
 // Coefficients for the Input Member Functions
 FIS_TYPE fis_gMFO0Coeff1[] = { -153, -102, -51 };
 FIS_TYPE fis_gMFO0Coeff2[] = { -102, -51, 0 };
@@ -102,21 +105,21 @@
 FIS_TYPE fis_gMFO0Coeff5[] = { 51, 102, 153 };
 FIS_TYPE* fis_gMFO0Coeff[] = { fis_gMFO0Coeff1, fis_gMFO0Coeff2, fis_gMFO0Coeff3, fis_gMFO0Coeff4, fis_gMFO0Coeff5 };
 FIS_TYPE** fis_gMFOCoeff[] = { fis_gMFO0Coeff };
-
+ 
 // Input membership function set
 int fis_gMFI0[] = { 0, 0, 0, 0, 0 };
 int* fis_gMFI[] = { fis_gMFI0};
-
+ 
 // Output membership function set
 int fis_gMFO0[] = { 0, 0, 0, 0, 0 };
 int* fis_gMFO[] = { fis_gMFO0};
-
+ 
 // Rule Weights
 FIS_TYPE fis_gRWeight[] = { 1, 1, 1, 1, 1 };
-
+ 
 // Rule Type
 int fis_gRType[] = { 1, 1, 1, 1, 1 };
-
+ 
 // Rule Inputs
 int fis_gRI0[] = { 1 };
 int fis_gRI1[] = { 2 };
@@ -124,7 +127,7 @@
 int fis_gRI3[] = { 4 };
 int fis_gRI4[] = { 5 };
 int* fis_gRI[] = { fis_gRI0, fis_gRI1, fis_gRI2, fis_gRI3, fis_gRI4 };
-
+ 
 // Rule Outputs
 int fis_gRO0[] = { 1 };
 int fis_gRO1[] = { 2 };
@@ -132,19 +135,19 @@
 int fis_gRO3[] = { 4 };
 int fis_gRO4[] = { 5 };
 int* fis_gRO[] = { fis_gRO0, fis_gRO1, fis_gRO2, fis_gRO3, fis_gRO4 };
-
+ 
 // Input range Min
 FIS_TYPE fis_gIMin[] = { -60 };
-
+ 
 // Input range Max
 FIS_TYPE fis_gIMax[] = { 60 };
-
+ 
 // Output range Min
 FIS_TYPE fis_gOMin[] = { -102 };
-
+ 
 // Output range Max
 FIS_TYPE fis_gOMax[] = { 102 };
-
+ 
 //***********************************************************************
 // Data dependent support functions for Fuzzy Inference System                          
 //***********************************************************************
@@ -152,7 +155,7 @@
 {
     FIS_TYPE mfOut;
     int r;
-
+ 
     for (r = 0; r < fis_gcR; ++r)
     {
         int index = fis_gRO[r][o];
@@ -170,12 +173,12 @@
         {
             mfOut = 0;
         }
-
+ 
         fuzzyRuleSet[0][r] = fis_min(mfOut, fuzzyRuleSet[1][r]);
     }
     return fis_array_operation(fuzzyRuleSet[0], fis_gcR, fis_max);
 }
-
+ 
 FIS_TYPE fis_defuzz_centroid(FIS_TYPE** fuzzyRuleSet, int o)
 {
     FIS_TYPE step = (fis_gOMax[o] - fis_gOMin[o]) / (FIS_RESOLUSION - 1);
@@ -183,7 +186,7 @@
     FIS_TYPE momentum = 0;
     FIS_TYPE dist, slice;
     int i;
-
+ 
     // calculate the area under the curve formed by the MF outputs
     for (i = 0; i < FIS_RESOLUSION; ++i){
         dist = fis_gOMin[o] + (step * i);
@@ -191,10 +194,10 @@
         area += slice;
         momentum += slice*dist;
     }
-
+ 
     return ((area == 0) ? ((fis_gOMax[o] + fis_gOMin[o]) / 2) : (momentum / area));
 }
-
+ 
 //***********************************************************************
 // Fuzzy Inference System                                                
 //***********************************************************************
@@ -208,7 +211,7 @@
     FIS_TYPE fuzzyFires[fis_gcR] = { 0 };
     FIS_TYPE* fuzzyRuleSet[] = { fuzzyRules, fuzzyFires };
     FIS_TYPE sW = 0;
-
+ 
     // Transforming input to fuzzy Input
     int i, j, r, o;
     for (i = 0; i < fis_gcI; ++i)
@@ -219,7 +222,7 @@
                 (fis_gMF[fis_gMFI[i][j]])(g_fisInput[i], fis_gMFICoeff[i][j]);
         }
     }
-
+ 
     int index = 0;
     for (r = 0; r < fis_gcR; ++r)
     {
@@ -251,11 +254,11 @@
                     fuzzyFires[r] = fis_max(fuzzyFires[r], 0);
             }
         }
-
+ 
         fuzzyFires[r] = fis_gRWeight[r] * fuzzyFires[r];
         sW += fuzzyFires[r];
     }
-
+ 
     if (sW == 0)
     {
         for (o = 0; o < fis_gcO; ++o)
@@ -271,7 +274,7 @@
         }
     }
 }
-
+ 
 int main(void)
 {
     MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
@@ -281,8 +284,8 @@
     // initialize the Analog pins for input.
     // Pin mode for Input: IMU
     //pinMode(0 , INPUT);
-
-
+ 
+ 
     // initialize the Analog pins for output.
     // Pin mode for Output: Velocidad
     //pinMode(1 , OUTPUT);
@@ -302,6 +305,15 @@
     g_fisOutput[0] = 0;
     fis_evaluate();
     // Set output vlaue: Velocidad
-    printf("%f\r\n",g_fisOutput[0]);
+    vel_ant=velocidad;
+    velocidad=(g_fisOutput[0])*7;
+    if(velocidad!= vel_ant){
+        speed1.printf("S%i\r\n",velocidad);
+        speed2.printf("S%i\r\n",velocidad);
+    }
+    //set_secuence();
+    printf("%f  %i\r\n",g_fisOutput[0],velocidad);
+    
+
     }
 }
\ No newline at end of file