Diff: main.cpp

Revision:

12:7f3aadd79f89

Parent:

11:b2feed92584a

Child:

13:22a96dc67e85

--- a/main.cpp	Sun Nov 05 16:03:02 2017 +0000
+++ b/main.cpp	Thu Nov 09 15:47:49 2017 +0000
@@ -2,6 +2,7 @@
 #include <string>
 #include "Sensor_head_revB.h"
 #include "HTU21D.h"
+#include "PID.h"
 
 //Commandes des servos
 #define PWM_SERVO_POUMON PB_15
@@ -16,16 +17,23 @@
 //Sortie en mode VT100, à commenter si on veut une sortie type Arduino
 //#define VT100
 
-//Mode Delta constant entre les servos à commenter si on en veut pas
+//Mode PID, STD et DELTA, à commenter / décommenter
+//#define STD_MODE
+#define PID_MODE
 //#define DELTA_MODE
+
 float delta = 0.0f;
 
-#ifndef DELTA_MODE
-int DELTA_FLAG = 0;
+#ifdef STD_MODE
+int MODE_FLAG = 0;
 #endif
 
 #ifdef DELTA_MODE
-int DELTA_FLAG = 1;
+int MODE_FLAG = 1;
+#endif
+
+#ifdef PID_MODE
+int MODE_FLAG = 2;
 #endif
 
 #if NEED_ANDROID_OUTPUT
@@ -54,6 +62,7 @@
 char  Android[sizeAndroid];
 volatile int indexAndroid = 0;
 bool newAndroidFlag = false;
+char to_android[100];
 
 //Variables de stockage des infos capteurs
 int co2 = 0;
@@ -67,6 +76,7 @@
 Timer REAL_RATE;
 float RATE = 0;
 float RATE_TRUE = 0;
+float Ref_Time = 1.0; //La durée de la boucle désirée...
 
 //HTU21D
 HTU21D temphumid(PB_9, PB_8); //Temp humid sensor || SDA, SCL
@@ -97,11 +107,22 @@
 float Limit_min_Servo_Fuite, Limit_max_Servo_Fuite;
 float Delta_FB_2;
 
-bool FLAG_COMMAND_SERVO = false;
+#ifdef PID_MODE
+//Paramètre du PID
+float Kc = 40;
+float Ti = 0;
+float Td = 0;
+float RATE_PID = Ref_Time;
+float Commande_PID;
+int consigne = 210;
+int Max_Input = 1000;
+int Min_Input = 100;
+int Max_Output = 100;
+int Min_Output = 0;
 
-//Buffer pour Commandes servo
-char commande_servo[50];
-int indexCommande;
+//Init PID
+PID control_Servo(Kc, Ti, Td, RATE_PID);//Kc, Ti, Td, interval
+#endif
 
 //Remap d'une valeur
 float remap(float x, float in_min, float in_max, float out_min, float out_max)
@@ -110,7 +131,7 @@
 }
 
 
