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?
User | Revision | Line number | New 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 | } |