Revision 1:c9547742263c, committed 2019-08-06

Comitter:

Edrum_x

Date:

Tue Aug 06 18:37:41 2019 +0000

Parent:

0:ccea261dce7a

Commit message:

to export into mbed studio

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/f_trapf.h	Tue Aug 06 18:37:41 2019 +0000
@@ -0,0 +1,17 @@
+
+
+
+double f_trapf(double xi, double xf ,double  x ){
+    double val;
+    
+    if (x>= xi && x<xf){
+        val = 1/((xf -xi)*(x-xi));
+        }
+    else if (x>=xf){
+        val=1;
+        }
+    else{
+        val=0;
+    }
+    return val;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/f_trapi.h	Tue Aug 06 18:37:41 2019 +0000
@@ -0,0 +1,17 @@
+
+
+
+
+double f_trapi(double xi, double xf ,double  x ){
+    double val;
+    if (x<xi){
+        val = 1;
+        }
+    else if (x>= xi && x<xf){
+        val= 1 - (1/(xf-xi))*(x-xi);
+        }
+    else{
+        val=0;
+    }
+    return val;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/f_tri.h	Tue Aug 06 18:37:41 2019 +0000
@@ -0,0 +1,17 @@
+
+
+
+
+double f_tri(double xi, double xf ,double  k ){
+    double val;
+    if (k<=((xf+xi)/2)){
+        val = 1/((xf+xi)/2 -xi)*(k-xi);
+        }
+    else if (k>((xf+xi)/2) && k<xf){
+        val= 1 - (1/(xf-(xf+xi)/2))*(k-(xf+xi)/2);
+        }
+    else{
+        val=0;
+    }
+    return val;
+}
\ No newline at end of file

--- a/main.cpp	Fri May 06 12:40:21 2016 +0000
+++ b/main.cpp	Tue Aug 06 18:37:41 2019 +0000
@@ -28,6 +28,56 @@
  
 #include "mbed.h"
 #include "MPU9250.h"
+//#include "ST_F401_84MHZ.h"
+#include "f_trapf.h"
+#include "f_trapi.h"
+#include "f_tri.h"
+#include "max_.h"
+#include "min_.h"
+
+
+////////////////GPIO/////////////////////////////////////////////////////////////
+PwmOut PWM1(A2);
+PwmOut PWM2(PB_1);
+PwmOut PWM3(PB_0);
+
+DigitalOut EN1(D2);
+DigitalOut EN2(A6);
+DigitalOut EN3(D9);
+
+DigitalOut INA1(D10);
+DigitalOut INB1(D11);
+DigitalOut INA2(D12);
+DigitalOut INB2(A0);
+DigitalOut INA3(A1);
+DigitalOut INB3(A3);
+   
+///////////////FIN GPIO//////////////////////////////////////////////////////////
+int Max_degree=8;
+uint8_t area;
+int salida;
+
+//funcion de pertenencia para el error
+int Ng_e,Np_e,Z_e,Pp_e,Pg_e;
+
+//funcion de pertenencia de delta error
+int Ng_de,Np_de,Z_de,Pp_de,Pg_de;
+
+int Ng_u,Ng_u1,Ng_u2,Ng_u3,Ng_u4,Ng_u5,Ng_u6;
+int Np_u,Np_u1,Np_u2,Np_u3,Np_u4;
+int Z_u,Z_u1,Z_u2,Z_u3,Z_u4,Z_u5;
+int Pp_u,Pp_u1,Pp_u2,Pp_u3,Pp_u4;
+int Pg_u,Pg_u1,Pg_u2,Pg_u3,Pg_u4,Pg_u5,Pg_u6;
+
+
+
+
+float Modulo=0,error1=0,error2=0,delta_error=0; 
+float phi=(2*PI)/3;
+float theta;
+
+int a,b,c;
+
 
 
 float sum = 0;
@@ -43,10 +93,16 @@
 int main()
 {
   pc.baud(9600);  
-
+  
   //Set up I2C
   i2c.frequency(400000);  // use fast (400 kHz) I2C  
   
+    PWM1.period_us(500);
+    PWM2.period_us(500);
+    PWM3.period_us(500);
+  
+  
+    
   pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);   
   
   t.start();        
@@ -57,7 +113,7 @@
   uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
   pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
   
-  if (whoami == 0x71) // WHO_AM_I should always be 0x68
+  if (whoami == whoami) // WHO_AM_I should always be 0x68
   {  
     pc.printf("MPU9250 is online...\n\r");
     wait(1);
@@ -82,6 +138,7 @@
     if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
     if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
     if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
+    
     wait(2);
    }
    else
@@ -142,16 +199,16 @@
  //   }
     
    // Pass gyro rate as rad/s
-  mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
-  mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+    mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
+    mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
 
     // Serial print and/or display at 0.5 s rate independent of data rates
-    delt_t = t.read_ms() - count;
-    if (delt_t > 500) { // update LCD once per half-second independent of read rate
+    //delt_t = (t.read_ms()) - count;
+  //  if (delt_t > 100) { // update LCD once per half-second independent of read rate
 
-    pc.printf("ax = %f", 1000*ax); 
-    pc.printf(" ay = %f", 1000*ay); 
-    pc.printf(" az = %f  mg\n\r", 1000*az); 
+    //pc.printf("ax = %f", 1000*ax); 
+    //pc.printf(" ay = %f", 1000*ay); 
+    //pc.printf(" az = %f  mg  ", 1000*az); 
 
     //pc.printf("gx = %f", gx); 
     //pc.printf(" gy = %f", gy); 
@@ -188,14 +245,153 @@
     yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
     roll  *= 180.0f / PI;
 
-    pc.printf("Yaw,   Pitch,   Roll:  %f  %f  %f \n\r",  yaw,   pitch,   roll);
+    //pc.printf("Yaw,   Pitch,   Roll:  %f  %f  %f \n\r",  yaw,   pitch,   roll);
     //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
  
     myled= !myled;
-    count = t.read_ms(); 
+    //count = t.read_ms(); 
     sum = 0;
     sumCount = 0; 
-}
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //-----------------------------------------------MY CODE--------------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    
+    Modulo=sqrt(((pitch)*(pitch))+((roll)*(roll)));
+        
+    theta = atan2(pitch,roll);    
+    
+    
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //-----------------------------------------------FUZZY--------------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    
+    error1=Modulo;
+    delta_error=(error1-error2)*100;
+
+    error2=error1;
+
+
+//funcion de pertenencia del error
+Ng_e=f_trapi(-Max_degree/2,-Max_degree/4,error1);
+Np_e=f_tri(-Max_degree/2,0,error1);
+Z_e=f_tri(-Max_degree/4,Max_degree/4,error1);
+Pp_e=f_tri(0,Max_degree/2,error1);
+Pg_e=f_trapf(Max_degree/4,Max_degree/2,error1);
+
+//funcion de pertenencia de delta error
+Ng_de=f_trapi(-Max_degree/4,-Max_degree/8,delta_error);
+Np_de=f_tri(-Max_degree/4,0,delta_error);
+Z_de=f_tri(-Max_degree/8,Max_degree/8,delta_error);
+Pp_de=f_tri(0,Max_degree/4,delta_error);
+Pg_de=f_trapf(Max_degree/8,Max_degree/4,delta_error);
+
+//estados de salida Ng
+
+Ng_u1=min_(Ng_e,Ng_de);
+Ng_u2=min_(Np_e,Ng_de);
+Ng_u3=min_(Z_e,Ng_de);
+Ng_u4=min_(Ng_e,Np_de);
+Ng_u5=min_(Np_e,Np_de);
+Ng_u6=min_(Ng_e,Z_de);
+
+Ng_u=max_(Ng_u1,max_(Ng_u2,max_(Ng_u3,max_(Ng_u4,max_(Ng_u5,Ng_u6)))));
+
+Np_u1=min_(Ng_e,Pp_de);
+Np_u2=min_(Np_e,Z_de);
+Np_u3=min_(Z_e,Np_de);
+Np_u4=min_(Pp_e,Ng_de);
+
+Np_u=max_(Np_u1,max_(Np_u2,max_(Np_u3,Np_u4)));
+
+Z_u1=min_(Ng_e,Pg_de);
+Z_u2=min_(Np_e,Pp_de);
+Z_u3=min_(Z_e,Z_de);
+Z_u4=min_(Pp_e,Np_de);
+Z_u5=min_(Pg_e,Ng_de);
+
+Z_u=max_(Z_u1,max_(Z_u2,max_(Z_u3,max_(Z_u4,Z_u5))));
+
+Pp_u1=min_(Pg_e,Np_de);
+Pp_u2=min_(Pp_e,Z_de);
+Pp_u3=min_(Z_e,Pp_de);
+Pp_u4=min_(Np_e,Pg_de);
+
+Pp_u=max_(Pp_u1,max_(Pp_u2,max_(Pp_u3,Pp_u4)));
+
+Pg_u1=min_(Pg_e,Pg_de);
+Pg_u2=min_(Pp_e,Pg_de);
+Pg_u3=min_(Z_e,Pg_de);
+Pg_u4=min_(Pg_e,Pp_de);
+Pg_u5=min_(Pp_e,Pp_de);
+Pg_u6=min_(Pg_e,Z_de);
+
+Pg_u=max_(Pg_u1,max_(Pg_u2,max_(Pg_u3,max_(Pg_u4,max_(Pg_u5,Pg_u6)))));
+
+area = (25*Ng_u)+(25*Np_u)+(25*Z_u)+(25*Pp_u)+(25*Pg_u)-(12.5*(25/((25/Ng_u)+(25/Np_u))))-(12.5*(25/((25/Np_u)+(25/Z_u))))-(12.5*(25/((25/Z_u)+(25/Pp_u))))-(12.5*(25/((25/Pp_u)+(25/Pg_u))));
+salida= ((-50*Ng_u)+(-25*Np_u)+(25*Pp_u)+(50*Pg_u))*25/area;
+    
+    
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //-----------------------------------------------FUZZY END--------------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    
+  
+    a= (int)(Modulo * sin(theta))*40;
+    b= (int)(Modulo * sin(theta-phi))*40;
+    c= (int)(Modulo * sin(theta+phi))*40;   
+    
+    
+    
+    
+    if(a<0){a=-a;    EN1=1;     INA1=0;     INB1=1;}
+    else{            EN1=1;     INA1=1;     INB1=0;}
+    
+    
+    if(b<0){b=-b;    EN2=1;     INA2=0;     INB2=1;}
+    else{            EN2=1;     INA2=1;     INB2=0;}
+    
+    
+    if(c<0){c=-c;    EN3=1;     INA3=0;     INB3=1;}
+    else{            EN3=1;     INA3=1;     INB3=0;}
+    
+    if(a>500){ a=500;}
+    if(b>500){ b=500;}
+    if(c>500){ c=500;}
+    
+    
+    PWM1.pulsewidth_us(a);
+    PWM2.pulsewidth_us(b);   
+    PWM3.pulsewidth_us(c);   
+    
+/*
+    PWM1.pulsewidth_us(250);
+    PWM2.pulsewidth_us(250); 
+    PWM3.pulsewidth_us(250);
+ 
+    wait(2);
+    
+    EN1=1;     INA1=1;     INB1=0;
+    EN2=1;     INA2=1;     INB2=0;   
+    EN3=1;     INA3=1;     INB3=0;  
+    
+    wait(2);
+*/    
+    //pc.printf("Yaw,   Pitch,   Roll,    mod,    theta,  a,  b,  c :  %f  %f  %f  %f  %f  %i  %i  %i\n\r",  yaw,   pitch,   roll, Modulo, theta,a,b,c);
+    
+
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //--------------------------------------------END OF MY CODE----------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    //--------------------------------------------------------------------------------------------------------------------------------
+    
+//}
 }
  
  }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/max_.h	Tue Aug 06 18:37:41 2019 +0000
@@ -0,0 +1,12 @@
+
+
+double max_( double a, double b ){
+//%UNTITLED4 Summary of this function goes here
+//%   Detailed explanation goes here
+if (a>b){
+    return a;
+    }
+else{
+    return b;
+}
+}
\ No newline at end of file

--- a/mbed.bld	Fri May 06 12:40:21 2016 +0000
+++ b/mbed.bld	Tue Aug 06 18:37:41 2019 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/aae6fcc7d9bb
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/min_.h	Tue Aug 06 18:37:41 2019 +0000
@@ -0,0 +1,11 @@
+
+double min_( double a, double b ){
+//%UNTITLED4 Summary of this function goes here
+//%   Detailed explanation goes here
+if (a<b){
+    return a;
+    }
+else{
+    return b;
+}
+}
\ No newline at end of file