Diff: main.cpp

Revision:

0:0abd1091adbf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 17 06:31:51 2020 +0000
@@ -0,0 +1,95 @@
+#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);
+              }
+     
+          
+          }
+    }
+}
\ No newline at end of file