![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Filter for 9250
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
diff -r ccea261dce7a -r c9547742263c f_trapf.h --- /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; +}
diff -r ccea261dce7a -r c9547742263c f_trapi.h --- /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; +}
diff -r ccea261dce7a -r c9547742263c f_tri.h --- /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
diff -r ccea261dce7a -r c9547742263c main.cpp --- 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---------------------------------------------------------------------- + //-------------------------------------------------------------------------------------------------------------------------------- + //-------------------------------------------------------------------------------------------------------------------------------- + +//} } }
diff -r ccea261dce7a -r c9547742263c max_.h --- /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
diff -r ccea261dce7a -r c9547742263c mbed.bld --- 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
diff -r ccea261dce7a -r c9547742263c min_.h --- /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