balancin farfan
Dependencies: mbed MPU6050f Map Math
Diff: main.cpp
- Revision:
- 0:0abd1091adbf
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