![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
changed adress
Dependencies: mbed MPU6050 mbed-rtos
Diff: main.cpp
- Revision:
- 3:2cd58c219b89
- Child:
- 5:0d84191fde21
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Feb 03 13:41:29 2020 +0000 @@ -0,0 +1,79 @@ +#include "mbed.h" +#include "MPU6050.h" +#include "rtos.h" +#include <math.h> + +//state +#define FinAcquisition 1 +#define PretPourAcquisition 0 + +Serial pc(USBTX, USBRX); // USB initialization +MPU6050 accelero(D4, D5); // MPU6050 library initialization + +// Variables globales +float AngleTan,AngleFiltre=0,AngleNonFiltre=0,A,B; +float Te_ms = 5, Te, Tau = 0.1; // Valeurs periode d'enchantillonage et tau + +char Flag=PretPourAcquisition; + +void calculAngle(void) +{ + int gx, gz, OmegaY; + + // acq de gx et gz et calcul de AngleTan + gx = accelero.getAcceleroRawX(); + gz = accelero.getAcceleroRawZ(); + AngleTan = atan2((double)gx,(double)gz); + + // acq de OmegaY + OmegaY = accelero.getGyroRawY()*Tau/1000; + + // calcul AngleNonFiltre + AngleNonFiltre = OmegaY + AngleTan; + + // filtre passe bas numérique + AngleFiltre = A*AngleNonFiltre+B*AngleFiltre; + + Flag = FinAcquisition; +} + +RtosTimer timer(mbed::callback(calculAngle),osTimerPeriodic); // definition of rtos timer + + +int main (void) +{ + float AngleDeg; + // calcul des coefficients du filtre de l'angle + Te = Te_ms/1000; + A = Te/(Te+Tau); + B = Tau/(Te+Tau); + + // Usb init + pc.baud(115200); + pc.printf("\r\n Debut prog\r\n"); + + // verification of connection + if (accelero.testConnection()) { + pc.printf("connected to MPU 6050\r\n"); + } else { + pc.printf("No device \r\n"); + while(1); // infinite pause + } + wait(0.2); + + // Scale init + accelero.setGyroRange(MPU6050_GYRO_RANGE_2000); + accelero.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); + + timer.start(Te_ms); + + while(1) { + //display of angles calculated + if (Flag == FinAcquisition) { + pc.printf("%f %f \r\n",AngleFiltre, AngleTan); + Flag = PretPourAcquisition ; + } + + } +} +/*