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?
User | Revision | Line number | New 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 | } |