-//Thread d'intérogation des capteurs, positions servo et constitution de la chaine 
+//Thread d'intérogation des capteurs, positions servo et constitution de la chaine
 void Get_Info_thread()
 {
     while (true) {
@@ -133,14 +154,14 @@
         //Retour position des servos
         volet_poumon_Position = 90 - remap(FeedBack_Servo_Poumon, Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90);
         volet_fuite_Position = remap(FeedBack_Servo_Fuite, Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90);
-        
+
         //Fabrication de la chaine Date / heure
         seconds = time(NULL);
         char Time_buf[32];
         strftime(Time_buf, 32, "%D %I-%M-%S ", localtime(&seconds));
 
         //Fabrication de la chaine à enregistrer
-        sprintf(to_store,"<%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%3.2f:%3.2f:%d>",
+        sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%3.2f:%3.2f:%d",
                 Time_buf,
                 co2,
                 ppO2,
@@ -152,13 +173,18 @@
                 CellO2_2,
                 volet_poumon_Position,
                 volet_fuite_Position,
-                DELTA_FLAG);
-    }    
+                MODE_FLAG);
+          
+        if (NEED_ANDROID_OUTPUT == 1) {
+        sprintf(to_android,"<%s>",to_store);
+        }    
+              
+     }
 }
 
 void Affichage_moniteur()
 {
-#ifndef VT100    
+#ifndef VT100
     printf("\r\n");
     printf("  CO2             = %d ppm\r\n"  , co2);
     printf("  PPO2            = %d mb\r\n", ppO2);
@@ -174,13 +200,13 @@
     printf("  Volet Poumon    = %3.2f%\r\n"  , volet_poumon_Position);
     printf("  Volet Fuite     = %3.2f%\r\n"  , volet_fuite_Position);
     printf("\r\n");
-    printf("Temps d execution de la boucle = %f secondes\n", (RATE + RATE_TRUE) / 1000);
+    printf("Temps d execution de la boucle = %f seconde(s)\n", (RATE + RATE_TRUE / 1000));
     printf("\r\n", "");
     printf("A enregistrer = %s\n", to_store);
     printf("\r\n");
 #endif
 
-#ifdef VT100   
+#ifdef VT100
     printf(HOME);
     printf("\x1b[30m");
     printf("\x1b[0m\r  CO2           = \x1b[1m\x1b[K%d ppm\n", co2);
@@ -199,11 +225,11 @@
     printf("\x1b[0m\r  Volet Poumon = \x1b[1m\x1b[K%3.2f%\n", volet_poumon_Position);
     printf("\x1b[0m\r  Volet Fuite  = \x1b[1m\x1b[K%3.2f%\n", volet_fuite_Position);
     printf("\n");
-    printf("\x1b[0m\r  Temps d execution de la boucle = \x1b[1m\x1b[K%f secondes\n", (RATE + RATE_TRUE) / 1000);
+    printf("\x1b[0m\r  Temps d execution de la boucle = \x1b[1m\x1b[K%f seconde(s)\n", (RATE + RATE_TRUE) / 1000);
     printf("\r\n", "");
     printf("\x1b[0m\r  A enregistrer = \x1b[1m\x1b[K%s\n", to_store);
     printf("\r\n", "");
-#endif    
+#endif
 }
 
 //Callback de l'intérruption des envois de commandes depuis le terminal
