guilhem mathieux / Mbed 2 deprecated GYRO_MPU6050

changed adress

Dependencies:   mbed MPU6050 mbed-rtos

main.cpp@3:2cd58c219b89, 2020-02-03 (annotated)

Committer:
guilhemMBED
Date:
Mon Feb 03 13:41:29 2020 +0000
Revision:
3:2cd58c219b89
Child:
5:0d84191fde21
changed Adress;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
guilhemMBED 3:2cd58c219b89 1 #include "mbed.h"
guilhemMBED 3:2cd58c219b89 2 #include "MPU6050.h"
guilhemMBED 3:2cd58c219b89 3 #include "rtos.h"
guilhemMBED 3:2cd58c219b89 4 #include <math.h>
guilhemMBED 3:2cd58c219b89 5
guilhemMBED 3:2cd58c219b89 6 //state
guilhemMBED 3:2cd58c219b89 7 #define FinAcquisition 1
guilhemMBED 3:2cd58c219b89 8 #define PretPourAcquisition 0
guilhemMBED 3:2cd58c219b89 9
guilhemMBED 3:2cd58c219b89 10 Serial pc(USBTX, USBRX); // USB initialization
guilhemMBED 3:2cd58c219b89 11 MPU6050 accelero(D4, D5); // MPU6050 library initialization
guilhemMBED 3:2cd58c219b89 12
guilhemMBED 3:2cd58c219b89 13 // Variables globales
guilhemMBED 3:2cd58c219b89 14 float AngleTan,AngleFiltre=0,AngleNonFiltre=0,A,B;
guilhemMBED 3:2cd58c219b89 15 float Te_ms = 5, Te, Tau = 0.1; // Valeurs periode d'enchantillonage et tau
guilhemMBED 3:2cd58c219b89 16
guilhemMBED 3:2cd58c219b89 17 char Flag=PretPourAcquisition;
guilhemMBED 3:2cd58c219b89 18
guilhemMBED 3:2cd58c219b89 19 void calculAngle(void)
guilhemMBED 3:2cd58c219b89 20 {
guilhemMBED 3:2cd58c219b89 21 int gx, gz, OmegaY;
guilhemMBED 3:2cd58c219b89 22
guilhemMBED 3:2cd58c219b89 23 // acq de gx et gz et calcul de AngleTan
guilhemMBED 3:2cd58c219b89 24 gx = accelero.getAcceleroRawX();
guilhemMBED 3:2cd58c219b89 25 gz = accelero.getAcceleroRawZ();
guilhemMBED 3:2cd58c219b89 26 AngleTan = atan2((double)gx,(double)gz);
guilhemMBED 3:2cd58c219b89 27
guilhemMBED 3:2cd58c219b89 28 // acq de OmegaY
guilhemMBED 3:2cd58c219b89 29 OmegaY = accelero.getGyroRawY()*Tau/1000;
guilhemMBED 3:2cd58c219b89 30
guilhemMBED 3:2cd58c219b89 31 // calcul AngleNonFiltre
guilhemMBED 3:2cd58c219b89 32 AngleNonFiltre = OmegaY + AngleTan;
guilhemMBED 3:2cd58c219b89 33
guilhemMBED 3:2cd58c219b89 34 // filtre passe bas numérique
guilhemMBED 3:2cd58c219b89 35 AngleFiltre = A*AngleNonFiltre+B*AngleFiltre;
guilhemMBED 3:2cd58c219b89 36
guilhemMBED 3:2cd58c219b89 37 Flag = FinAcquisition;
guilhemMBED 3:2cd58c219b89 38 }
guilhemMBED 3:2cd58c219b89 39
guilhemMBED 3:2cd58c219b89 40 RtosTimer timer(mbed::callback(calculAngle),osTimerPeriodic); // definition of rtos timer
guilhemMBED 3:2cd58c219b89 41
guilhemMBED 3:2cd58c219b89 42
guilhemMBED 3:2cd58c219b89 43 int main (void)
guilhemMBED 3:2cd58c219b89 44 {
guilhemMBED 3:2cd58c219b89 45 float AngleDeg;
guilhemMBED 3:2cd58c219b89 46 // calcul des coefficients du filtre de l'angle
guilhemMBED 3:2cd58c219b89 47 Te = Te_ms/1000;
guilhemMBED 3:2cd58c219b89 48 A = Te/(Te+Tau);
guilhemMBED 3:2cd58c219b89 49 B = Tau/(Te+Tau);
guilhemMBED 3:2cd58c219b89 50
guilhemMBED 3:2cd58c219b89 51 // Usb init
guilhemMBED 3:2cd58c219b89 52 pc.baud(115200);
guilhemMBED 3:2cd58c219b89 53 pc.printf("\r\n Debut prog\r\n");
guilhemMBED 3:2cd58c219b89 54
guilhemMBED 3:2cd58c219b89 55 // verification of connection
guilhemMBED 3:2cd58c219b89 56 if (accelero.testConnection()) {
guilhemMBED 3:2cd58c219b89 57 pc.printf("connected to MPU 6050\r\n");
guilhemMBED 3:2cd58c219b89 58 } else {
guilhemMBED 3:2cd58c219b89 59 pc.printf("No device \r\n");
guilhemMBED 3:2cd58c219b89 60 while(1); // infinite pause
guilhemMBED 3:2cd58c219b89 61 }
guilhemMBED 3:2cd58c219b89 62 wait(0.2);
guilhemMBED 3:2cd58c219b89 63
guilhemMBED 3:2cd58c219b89 64 // Scale init
guilhemMBED 3:2cd58c219b89 65 accelero.setGyroRange(MPU6050_GYRO_RANGE_2000);
guilhemMBED 3:2cd58c219b89 66 accelero.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
guilhemMBED 3:2cd58c219b89 67
guilhemMBED 3:2cd58c219b89 68 timer.start(Te_ms);
guilhemMBED 3:2cd58c219b89 69
guilhemMBED 3:2cd58c219b89 70 while(1) {
guilhemMBED 3:2cd58c219b89 71 //display of angles calculated
guilhemMBED 3:2cd58c219b89 72 if (Flag == FinAcquisition) {
guilhemMBED 3:2cd58c219b89 73 pc.printf("%f %f \r\n",AngleFiltre, AngleTan);
guilhemMBED 3:2cd58c219b89 74 Flag = PretPourAcquisition ;
guilhemMBED 3:2cd58c219b89 75 }
guilhemMBED 3:2cd58c219b89 76
guilhemMBED 3:2cd58c219b89 77 }
guilhemMBED 3:2cd58c219b89 78 }
guilhemMBED 3:2cd58c219b89 79 /*