Efrain Duarte / Mbed 2 deprecated Control

Pendulo

Dependencies:   mbed

Fork of Rami by Ramiro Rubio

main.cpp@0:49465eeab179, 2017-11-06 (annotated)

Committer:
RAMIRORUBIO
Date:
Mon Nov 06 22:37:56 2017 +0000
Revision:
0:49465eeab179
Child:
1:b00ebb75099f
rami;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RAMIRORUBIO 0:49465eeab179 1 #include "mbed.h"
RAMIRORUBIO 0:49465eeab179 2 #include "MMA8451Q.h"
RAMIRORUBIO 0:49465eeab179 3 #include "fis_header.h"
RAMIRORUBIO 0:49465eeab179 4 #if defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
RAMIRORUBIO 0:49465eeab179 5 PinName const SDA = PTE25;
RAMIRORUBIO 0:49465eeab179 6 PinName const SCL = PTE24;
RAMIRORUBIO 0:49465eeab179 7 #elif defined (TARGET_KL05Z)
RAMIRORUBIO 0:49465eeab179 8 PinName const SDA = PTB4;
RAMIRORUBIO 0:49465eeab179 9 PinName const SCL = PTB3;
RAMIRORUBIO 0:49465eeab179 10 #elif defined (TARGET_K20D50M)
RAMIRORUBIO 0:49465eeab179 11 PinName const SDA = PTB1;
RAMIRORUBIO 0:49465eeab179 12 PinName const SCL = PTB0;
RAMIRORUBIO 0:49465eeab179 13 #else
RAMIRORUBIO 0:49465eeab179 14 #error TARGET NOT DEFINED
RAMIRORUBIO 0:49465eeab179 15 #endif
RAMIRORUBIO 0:49465eeab179 16 #define max(a,b) (((a) > (b)) ? (a) : (b))
RAMIRORUBIO 0:49465eeab179 17 #define min(a,b) (((a) < (b)) ? (a) : (b))
RAMIRORUBIO 0:49465eeab179 18 #define MMA8451_I2C_ADDRESS (0x1d<<1)
RAMIRORUBIO 0:49465eeab179 19 // Number of inputs to the fuzzy inference system
RAMIRORUBIO 0:49465eeab179 20 const int fis_gcI = 1;
RAMIRORUBIO 0:49465eeab179 21 // Number of outputs to the fuzzy inference system
RAMIRORUBIO 0:49465eeab179 22 const int fis_gcO = 1;
RAMIRORUBIO 0:49465eeab179 23 // Number of rules to the fuzzy inference system
RAMIRORUBIO 0:49465eeab179 24 const int fis_gcR = 5;
RAMIRORUBIO 0:49465eeab179 25
RAMIRORUBIO 0:49465eeab179 26 FIS_TYPE g_fisInput[fis_gcI];
RAMIRORUBIO 0:49465eeab179 27 FIS_TYPE g_fisOutput[fis_gcO];
RAMIRORUBIO 0:49465eeab179 28
RAMIRORUBIO 0:49465eeab179 29 //***********************************************************************
RAMIRORUBIO 0:49465eeab179 30 // Support functions for Fuzzy Inference System
RAMIRORUBIO 0:49465eeab179 31 //***********************************************************************
RAMIRORUBIO 0:49465eeab179 32 // Triangular Member Function
RAMIRORUBIO 0:49465eeab179 33 FIS_TYPE fis_trimf(FIS_TYPE x, FIS_TYPE* p)
RAMIRORUBIO 0:49465eeab179 34 {
RAMIRORUBIO 0:49465eeab179 35 FIS_TYPE a = p[0], b = p[1], c = p[2];
RAMIRORUBIO 0:49465eeab179 36 FIS_TYPE t1 = (x - a) / (b - a);
RAMIRORUBIO 0:49465eeab179 37 FIS_TYPE t2 = (c - x) / (c - b);
RAMIRORUBIO 0:49465eeab179 38 if ((a == b) && (b == c)) return (FIS_TYPE) (x == a);
RAMIRORUBIO 0:49465eeab179 39 if (a == b) return (FIS_TYPE) (t2*(b <= x)*(x <= c));
RAMIRORUBIO 0:49465eeab179 40 if (b == c) return (FIS_TYPE) (t1*(a <= x)*(x <= b));
RAMIRORUBIO 0:49465eeab179 41 t1 = min(t1, t2);
RAMIRORUBIO 0:49465eeab179 42 return (FIS_TYPE) max(t1, 0);
RAMIRORUBIO 0:49465eeab179 43 }
RAMIRORUBIO 0:49465eeab179 44
RAMIRORUBIO 0:49465eeab179 45 FIS_TYPE fis_min(FIS_TYPE a, FIS_TYPE b)
RAMIRORUBIO 0:49465eeab179 46 {
RAMIRORUBIO 0:49465eeab179 47 return min(a, b);
RAMIRORUBIO 0:49465eeab179 48 }
RAMIRORUBIO 0:49465eeab179 49
RAMIRORUBIO 0:49465eeab179 50 FIS_TYPE fis_max(FIS_TYPE a, FIS_TYPE b)
RAMIRORUBIO 0:49465eeab179 51 {
RAMIRORUBIO 0:49465eeab179 52 return max(a, b);
RAMIRORUBIO 0:49465eeab179 53 }
RAMIRORUBIO 0:49465eeab179 54
RAMIRORUBIO 0:49465eeab179 55 FIS_TYPE fis_array_operation(FIS_TYPE *array, int size, _FIS_ARR_OP pfnOp)
RAMIRORUBIO 0:49465eeab179 56 {
RAMIRORUBIO 0:49465eeab179 57 int i;
RAMIRORUBIO 0:49465eeab179 58 FIS_TYPE ret = 0;
RAMIRORUBIO 0:49465eeab179 59
RAMIRORUBIO 0:49465eeab179 60 if (size == 0) return ret;
RAMIRORUBIO 0:49465eeab179 61 if (size == 1) return array[0];
RAMIRORUBIO 0:49465eeab179 62
RAMIRORUBIO 0:49465eeab179 63 ret = array[0];
RAMIRORUBIO 0:49465eeab179 64 for (i = 1; i < size; i++)
RAMIRORUBIO 0:49465eeab179 65 {
RAMIRORUBIO 0:49465eeab179 66 ret = (*pfnOp)(ret, array[i]);
RAMIRORUBIO 0:49465eeab179 67 }
RAMIRORUBIO 0:49465eeab179 68
RAMIRORUBIO 0:49465eeab179 69 return ret;
RAMIRORUBIO 0:49465eeab179 70 }
RAMIRORUBIO 0:49465eeab179 71
RAMIRORUBIO 0:49465eeab179 72
RAMIRORUBIO 0:49465eeab179 73 //***********************************************************************
RAMIRORUBIO 0:49465eeab179 74 // Data for Fuzzy Inference System
RAMIRORUBIO 0:49465eeab179 75 //***********************************************************************
RAMIRORUBIO 0:49465eeab179 76 // Pointers to the implementations of member functions
RAMIRORUBIO 0:49465eeab179 77 _FIS_MF fis_gMF[] =
RAMIRORUBIO 0:49465eeab179 78 {
RAMIRORUBIO 0:49465eeab179 79 fis_trimf
RAMIRORUBIO 0:49465eeab179 80 };
RAMIRORUBIO 0:49465eeab179 81
RAMIRORUBIO 0:49465eeab179 82 // Count of member function for each Input
RAMIRORUBIO 0:49465eeab179 83 int fis_gIMFCount[] = { 5 };
RAMIRORUBIO 0:49465eeab179 84
RAMIRORUBIO 0:49465eeab179 85 // Count of member function for each Output
RAMIRORUBIO 0:49465eeab179 86 int fis_gOMFCount[] = { 5 };
RAMIRORUBIO 0:49465eeab179 87
RAMIRORUBIO 0:49465eeab179 88 // Coefficients for the Input Member Functions
RAMIRORUBIO 0:49465eeab179 89 FIS_TYPE fis_gMFI0Coeff1[] = { -60, -40, -20 };
RAMIRORUBIO 0:49465eeab179 90 FIS_TYPE fis_gMFI0Coeff2[] = { -40, -20, 0 };
RAMIRORUBIO 0:49465eeab179 91 FIS_TYPE fis_gMFI0Coeff3[] = { -20, 0, 20 };
RAMIRORUBIO 0:49465eeab179 92 FIS_TYPE fis_gMFI0Coeff4[] = { 0, 20, 40 };
RAMIRORUBIO 0:49465eeab179 93 FIS_TYPE fis_gMFI0Coeff5[] = { 20, 40, 60 };
RAMIRORUBIO 0:49465eeab179 94 FIS_TYPE* fis_gMFI0Coeff[] = { fis_gMFI0Coeff1, fis_gMFI0Coeff2, fis_gMFI0Coeff3, fis_gMFI0Coeff4, fis_gMFI0Coeff5 };
RAMIRORUBIO 0:49465eeab179 95 FIS_TYPE** fis_gMFICoeff[] = { fis_gMFI0Coeff };
RAMIRORUBIO 0:49465eeab179 96
RAMIRORUBIO 0:49465eeab179 97 // Coefficients for the Input Member Functions
RAMIRORUBIO 0:49465eeab179 98 FIS_TYPE fis_gMFO0Coeff1[] = { -153, -102, -51 };
RAMIRORUBIO 0:49465eeab179 99 FIS_TYPE fis_gMFO0Coeff2[] = { -102, -51, 0 };
RAMIRORUBIO 0:49465eeab179 100 FIS_TYPE fis_gMFO0Coeff3[] = { -51, -1.332e-15, 51 };
RAMIRORUBIO 0:49465eeab179 101 FIS_TYPE fis_gMFO0Coeff4[] = { 0, 51, 102 };
RAMIRORUBIO 0:49465eeab179 102 FIS_TYPE fis_gMFO0Coeff5[] = { 51, 102, 153 };
RAMIRORUBIO 0:49465eeab179 103 FIS_TYPE* fis_gMFO0Coeff[] = { fis_gMFO0Coeff1, fis_gMFO0Coeff2, fis_gMFO0Coeff3, fis_gMFO0Coeff4, fis_gMFO0Coeff5 };
RAMIRORUBIO 0:49465eeab179 104 FIS_TYPE** fis_gMFOCoeff[] = { fis_gMFO0Coeff };
RAMIRORUBIO 0:49465eeab179 105
RAMIRORUBIO 0:49465eeab179 106 // Input membership function set
RAMIRORUBIO 0:49465eeab179 107 int fis_gMFI0[] = { 0, 0, 0, 0, 0 };
RAMIRORUBIO 0:49465eeab179 108 int* fis_gMFI[] = { fis_gMFI0};
RAMIRORUBIO 0:49465eeab179 109
RAMIRORUBIO 0:49465eeab179 110 // Output membership function set
RAMIRORUBIO 0:49465eeab179 111 int fis_gMFO0[] = { 0, 0, 0, 0, 0 };
RAMIRORUBIO 0:49465eeab179 112 int* fis_gMFO[] = { fis_gMFO0};
RAMIRORUBIO 0:49465eeab179 113
RAMIRORUBIO 0:49465eeab179 114 // Rule Weights
RAMIRORUBIO 0:49465eeab179 115 FIS_TYPE fis_gRWeight[] = { 1, 1, 1, 1, 1 };
RAMIRORUBIO 0:49465eeab179 116
RAMIRORUBIO 0:49465eeab179 117 // Rule Type
RAMIRORUBIO 0:49465eeab179 118 int fis_gRType[] = { 1, 1, 1, 1, 1 };
RAMIRORUBIO 0:49465eeab179 119
RAMIRORUBIO 0:49465eeab179 120 // Rule Inputs
RAMIRORUBIO 0:49465eeab179 121 int fis_gRI0[] = { 1 };
RAMIRORUBIO 0:49465eeab179 122 int fis_gRI1[] = { 2 };
RAMIRORUBIO 0:49465eeab179 123 int fis_gRI2[] = { 3 };
RAMIRORUBIO 0:49465eeab179 124 int fis_gRI3[] = { 4 };
RAMIRORUBIO 0:49465eeab179 125 int fis_gRI4[] = { 5 };
RAMIRORUBIO 0:49465eeab179 126 int* fis_gRI[] = { fis_gRI0, fis_gRI1, fis_gRI2, fis_gRI3, fis_gRI4 };
RAMIRORUBIO 0:49465eeab179 127
RAMIRORUBIO 0:49465eeab179 128 // Rule Outputs
RAMIRORUBIO 0:49465eeab179 129 int fis_gRO0[] = { 1 };
RAMIRORUBIO 0:49465eeab179 130 int fis_gRO1[] = { 2 };
RAMIRORUBIO 0:49465eeab179 131 int fis_gRO2[] = { 3 };
RAMIRORUBIO 0:49465eeab179 132 int fis_gRO3[] = { 4 };
RAMIRORUBIO 0:49465eeab179 133 int fis_gRO4[] = { 5 };
RAMIRORUBIO 0:49465eeab179 134 int* fis_gRO[] = { fis_gRO0, fis_gRO1, fis_gRO2, fis_gRO3, fis_gRO4 };
RAMIRORUBIO 0:49465eeab179 135
RAMIRORUBIO 0:49465eeab179 136 // Input range Min
RAMIRORUBIO 0:49465eeab179 137 FIS_TYPE fis_gIMin[] = { -60 };
RAMIRORUBIO 0:49465eeab179 138
RAMIRORUBIO 0:49465eeab179 139 // Input range Max
RAMIRORUBIO 0:49465eeab179 140 FIS_TYPE fis_gIMax[] = { 60 };
RAMIRORUBIO 0:49465eeab179 141
RAMIRORUBIO 0:49465eeab179 142 // Output range Min
RAMIRORUBIO 0:49465eeab179 143 FIS_TYPE fis_gOMin[] = { -102 };
RAMIRORUBIO 0:49465eeab179 144
RAMIRORUBIO 0:49465eeab179 145 // Output range Max
RAMIRORUBIO 0:49465eeab179 146 FIS_TYPE fis_gOMax[] = { 102 };
RAMIRORUBIO 0:49465eeab179 147
RAMIRORUBIO 0:49465eeab179 148 //***********************************************************************
RAMIRORUBIO 0:49465eeab179 149 // Data dependent support functions for Fuzzy Inference System
RAMIRORUBIO 0:49465eeab179 150 //***********************************************************************
RAMIRORUBIO 0:49465eeab179 151 FIS_TYPE fis_MF_out(FIS_TYPE** fuzzyRuleSet, FIS_TYPE x, int o)
RAMIRORUBIO 0:49465eeab179 152 {
RAMIRORUBIO 0:49465eeab179 153 FIS_TYPE mfOut;
RAMIRORUBIO 0:49465eeab179 154 int r;
RAMIRORUBIO 0:49465eeab179 155
RAMIRORUBIO 0:49465eeab179 156 for (r = 0; r < fis_gcR; ++r)
RAMIRORUBIO 0:49465eeab179 157 {
RAMIRORUBIO 0:49465eeab179 158 int index = fis_gRO[r][o];
RAMIRORUBIO 0:49465eeab179 159 if (index > 0)
RAMIRORUBIO 0:49465eeab179 160 {
RAMIRORUBIO 0:49465eeab179 161 index = index - 1;
RAMIRORUBIO 0:49465eeab179 162 mfOut = (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
RAMIRORUBIO 0:49465eeab179 163 }
RAMIRORUBIO 0:49465eeab179 164 else if (index < 0)
RAMIRORUBIO 0:49465eeab179 165 {
RAMIRORUBIO 0:49465eeab179 166 index = -index - 1;
RAMIRORUBIO 0:49465eeab179 167 mfOut = 1 - (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
RAMIRORUBIO 0:49465eeab179 168 }
RAMIRORUBIO 0:49465eeab179 169 else
RAMIRORUBIO 0:49465eeab179 170 {
RAMIRORUBIO 0:49465eeab179 171 mfOut = 0;
RAMIRORUBIO 0:49465eeab179 172 }
RAMIRORUBIO 0:49465eeab179 173
RAMIRORUBIO 0:49465eeab179 174 fuzzyRuleSet[0][r] = fis_min(mfOut, fuzzyRuleSet[1][r]);
RAMIRORUBIO 0:49465eeab179 175 }
RAMIRORUBIO 0:49465eeab179 176 return fis_array_operation(fuzzyRuleSet[0], fis_gcR, fis_max);
RAMIRORUBIO 0:49465eeab179 177 }
RAMIRORUBIO 0:49465eeab179 178
RAMIRORUBIO 0:49465eeab179 179 FIS_TYPE fis_defuzz_centroid(FIS_TYPE** fuzzyRuleSet, int o)
RAMIRORUBIO 0:49465eeab179 180 {
RAMIRORUBIO 0:49465eeab179 181 FIS_TYPE step = (fis_gOMax[o] - fis_gOMin[o]) / (FIS_RESOLUSION - 1);
RAMIRORUBIO 0:49465eeab179 182 FIS_TYPE area = 0;
RAMIRORUBIO 0:49465eeab179 183 FIS_TYPE momentum = 0;
RAMIRORUBIO 0:49465eeab179 184 FIS_TYPE dist, slice;
RAMIRORUBIO 0:49465eeab179 185 int i;
RAMIRORUBIO 0:49465eeab179 186
RAMIRORUBIO 0:49465eeab179 187 // calculate the area under the curve formed by the MF outputs
RAMIRORUBIO 0:49465eeab179 188 for (i = 0; i < FIS_RESOLUSION; ++i){
RAMIRORUBIO 0:49465eeab179 189 dist = fis_gOMin[o] + (step * i);
RAMIRORUBIO 0:49465eeab179 190 slice = step * fis_MF_out(fuzzyRuleSet, dist, o);
RAMIRORUBIO 0:49465eeab179 191 area += slice;
RAMIRORUBIO 0:49465eeab179 192 momentum += slice*dist;
RAMIRORUBIO 0:49465eeab179 193 }
RAMIRORUBIO 0:49465eeab179 194
RAMIRORUBIO 0:49465eeab179 195 return ((area == 0) ? ((fis_gOMax[o] + fis_gOMin[o]) / 2) : (momentum / area));
RAMIRORUBIO 0:49465eeab179 196 }
RAMIRORUBIO 0:49465eeab179 197
RAMIRORUBIO 0:49465eeab179 198 //***********************************************************************
RAMIRORUBIO 0:49465eeab179 199 // Fuzzy Inference System
RAMIRORUBIO 0:49465eeab179 200 //***********************************************************************
RAMIRORUBIO 0:49465eeab179 201 void fis_evaluate()
RAMIRORUBIO 0:49465eeab179 202 {
RAMIRORUBIO 0:49465eeab179 203 FIS_TYPE fuzzyInput0[] = { 0, 0, 0, 0, 0 };
RAMIRORUBIO 0:49465eeab179 204 FIS_TYPE* fuzzyInput[fis_gcI] = { fuzzyInput0, };
RAMIRORUBIO 0:49465eeab179 205 FIS_TYPE fuzzyOutput0[] = { 0, 0, 0, 0, 0 };
RAMIRORUBIO 0:49465eeab179 206 FIS_TYPE* fuzzyOutput[fis_gcO] = { fuzzyOutput0, };
RAMIRORUBIO 0:49465eeab179 207 FIS_TYPE fuzzyRules[fis_gcR] = { 0 };
RAMIRORUBIO 0:49465eeab179 208 FIS_TYPE fuzzyFires[fis_gcR] = { 0 };
RAMIRORUBIO 0:49465eeab179 209 FIS_TYPE* fuzzyRuleSet[] = { fuzzyRules, fuzzyFires };
RAMIRORUBIO 0:49465eeab179 210 FIS_TYPE sW = 0;
RAMIRORUBIO 0:49465eeab179 211
RAMIRORUBIO 0:49465eeab179 212 // Transforming input to fuzzy Input
RAMIRORUBIO 0:49465eeab179 213 int i, j, r, o;
RAMIRORUBIO 0:49465eeab179 214 for (i = 0; i < fis_gcI; ++i)
RAMIRORUBIO 0:49465eeab179 215 {
RAMIRORUBIO 0:49465eeab179 216 for (j = 0; j < fis_gIMFCount[i]; ++j)
RAMIRORUBIO 0:49465eeab179 217 {
RAMIRORUBIO 0:49465eeab179 218 fuzzyInput[i][j] =
RAMIRORUBIO 0:49465eeab179 219 (fis_gMF[fis_gMFI[i][j]])(g_fisInput[i], fis_gMFICoeff[i][j]);
RAMIRORUBIO 0:49465eeab179 220 }
RAMIRORUBIO 0:49465eeab179 221 }
RAMIRORUBIO 0:49465eeab179 222
RAMIRORUBIO 0:49465eeab179 223 int index = 0;
RAMIRORUBIO 0:49465eeab179 224 for (r = 0; r < fis_gcR; ++r)
RAMIRORUBIO 0:49465eeab179 225 {
RAMIRORUBIO 0:49465eeab179 226 if (fis_gRType[r] == 1)
RAMIRORUBIO 0:49465eeab179 227 {
RAMIRORUBIO 0:49465eeab179 228 fuzzyFires[r] = FIS_MAX;
RAMIRORUBIO 0:49465eeab179 229 for (i = 0; i < fis_gcI; ++i)
RAMIRORUBIO 0:49465eeab179 230 {
RAMIRORUBIO 0:49465eeab179 231 index = fis_gRI[r][i];
RAMIRORUBIO 0:49465eeab179 232 if (index > 0)
RAMIRORUBIO 0:49465eeab179 233 fuzzyFires[r] = fis_min(fuzzyFires[r], fuzzyInput[i][index - 1]);
RAMIRORUBIO 0:49465eeab179 234 else if (index < 0)
RAMIRORUBIO 0:49465eeab179 235 fuzzyFires[r] = fis_min(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
RAMIRORUBIO 0:49465eeab179 236 else
RAMIRORUBIO 0:49465eeab179 237 fuzzyFires[r] = fis_min(fuzzyFires[r], 1);
RAMIRORUBIO 0:49465eeab179 238 }
RAMIRORUBIO 0:49465eeab179 239 }
RAMIRORUBIO 0:49465eeab179 240 else
RAMIRORUBIO 0:49465eeab179 241 {
RAMIRORUBIO 0:49465eeab179 242 fuzzyFires[r] = FIS_MIN;
RAMIRORUBIO 0:49465eeab179 243 for (i = 0; i < fis_gcI; ++i)
RAMIRORUBIO 0:49465eeab179 244 {
RAMIRORUBIO 0:49465eeab179 245 index = fis_gRI[r][i];
RAMIRORUBIO 0:49465eeab179 246 if (index > 0)
RAMIRORUBIO 0:49465eeab179 247 fuzzyFires[r] = fis_max(fuzzyFires[r], fuzzyInput[i][index - 1]);
RAMIRORUBIO 0:49465eeab179 248 else if (index < 0)
RAMIRORUBIO 0:49465eeab179 249 fuzzyFires[r] = fis_max(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
RAMIRORUBIO 0:49465eeab179 250 else
RAMIRORUBIO 0:49465eeab179 251 fuzzyFires[r] = fis_max(fuzzyFires[r], 0);
RAMIRORUBIO 0:49465eeab179 252 }
RAMIRORUBIO 0:49465eeab179 253 }
RAMIRORUBIO 0:49465eeab179 254
RAMIRORUBIO 0:49465eeab179 255 fuzzyFires[r] = fis_gRWeight[r] * fuzzyFires[r];
RAMIRORUBIO 0:49465eeab179 256 sW += fuzzyFires[r];
RAMIRORUBIO 0:49465eeab179 257 }
RAMIRORUBIO 0:49465eeab179 258
RAMIRORUBIO 0:49465eeab179 259 if (sW == 0)
RAMIRORUBIO 0:49465eeab179 260 {
RAMIRORUBIO 0:49465eeab179 261 for (o = 0; o < fis_gcO; ++o)
RAMIRORUBIO 0:49465eeab179 262 {
RAMIRORUBIO 0:49465eeab179 263 g_fisOutput[o] = ((fis_gOMax[o] + fis_gOMin[o]) / 2);
RAMIRORUBIO 0:49465eeab179 264 }
RAMIRORUBIO 0:49465eeab179 265 }
RAMIRORUBIO 0:49465eeab179 266 else
RAMIRORUBIO 0:49465eeab179 267 {
RAMIRORUBIO 0:49465eeab179 268 for (o = 0; o < fis_gcO; ++o)
RAMIRORUBIO 0:49465eeab179 269 {
RAMIRORUBIO 0:49465eeab179 270 g_fisOutput[o] = fis_defuzz_centroid(fuzzyRuleSet, o);
RAMIRORUBIO 0:49465eeab179 271 }
RAMIRORUBIO 0:49465eeab179 272 }
RAMIRORUBIO 0:49465eeab179 273 }
RAMIRORUBIO 0:49465eeab179 274
RAMIRORUBIO 0:49465eeab179 275 int main(void)
RAMIRORUBIO 0:49465eeab179 276 {
RAMIRORUBIO 0:49465eeab179 277 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
RAMIRORUBIO 0:49465eeab179 278 PwmOut rled(LED1);
RAMIRORUBIO 0:49465eeab179 279 PwmOut gled(LED2);
RAMIRORUBIO 0:49465eeab179 280 PwmOut bled(LED3);
RAMIRORUBIO 0:49465eeab179 281 // initialize the Analog pins for input.
RAMIRORUBIO 0:49465eeab179 282 // Pin mode for Input: IMU
RAMIRORUBIO 0:49465eeab179 283 //pinMode(0 , INPUT);
RAMIRORUBIO 0:49465eeab179 284
RAMIRORUBIO 0:49465eeab179 285
RAMIRORUBIO 0:49465eeab179 286 // initialize the Analog pins for output.
RAMIRORUBIO 0:49465eeab179 287 // Pin mode for Output: Velocidad
RAMIRORUBIO 0:49465eeab179 288 //pinMode(1 , OUTPUT);
RAMIRORUBIO 0:49465eeab179 289 while (true) {
RAMIRORUBIO 0:49465eeab179 290 float x, y, z;
RAMIRORUBIO 0:49465eeab179 291 int16_t ry;
RAMIRORUBIO 0:49465eeab179 292 ry = (int16_t)(acc.getAccY()*90);
RAMIRORUBIO 0:49465eeab179 293 x = abs(acc.getAccX());
RAMIRORUBIO 0:49465eeab179 294 y = abs(acc.getAccY());
RAMIRORUBIO 0:49465eeab179 295 z = abs(acc.getAccZ());
RAMIRORUBIO 0:49465eeab179 296 rled = 1.0f - x;
RAMIRORUBIO 0:49465eeab179 297 gled = 1.0f - y;
RAMIRORUBIO 0:49465eeab179 298 bled = 1.0f - z;
RAMIRORUBIO 0:49465eeab179 299 wait(0.1f);
RAMIRORUBIO 0:49465eeab179 300 // Read Input: IMU
RAMIRORUBIO 0:49465eeab179 301 g_fisInput[0] = ry;
RAMIRORUBIO 0:49465eeab179 302 g_fisOutput[0] = 0;
RAMIRORUBIO 0:49465eeab179 303 fis_evaluate();
RAMIRORUBIO 0:49465eeab179 304 // Set output vlaue: Velocidad
RAMIRORUBIO 0:49465eeab179 305 printf("%f\r\n",g_fisOutput[0]);
RAMIRORUBIO 0:49465eeab179 306 }
RAMIRORUBIO 0:49465eeab179 307 }