Diff: main.cpp

Revision:

4:c79a3c86ab36

Parent:

2:986e8a434da3

Child:

5:96fff32333e8

--- a/main.cpp	Thu Sep 22 16:34:10 2016 +0000
+++ b/main.cpp	Thu Sep 22 22:39:40 2016 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 
+/*Timer*/
+Timer t;
 
 /* defines the axis for acc */
 #define ACC_NOOF_AXIS       3
@@ -8,10 +10,13 @@
 /* bmi160 slave address */
 #define BMI160_ADDR         ((0x68)<<1)
 
+/*Valor para Transformar de Radiano para Graus*/
 #define RAD_DEG           57.29577951
- 
+
+/*Comunicacao LPCXpresso4337*/ 
 Serial pc(USBTX, USBRX); // tx, rx
 
+/*Comunicacao I2C*/
 I2C i2c(P2_3, P2_4);
 
 
@@ -102,9 +107,20 @@
         accel_ang_x=atan(acc_result_buffer[0]/sqrt(pow(acc_result_buffer[1],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
         accel_ang_y=atan(acc_result_buffer[1]/sqrt(pow(acc_result_buffer[0],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
         
+        /*Para o Timer*/
+        t.stop();
+                
         /*Calcula os Angulos de Rotacao com valor do Giroscopio e aplica filtro complementar realizando a fusao*/
-        tiltx = (0.98*(tiltx_prev+(gyr_result_buffer[0]*0.001)))+(0.02*(accel_ang_x));
-        tilty = (0.98*(tilty_prev+(gyr_result_buffer[1]*0.001)))+(0.02*(accel_ang_y));
+        tiltx = (0.965*(tiltx_prev+(gyr_result_buffer[0]*t.read())))+(0.035*(accel_ang_x));
+        tilty = (0.965*(tilty_prev+(gyr_result_buffer[1]*t.read())))+(0.035*(accel_ang_y));
+        
+        /*Debug para encontrar o tempo do loop*/
+        //pc.printf("%f",t.read());
+        
+        /*Reseta o Timer*/
+        t.reset();
+        /*Inicia o Timer*/
+        t.start();
         
         tiltx_prev = tiltx;
         tilty_prev = tilty;