guilhem mathieux / Mbed 2 deprecated GYRO_MPU6050

changed adress

Dependencies:   mbed MPU6050 mbed-rtos

main.cpp@5:0d84191fde21, 2020-02-07 (annotated)

Committer:
guilhemMBED
Date:
Fri Feb 07 07:32:50 2020 +0000
Revision:
5:0d84191fde21
Parent:
3:2cd58c219b89 
gyroscope prgm

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 5:0d84191fde21 11 Serial HC06(D1,D0); // TX - RX du microcontroler
guilhemMBED 3:2cd58c219b89 12 MPU6050 accelero(D4, D5); // MPU6050 library initialization
guilhemMBED 3:2cd58c219b89 13
guilhemMBED 5:0d84191fde21 14 PwmOut Mot_G(D12);
guilhemMBED 5:0d84191fde21 15 PwmOut Mot_D(D10);
guilhemMBED 5:0d84191fde21 16
guilhemMBED 5:0d84191fde21 17 DigitalOut SensMotG(D11);
guilhemMBED 5:0d84191fde21 18 DigitalOut SensMotD(D9);
guilhemMBED 5:0d84191fde21 19
guilhemMBED 5:0d84191fde21 20 struct Trame {
guilhemMBED 5:0d84191fde21 21 float kp;
guilhemMBED 5:0d84191fde21 22 float kd;
guilhemMBED 5:0d84191fde21 23 float val3;
guilhemMBED 5:0d84191fde21 24 float val4;
guilhemMBED 5:0d84191fde21 25 float val5;
guilhemMBED 5:0d84191fde21 26 float val6;
guilhemMBED 5:0d84191fde21 27 float val7;
guilhemMBED 5:0d84191fde21 28 };
guilhemMBED 5:0d84191fde21 29
guilhemMBED 5:0d84191fde21 30 Trame trame;
guilhemMBED 5:0d84191fde21 31
guilhemMBED 5:0d84191fde21 32
guilhemMBED 5:0d84191fde21 33 // filtre compléméntaire
guilhemMBED 5:0d84191fde21 34 float Te_ms = 5, Te, Tau = 0.1,A,B;
guilhemMBED 5:0d84191fde21 35 int gx, gz, OmegaY;
guilhemMBED 5:0d84191fde21 36 float AngleTan,AngleFiltre=0,AngleNonFiltre=0;
guilhemMBED 5:0d84191fde21 37 char Flag=PretPourAcquisition;
guilhemMBED 3:2cd58c219b89 38
guilhemMBED 5:0d84191fde21 39 //asservissement angulaire
guilhemMBED 5:0d84191fde21 40 float Kp=0.01,Kd=0.05;
guilhemMBED 5:0d84191fde21 41 float Alpha, Erreur;
guilhemMBED 5:0d84191fde21 42 float AngleOffset=0, AngleCons=0;
guilhemMBED 5:0d84191fde21 43
guilhemMBED 5:0d84191fde21 44 //asservissement angle consigne
guilhemMBED 5:0d84191fde21 45 float TauA=0.1,KpA=0.1;
guilhemMBED 5:0d84191fde21 46
guilhemMBED 5:0d84191fde21 47
guilhemMBED 5:0d84191fde21 48 void rotationMoteur(float motG, float motD)
guilhemMBED 5:0d84191fde21 49 {
guilhemMBED 5:0d84191fde21 50 Mot_D.period_us(40);
guilhemMBED 5:0d84191fde21 51 Mot_G.period_us(40);
guilhemMBED 5:0d84191fde21 52
guilhemMBED 5:0d84191fde21 53 if (motG < 0) {
guilhemMBED 5:0d84191fde21 54 SensMotG = 1;
guilhemMBED 5:0d84191fde21 55 motG = 1 + motG;
guilhemMBED 5:0d84191fde21 56 } else SensMotG = 0;
guilhemMBED 5:0d84191fde21 57
guilhemMBED 5:0d84191fde21 58 if (motD<0) {
guilhemMBED 5:0d84191fde21 59 SensMotD = 1;
guilhemMBED 5:0d84191fde21 60 motD = 1 + motD;
guilhemMBED 5:0d84191fde21 61 } else SensMotD = 0;
guilhemMBED 5:0d84191fde21 62
guilhemMBED 5:0d84191fde21 63 Mot_G.write(motG);
guilhemMBED 5:0d84191fde21 64 Mot_D.write(motD);
guilhemMBED 5:0d84191fde21 65 }
guilhemMBED 3:2cd58c219b89 66
guilhemMBED 3:2cd58c219b89 67 void calculAngle(void)
guilhemMBED 3:2cd58c219b89 68 {
guilhemMBED 5:0d84191fde21 69 // acq de l'acc linéaire (G) et calcul de AngleTan
guilhemMBED 3:2cd58c219b89 70 gx = accelero.getAcceleroRawX();
guilhemMBED 3:2cd58c219b89 71 gz = accelero.getAcceleroRawZ();
guilhemMBED 3:2cd58c219b89 72 AngleTan = atan2((double)gx,(double)gz);
guilhemMBED 5:0d84191fde21 73
guilhemMBED 5:0d84191fde21 74 // acq de vitesse angulaire
guilhemMBED 5:0d84191fde21 75 OmegaY = accelero.getGyroRawY();
guilhemMBED 3:2cd58c219b89 76
guilhemMBED 3:2cd58c219b89 77 // calcul AngleNonFiltre
guilhemMBED 5:0d84191fde21 78 AngleNonFiltre = OmegaY*Tau/1000 + AngleTan;
guilhemMBED 3:2cd58c219b89 79
guilhemMBED 3:2cd58c219b89 80 // filtre passe bas numérique
guilhemMBED 3:2cd58c219b89 81 AngleFiltre = A*AngleNonFiltre+B*AngleFiltre;
guilhemMBED 5:0d84191fde21 82
guilhemMBED 3:2cd58c219b89 83 Flag = FinAcquisition;
guilhemMBED 3:2cd58c219b89 84 }
guilhemMBED 3:2cd58c219b89 85
guilhemMBED 5:0d84191fde21 86 RtosTimer timer(mbed::callback(calculAngle),osTimerPeriodic); // definition of rtos timer
guilhemMBED 5:0d84191fde21 87
guilhemMBED 5:0d84191fde21 88 void asservissmentAngulaire(void)
guilhemMBED 5:0d84191fde21 89 {
guilhemMBED 5:0d84191fde21 90 Erreur = AngleCons + AngleOffset - AngleFiltre; // calcul erreur
guilhemMBED 5:0d84191fde21 91 Alpha = Kp*AngleFiltre + Kd*OmegaY; // proportionel + derivé
guilhemMBED 5:0d84191fde21 92
guilhemMBED 5:0d84191fde21 93 // saturation
guilhemMBED 5:0d84191fde21 94 if (Alpha < -0.35) Alpha = -0.35;
guilhemMBED 5:0d84191fde21 95 if (Alpha > 0.35) Alpha = 0.35;
guilhemMBED 5:0d84191fde21 96
guilhemMBED 5:0d84191fde21 97 rotationMoteur(Alpha,Alpha);
guilhemMBED 5:0d84191fde21 98 }
guilhemMBED 5:0d84191fde21 99
guilhemMBED 5:0d84191fde21 100
guilhemMBED 5:0d84191fde21 101 void receptionBluetooth(void)
guilhemMBED 5:0d84191fde21 102 {
guilhemMBED 5:0d84191fde21 103 int i=0;
guilhemMBED 5:0d84191fde21 104 char val[500];
guilhemMBED 3:2cd58c219b89 105
guilhemMBED 5:0d84191fde21 106 // stockage de la trame
guilhemMBED 5:0d84191fde21 107 val[0]= HC06.getc();
guilhemMBED 5:0d84191fde21 108 while(val[i]!='\n') {
guilhemMBED 5:0d84191fde21 109 i++;
guilhemMBED 5:0d84191fde21 110 val[i]= HC06.getc();
guilhemMBED 5:0d84191fde21 111 }
guilhemMBED 5:0d84191fde21 112
guilhemMBED 5:0d84191fde21 113 sscanf(val,"%f:%f:%f:%f:%f:%f:%f",
guilhemMBED 5:0d84191fde21 114 &trame.kp,
guilhemMBED 5:0d84191fde21 115 &trame.kd,
guilhemMBED 5:0d84191fde21 116 &trame.val3,
guilhemMBED 5:0d84191fde21 117 &trame.val4,
guilhemMBED 5:0d84191fde21 118 &trame.val5,
guilhemMBED 5:0d84191fde21 119 &trame.val6,
guilhemMBED 5:0d84191fde21 120 &trame.val7);
guilhemMBED 5:0d84191fde21 121
guilhemMBED 5:0d84191fde21 122 // mise à jour des variables
guilhemMBED 5:0d84191fde21 123 Kp = trame.kp ;
guilhemMBED 5:0d84191fde21 124 Kd = trame.kd ;
guilhemMBED 5:0d84191fde21 125 }
guilhemMBED 3:2cd58c219b89 126
guilhemMBED 3:2cd58c219b89 127 int main (void)
guilhemMBED 3:2cd58c219b89 128 {
guilhemMBED 3:2cd58c219b89 129 // calcul des coefficients du filtre de l'angle
guilhemMBED 3:2cd58c219b89 130 Te = Te_ms/1000;
guilhemMBED 3:2cd58c219b89 131 A = Te/(Te+Tau);
guilhemMBED 3:2cd58c219b89 132 B = Tau/(Te+Tau);
guilhemMBED 5:0d84191fde21 133
guilhemMBED 3:2cd58c219b89 134 // Usb init
guilhemMBED 3:2cd58c219b89 135 pc.baud(115200);
guilhemMBED 3:2cd58c219b89 136 pc.printf("\r\n Debut prog\r\n");
guilhemMBED 3:2cd58c219b89 137
guilhemMBED 5:0d84191fde21 138 // bluetooth init
guilhemMBED 5:0d84191fde21 139 HC06.baud(9600);
guilhemMBED 5:0d84191fde21 140
guilhemMBED 3:2cd58c219b89 141 // verification of connection
guilhemMBED 3:2cd58c219b89 142 if (accelero.testConnection()) {
guilhemMBED 3:2cd58c219b89 143 pc.printf("connected to MPU 6050\r\n");
guilhemMBED 3:2cd58c219b89 144 } else {
guilhemMBED 3:2cd58c219b89 145 pc.printf("No device \r\n");
guilhemMBED 3:2cd58c219b89 146 while(1); // infinite pause
guilhemMBED 3:2cd58c219b89 147 }
guilhemMBED 3:2cd58c219b89 148 wait(0.2);
guilhemMBED 5:0d84191fde21 149
guilhemMBED 3:2cd58c219b89 150 // Scale init
guilhemMBED 3:2cd58c219b89 151 accelero.setGyroRange(MPU6050_GYRO_RANGE_2000);
guilhemMBED 3:2cd58c219b89 152 accelero.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
guilhemMBED 5:0d84191fde21 153
guilhemMBED 3:2cd58c219b89 154 timer.start(Te_ms);
guilhemMBED 5:0d84191fde21 155
guilhemMBED 3:2cd58c219b89 156 while(1) {
guilhemMBED 5:0d84191fde21 157
guilhemMBED 5:0d84191fde21 158 if (HC06.readable()) {
guilhemMBED 5:0d84191fde21 159 receptionBluetooth();
guilhemMBED 5:0d84191fde21 160 /* Affichage valeur recues
guilhemMBED 5:0d84191fde21 161 pc.printf("\r\nkp %f kd %f val3 %f val4 %f val5 %f val6 %f val7 %f",
guilhemMBED 5:0d84191fde21 162 trame.kp,
guilhemMBED 5:0d84191fde21 163 trame.kd,
guilhemMBED 5:0d84191fde21 164 trame.val3,
guilhemMBED 5:0d84191fde21 165 trame.val4,
guilhemMBED 5:0d84191fde21 166 trame.val5,
guilhemMBED 5:0d84191fde21 167 trame.val6,
guilhemMBED 5:0d84191fde21 168 trame.val7);*/
guilhemMBED 5:0d84191fde21 169 }
guilhemMBED 5:0d84191fde21 170
guilhemMBED 5:0d84191fde21 171 // Nouvelle valeur d'angle
guilhemMBED 3:2cd58c219b89 172 if (Flag == FinAcquisition) {
guilhemMBED 5:0d84191fde21 173 asservissmentAngulaire();
guilhemMBED 5:0d84191fde21 174 // pc.printf("%f %f \r\n",AngleFiltre, AngleTan);
guilhemMBED 3:2cd58c219b89 175 Flag = PretPourAcquisition ;
guilhemMBED 3:2cd58c219b89 176 }
guilhemMBED 5:0d84191fde21 177
guilhemMBED 5:0d84191fde21 178 rotationMoteur(0,0);
guilhemMBED 5:0d84191fde21 179
guilhemMBED 3:2cd58c219b89 180 }
guilhemMBED 3:2cd58c219b89 181 }
guilhemMBED 5:0d84191fde21 182