dimel contreras / Mbed 2 deprecated Algoritmo_Fuzzy

algoritmo logica difusa sensores navegacion

Dependencies:   GPS MODI2C SRF05 mbed HMC5883

main.cpp@0:1c15748ff0e1, 2014-07-19 (annotated)

Committer:
arturocontreras
Date:
Sat Jul 19 05:35:58 2014 +0000
Revision:
0:1c15748ff0e1
logica difusa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arturocontreras 0:1c15748ff0e1 1 #include "mbed.h" // Standard Library
arturocontreras 0:1c15748ff0e1 2 #include "HMC5883.h" // Comp (Compass)
arturocontreras 0:1c15748ff0e1 3 #include "GPS.h"
arturocontreras 0:1c15748ff0e1 4 #include "SRF05.h"
arturocontreras 0:1c15748ff0e1 5
arturocontreras 0:1c15748ff0e1 6 #define PI 3.1415926535897932384626433832795
arturocontreras 0:1c15748ff0e1 7 DigitalOut myled(LED1);
arturocontreras 0:1c15748ff0e1 8 I2C I2CBus(p9, p10);
arturocontreras 0:1c15748ff0e1 9 Timer GlobalTime;
arturocontreras 0:1c15748ff0e1 10
arturocontreras 0:1c15748ff0e1 11 SRF05 Ult1(p5, p6);
arturocontreras 0:1c15748ff0e1 12 SRF05 Ult2(p7, p8);
arturocontreras 0:1c15748ff0e1 13 SRF05 Ult3(p11, p12);
arturocontreras 0:1c15748ff0e1 14 SRF05 Ult4(p16, p15);
arturocontreras 0:1c15748ff0e1 15
arturocontreras 0:1c15748ff0e1 16 Serial pc(USBTX, USBRX);
arturocontreras 0:1c15748ff0e1 17 HMC5883 Mag(I2CBus, GlobalTime);
arturocontreras 0:1c15748ff0e1 18 GPS gps(p13, p14);
arturocontreras 0:1c15748ff0e1 19
arturocontreras 0:1c15748ff0e1 20 float longitud,latitud,ul1,ul2,ul3,ul4;
arturocontreras 0:1c15748ff0e1 21 int orientacion;
arturocontreras 0:1c15748ff0e1 22
arturocontreras 0:1c15748ff0e1 23
arturocontreras 0:1c15748ff0e1 24 void GPfuzzy(float P[],int n,float v,float GP[],int mf[]);
arturocontreras 0:1c15748ff0e1 25 float min(float a,float b);
arturocontreras 0:1c15748ff0e1 26 float Sugeno(int v1,int v2,float P[],int n);
arturocontreras 0:1c15748ff0e1 27 float Centroide(int v1,int v2,float P[],int n);
arturocontreras 0:1c15748ff0e1 28 void GPIfuzzy(float P[],int n,int gx,float GP,float x[]);
arturocontreras 0:1c15748ff0e1 29 void EntradaU(float F[],float O[],int ultrasonico,float tetha,float entrada[],float gp[],int mf[]);
arturocontreras 0:1c15748ff0e1 30 int reglas(int mf[],int mfrueda[],float gpin[],float gprueda[]);
arturocontreras 0:1c15748ff0e1 31 void desfuzzy(float Pm[],int n,int mfrueda[],float gprueda[],int size,float dutty[]);
arturocontreras 0:1c15748ff0e1 32
arturocontreras 0:1c15748ff0e1 33
arturocontreras 0:1c15748ff0e1 34 int HMC5883_getAngle(float x, float y)
arturocontreras 0:1c15748ff0e1 35 {
arturocontreras 0:1c15748ff0e1 36 float heading = atan2((float)y,(float)x);
arturocontreras 0:1c15748ff0e1 37 // Your mrad result / 1000.00 (to turn it into radians).
arturocontreras 0:1c15748ff0e1 38 float declinationAngle = 21.18 / 1000.0;
arturocontreras 0:1c15748ff0e1 39 // If you have an EAST declination, use += declinationAngle, if you have a WEST declination, use -= declinationAngle
arturocontreras 0:1c15748ff0e1 40 heading += declinationAngle;
arturocontreras 0:1c15748ff0e1 41 if(heading < 0) heading += 2*PI;// Correct for when signs are reversed.
arturocontreras 0:1c15748ff0e1 42 if(heading > 2*PI) heading -= 2*PI; // Check for wrap due to addition of declination.
arturocontreras 0:1c15748ff0e1 43 return(heading * 180/PI); // Convert radians to degrees for readability.
arturocontreras 0:1c15748ff0e1 44 }
arturocontreras 0:1c15748ff0e1 45
arturocontreras 0:1c15748ff0e1 46 int main() { // main programm for initialisation and debug output
arturocontreras 0:1c15748ff0e1 47 // pc.baud(9600);
arturocontreras 0:1c15748ff0e1 48
arturocontreras 0:1c15748ff0e1 49 I2CBus.frequency(400000);
arturocontreras 0:1c15748ff0e1 50 GlobalTime.start();
arturocontreras 0:1c15748ff0e1 51
arturocontreras 0:1c15748ff0e1 52 Mag.Init();
arturocontreras 0:1c15748ff0e1 53 #if 1
arturocontreras 0:1c15748ff0e1 54 Mag.AutoCalibration= 1; //In Echtzeit kalibrieren
arturocontreras 0:1c15748ff0e1 55 #else
arturocontreras 0:1c15748ff0e1 56 short MagRawMin[3]= {-400, -400, -400}; //Gespeicherte Werte
arturocontreras 0:1c15748ff0e1 57 short MagRawMax[3]= {400, 400, 400};
arturocontreras 0:1c15748ff0e1 58 Mag.Calibrate(MagRawMin, MagRawMax);
arturocontreras 0:1c15748ff0e1 59 #endif
arturocontreras 0:1c15748ff0e1 60
arturocontreras 0:1c15748ff0e1 61 int size,n=5,ultrasonico=3,mfs[4],mfrueda[4];
arturocontreras 0:1c15748ff0e1 62 float dutty[2],entrada[2],tetha,gradosp[4],gprueda[4];
arturocontreras 0:1c15748ff0e1 63 float Pm[5]={0,25,50,75,100};//RANGO DE DUTTY PARA MOTORES (p.e. de 0 a 100)
arturocontreras 0:1c15748ff0e1 64
arturocontreras 0:1c15748ff0e1 65 while(1) {
arturocontreras 0:1c15748ff0e1 66
arturocontreras 0:1c15748ff0e1 67 Mag.Update();
arturocontreras 0:1c15748ff0e1 68
arturocontreras 0:1c15748ff0e1 69 printf("grader = %i \n",HMC5883_getAngle(Mag.Mag[0],Mag.Mag[1]));
arturocontreras 0:1c15748ff0e1 70 wait_ms(20);
arturocontreras 0:1c15748ff0e1 71
arturocontreras 0:1c15748ff0e1 72
arturocontreras 0:1c15748ff0e1 73 if(gps.sample()) {
arturocontreras 0:1c15748ff0e1 74 myled = 1;//indica si el GPS esta enviando valores buenos
arturocontreras 0:1c15748ff0e1 75 latitud=gps.latitude;
arturocontreras 0:1c15748ff0e1 76 longitud=gps.longitude;
arturocontreras 0:1c15748ff0e1 77 }
arturocontreras 0:1c15748ff0e1 78
arturocontreras 0:1c15748ff0e1 79
arturocontreras 0:1c15748ff0e1 80 orientacion=HMC5883_getAngle(Mag.Mag[0],Mag.Mag[1]);
arturocontreras 0:1c15748ff0e1 81
arturocontreras 0:1c15748ff0e1 82 // beta=atan((F[0]-O[0])/(F[1]-O[1]))*57.29582790879777;
arturocontreras 0:1c15748ff0e1 83
arturocontreras 0:1c15748ff0e1 84
arturocontreras 0:1c15748ff0e1 85
arturocontreras 0:1c15748ff0e1 86 //CONFIGURACION DE ENTRADAS A LA LOGICA FUZZY********************************************
arturocontreras 0:1c15748ff0e1 87
arturocontreras 0:1c15748ff0e1 88 float O[]={0,0};//POSICION INICIAL SEGUN GPS, EN EL PLANO
arturocontreras 0:1c15748ff0e1 89 float F[]={3,4};//OBJETIVO O POSICION FINAL
arturocontreras 0:1c15748ff0e1 90 tetha=orientacion;//en sexagesimales, ang de la brujula medido desde el eje NORTE sentido horario(positivo)
arturocontreras 0:1c15748ff0e1 91 ultrasonico=1;
arturocontreras 0:1c15748ff0e1 92 //fin de CONFIG DE ENTRADAS******************************************************
arturocontreras 0:1c15748ff0e1 93
arturocontreras 0:1c15748ff0e1 94
arturocontreras 0:1c15748ff0e1 95 EntradaU(F,O,ultrasonico,tetha,entrada,gradosp,mfs);//hace la fuzzyfication
arturocontreras 0:1c15748ff0e1 96
arturocontreras 0:1c15748ff0e1 97 size=reglas(mfs,mfrueda,gradosp,gprueda);//esta funcion bota los vectores mfrueda y sus gprueda
arturocontreras 0:1c15748ff0e1 98
arturocontreras 0:1c15748ff0e1 99 desfuzzy(Pm,n,mfrueda,gprueda,size,dutty);
arturocontreras 0:1c15748ff0e1 100 printf("\fdutty1 %f \ndutty2 %f",dutty[0],dutty[1]);
arturocontreras 0:1c15748ff0e1 101 //wait_ms(100);
arturocontreras 0:1c15748ff0e1 102
arturocontreras 0:1c15748ff0e1 103
arturocontreras 0:1c15748ff0e1 104 }
arturocontreras 0:1c15748ff0e1 105 }
arturocontreras 0:1c15748ff0e1 106 //*****************DESARROLLO DE FUNCIONES**************************
arturocontreras 0:1c15748ff0e1 107 void GPfuzzy(float P[],int n,float v,float GP[],int mf[]){//v==> valor eje x
arturocontreras 0:1c15748ff0e1 108 int i;
arturocontreras 0:1c15748ff0e1 109 for( i=0;i<n;i++){//
arturocontreras 0:1c15748ff0e1 110 if(P[i]<=v && v<=P[i+1]){
arturocontreras 0:1c15748ff0e1 111 GP[0]=1.0-(v-P[i])*1.3333/(P[i+1]-P[i]);
arturocontreras 0:1c15748ff0e1 112 GP[1]=1.0+1.33333*(v-P[i+1])/(P[i+1]-P[i]);
arturocontreras 0:1c15748ff0e1 113 if(GP[1]<0){GP[1]=0;}
arturocontreras 0:1c15748ff0e1 114 if(GP[0]<0){GP[0]=0;}
arturocontreras 0:1c15748ff0e1 115 mf[0]=i+1;
arturocontreras 0:1c15748ff0e1 116 mf[1]=i+2;
arturocontreras 0:1c15748ff0e1 117 break;
arturocontreras 0:1c15748ff0e1 118 }
arturocontreras 0:1c15748ff0e1 119 }
arturocontreras 0:1c15748ff0e1 120 }
arturocontreras 0:1c15748ff0e1 121 float min(float a,float b){
arturocontreras 0:1c15748ff0e1 122 if(a<=b)return a;
arturocontreras 0:1c15748ff0e1 123 else return b;
arturocontreras 0:1c15748ff0e1 124 }
arturocontreras 0:1c15748ff0e1 125 float max(float a,float b){
arturocontreras 0:1c15748ff0e1 126 if(a>=b)return a;
arturocontreras 0:1c15748ff0e1 127 else return b;
arturocontreras 0:1c15748ff0e1 128 }
arturocontreras 0:1c15748ff0e1 129 float Sugeno(int v1,int v2,float P[],int n){//bota el centroide
arturocontreras 0:1c15748ff0e1 130 int i;
arturocontreras 0:1c15748ff0e1 131 float a,GP1[3],GP2[3],num=0,area=0,mf[3];
arturocontreras 0:1c15748ff0e1 132 for(i=1;i<=(v2-v1);i++){
arturocontreras 0:1c15748ff0e1 133 /* GPfuzzy(P,n,v1+i-1,GP1,mf);
arturocontreras 0:1c15748ff0e1 134 GPfuzzy(P,n,v1+i,GP2,mf);*/
arturocontreras 0:1c15748ff0e1 135 a=(max(GP1[0],GP1[1])+max(GP2[0],GP2[1]))/2;
arturocontreras 0:1c15748ff0e1 136 num=num+a*max(GP1[0],GP1[1])/2;
arturocontreras 0:1c15748ff0e1 137 area=area+a;
arturocontreras 0:1c15748ff0e1 138 }
arturocontreras 0:1c15748ff0e1 139
arturocontreras 0:1c15748ff0e1 140 return num/area;
arturocontreras 0:1c15748ff0e1 141 }
arturocontreras 0:1c15748ff0e1 142
arturocontreras 0:1c15748ff0e1 143 float Centroide(int v1,int v2,float P[],int n){//bota el centroide
arturocontreras 0:1c15748ff0e1 144 int i,L=50;
arturocontreras 0:1c15748ff0e1 145 float a,GP1[3],GP2[3],num=0,area=0;
arturocontreras 0:1c15748ff0e1 146 for(i=1;i<=L;i++){
arturocontreras 0:1c15748ff0e1 147 //GPfuzzy(P,n,v1+i-1,GP1,mf);
arturocontreras 0:1c15748ff0e1 148 //GPfuzzy(P,n,v1+i,GP2,mf);
arturocontreras 0:1c15748ff0e1 149 a=(max(GP1[0],GP1[1])+max(GP2[0],GP2[1]))/2;
arturocontreras 0:1c15748ff0e1 150 num=num+a*max(GP1[0],GP1[1])/2;
arturocontreras 0:1c15748ff0e1 151 area=area+a;
arturocontreras 0:1c15748ff0e1 152 }
arturocontreras 0:1c15748ff0e1 153 return num/area;
arturocontreras 0:1c15748ff0e1 154 }
arturocontreras 0:1c15748ff0e1 155
arturocontreras 0:1c15748ff0e1 156
arturocontreras 0:1c15748ff0e1 157 void GPIfuzzy(float P[],int n,int gx,float GP,float x[]){//1<=gx<=n ,, 0<=GP<=1
arturocontreras 0:1c15748ff0e1 158
arturocontreras 0:1c15748ff0e1 159 if(gx==1){
arturocontreras 0:1c15748ff0e1 160 x[0]=P[0];
arturocontreras 0:1c15748ff0e1 161 x[1]=(1-GP)*(P[1]-P[0])/1.33333+P[0];
arturocontreras 0:1c15748ff0e1 162 }
arturocontreras 0:1c15748ff0e1 163 else{
arturocontreras 0:1c15748ff0e1 164 if(gx<n){
arturocontreras 0:1c15748ff0e1 165 x[0]=(GP-1)*(P[gx-1]-P[gx-2])/1.333333+P[gx-1];
arturocontreras 0:1c15748ff0e1 166 x[1]=(1-GP)*(P[gx]-P[gx-1])/1.333333+P[gx-1];
arturocontreras 0:1c15748ff0e1 167 }
arturocontreras 0:1c15748ff0e1 168 if(gx==n){
arturocontreras 0:1c15748ff0e1 169 x[0]=(GP-1)*(P[gx-1]-P[gx-2])/1.333333+P[gx-1];
arturocontreras 0:1c15748ff0e1 170 x[1]=P[n-1];
arturocontreras 0:1c15748ff0e1 171 }
arturocontreras 0:1c15748ff0e1 172
arturocontreras 0:1c15748ff0e1 173 }
arturocontreras 0:1c15748ff0e1 174 }
arturocontreras 0:1c15748ff0e1 175
arturocontreras 0:1c15748ff0e1 176
arturocontreras 0:1c15748ff0e1 177 void EntradaU(float F[],float O[],int ultrasonico,float tetha,float entrada[],float gp[],int mf[]){
arturocontreras 0:1c15748ff0e1 178 // ex_izq muy_izqu izqui frente dere muy_der ex_dere
arturocontreras 0:1c15748ff0e1 179 //salidau 1 2 3 4 5 6 7
arturocontreras 0:1c15748ff0e1 180 // entrada[0]==> entrada fuzzy phi brujula
arturocontreras 0:1c15748ff0e1 181 // entrada[1] ==> entrada fuzzy ultrasonico
arturocontreras 0:1c15748ff0e1 182 //para el phi brujula
arturocontreras 0:1c15748ff0e1 183 float gradop[2] = {0.0,0.0};
arturocontreras 0:1c15748ff0e1 184 int mfi[2]={0,0};
arturocontreras 0:1c15748ff0e1 185 float P[5]={-180,-90,0,90,180};//es decir entre -180 y 180
arturocontreras 0:1c15748ff0e1 186 float beta;
arturocontreras 0:1c15748ff0e1 187 beta=atan((F[0]-O[0])/(F[1]-O[1]))*57.29582790879777;
arturocontreras 0:1c15748ff0e1 188 entrada[0]=beta-tetha;//degrees phi
arturocontreras 0:1c15748ff0e1 189 switch(ultrasonico){
arturocontreras 0:1c15748ff0e1 190 case 0:{
arturocontreras 0:1c15748ff0e1 191 if(entrada[0]>0)entrada[1]=7;//entrada[1] de ultrasonico
arturocontreras 0:1c15748ff0e1 192 if(entrada[0]<0)entrada[1]=1;
arturocontreras 0:1c15748ff0e1 193 break;}
arturocontreras 0:1c15748ff0e1 194 case 2:{
arturocontreras 0:1c15748ff0e1 195 if(entrada[0]>0)entrada[1]=7;
arturocontreras 0:1c15748ff0e1 196 if(entrada[0]<0)entrada[1]=1;
arturocontreras 0:1c15748ff0e1 197 break;}
arturocontreras 0:1c15748ff0e1 198 case 5:{
arturocontreras 0:1c15748ff0e1 199 if(entrada[0]>0)entrada[1]=6;
arturocontreras 0:1c15748ff0e1 200 if(entrada[0]<0)entrada[1]=2;
arturocontreras 0:1c15748ff0e1 201 break;}
arturocontreras 0:1c15748ff0e1 202 case 1:{entrada[1]=6;
arturocontreras 0:1c15748ff0e1 203 break;}
arturocontreras 0:1c15748ff0e1 204 case 3:{entrada[1]=5;
arturocontreras 0:1c15748ff0e1 205 break;}
arturocontreras 0:1c15748ff0e1 206 case 7:{if(entrada[0]>0)entrada[1]=6;//entrada[1] de ultrasonico
arturocontreras 0:1c15748ff0e1 207 if(entrada[0]<0)entrada[1]=2;//*cambie orden 7 con 1
arturocontreras 0:1c15748ff0e1 208 break;}
arturocontreras 0:1c15748ff0e1 209 case 6:{entrada[1]=3;
arturocontreras 0:1c15748ff0e1 210 break;}
arturocontreras 0:1c15748ff0e1 211 case 4:{entrada[1]=2;
arturocontreras 0:1c15748ff0e1 212 break;}
arturocontreras 0:1c15748ff0e1 213 }
arturocontreras 0:1c15748ff0e1 214 /*//para el phi brujula
arturocontreras 0:1c15748ff0e1 215 float gradop[2] = {0.0,0.0};
arturocontreras 0:1c15748ff0e1 216 int mfi[2]={0,0};
arturocontreras 0:1c15748ff0e1 217 float P[5]={-144,-72,0,72,144};//es decir entre -180 y 180*/
arturocontreras 0:1c15748ff0e1 218 //printf("\nentrada[0]%f\n",entrada[0]);
arturocontreras 0:1c15748ff0e1 219 GPfuzzy(P,5,entrada[0],gradop,mfi);
arturocontreras 0:1c15748ff0e1 220 gp[0]=gradop[0]; mf[0]=mfi[0];
arturocontreras 0:1c15748ff0e1 221 gp[1]=gradop[1]; mf[1]=mfi[1];
arturocontreras 0:1c15748ff0e1 222
arturocontreras 0:1c15748ff0e1 223 //para el ultrasonico
arturocontreras 0:1c15748ff0e1 224 //GPfuzzy(P,5,entrada[1],gradop,mf);
arturocontreras 0:1c15748ff0e1 225 gp[2]=1; mf[2]=entrada[1];
arturocontreras 0:1c15748ff0e1 226 gp[3]=0.5; mf[3]=entrada[1]+1;
arturocontreras 0:1c15748ff0e1 227 }
arturocontreras 0:1c15748ff0e1 228
arturocontreras 0:1c15748ff0e1 229
arturocontreras 0:1c15748ff0e1 230 //reglas fuzzy
arturocontreras 0:1c15748ff0e1 231 int reglas(int mf[],int mfrueda[],float gpin[],float gprueda[]){
arturocontreras 0:1c15748ff0e1 232 //mf[0]==>mf phi brujula
arturocontreras 0:1c15748ff0e1 233 //mf[1]==>mf sgte brujula
arturocontreras 0:1c15748ff0e1 234 //mf[2] mf ultrasonico
arturocontreras 0:1c15748ff0e1 235 //mf[3] mf sgte ultra
arturocontreras 0:1c15748ff0e1 236 //gpin[0] brujula primer grado de pertenencia
arturocontreras 0:1c15748ff0e1 237 //gpin[1] brujula segundo grado de pertencia
arturocontreras 0:1c15748ff0e1 238 //gpin[2] ultrasonico pro=imer gp
arturocontreras 0:1c15748ff0e1 239 //gpin[3] ultrasonico segundo gp
arturocontreras 0:1c15748ff0e1 240 //rueda[0]==>rueda1
arturocontreras 0:1c15748ff0e1 241 //rueda[1]==>rueda2
arturocontreras 0:1c15748ff0e1 242
arturocontreras 0:1c15748ff0e1 243 //mfgps[0]==>latitud
arturocontreras 0:1c15748ff0e1 244 //mfgps[1]==>longtud
arturocontreras 0:1c15748ff0e1 245 //k==> ultimo elem: gprueda[k+1]
arturocontreras 0:1c15748ff0e1 246 int i,j;
arturocontreras 0:1c15748ff0e1 247 int k=0;
arturocontreras 0:1c15748ff0e1 248 //printf("\ngpin[]phi %f %f\n",gpin[0],gpin[1]);
arturocontreras 0:1c15748ff0e1 249 //printf("\ngpin[]ultrasonico %f %f\n",gpin[2],gpin[3]);
arturocontreras 0:1c15748ff0e1 250 //printf("\nmf[] %d %d %d %d\n",mf[0],mf[1],mf[2],mf[3]);
arturocontreras 0:1c15748ff0e1 251 //mfrueda[6]=7;
arturocontreras 0:1c15748ff0e1 252 //mfrueda[5]=7;
arturocontreras 0:1c15748ff0e1 253
arturocontreras 0:1c15748ff0e1 254 //falta ve lo de la interrupcion del gps XD
arturocontreras 0:1c15748ff0e1 255 //if((mfgps[0]==1)&&(mfgps[1]==1)){gprueda[0]=gpgps(0);gprueda[1]=gpgps[1];}//a parte del phi y el ultrasonico
arturocontreras 0:1c15748ff0e1 256 if(mf[0]==3||mf[1]==3){i=1;if(mf[0]==3)i=0;
arturocontreras 0:1c15748ff0e1 257 if(mf[2]==1||mf[3]==1){j=2;if(mf[3]==1)j=3;mfrueda[k]=1;mfrueda[k+1]=4;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 258 if(mf[2]==2||mf[3]==2){j=2;if(mf[3]==2)j=3;mfrueda[k]=1;mfrueda[k+1]=4;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 259 if(mf[2]==3||mf[3]==3){j=2;if(mf[3]==3)j=3;mfrueda[k]=2;mfrueda[k+1]=3;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 260 if(mf[2]==7||mf[3]==7){j=2;if(mf[3]==7)j=3;mfrueda[k]=3;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 261 if(mf[2]==6||mf[3]==6){j=2;if(mf[3]==6)j=3;mfrueda[k]=2;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 262 if(mf[2]==5||mf[3]==5){j=2;if(mf[3]==5)j=3;mfrueda[k]=3;mfrueda[k+1]=2;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 263 if(mf[2]==4||mf[3]==4){j=2;if(mf[3]==4)j=3;mfrueda[k]=3;mfrueda[k+1]=3;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 264 }
arturocontreras 0:1c15748ff0e1 265 //printf("\n1gprueda[]phi %f %f\n",gpin[0],gpin[1]);
arturocontreras 0:1c15748ff0e1 266
arturocontreras 0:1c15748ff0e1 267 if(mf[0]==4||mf[1]==4){i=1;if(mf[0]==4)i=0;//si brujula es POS
arturocontreras 0:1c15748ff0e1 268 if(mf[2]==1||mf[3]==1){j=2;if(mf[3]==1)j=3;mfrueda[k]=1;mfrueda[k+1]=4;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 269 if(mf[2]==3||mf[3]==3){j=2;if(mf[3]==3)j=3;mfrueda[k]=1;mfrueda[k+1]=3;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 270 if(mf[2]==3||mf[3]==3){j=2;if(mf[3]==3)j=3;mfrueda[k]=4;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 271 if(mf[2]==5||mf[3]==5){j=2;if(mf[3]==5)j=3;mfrueda[k]=3;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 272 if(mf[2]==4||mf[3]==4){j=2;if(mf[3]==4)j=3;mfrueda[k]=1;mfrueda[k+1]=3;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 273 }
arturocontreras 0:1c15748ff0e1 274
arturocontreras 0:1c15748ff0e1 275 if(mf[0]==5||mf[1]==5){i=1;if(mf[0]==5)i=0;//si brujula es APOS
arturocontreras 0:1c15748ff0e1 276 if(mf[2]==3||mf[3]==3){j=2;if(mf[3]==3)j=3;mfrueda[k]=1;mfrueda[k+1]=5;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 277 if(mf[2]==5||mf[3]==5){j=2;if(mf[3]==5)j=3;mfrueda[k]=5;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 278 if(mf[2]==4||mf[3]==4){j=2;if(mf[3]==4)j=3;mfrueda[k]=1;mfrueda[k+1]=5;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 279 }
arturocontreras 0:1c15748ff0e1 280 if(mf[0]==2||mf[1]==2){i=1;if(mf[0]==2)i=0;//si brujula es NEG
arturocontreras 0:1c15748ff0e1 281 if(mf[2]==4||mf[3]==4){j=2;if(mf[3]==4)j=3;mfrueda[k]=1;mfrueda[k+1]=5;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 282 }
arturocontreras 0:1c15748ff0e1 283 if(mf[0]==1||mf[1]==1){i=1;if(mf[0]==1)i=0;//si brujula es ANEG
arturocontreras 0:1c15748ff0e1 284 if(mf[2]==4||mf[3]==4){j=2;if(mf[3]==4)j=3;mfrueda[k]=5;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
arturocontreras 0:1c15748ff0e1 285 }//llega hasta gprueda[k+1]
arturocontreras 0:1c15748ff0e1 286 return k;//size de mf y gprueda[]
arturocontreras 0:1c15748ff0e1 287 }
arturocontreras 0:1c15748ff0e1 288 //desfuzzyfication
arturocontreras 0:1c15748ff0e1 289 void desfuzzy(float Pm[],int n,int mfrueda[],float gprueda[],int size,float dutty[]){
arturocontreras 0:1c15748ff0e1 290 //rueda1*********************************
arturocontreras 0:1c15748ff0e1 291 float x[3],ar=0,num=0,x1,area=0,area1,x2,area2;
arturocontreras 0:1c15748ff0e1 292 int a,b,i,mfmin,mfmax=1;
arturocontreras 0:1c15748ff0e1 293 mfmin=n;
arturocontreras 0:1c15748ff0e1 294 //el sgte for fue modificado para casos de mfmax y mfmin q se repiten despues de las reglas
arturocontreras 0:1c15748ff0e1 295 for(i=0;i<size;i=i+2){//ojo desde i=0
arturocontreras 0:1c15748ff0e1 296 if(mfrueda[i]<=mfmin){if(mfrueda[i]<mfmin){mfmin=mfrueda[i];a=i;}
arturocontreras 0:1c15748ff0e1 297 if(mfrueda[i]=mfmin)b=i;}
arturocontreras 0:1c15748ff0e1 298 if(mfrueda[i]>mfmax){mfmax=mfrueda[i];b=i;}
arturocontreras 0:1c15748ff0e1 299 }
arturocontreras 0:1c15748ff0e1 300
arturocontreras 0:1c15748ff0e1 301
arturocontreras 0:1c15748ff0e1 302 //hallar %dutty de rueda1
arturocontreras 0:1c15748ff0e1 303 //centroide de area1
arturocontreras 0:1c15748ff0e1 304 GPIfuzzy(Pm,n,mfmin,gprueda[a],x);
arturocontreras 0:1c15748ff0e1 305
arturocontreras 0:1c15748ff0e1 306 for(i=0;i<gprueda[a]*100;i++){//*ojo con gprueda[a]*100
arturocontreras 0:1c15748ff0e1 307 ar=0.01*(Pm[mfmin-1]-(i*0.01-1)/1.3333*(Pm[mfmin]-Pm[mfmin-1])-x[0]);
arturocontreras 0:1c15748ff0e1 308 area=area+ar;
arturocontreras 0:1c15748ff0e1 309 num=num+(ar*100/2+x[0])*ar;//ahhhhhhhhhhhhhhhhhhh
arturocontreras 0:1c15748ff0e1 310 }
arturocontreras 0:1c15748ff0e1 311 if(area!=0)x1=num/area;
arturocontreras 0:1c15748ff0e1 312 else x1=0;
arturocontreras 0:1c15748ff0e1 313 area1=area;
arturocontreras 0:1c15748ff0e1 314
arturocontreras 0:1c15748ff0e1 315 //centroide de area2
arturocontreras 0:1c15748ff0e1 316 ar=0,num=0,area=0;
arturocontreras 0:1c15748ff0e1 317 GPIfuzzy(Pm,n,mfmax,gprueda[b],x);
arturocontreras 0:1c15748ff0e1 318
arturocontreras 0:1c15748ff0e1 319 for(i=0;i<gprueda[b]*100;i++){
arturocontreras 0:1c15748ff0e1 320 ar=0.01*(x[1]-(Pm[mfmax-1]+(i*0.01-1)/1.333333*(Pm[mfmax-1]-Pm[mfmax-2])));
arturocontreras 0:1c15748ff0e1 321 area=area+ar;
arturocontreras 0:1c15748ff0e1 322 num=num+(100*ar/2+ (x[1]-ar*100) )*ar;
arturocontreras 0:1c15748ff0e1 323 }
arturocontreras 0:1c15748ff0e1 324 if(area!=0)x2=num/area;else x2=0;
arturocontreras 0:1c15748ff0e1 325 area2=area;
arturocontreras 0:1c15748ff0e1 326
arturocontreras 0:1c15748ff0e1 327 dutty[0]=(x1*area1+x2*area2)/(area1+area2);
arturocontreras 0:1c15748ff0e1 328
arturocontreras 0:1c15748ff0e1 329 //rueda2************************************************
arturocontreras 0:1c15748ff0e1 330
arturocontreras 0:1c15748ff0e1 331
arturocontreras 0:1c15748ff0e1 332 mfmin=n,mfmax=1;ar=0,area=0,num=0;
arturocontreras 0:1c15748ff0e1 333 //el sgte for fue modificado para casos de mfmax y mfmin q se repiten despues de las reglas
arturocontreras 0:1c15748ff0e1 334 for(i=1;i<size;i=i+2){//ojo desde i=1
arturocontreras 0:1c15748ff0e1 335 if(mfrueda[i]<=mfmin){if(mfrueda[i]<mfmin){mfmin=mfrueda[i];a=i;}
arturocontreras 0:1c15748ff0e1 336 if(mfrueda[i]=mfmin)b=i;}
arturocontreras 0:1c15748ff0e1 337 if(mfrueda[i]>mfmax){mfmax=mfrueda[i];b=i;}
arturocontreras 0:1c15748ff0e1 338 }
arturocontreras 0:1c15748ff0e1 339
arturocontreras 0:1c15748ff0e1 340 //hallar %dutty de rueda2
arturocontreras 0:1c15748ff0e1 341 //centroide de area1
arturocontreras 0:1c15748ff0e1 342 GPIfuzzy(Pm,n,mfmin,gprueda[a],x);
arturocontreras 0:1c15748ff0e1 343 for(i=0;i<gprueda[a]*100;i++){//*ojo con gprueda[a]*100
arturocontreras 0:1c15748ff0e1 344 ar=0.01*(Pm[mfmin-1]-(i*0.01-1)/1.3333*(Pm[mfmin]-Pm[mfmin-1])-x[0]);
arturocontreras 0:1c15748ff0e1 345 area=area+ar;
arturocontreras 0:1c15748ff0e1 346 num=num+(ar*100/2+x[0])*ar;//ahhhhhhhhhhhhhhhhhhh
arturocontreras 0:1c15748ff0e1 347 }
arturocontreras 0:1c15748ff0e1 348 if(area!=0) x1=num/area;
arturocontreras 0:1c15748ff0e1 349 else x1=0;
arturocontreras 0:1c15748ff0e1 350 area1=area;
arturocontreras 0:1c15748ff0e1 351 //centroide de area2
arturocontreras 0:1c15748ff0e1 352 ar=0,num=0,area=0;
arturocontreras 0:1c15748ff0e1 353 GPIfuzzy(Pm,n,mfmax,gprueda[b],x);
arturocontreras 0:1c15748ff0e1 354 for(i=0;i<gprueda[b]*100;i++){
arturocontreras 0:1c15748ff0e1 355 ar=0.01*(x[1]-(Pm[mfmax-1]+(i*0.01-1)/1.333333*(Pm[mfmax-1]-Pm[mfmax-2])));
arturocontreras 0:1c15748ff0e1 356 area=area+ar;
arturocontreras 0:1c15748ff0e1 357 num=num+(100*ar/2+ (x[1]-ar*100) )*ar;//ahhhhhhhhhh
arturocontreras 0:1c15748ff0e1 358 }
arturocontreras 0:1c15748ff0e1 359 if(area!=0)x2=num/area;
arturocontreras 0:1c15748ff0e1 360 else x2=0;
arturocontreras 0:1c15748ff0e1 361 area2=area;
arturocontreras 0:1c15748ff0e1 362
arturocontreras 0:1c15748ff0e1 363 dutty[1]=(x1*area1+x2*area2)/(area1+area2);
arturocontreras 0:1c15748ff0e1 364 }