Diff: main.cpp

Revision:

10:aca745a66d51

Parent:

9:04bfdfc029cb

Child:

11:b2feed92584a

--- a/main.cpp	Tue Oct 03 15:55:23 2017 +0000
+++ b/main.cpp	Thu Nov 02 10:39:53 2017 +0000
@@ -3,10 +3,18 @@
 #include "Sensor_head_revB.h"
 #include "HTU21D.h"
 
+//Commandes des servos
+#define PWM_SERVO_POUMON PB_15
+#define PWM_SERVO_FUITE PB_1
+//Retour des servos
+#define FEED_BACK_SERVO_POUMON PC_2
+#define FEED_BACK_SERVO_FUITE PC_3
 
 //Ecrit dans le moniteur série de la tablette à 9600 bds si sur 1, penser à mettre NEED_CONSOLE_OUTPUT à 0
 #define NEED_ANDROID_OUTPUT 1
 
+//Sortie en mode VT100, à commenter si on veut une sortie type Arduino
+//#define VT100
 
 #if NEED_ANDROID_OUTPUT
 #define ANDROID(...) { android.printf(__VA_ARGS__); }
@@ -16,6 +24,7 @@
 
 //Moniteur série
 Serial serialMonit(USBTX,USBRX,9600);
+
 //COM Série vers Android, Serial 3
 Serial android(PC_10,PC_11,9600);
 
@@ -62,7 +71,34 @@
 //Thread d'intérogation des capteurs
 Thread thread;
 
-void Sensors_thread()
+//Contrôle des servos
+PwmOut servo_poumon(PWM_SERVO_POUMON);
+AnalogIn FeedBack_Servo_Poumon(FEED_BACK_SERVO_POUMON);
+float volet_poumon_Position;
+float Limit_min_Servo_Poumon, Limit_max_Servo_Poumon;
+float Delta_FB_1;
+
+PwmOut servo_fuite(PWM_SERVO_FUITE);
+AnalogIn FeedBack_Servo_Fuite(FEED_BACK_SERVO_FUITE);
+float volet_fuite_Position;
+float Limit_min_Servo_Fuite, Limit_max_Servo_Fuite;
+float Delta_FB_2;
+
+bool FLAG_COMMAND_SERVO = false;
+
+//Buffer pour Commandes servo
+char commande_servo[50];
+int indexCommande;
+
+//Remap d'une valeur
+float remap(float x, float in_min, float in_max, float out_min, float out_max)
+{
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+
+//Thread d'intérogation des capteurs, positions servo et constitution de la chaine 
+void Get_Info_thread()
 {
     while (true) {
 
@@ -81,13 +117,17 @@
         Temp2 = temphumid.sample_ctemp();
         Humid = temphumid.sample_humid();
 
+        //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>",
+        sprintf(to_store,"<%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%3.2f:%3.2f>",
                 Time_buf,
                 co2,
                 ppO2,
@@ -96,30 +136,37 @@
                 Temp2,
                 Humid,
                 CellO2_1,
-                CellO2_2);
-    }
+                CellO2_2,
+                volet_poumon_Position,
+                volet_fuite_Position);
+    }    
 }
 
 void Affichage_moniteur()
 {
+#ifndef VT100    
     printf("\r\n");
     printf("  CO2             = %d ppm\r\n"  , co2);
     printf("  PPO2            = %d mb\r\n", ppO2);
     printf("  Pression        = %f msw\r\n", pression);
-    printf("  Temperature 1   = %f C\r\n", Temp1);
-    printf("  Temperature 2   = %f C\n\r", Temp2);
+    printf("  Temp MS5837     = %f C\r\n", Temp1);
+    printf("  Temp HTU21D     = %f C\n\r", Temp2);
     printf("  Humidity        = %d %%\n\r", Humid);
     printf("\n\r");
     printf("  Cell O2 n 1     = %d\r\n"  , CellO2_1);
     printf("  Cell O2 n 2     = %d\r\n"  , CellO2_2);
     printf("\r\n");
     printf("\n");
+    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);
     printf("\r\n", "");
     printf("A enregistrer = %s\n", to_store);
     printf("\r\n");
+#endif
 
