main.cpp

00001 #include "mbed.h"       // Standard Library
00002 #include "HMC5883.h"    // Comp (Compass)
00003 #include "GPS.h"
00004 #include "SRF05.h"
00005 
00006 #define PI             3.1415926535897932384626433832795
00007 DigitalOut myled(LED1);
00008 I2C I2CBus(p9, p10);
00009 Timer GlobalTime;
00010 
00011 SRF05 Ult1(p5, p6);
00012 SRF05 Ult2(p7, p8);
00013 SRF05 Ult3(p11, p12);
00014 SRF05 Ult4(p16, p15);
00015 
00016 Serial pc(USBTX, USBRX);
00017 HMC5883     Mag(I2CBus, GlobalTime);
00018 GPS gps(p13, p14);
00019 
00020 float longitud,latitud,ul1,ul2,ul3,ul4;
00021 int orientacion;
00022 
00023 
00024 void GPfuzzy(float P[],int n,float v,float GP[],int mf[]);         
00025 float min(float a,float b);
00026 float Sugeno(int v1,int v2,float P[],int n);
00027 float Centroide(int v1,int v2,float P[],int n);
00028 void GPIfuzzy(float P[],int n,int gx,float GP,float x[]);
00029 void EntradaU(float F[],float O[],int ultrasonico,float tetha,float entrada[],float gp[],int mf[]);
00030 int  reglas(int mf[],int mfrueda[],float gpin[],float gprueda[]);
00031 void desfuzzy(float Pm[],int n,int mfrueda[],float gprueda[],int size,float dutty[]);
00032 
00033 
00034 int HMC5883_getAngle(float x, float y)
00035 {
00036 float heading = atan2((float)y,(float)x); 
00037 // Your mrad result / 1000.00 (to turn it into radians).
00038   float declinationAngle = 21.18  / 1000.0;
00039   // If you have an EAST declination, use += declinationAngle, if you have a WEST declination, use -= declinationAngle
00040   heading += declinationAngle;  
00041 if(heading < 0)  heading += 2*PI;// Correct for when signs are reversed.
00042 if(heading > 2*PI)   heading -= 2*PI; // Check for wrap due to addition of declination.
00043 return(heading * 180/PI); // Convert radians to degrees for readability.
00044 }
00045 
00046 int main() { // main programm for initialisation and debug output
00047       // pc.baud(9600);
00048   
00049     I2CBus.frequency(400000);
00050     GlobalTime.start();
00051 
00052     Mag.Init();
00053     #if 1
00054         Mag.AutoCalibration= 1;                     //In Echtzeit kalibrieren
00055     #else
00056         short MagRawMin[3]= {-400, -400, -400};     //Gespeicherte Werte
00057         short MagRawMax[3]= {400, 400, 400};
00058         Mag.Calibrate(MagRawMin, MagRawMax);
00059     #endif
00060     
00061    int size,n=5,ultrasonico=3,mfs[4],mfrueda[4];
00062    float dutty[2],entrada[2],tetha,gradosp[4],gprueda[4];
00063    float Pm[5]={0,25,50,75,100};//RANGO DE DUTTY PARA MOTORES (p.e. de 0 a 100)
00064     
00065     while(1) { 
00066     
00067     Mag.Update();
00068        
00069     printf("grader = %i \n",HMC5883_getAngle(Mag.Mag[0],Mag.Mag[1])); 
00070     wait_ms(20);
00071     
00072  
00073     if(gps.sample()) {
00074     myled = 1;//indica si el GPS esta enviando valores buenos
00075     latitud=gps.latitude;
00076     longitud=gps.longitude;
00077     } 
00078     
00079 
00080     orientacion=HMC5883_getAngle(Mag.Mag[0],Mag.Mag[1]);
00081     
00082   //  beta=atan((F[0]-O[0])/(F[1]-O[1]))*57.29582790879777;
00083     
00084 
00085    
00086 //CONFIGURACION DE ENTRADAS A LA LOGICA FUZZY********************************************
00087    
00088    float O[]={0,0};//POSICION INICIAL SEGUN GPS, EN EL PLANO
00089    float F[]={3,4};//OBJETIVO O POSICION FINAL
00090    tetha=orientacion;//en sexagesimales, ang de la brujula medido desde el eje NORTE sentido horario(positivo)
00091    ultrasonico=1;
00092 //fin de CONFIG DE ENTRADAS******************************************************
00093 
00094  
00095     EntradaU(F,O,ultrasonico,tetha,entrada,gradosp,mfs);//hace la fuzzyfication
00096 
00097    size=reglas(mfs,mfrueda,gradosp,gprueda);//esta funcion bota los vectores mfrueda y sus gprueda
00098 
00099     desfuzzy(Pm,n,mfrueda,gprueda,size,dutty);
00100     printf("\fdutty1 %f \ndutty2 %f",dutty[0],dutty[1]);
00101     //wait_ms(100);
00102     
00103     
00104     }
00105 }
00106 //*****************DESARROLLO DE FUNCIONES**************************
00107 void GPfuzzy(float P[],int n,float v,float GP[],int mf[]){//v==> valor eje x
00108     int i;
00109     for( i=0;i<n;i++){//
00110         if(P[i]<=v && v<=P[i+1]){
00111             GP[0]=1.0-(v-P[i])*1.3333/(P[i+1]-P[i]);
00112             GP[1]=1.0+1.33333*(v-P[i+1])/(P[i+1]-P[i]);
00113             if(GP[1]<0){GP[1]=0;}
00114             if(GP[0]<0){GP[0]=0;}
00115             mf[0]=i+1;
00116             mf[1]=i+2;
00117             break;
00118         }
00119     }
00120 }
00121 float min(float a,float b){
00122     if(a<=b)return a;
00123     else return b;
00124 }
00125 float max(float a,float b){
00126     if(a>=b)return a;
00127     else return b;
00128 }
00129 float Sugeno(int v1,int v2,float P[],int n){//bota el centroide
00130     int i;
00131     float a,GP1[3],GP2[3],num=0,area=0,mf[3];
00132     for(i=1;i<=(v2-v1);i++){
00133       /*  GPfuzzy(P,n,v1+i-1,GP1,mf);
00134         GPfuzzy(P,n,v1+i,GP2,mf);*/
00135         a=(max(GP1[0],GP1[1])+max(GP2[0],GP2[1]))/2;
00136         num=num+a*max(GP1[0],GP1[1])/2;
00137         area=area+a;
00138     }
00139 
00140     return num/area;
00141 }
00142 
00143 float Centroide(int v1,int v2,float P[],int n){//bota el centroide
00144     int i,L=50;
00145     float a,GP1[3],GP2[3],num=0,area=0;
00146     for(i=1;i<=L;i++){
00147         //GPfuzzy(P,n,v1+i-1,GP1,mf);
00148         //GPfuzzy(P,n,v1+i,GP2,mf);
00149         a=(max(GP1[0],GP1[1])+max(GP2[0],GP2[1]))/2;
00150         num=num+a*max(GP1[0],GP1[1])/2;
00151         area=area+a;
00152     }
00153     return num/area;
00154 }
00155 
00156 
00157 void GPIfuzzy(float P[],int n,int gx,float GP,float x[]){//1<=gx<=n ,, 0<=GP<=1
00158     
00159     if(gx==1){
00160         x[0]=P[0];
00161         x[1]=(1-GP)*(P[1]-P[0])/1.33333+P[0];
00162     }
00163     else{
00164         if(gx<n){
00165         x[0]=(GP-1)*(P[gx-1]-P[gx-2])/1.333333+P[gx-1];
00166         x[1]=(1-GP)*(P[gx]-P[gx-1])/1.333333+P[gx-1];
00167         }
00168         if(gx==n){
00169         x[0]=(GP-1)*(P[gx-1]-P[gx-2])/1.333333+P[gx-1];
00170         x[1]=P[n-1];
00171         }
00172 
00173     }
00174 }
00175 
00176 
00177 void EntradaU(float F[],float O[],int ultrasonico,float tetha,float entrada[],float gp[],int mf[]){
00178     //        ex_izq  muy_izqu  izqui  frente  dere  muy_der  ex_dere
00179     //salidau   1        2        3      4      5      6         7
00180     // entrada[0]==> entrada  fuzzy phi brujula
00181     // entrada[1] ==> entrada fuzzy ultrasonico
00182     //para el phi brujula
00183     float gradop[2] = {0.0,0.0};
00184     int mfi[2]={0,0};
00185     float P[5]={-180,-90,0,90,180};//es decir entre -180 y 180
00186    float beta;
00187    beta=atan((F[0]-O[0])/(F[1]-O[1]))*57.29582790879777;
00188     entrada[0]=beta-tetha;//degrees  phi
00189     switch(ultrasonico){
00190         case 0:{
00191         if(entrada[0]>0)entrada[1]=7;//entrada[1] de ultrasonico
00192         if(entrada[0]<0)entrada[1]=1;
00193         break;}
00194         case 2:{
00195         if(entrada[0]>0)entrada[1]=7;
00196         if(entrada[0]<0)entrada[1]=1;
00197         break;}
00198         case 5:{
00199         if(entrada[0]>0)entrada[1]=6;
00200         if(entrada[0]<0)entrada[1]=2;
00201         break;}
00202         case 1:{entrada[1]=6;
00203         break;}
00204         case 3:{entrada[1]=5;
00205         break;}
00206         case 7:{if(entrada[0]>0)entrada[1]=6;//entrada[1] de ultrasonico
00207             if(entrada[0]<0)entrada[1]=2;//*cambie orden 7 con  1
00208             break;}
00209         case 6:{entrada[1]=3;
00210         break;}
00211         case 4:{entrada[1]=2;
00212         break;}
00213     }
00214     /*//para el phi brujula
00215     float gradop[2] = {0.0,0.0};
00216     int mfi[2]={0,0};
00217     float P[5]={-144,-72,0,72,144};//es decir entre -180 y 180*/
00218     //printf("\nentrada[0]%f\n",entrada[0]);
00219     GPfuzzy(P,5,entrada[0],gradop,mfi);
00220     gp[0]=gradop[0];  mf[0]=mfi[0];
00221     gp[1]=gradop[1];  mf[1]=mfi[1];
00222 
00223     //para el ultrasonico
00224     //GPfuzzy(P,5,entrada[1],gradop,mf);
00225     gp[2]=1;  mf[2]=entrada[1];
00226     gp[3]=0.5;  mf[3]=entrada[1]+1;
00227 }
00228 
00229 
00230 //reglas fuzzy
00231 int  reglas(int mf[],int mfrueda[],float gpin[],float gprueda[]){
00232     //mf[0]==>mf      phi brujula
00233     //mf[1]==>mf sgte brujula
00234     //mf[2] mf ultrasonico
00235     //mf[3]  mf sgte ultra
00236     //gpin[0]  brujula primer grado de pertenencia
00237     //gpin[1]  brujula segundo grado de pertencia
00238     //gpin[2]  ultrasonico pro=imer gp
00239     //gpin[3]  ultrasonico segundo gp
00240     //rueda[0]==>rueda1
00241     //rueda[1]==>rueda2
00242 
00243     //mfgps[0]==>latitud
00244     //mfgps[1]==>longtud
00245     //k==> ultimo elem: gprueda[k+1]
00246     int i,j;
00247     int k=0;
00248     //printf("\ngpin[]phi %f %f\n",gpin[0],gpin[1]);
00249     //printf("\ngpin[]ultrasonico %f %f\n",gpin[2],gpin[3]);
00250     //printf("\nmf[] %d %d %d %d\n",mf[0],mf[1],mf[2],mf[3]);
00251     //mfrueda[6]=7;
00252     //mfrueda[5]=7;
00253 
00254 //falta ve lo de la interrupcion del gps XD
00255     //if((mfgps[0]==1)&&(mfgps[1]==1)){gprueda[0]=gpgps(0);gprueda[1]=gpgps[1];}//a parte del phi y el ultrasonico
00256     if(mf[0]==3||mf[1]==3){i=1;if(mf[0]==3)i=0;
00257         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;}
00258         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;}
00259         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;}
00260         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;}
00261         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;}
00262         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;}
00263         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;}
00264     }
00265 //printf("\n1gprueda[]phi %f %f\n",gpin[0],gpin[1]);
00266 
00267     if(mf[0]==4||mf[1]==4){i=1;if(mf[0]==4)i=0;//si brujula es POS
00268         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;}
00269         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;}
00270         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;}
00271         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;}
00272         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;}
00273     }
00274 
00275     if(mf[0]==5||mf[1]==5){i=1;if(mf[0]==5)i=0;//si brujula es APOS
00276         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;}
00277         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;}
00278         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;}
00279     }
00280     if(mf[0]==2||mf[1]==2){i=1;if(mf[0]==2)i=0;//si brujula es NEG
00281         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;}
00282     }
00283     if(mf[0]==1||mf[1]==1){i=1;if(mf[0]==1)i=0;//si brujula es ANEG
00284         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;}
00285     }//llega hasta gprueda[k+1]
00286 return k;//size de mf y gprueda[]
00287 }
00288 //desfuzzyfication
00289 void desfuzzy(float Pm[],int n,int mfrueda[],float gprueda[],int size,float dutty[]){
00290       //rueda1*********************************
00291     float x[3],ar=0,num=0,x1,area=0,area1,x2,area2;
00292     int a,b,i,mfmin,mfmax=1;
00293    mfmin=n;
00294 //el sgte for fue modificado para casos de mfmax y mfmin q se repiten despues de las reglas
00295     for(i=0;i<size;i=i+2){//ojo desde i=0
00296         if(mfrueda[i]<=mfmin){if(mfrueda[i]<mfmin){mfmin=mfrueda[i];a=i;}
00297                                 if(mfrueda[i]=mfmin)b=i;}
00298         if(mfrueda[i]>mfmax){mfmax=mfrueda[i];b=i;}
00299     }
00300 
00301     
00302     //hallar %dutty de rueda1
00303         //centroide de area1
00304     GPIfuzzy(Pm,n,mfmin,gprueda[a],x);
00305 
00306     for(i=0;i<gprueda[a]*100;i++){//*ojo con gprueda[a]*100
00307         ar=0.01*(Pm[mfmin-1]-(i*0.01-1)/1.3333*(Pm[mfmin]-Pm[mfmin-1])-x[0]);
00308         area=area+ar;
00309         num=num+(ar*100/2+x[0])*ar;//ahhhhhhhhhhhhhhhhhhh
00310     }
00311     if(area!=0)x1=num/area;
00312    else x1=0;
00313     area1=area;
00314 
00315         //centroide de area2
00316     ar=0,num=0,area=0;
00317     GPIfuzzy(Pm,n,mfmax,gprueda[b],x);
00318 
00319     for(i=0;i<gprueda[b]*100;i++){
00320         ar=0.01*(x[1]-(Pm[mfmax-1]+(i*0.01-1)/1.333333*(Pm[mfmax-1]-Pm[mfmax-2])));
00321         area=area+ar;
00322        num=num+(100*ar/2+ (x[1]-ar*100)   )*ar;
00323     }
00324     if(area!=0)x2=num/area;else x2=0;
00325     area2=area;
00326 
00327    dutty[0]=(x1*area1+x2*area2)/(area1+area2);
00328 
00329     //rueda2************************************************
00330 
00331    
00332     mfmin=n,mfmax=1;ar=0,area=0,num=0;
00333 //el sgte for fue modificado para casos de mfmax y mfmin q se repiten despues de las reglas
00334      for(i=1;i<size;i=i+2){//ojo desde i=1
00335         if(mfrueda[i]<=mfmin){if(mfrueda[i]<mfmin){mfmin=mfrueda[i];a=i;}
00336                                 if(mfrueda[i]=mfmin)b=i;}
00337         if(mfrueda[i]>mfmax){mfmax=mfrueda[i];b=i;}
00338     }
00339   
00340     //hallar %dutty de rueda2
00341         //centroide de area1
00342     GPIfuzzy(Pm,n,mfmin,gprueda[a],x);
00343     for(i=0;i<gprueda[a]*100;i++){//*ojo con gprueda[a]*100
00344         ar=0.01*(Pm[mfmin-1]-(i*0.01-1)/1.3333*(Pm[mfmin]-Pm[mfmin-1])-x[0]);
00345         area=area+ar;
00346         num=num+(ar*100/2+x[0])*ar;//ahhhhhhhhhhhhhhhhhhh
00347     }
00348     if(area!=0) x1=num/area;
00349    else x1=0;
00350     area1=area;
00351         //centroide de area2
00352     ar=0,num=0,area=0;
00353     GPIfuzzy(Pm,n,mfmax,gprueda[b],x);
00354     for(i=0;i<gprueda[b]*100;i++){
00355        ar=0.01*(x[1]-(Pm[mfmax-1]+(i*0.01-1)/1.333333*(Pm[mfmax-1]-Pm[mfmax-2])));
00356         area=area+ar;
00357        num=num+(100*ar/2+ (x[1]-ar*100)   )*ar;//ahhhhhhhhhh
00358     }
00359     if(area!=0)x2=num/area;
00360    else x2=0;
00361     area2=area;
00362 
00363     dutty[1]=(x1*area1+x2*area2)/(area1+area2);
00364 }