dimel contreras
algoritmo logica difusa sensores navegacion
Dependencies: GPS MODI2C SRF05 mbed HMC5883
Diff: main.cpp
- Revision:
- 0:1c15748ff0e1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,364 @@
+#include "mbed.h" // Standard Library
+#include "HMC5883.h" // Comp (Compass)
+#include "GPS.h"
+#include "SRF05.h"
+
+#define PI 3.1415926535897932384626433832795
+DigitalOut myled(LED1);
+I2C I2CBus(p9, p10);
+Timer GlobalTime;
+
+SRF05 Ult1(p5, p6);
+SRF05 Ult2(p7, p8);
+SRF05 Ult3(p11, p12);
+SRF05 Ult4(p16, p15);
+
+Serial pc(USBTX, USBRX);
+HMC5883 Mag(I2CBus, GlobalTime);
+GPS gps(p13, p14);
+
+float longitud,latitud,ul1,ul2,ul3,ul4;
+int orientacion;
+
+
+void GPfuzzy(float P[],int n,float v,float GP[],int mf[]);
+float min(float a,float b);
+float Sugeno(int v1,int v2,float P[],int n);
+float Centroide(int v1,int v2,float P[],int n);
+void GPIfuzzy(float P[],int n,int gx,float GP,float x[]);
+void EntradaU(float F[],float O[],int ultrasonico,float tetha,float entrada[],float gp[],int mf[]);
+int reglas(int mf[],int mfrueda[],float gpin[],float gprueda[]);
+void desfuzzy(float Pm[],int n,int mfrueda[],float gprueda[],int size,float dutty[]);
+
+
+int HMC5883_getAngle(float x, float y)
+{
+float heading = atan2((float)y,(float)x);
+// Your mrad result / 1000.00 (to turn it into radians).
+ float declinationAngle = 21.18 / 1000.0;
+ // If you have an EAST declination, use += declinationAngle, if you have a WEST declination, use -= declinationAngle
+ heading += declinationAngle;
+if(heading < 0) heading += 2*PI;// Correct for when signs are reversed.
+if(heading > 2*PI) heading -= 2*PI; // Check for wrap due to addition of declination.
+return(heading * 180/PI); // Convert radians to degrees for readability.
+}
+
+int main() { // main programm for initialisation and debug output
+ // pc.baud(9600);
+
+ I2CBus.frequency(400000);
+ GlobalTime.start();
+
+ Mag.Init();
+ #if 1
+ Mag.AutoCalibration= 1; //In Echtzeit kalibrieren
+ #else
+ short MagRawMin[3]= {-400, -400, -400}; //Gespeicherte Werte
+ short MagRawMax[3]= {400, 400, 400};
+ Mag.Calibrate(MagRawMin, MagRawMax);
+ #endif
+
+ int size,n=5,ultrasonico=3,mfs[4],mfrueda[4];
+ float dutty[2],entrada[2],tetha,gradosp[4],gprueda[4];
+ float Pm[5]={0,25,50,75,100};//RANGO DE DUTTY PARA MOTORES (p.e. de 0 a 100)
+
+ while(1) {
+
+ Mag.Update();
+
+ printf("grader = %i \n",HMC5883_getAngle(Mag.Mag[0],Mag.Mag[1]));
+ wait_ms(20);
+
+
+ if(gps.sample()) {
+ myled = 1;//indica si el GPS esta enviando valores buenos
+ latitud=gps.latitude;
+ longitud=gps.longitude;
+ }
+
+
+ orientacion=HMC5883_getAngle(Mag.Mag[0],Mag.Mag[1]);
+
+ // beta=atan((F[0]-O[0])/(F[1]-O[1]))*57.29582790879777;
+
+
+
+//CONFIGURACION DE ENTRADAS A LA LOGICA FUZZY********************************************
+
+ float O[]={0,0};//POSICION INICIAL SEGUN GPS, EN EL PLANO
+ float F[]={3,4};//OBJETIVO O POSICION FINAL
+ tetha=orientacion;//en sexagesimales, ang de la brujula medido desde el eje NORTE sentido horario(positivo)
+ ultrasonico=1;
+//fin de CONFIG DE ENTRADAS******************************************************
+
+
+ EntradaU(F,O,ultrasonico,tetha,entrada,gradosp,mfs);//hace la fuzzyfication
+
+ size=reglas(mfs,mfrueda,gradosp,gprueda);//esta funcion bota los vectores mfrueda y sus gprueda
+
+ desfuzzy(Pm,n,mfrueda,gprueda,size,dutty);
+ printf("\fdutty1 %f \ndutty2 %f",dutty[0],dutty[1]);
+ //wait_ms(100);
+
+
+ }
+}
+//*****************DESARROLLO DE FUNCIONES**************************
+void GPfuzzy(float P[],int n,float v,float GP[],int mf[]){//v==> valor eje x
+ int i;
+ for( i=0;i<n;i++){//
+ if(P[i]<=v && v<=P[i+1]){
+ GP[0]=1.0-(v-P[i])*1.3333/(P[i+1]-P[i]);
+ GP[1]=1.0+1.33333*(v-P[i+1])/(P[i+1]-P[i]);
+ if(GP[1]<0){GP[1]=0;}
+ if(GP[0]<0){GP[0]=0;}
+ mf[0]=i+1;
+ mf[1]=i+2;
+ break;
+ }
+ }
+}
+float min(float a,float b){
+ if(a<=b)return a;
+ else return b;
+}
+float max(float a,float b){
+ if(a>=b)return a;
+ else return b;
+}
+float Sugeno(int v1,int v2,float P[],int n){//bota el centroide
+ int i;
+ float a,GP1[3],GP2[3],num=0,area=0,mf[3];
+ for(i=1;i<=(v2-v1);i++){
+ /* GPfuzzy(P,n,v1+i-1,GP1,mf);
+ GPfuzzy(P,n,v1+i,GP2,mf);*/
+ a=(max(GP1[0],GP1[1])+max(GP2[0],GP2[1]))/2;
+ num=num+a*max(GP1[0],GP1[1])/2;
+ area=area+a;
+ }
+
+ return num/area;
+}
+
+float Centroide(int v1,int v2,float P[],int n){//bota el centroide
+ int i,L=50;
+ float a,GP1[3],GP2[3],num=0,area=0;
+ for(i=1;i<=L;i++){
+ //GPfuzzy(P,n,v1+i-1,GP1,mf);
+ //GPfuzzy(P,n,v1+i,GP2,mf);
+ a=(max(GP1[0],GP1[1])+max(GP2[0],GP2[1]))/2;
+ num=num+a*max(GP1[0],GP1[1])/2;
+ area=area+a;
+ }
+ return num/area;
+}
+
+
+void GPIfuzzy(float P[],int n,int gx,float GP,float x[]){//1<=gx<=n ,, 0<=GP<=1
+
+ if(gx==1){
+ x[0]=P[0];
+ x[1]=(1-GP)*(P[1]-P[0])/1.33333+P[0];
+ }
+ else{
+ if(gx<n){
+ x[0]=(GP-1)*(P[gx-1]-P[gx-2])/1.333333+P[gx-1];
+ x[1]=(1-GP)*(P[gx]-P[gx-1])/1.333333+P[gx-1];
+ }
+ if(gx==n){
+ x[0]=(GP-1)*(P[gx-1]-P[gx-2])/1.333333+P[gx-1];
+ x[1]=P[n-1];
+ }
+
+ }
+}
+
+
+void EntradaU(float F[],float O[],int ultrasonico,float tetha,float entrada[],float gp[],int mf[]){
+ // ex_izq muy_izqu izqui frente dere muy_der ex_dere
+ //salidau 1 2 3 4 5 6 7
+ // entrada[0]==> entrada fuzzy phi brujula
+ // entrada[1] ==> entrada fuzzy ultrasonico
+ //para el phi brujula
+ float gradop[2] = {0.0,0.0};
+ int mfi[2]={0,0};
+ float P[5]={-180,-90,0,90,180};//es decir entre -180 y 180
+ float beta;
+ beta=atan((F[0]-O[0])/(F[1]-O[1]))*57.29582790879777;
+ entrada[0]=beta-tetha;//degrees phi
+ switch(ultrasonico){
+ case 0:{
+ if(entrada[0]>0)entrada[1]=7;//entrada[1] de ultrasonico
+ if(entrada[0]<0)entrada[1]=1;
+ break;}
+ case 2:{
+ if(entrada[0]>0)entrada[1]=7;
+ if(entrada[0]<0)entrada[1]=1;
+ break;}
+ case 5:{
+ if(entrada[0]>0)entrada[1]=6;
+ if(entrada[0]<0)entrada[1]=2;
+ break;}
+ case 1:{entrada[1]=6;
+ break;}
+ case 3:{entrada[1]=5;
+ break;}
+ case 7:{if(entrada[0]>0)entrada[1]=6;//entrada[1] de ultrasonico
+ if(entrada[0]<0)entrada[1]=2;//*cambie orden 7 con 1
+ break;}
+ case 6:{entrada[1]=3;
+ break;}
+ case 4:{entrada[1]=2;
+ break;}
+ }
+ /*//para el phi brujula
+ float gradop[2] = {0.0,0.0};
+ int mfi[2]={0,0};
+ float P[5]={-144,-72,0,72,144};//es decir entre -180 y 180*/
+ //printf("\nentrada[0]%f\n",entrada[0]);
+ GPfuzzy(P,5,entrada[0],gradop,mfi);
+ gp[0]=gradop[0]; mf[0]=mfi[0];
+ gp[1]=gradop[1]; mf[1]=mfi[1];
+
+ //para el ultrasonico
+ //GPfuzzy(P,5,entrada[1],gradop,mf);
+ gp[2]=1; mf[2]=entrada[1];
+ gp[3]=0.5; mf[3]=entrada[1]+1;
+}
+
+
+//reglas fuzzy
+int reglas(int mf[],int mfrueda[],float gpin[],float gprueda[]){
+ //mf[0]==>mf phi brujula
+ //mf[1]==>mf sgte brujula
+ //mf[2] mf ultrasonico
+ //mf[3] mf sgte ultra
+ //gpin[0] brujula primer grado de pertenencia
+ //gpin[1] brujula segundo grado de pertencia
+ //gpin[2] ultrasonico pro=imer gp
+ //gpin[3] ultrasonico segundo gp
+ //rueda[0]==>rueda1
+ //rueda[1]==>rueda2
+
+ //mfgps[0]==>latitud
+ //mfgps[1]==>longtud
+ //k==> ultimo elem: gprueda[k+1]
+ int i,j;
+ int k=0;
+ //printf("\ngpin[]phi %f %f\n",gpin[0],gpin[1]);
+ //printf("\ngpin[]ultrasonico %f %f\n",gpin[2],gpin[3]);
+ //printf("\nmf[] %d %d %d %d\n",mf[0],mf[1],mf[2],mf[3]);
+ //mfrueda[6]=7;
+ //mfrueda[5]=7;
+
+//falta ve lo de la interrupcion del gps XD
+ //if((mfgps[0]==1)&&(mfgps[1]==1)){gprueda[0]=gpgps(0);gprueda[1]=gpgps[1];}//a parte del phi y el ultrasonico
+ if(mf[0]==3||mf[1]==3){i=1;if(mf[0]==3)i=0;
+ 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;}
+ 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;}
+ 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;}
+ 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;}
+ 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;}
+ 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;}
+ 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;}
+ }
+//printf("\n1gprueda[]phi %f %f\n",gpin[0],gpin[1]);
+
+ if(mf[0]==4||mf[1]==4){i=1;if(mf[0]==4)i=0;//si brujula es POS
+ 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;}
+ 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;}
+ 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;}
+ 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;}
+ 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;}
+ }
+
+ if(mf[0]==5||mf[1]==5){i=1;if(mf[0]==5)i=0;//si brujula es APOS
+ 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;}
+ 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;}
+ 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;}
+ }
+ if(mf[0]==2||mf[1]==2){i=1;if(mf[0]==2)i=0;//si brujula es NEG
+ 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;}
+ }
+ if(mf[0]==1||mf[1]==1){i=1;if(mf[0]==1)i=0;//si brujula es ANEG
+ 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;}
+ }//llega hasta gprueda[k+1]
+return k;//size de mf y gprueda[]
+}
+//desfuzzyfication
+void desfuzzy(float Pm[],int n,int mfrueda[],float gprueda[],int size,float dutty[]){
+ //rueda1*********************************
+ float x[3],ar=0,num=0,x1,area=0,area1,x2,area2;
+ int a,b,i,mfmin,mfmax=1;
+ mfmin=n;
+//el sgte for fue modificado para casos de mfmax y mfmin q se repiten despues de las reglas
+ for(i=0;i<size;i=i+2){//ojo desde i=0
+ if(mfrueda[i]<=mfmin){if(mfrueda[i]<mfmin){mfmin=mfrueda[i];a=i;}
+ if(mfrueda[i]=mfmin)b=i;}
+ if(mfrueda[i]>mfmax){mfmax=mfrueda[i];b=i;}
+ }
+
+
+ //hallar %dutty de rueda1
+ //centroide de area1
+ GPIfuzzy(Pm,n,mfmin,gprueda[a],x);
+
+ for(i=0;i<gprueda[a]*100;i++){//*ojo con gprueda[a]*100
+ ar=0.01*(Pm[mfmin-1]-(i*0.01-1)/1.3333*(Pm[mfmin]-Pm[mfmin-1])-x[0]);
+ area=area+ar;
+ num=num+(ar*100/2+x[0])*ar;//ahhhhhhhhhhhhhhhhhhh
+ }
+ if(area!=0)x1=num/area;
+ else x1=0;
+ area1=area;
+
+ //centroide de area2
+ ar=0,num=0,area=0;
+ GPIfuzzy(Pm,n,mfmax,gprueda[b],x);
+
+ for(i=0;i<gprueda[b]*100;i++){
+ ar=0.01*(x[1]-(Pm[mfmax-1]+(i*0.01-1)/1.333333*(Pm[mfmax-1]-Pm[mfmax-2])));
+ area=area+ar;
+ num=num+(100*ar/2+ (x[1]-ar*100) )*ar;
+ }
+ if(area!=0)x2=num/area;else x2=0;
+ area2=area;
+
+ dutty[0]=(x1*area1+x2*area2)/(area1+area2);
+
+ //rueda2************************************************
+
+
+ mfmin=n,mfmax=1;ar=0,area=0,num=0;
+//el sgte for fue modificado para casos de mfmax y mfmin q se repiten despues de las reglas
+ for(i=1;i<size;i=i+2){//ojo desde i=1
+ if(mfrueda[i]<=mfmin){if(mfrueda[i]<mfmin){mfmin=mfrueda[i];a=i;}
+ if(mfrueda[i]=mfmin)b=i;}
+ if(mfrueda[i]>mfmax){mfmax=mfrueda[i];b=i;}
+ }
+
+ //hallar %dutty de rueda2
+ //centroide de area1
+ GPIfuzzy(Pm,n,mfmin,gprueda[a],x);
+ for(i=0;i<gprueda[a]*100;i++){//*ojo con gprueda[a]*100
+ ar=0.01*(Pm[mfmin-1]-(i*0.01-1)/1.3333*(Pm[mfmin]-Pm[mfmin-1])-x[0]);
+ area=area+ar;
+ num=num+(ar*100/2+x[0])*ar;//ahhhhhhhhhhhhhhhhhhh
+ }
+ if(area!=0) x1=num/area;
+ else x1=0;
+ area1=area;
+ //centroide de area2
+ ar=0,num=0,area=0;
+ GPIfuzzy(Pm,n,mfmax,gprueda[b],x);
+ for(i=0;i<gprueda[b]*100;i++){
+ ar=0.01*(x[1]-(Pm[mfmax-1]+(i*0.01-1)/1.333333*(Pm[mfmax-1]-Pm[mfmax-2])));
+ area=area+ar;
+ num=num+(100*ar/2+ (x[1]-ar*100) )*ar;//ahhhhhhhhhh
+ }
+ if(area!=0)x2=num/area;
+ else x2=0;
+ area2=area;
+
+ dutty[1]=(x1*area1+x2*area2)/(area1+area2);
+}
\ No newline at end of file