File content as of revision 0:1c15748ff0e1:

#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);
}