Giovanni Farfan / Mbed 2 deprecated Balanceo

balancin farfan

Dependencies:   mbed MPU6050f Map Math

main.cpp@0:0abd1091adbf, 2020-11-17 (annotated)

Committer:
gfarfan18
Date:
Tue Nov 17 06:31:51 2020 +0000
Revision:
0:0abd1091adbf
balancin farfan;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gfarfan18 0:0abd1091adbf 1 #include "mbed.h"
gfarfan18 0:0abd1091adbf 2 #include "MPU6050.h"
gfarfan18 0:0abd1091adbf 3 #include "Math.h"
gfarfan18 0:0abd1091adbf 4 #include "Map.hpp"
gfarfan18 0:0abd1091adbf 5
gfarfan18 0:0abd1091adbf 6 Serial pc(SERIAL_TX, SERIAL_RX);
gfarfan18 0:0abd1091adbf 7 MPU6050 ark(PB_9,PB_8);
gfarfan18 0:0abd1091adbf 8
gfarfan18 0:0abd1091adbf 9 //Mototores
gfarfan18 0:0abd1091adbf 10 PwmOut m1der(D11);
gfarfan18 0:0abd1091adbf 11 PwmOut m1izq(D10);
gfarfan18 0:0abd1091adbf 12 PwmOut m2der(D6);
gfarfan18 0:0abd1091adbf 13 PwmOut m2izq(D5);
gfarfan18 0:0abd1091adbf 14
gfarfan18 0:0abd1091adbf 15 Timer t;
gfarfan18 0:0abd1091adbf 16 Ticker flipper;
gfarfan18 0:0abd1091adbf 17 static int estado = 0;
gfarfan18 0:0abd1091adbf 18 void flip();
gfarfan18 0:0abd1091adbf 19
gfarfan18 0:0abd1091adbf 20 //Tiempo para el angulo
gfarfan18 0:0abd1091adbf 21 int dt, end;
gfarfan18 0:0abd1091adbf 22 float gyro[3];
gfarfan18 0:0abd1091adbf 23 float girosc_ang_x, girosc_ang_y;
gfarfan18 0:0abd1091adbf 24 float girosc_ang_x_prev, girosc_ang_y_prev;
gfarfan18 0:0abd1091adbf 25
gfarfan18 0:0abd1091adbf 26 DigitalOut led1(LED1);
gfarfan18 0:0abd1091adbf 27
gfarfan18 0:0abd1091adbf 28 float acce[3];
gfarfan18 0:0abd1091adbf 29 float accel_ang_x;
gfarfan18 0:0abd1091adbf 30 float accel_ang_y;
gfarfan18 0:0abd1091adbf 31
gfarfan18 0:0abd1091adbf 32 //Angulos
gfarfan18 0:0abd1091adbf 33 float ang_x, ang_y;
gfarfan18 0:0abd1091adbf 34 float ang_x_prev, ang_y_prev;
gfarfan18 0:0abd1091adbf 35 ;
gfarfan18 0:0abd1091adbf 36
gfarfan18 0:0abd1091adbf 37 void flip() {
gfarfan18 0:0abd1091adbf 38 led1 = !led1;
gfarfan18 0:0abd1091adbf 39 }
gfarfan18 0:0abd1091adbf 40
gfarfan18 0:0abd1091adbf 41 int main() {
gfarfan18 0:0abd1091adbf 42 t.start();
gfarfan18 0:0abd1091adbf 43 flipper.attach_us(&flip, 500); // the address of the function to be attached (flip) and the interval (2 seconds)
gfarfan18 0:0abd1091adbf 44 led1 = 1;
gfarfan18 0:0abd1091adbf 45 // spin in a main loop. flipper will interrupt it to call flip
gfarfan18 0:0abd1091adbf 46 while(1) {
gfarfan18 0:0abd1091adbf 47 estado = led1;
gfarfan18 0:0abd1091adbf 48 if (estado == 0){
gfarfan18 0:0abd1091adbf 49 ark.getAccelero(acce);
gfarfan18 0:0abd1091adbf 50 // pc.printf("Acce0=%f,Acce1=%f,Acce2=%f\r\n",acce[0],acce[1],acce[2]);
gfarfan18 0:0abd1091adbf 51 accel_ang_x = atan(acce[0]/sqrt(pow(acce[1],2) + pow(acce[2],2)))*(180.0/3.14);
gfarfan18 0:0abd1091adbf 52 accel_ang_y = atan(acce[1]/sqrt(pow(acce[0],2) + pow(acce[2],2)))*(180.0/3.14);
gfarfan18 0:0abd1091adbf 53 //pc.printf("%f\n",accel_ang_x);
gfarfan18 0:0abd1091adbf 54 //pc.printf("y%f\n",accel_ang_y);
gfarfan18 0:0abd1091adbf 55 }
gfarfan18 0:0abd1091adbf 56 if (estado == 1){
gfarfan18 0:0abd1091adbf 57 dt = t.read_ms() - end;
gfarfan18 0:0abd1091adbf 58 end = t.read_ms();
gfarfan18 0:0abd1091adbf 59
gfarfan18 0:0abd1091adbf 60 ark.getGyro(gyro);
gfarfan18 0:0abd1091adbf 61 //girosc_ang_x = (gyro[0]/131)*dt/1000.0 + girosc_ang_x_prev;
gfarfan18 0:0abd1091adbf 62 //girosc_ang_y = (gyro[1]/131)*dt/1000.0 + girosc_ang_y_prev;
gfarfan18 0:0abd1091adbf 63
gfarfan18 0:0abd1091adbf 64 ang_x = 0.98*(ang_x_prev+(gyro[0]/131)*dt) + 0.02*accel_ang_x;
gfarfan18 0:0abd1091adbf 65 ang_y = 0.98*(ang_y_prev+(gyro[1]/131)*dt) + 0.02*accel_ang_y;
gfarfan18 0:0abd1091adbf 66
gfarfan18 0:0abd1091adbf 67 //girosc_ang_x_prev = girosc_ang_x;
gfarfan18 0:0abd1091adbf 68 //girosc_ang_y_prev = girosc_ang_y;
gfarfan18 0:0abd1091adbf 69
gfarfan18 0:0abd1091adbf 70 ang_x_prev = ang_x;
gfarfan18 0:0abd1091adbf 71 ang_y_prev = ang_y;
gfarfan18 0:0abd1091adbf 72
gfarfan18 0:0abd1091adbf 73
gfarfan18 0:0abd1091adbf 74
gfarfan18 0:0abd1091adbf 75 //pc.printf("%f\n", ang_x);
gfarfan18 0:0abd1091adbf 76 //pc.printf("Y=%f\n",girosc_ang_y);
gfarfan18 0:0abd1091adbf 77 if (ang_x > -4 && ang_x <-2){
gfarfan18 0:0abd1091adbf 78 m1der = 0.2;
gfarfan18 0:0abd1091adbf 79 m1izq = 0.00F;
gfarfan18 0:0abd1091adbf 80 m2der = 0.00F;
gfarfan18 0:0abd1091adbf 81 m2izq = 0.2;
gfarfan18 0:0abd1091adbf 82 pc.printf("%f\n", ang_x);
gfarfan18 0:0abd1091adbf 83 }
gfarfan18 0:0abd1091adbf 84 if (ang_x < -4 && ang_x > -6){
gfarfan18 0:0abd1091adbf 85 m1der = 0.00f;
gfarfan18 0:0abd1091adbf 86 m1izq = 0.2;
gfarfan18 0:0abd1091adbf 87 m2der = 0.2;
gfarfan18 0:0abd1091adbf 88 m2izq = 0.00f;
gfarfan18 0:0abd1091adbf 89 pc.printf("%f\n", ang_x);
gfarfan18 0:0abd1091adbf 90 }
gfarfan18 0:0abd1091adbf 91
gfarfan18 0:0abd1091adbf 92
gfarfan18 0:0abd1091adbf 93 }
gfarfan18 0:0abd1091adbf 94 }
gfarfan18 0:0abd1091adbf 95 }