Revision 5:0d84191fde21, committed 2020-02-07

Comitter:

guilhemMBED

Date:

Fri Feb 07 07:32:50 2020 +0000

Parent:

4:9b54c1ccc703

Commit message:

gyroscope prgm

Changed in this revision

diff -r 9b54c1ccc703 -r 0d84191fde21 main.cpp
--- a/main.cpp	Mon Feb 03 13:43:24 2020 +0000
+++ b/main.cpp	Fri Feb 07 07:32:50 2020 +0000
@@ -8,50 +8,136 @@
 #define PretPourAcquisition  0
 
 Serial pc(USBTX, USBRX);     // USB initialization
+Serial HC06(D1,D0);          // TX - RX du microcontroler
 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
+PwmOut Mot_G(D12);
+PwmOut Mot_D(D10);
+
+DigitalOut SensMotG(D11);
+DigitalOut SensMotD(D9);
+
+struct Trame {
+    float kp;
+    float kd;
+    float val3;
+    float val4;
+    float val5;
+    float val6;
+    float val7;
+};
+
+Trame trame;
+
+
+// filtre compléméntaire
+float Te_ms = 5, Te, Tau = 0.1,A,B;  
+int gx, gz, OmegaY;
+float AngleTan,AngleFiltre=0,AngleNonFiltre=0; 
+char Flag=PretPourAcquisition;
 
-char Flag=PretPourAcquisition;
+//asservissement angulaire
+float Kp=0.01,Kd=0.05;
+float Alpha, Erreur;
+float AngleOffset=0, AngleCons=0;
+
+//asservissement angle consigne 
+float TauA=0.1,KpA=0.1;
+
+
+void rotationMoteur(float motG, float motD)
+{
+    Mot_D.period_us(40);
+    Mot_G.period_us(40);
+    
+    if (motG < 0) {
+        SensMotG = 1;
+        motG = 1 + motG;
+    } else SensMotG = 0;
+
+    if (motD<0) {
+        SensMotD = 1;
+        motD = 1 + motD;
+    } else SensMotD = 0;
+
+    Mot_G.write(motG);
+    Mot_D.write(motD);
+}
 
 void calculAngle(void)
 {
-    int gx, gz, OmegaY;
-    
-    // acq de gx et gz et calcul de AngleTan
+    // acq de l'acc linéaire (G) et calcul de AngleTan
     gx = accelero.getAcceleroRawX();
     gz = accelero.getAcceleroRawZ();
     AngleTan = atan2((double)gx,(double)gz);
-    
-    // acq de OmegaY
-    OmegaY = accelero.getGyroRawY()*Tau/1000;
+
+    // acq de vitesse angulaire
+    OmegaY = accelero.getGyroRawY();
 
     // calcul AngleNonFiltre
-    AngleNonFiltre = OmegaY + AngleTan;
+    AngleNonFiltre = OmegaY*Tau/1000 + AngleTan;
 
     // filtre passe bas numérique
     AngleFiltre = A*AngleNonFiltre+B*AngleFiltre;
-    
+
     Flag = FinAcquisition;
 }
 
-RtosTimer timer(mbed::callback(calculAngle),osTimerPeriodic); // definition of rtos timer 
+RtosTimer timer(mbed::callback(calculAngle),osTimerPeriodic); // definition of rtos timer
+
+void asservissmentAngulaire(void)
+{ 
+    Erreur = AngleCons + AngleOffset - AngleFiltre; // calcul erreur
+    Alpha = Kp*AngleFiltre + Kd*OmegaY;              // proportionel + derivé
+    
+    // saturation 
+    if (Alpha < -0.35)  Alpha = -0.35;
+    if (Alpha > 0.35)   Alpha = 0.35;
+    
+    rotationMoteur(Alpha,Alpha);
+}
+
+
+void receptionBluetooth(void)
+{
+    int i=0;
+    char val[500];
 
+    // stockage de la trame 
+    val[0]= HC06.getc();
+    while(val[i]!='\n') {
+        i++;
+        val[i]= HC06.getc();
+    }
+
+    sscanf(val,"%f:%f:%f:%f:%f:%f:%f",
+                       &trame.kp,
+                       &trame.kd,
+                       &trame.val3,
+                       &trame.val4,
+                       &trame.val5,
+                       &trame.val6,
+                       &trame.val7);
+                       
+    // mise à jour des variables
+    Kp = trame.kp  ;
+    Kd = trame.kd ;
+}
 
 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");
 
+    // bluetooth init
+    HC06.baud(9600);
+
     // verification of connection
     if (accelero.testConnection()) {
         pc.printf("connected to MPU 6050\r\n");
@@ -60,20 +146,37 @@
         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 (HC06.readable()) {
+            receptionBluetooth();
+            /* Affichage valeur recues
+            pc.printf("\r\nkp %f   kd %f   val3 %f   val4 %f   val5 %f   val6 %f   val7 %f",
+                      trame.kp,
+                      trame.kd,
+                      trame.val3,
+                      trame.val4,
+                      trame.val5,
+                      trame.val6,
+                      trame.val7);*/
+        }
+
+        // Nouvelle valeur d'angle
         if (Flag == FinAcquisition) {
-            pc.printf("%f %f \r\n",AngleFiltre, AngleTan);
+            asservissmentAngulaire();
+            // pc.printf("%f %f \r\n",AngleFiltre, AngleTan);
             Flag = PretPourAcquisition ;
         }
-       
+
+        rotationMoteur(0,0);
+
     }
 }
-/*
+