mbed-os github
Dependencies: ADS1015 Faulhaber HTU21D_mod MS5837_potless Sensor_Head_RevB_3 USBDevice_dfu Utilsdfu beep
Fork of ARNSRS_testDFU by
Diff: main.cpp
- Revision:
- 12:7f3aadd79f89
- Parent:
- 11:b2feed92584a
- Child:
- 13:22a96dc67e85
diff -r b2feed92584a -r 7f3aadd79f89 main.cpp --- 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); } }