Eduardo Vergara
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