-    /*
+#ifdef VT100   
     printf(HOME);
     printf("\x1b[30m");
     printf("\x1b[0m\r  CO2           = \x1b[1m\x1b[K%d ppm\n", co2);
@@ -127,19 +174,22 @@
     printf("\n");
     printf("\x1b[0m\r  Pression      = \x1b[1m\x1b[K%.2f msw\n", pression);
     printf("\n");
-    printf("\x1b[0m\r  Temperature 1 = \x1b[1m\x1b[K%.2f C\n", Temp1);
-    printf("\x1b[0m\r  Temperature 2 = \x1b[1m\x1b[K%.2f C\n", Temp2);
+    printf("\x1b[0m\r  Temp MS5837   = \x1b[1m\x1b[K%.2f C\n", Temp1);
+    printf("\x1b[0m\r  Temp HTU21D   = \x1b[1m\x1b[K%.2f C\n", Temp2);
     printf("\n");
     printf("\x1b[0m\r  Humidity      = \x1b[1m\x1b[K%d %\n", Humid);
     printf("\n");
     printf("\x1b[0m\r  Cell O2 n 1  = \x1b[1m\x1b[K%d\n", CellO2_1);
     printf("\x1b[0m\r  Cell O2 n 2  = \x1b[1m\x1b[K%d\n", CellO2_2);
     printf("\n");
+    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);
     printf("\r\n", "");
     printf("\x1b[0m\r  A enregistrer = \x1b[1m\x1b[K%s\n", to_store);
     printf("\r\n", "");
-    */
+#endif    
 }
 
 //Callback de l'intérruption des envois de commandes au Cozir
@@ -181,7 +231,15 @@
 
     if ((char)commande == 'T') {
         set_time(valeur);
-    } else {
+    } else if ((char)commande == 'P'){
+        servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f;
+        printf("  Servo Poumon    = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f);
+    } else if ((char)commande == 'F'){
+        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 == 'R') {
+        NVIC_SystemReset();
+    }else {
         sensors.cozirSend(message);
     }
 
@@ -200,6 +258,12 @@
 
     if ((char)commande == 'T') {
         set_time(valeur);
+    } else if ((char)commande == 'P'){
+        servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f;
+        printf("  Servo Poumon    = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f);
+    } else if ((char)commande == 'F'){
+        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 == 'R') {
         NVIC_SystemReset();
     }
@@ -210,11 +274,62 @@
     newAndroidFlag = false;
 }
 
+//Calibration des limites de feedback des servos
+void Calibration_servo()
+{
+    //Servos, calibration du feedback
+    printf("  Check Servos :\r\n");
+    printf("\r\n");
+    servo_poumon.period(0.001); // à mettre dans le setup si cette calib n'est pas appelée (je l ai trouvée commentée)
+    servo_fuite.period(0.001);
+    //Rentrer les servos à fond
+    servo_poumon = 0;
+    servo_fuite = 0;
+    wait(4);
+
+    Limit_min_Servo_Poumon = FeedBack_Servo_Poumon.read();
+    wait_ms(100);
+    Limit_min_Servo_Fuite = FeedBack_Servo_Fuite.read();
+
+    printf("  FeedBack Servo POUMON min = %f\n", Limit_min_Servo_Poumon);
+    printf("  FeedBack Servo FUITE  min = %f\n", Limit_min_Servo_Fuite);
+    printf("\r\n");
+
+    //Sortir les servos à fond
+    servo_poumon = 1;
+    servo_fuite = 1;
+    wait(4);
+
+    Limit_max_Servo_Poumon = FeedBack_Servo_Poumon.read();
+    wait_ms(100);
+    Limit_max_Servo_Fuite = FeedBack_Servo_Fuite.read();
+
+    printf("  FeedBack Servo POUMON Max = %f\n", Limit_max_Servo_Poumon);
+    printf("  FeedBack Servo FUITE  Max = %f\n", Limit_max_Servo_Fuite);
+    printf("\r\n");
+
+    //Position milieu
+    servo_poumon = 0.5;
+    servo_fuite = 0.5;
+    wait(4);
+
+    //Mesure du delta consigne / feedback
+    Delta_FB_1 = abs(remap(FeedBack_Servo_Poumon.read(), Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90) - 45);
+    Delta_FB_2 = abs(remap(FeedBack_Servo_Fuite.read(), Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90) - 45);
+
+    printf("  Delta Servo 1 = %f\n", Delta_FB_1);
+    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");
+}
+
 int main()
 {
+    
     //UNIX TIMESTAMP depuis le erminal MAC =       date +%s         + 7200 pour heure d'été.....
-
-    sensors.Sensors_INIT(false, true, 5, SPOOLING, DIGI_FILTER32, CALIB_AIR);
+    Calibration_servo();
+    
+    sensors.Sensors_INIT(true, true, 5, SPOOLING, DIGI_FILTER32, CALIB_AIR);
 
     serialMonit.attach(&callbackParam, Serial::RxIrq);
 
@@ -222,7 +337,7 @@
 
     serialMonit.printf("  Demarrage...\r\n\r\n  Entrez les comandes COZIR si besoin :\r\n");
 
-    thread.start(Sensors_thread);
+    thread.start(Get_Info_thread);
 
     thread.set_priority(osPriorityRealtime);