@@ -245,33 +271,33 @@
 
     if ((char)commande == 'T') {
         set_time(valeur);
-    } else if ((char)commande == 'I'){
+    } else if ((char)commande == 'I') {
         servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f;
-        #ifdef DELTA_MODE
+#ifdef DELTA_MODE
         float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
         if(Sf >= 0 && Sf <= 90)
-        servo_fuite = Sf;
-        #endif
+            servo_fuite = Sf;
+#endif
         printf("  Servo Poumon    = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f);
-    } else if ((char)commande == 'O'){
+    } else if ((char)commande == 'O') {
         servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f;
-        printf("  Servo Fuite     =  %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f);    
+        printf("  Servo Fuite     =  %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f);
     } else if ((char)commande == 'D') {
         delta = valeur;
-        #ifdef DELTA_MODE
+#ifdef DELTA_MODE
         float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
         if(Sf >= 0 && Sf <= 90)
-        servo_fuite = Sf;
-        #endif
+            servo_fuite = Sf;
+#endif
     } else if ((char)commande == 'R') {
         NVIC_SystemReset();
-    /////////////////////////////////////////    
-    //Pour rajouter une commande    
-    //} else if ((char)commande == 'X') {
-    //  attribuer à une VARIABLE = valeur;
-    //  ou une action, avec ou sans valeur
-    /////////////////////////////////////////   
-    }else {
+        /////////////////////////////////////////
+        //Pour rajouter une commande
+        //} else if ((char)commande == 'X') {
+        //  attribuer à une VARIABLE = valeur;
+        //  ou une action, avec ou sans valeur
+        /////////////////////////////////////////
+    } else {
         sensors.cozirSend(message);
     }
 
@@ -290,36 +316,51 @@
 
     if ((char)commande == 'T') {
         set_time(valeur);
-    } else if ((char)commande == 'I'){
+    } else if ((char)commande == 'I') {
         servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f;
-        #ifdef DELTA_MODE
+#ifdef DELTA_MODE
         float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
         if(Sf >= 0 && Sf <= 90)
-        servo_fuite = Sf;
-        #endif
+            servo_fuite = Sf;
+#endif
         printf("  Servo Poumon    = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f);
-    } else if ((char)commande == 'O'){
+    } else if ((char)commande == 'O') {
         servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f;
         printf("  Servo Fuite     =  %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f);
     } else if ((char)commande == 'D') {
         delta = valeur;
-        #ifdef DELTA_MODE
+#ifdef DELTA_MODE
         float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
         if(Sf >= 0 && Sf <= 90)
-        servo_fuite = Sf;
-        #endif
-    /////////////////////////////////////////    
-    //Pour rajouter une commande    
-    //} else if ((char)commande == 'X') {
-    //  attribuer à une VARIABLE = valeur;
-    //  ou une action, avec ou sans valeur
-    /////////////////////////////////////////          
+            servo_fuite = Sf;
+#endif
+        /////////////////////////////////////////
+        //Pour rajouter une commande
+        //} else if ((char)commande == 'X') {
+        //  attribuer à une VARIABLE = valeur;
+        //  ou une action, avec ou sans valeur
+        /////////////////////////////////////////
     } else if ((char)commande == 'R') {
         NVIC_SystemReset();
-    } else {
-        sensors.cozirSend(message);
     }
-
+#ifdef PID_MODE
+    else if ((char)commande == 'p') {
+        Kc = (float)valeur;
+        control_Servo.setTunings(Kc, Ti, Td);
+        printf("  UPDATE PID -->  Kc = %f   Ti = %f   Td = %f\r\n", Kc, Ti, Td);
+    }else if ((char)commande == 'i') {
+        Ti = (float)valeur;
+        control_Servo.setTunings(Kc, Ti, Td);
+        printf("  UPDATE PID -->  Kc = %f   Ti = %f   Td = %f\r\n", Kc, Ti, Td);
+    }else if ((char)commande == 'd') {
+        Td = (float)valeur;
+        control_Servo.setTunings(Kc, Ti, Td);
+        printf("  UPDATE PID -->  Kc = %f   Ti = %f   Td = %f\r\n", Kc, Ti, Td);
+    }else if ((char)commande == 'c') {
+        consigne = valeur;
+        printf("  UPDATE CONSIGNE PID -->  Consigne = %d\r\n", consigne);
+    }
+#endif    
     //wait_ms(100);
     strcpy(Android," ");
     indexAndroid = 0;
@@ -373,20 +414,20 @@
     printf("  Delta Servo 2 = %f\n", Delta_FB_2);
 
     if(Delta_FB_1 > 10 || Delta_FB_2 > 10)  printf("  Delta Servos non satisfaisant...\r\n\r\n");
-    
+
 #ifdef DELTA_MODE
     //Position initial delta
     servo_poumon = 0.5;
     servo_fuite = 0.5 + remap(delta, 0, 90, 0, 100) / 100.f;
-#endif        
+#endif
 }
 
 int main()
-{   
+{
     //UNIX TIMESTAMP depuis le erminal MAC =       date +%s         + 7200 pour heure d'été.....
-    
+
     Calibration_servo();
-    
+
     sensors.Sensors_INIT(false, true, 5, SPOOLING, DIGI_FILTER32, CALIB_AIR);
 
     serialMonit.attach(&callbackParam, Serial::RxIrq);
@@ -398,7 +439,20 @@
     thread.start(Get_Info_thread);
 
     thread.set_priority(osPriorityRealtime);
-    
+
+#ifdef PID_MODE
+    //Init PID
+    //Entrée PPO2 entre 100 et 1000 mb
+    control_Servo.setInputLimits(Min_Input, Max_Input);
+    //Sortie servo entre 0 et 100 %
+    control_Servo.setOutputLimits(Min_Output, Max_Output);
+    //Mode manuel au démarrage
+    control_Servo.setMode(AUTO_MODE);
+    //Consigne à x mb
+    control_Servo.setSetPoint(consigne);
+
+#endif 
+   
 #ifdef VT100
     printf(CLS);
 #endif
@@ -422,20 +476,31 @@
 
         //Vers Android
         if (NEED_ANDROID_OUTPUT == 1) {
-            ANDROID(to_store);
+            ANDROID(to_android);
         }
-
+        
         sensors.Write_SD((string)to_store);
-
+        
+#ifdef PID_MODE 
+            //Adéquation du RATE du PID avec la contrainte du temps du loop
+            control_Servo.setInterval(RATE_PID);
+            //Update du PID
+            control_Servo.setProcessValue(ppO2);
+            //Nouvelle sortie servo
+            Commande_PID = control_Servo.compute();
+            //Appliquer la consigne
+            servo_fuite = Commande_PID / 100.0f;
+#endif
+        
         //Arrêt du Timer mesurant le temps d'éxecution du code
         REAL_RATE.stop();
         //Définition de la nouvelle valeur du temps d'échantillonage du PID.
         RATE = REAL_RATE.read();
         //Reset du Timer
         REAL_RATE.reset();
-        
+
         //Pour ralentir le code à 1 seconde fixe quelque soit les intéruptions du loop....
-        RATE_TRUE = (1000 - (RATE / 1000));
+        RATE_TRUE = (Ref_Time - RATE) * 1000;
         wait_ms(RATE_TRUE);
     }
 }