Giovanni Farfan
balancin farfan
Dependencies: mbed MPU6050f Map Math
Revision 0:0abd1091adbf, committed 2020-11-17
- Comitter:
- gfarfan18
- Date:
- Tue Nov 17 06:31:51 2020 +0000
- Commit message:
- balancin farfan;
Changed in this revision
diff -r 000000000000 -r 0abd1091adbf MPU6050.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Tue Nov 17 06:31:51 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/gfarfan18/code/MPU6050f/#2f09dad1b2d8
diff -r 000000000000 -r 0abd1091adbf Map.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Map.lib Tue Nov 17 06:31:51 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/Generic/code/Map/#dad975e2e150
diff -r 000000000000 -r 0abd1091adbf Math.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Math.lib Tue Nov 17 06:31:51 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/inst/code/Math/#067c036b09e0
diff -r 000000000000 -r 0abd1091adbf main.cpp
--- /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
diff -r 000000000000 -r 0abd1091adbf mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Nov 17 06:31:51 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file