File content as of revision 0:0abd1091adbf:

#include "mbed.h"
#include "MPU6050.h"
#include "Math.h"
#include "Map.hpp"
 
Serial pc(SERIAL_TX, SERIAL_RX);
MPU6050 ark(PB_9,PB_8);

//Mototores
PwmOut m1der(D11);
PwmOut m1izq(D10);
PwmOut m2der(D6);
PwmOut m2izq(D5);
 
Timer t;
Ticker flipper;
static int estado = 0;
void flip();

//Tiempo para el angulo
int dt, end;
float gyro[3]; 
float girosc_ang_x, girosc_ang_y;
float girosc_ang_x_prev, girosc_ang_y_prev;

DigitalOut led1(LED1);

float acce[3]; 
float accel_ang_x;
float accel_ang_y;

//Angulos
float ang_x, ang_y;
float ang_x_prev, ang_y_prev;
;
          
void flip() {
    led1 = !led1;
}
 
int main() {
    t.start();
    flipper.attach_us(&flip, 500); // the address of the function to be attached (flip) and the interval (2 seconds)
    led1 = 1;
    // spin in a main loop. flipper will interrupt it to call flip
    while(1) {
      estado = led1;
      if (estado == 0){
          ark.getAccelero(acce);
//        pc.printf("Acce0=%f,Acce1=%f,Acce2=%f\r\n",acce[0],acce[1],acce[2]);
          accel_ang_x = atan(acce[0]/sqrt(pow(acce[1],2) + pow(acce[2],2)))*(180.0/3.14);
          accel_ang_y = atan(acce[1]/sqrt(pow(acce[0],2) + pow(acce[2],2)))*(180.0/3.14);
          //pc.printf("%f\n",accel_ang_x);
          //pc.printf("y%f\n",accel_ang_y);
          }
      if (estado == 1){
          dt = t.read_ms() - end;
          end = t.read_ms();
          
          ark.getGyro(gyro);     
          //girosc_ang_x = (gyro[0]/131)*dt/1000.0 + girosc_ang_x_prev;
          //girosc_ang_y = (gyro[1]/131)*dt/1000.0 + girosc_ang_y_prev;
          
          ang_x = 0.98*(ang_x_prev+(gyro[0]/131)*dt) + 0.02*accel_ang_x;
          ang_y = 0.98*(ang_y_prev+(gyro[1]/131)*dt) + 0.02*accel_ang_y;
          
          //girosc_ang_x_prev = girosc_ang_x;
          //girosc_ang_y_prev = girosc_ang_y;
            
          ang_x_prev = ang_x;
          ang_y_prev = ang_y;
          
          
          
          //pc.printf("%f\n", ang_x);
          //pc.printf("Y=%f\n",girosc_ang_y);
          if (ang_x > -4 && ang_x <-2){
                m1der = 0.2;
                m1izq = 0.00F;
                m2der = 0.00F;
                m2izq = 0.2;
                pc.printf("%f\n", ang_x);
              }
          if (ang_x < -4 && ang_x > -6){
                  m1der = 0.00f;
                  m1izq = 0.2;
                  m2der = 0.2;
                  m2izq = 0.00f;
                  pc.printf("%f\n", ang_x);
              }
     
          
          }
    }
}