![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
mbed-os github
Dependencies: ADS1015 Faulhaber HTU21D_mod MS5837_potless Sensor_Head_RevB_3 USBDevice_dfu Utilsdfu beep
Fork of ARNSRS_testDFU by
main.cpp
- Committer:
- POTLESS_2
- Date:
- 2017-11-09
- Revision:
- 12:7f3aadd79f89
- Parent:
- 11:b2feed92584a
- Child:
- 13:22a96dc67e85
File content as of revision 12:7f3aadd79f89:
#include "mbed.h" #include <string> #include "Sensor_head_revB.h" #include "HTU21D.h" #include "PID.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 //Mode PID, STD et DELTA, à commenter / décommenter //#define STD_MODE #define PID_MODE //#define DELTA_MODE float delta = 0.0f; #ifdef STD_MODE int MODE_FLAG = 0; #endif #ifdef DELTA_MODE int MODE_FLAG = 1; #endif #ifdef PID_MODE int MODE_FLAG = 2; #endif #if NEED_ANDROID_OUTPUT #define ANDROID(...) { android.printf(__VA_ARGS__); } #else #define ANDROID(...) #endif //Moniteur série Serial serialMonit(USBTX,USBRX,9600); //COM Série vers Android, Serial 3 Serial android(PC_10,PC_11,9600); //Init de la lib ARNSRS; SENSOR_HEAD_REV_B sensors; //pour Param Cozir const int sizeParam = 20; char param[sizeParam]; volatile int indexParam = 0; bool newParamFlag = false; //pour Commandes Android const int sizeAndroid = 20; char Android[sizeAndroid]; volatile int indexAndroid = 0; bool newAndroidFlag = false; char to_android[100]; //Variables de stockage des infos capteurs int co2 = 0; float pression = 0; float Temp1 = 0; int ppO2 = 0; int CellO2_1 = 0; int CellO2_2 = 0; //Mesure du tempsd'éxecution du loop 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 float Temp2; int Humid; //Data LOG char to_store[50]; time_t seconds; //VT100 static const char CLS[] = "\x1B[2J"; static const char HOME[] = "\x1B[H"; //Thread d'intérogation des capteurs Thread 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; #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; //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) { 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) { //CO2 sur Cozir co2 = sensors.requestCO2(); //P / T sur MS5837 pression = sensors.requestPress(); Temp1 = sensors.requestTemp(); //PPO2 sur ADS1015 ppO2 = sensors.requestPpO2(); //Cell O2 en mV CellO2_1 = sensors.requestCellO2_1(); CellO2_2 = sensors.requestCellO2_2(); //HTU21D 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:%3.2f:%3.2f:%d", Time_buf, co2, ppO2, pression, Temp1, Temp2, Humid, CellO2_1, CellO2_2, volet_poumon_Position, volet_fuite_Position, MODE_FLAG); if (NEED_ANDROID_OUTPUT == 1) { sprintf(to_android,"<%s>",to_store); } } } 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(" 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 seconde(s)\n", (RATE + RATE_TRUE / 1000)); 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); printf("\x1b[0m\r PPO2 = \x1b[1m\x1b[K%d mb\n", ppO2); printf("\n"); printf("\x1b[0m\r Pression = \x1b[1m\x1b[K%.2f msw\n", pression); printf("\n"); 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 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 } //Callback de l'intérruption des envois de commandes depuis le terminal void callbackParam() { while(serialMonit.readable()) { if (indexParam == sizeParam) //éviter la saturation du buffer serialMonit.getc(); else param [indexParam++] = serialMonit.getc();//chargement du buffer dans le message if ((indexParam == sizeParam) || (param[indexParam -1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut... param[indexParam] = 0; newParamFlag = true; } } } //Callback de l'intérruption des envois de commandes depuis Android void callbackAndroid() { while(android.readable()) { if (indexAndroid == sizeAndroid) //éviter la saturation du buffer android.getc(); else Android [indexAndroid++] = android.getc();//chargement du buffer dans le message if ((indexAndroid == sizeAndroid) || (Android[indexAndroid -1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut... Android[indexAndroid] = 0; newAndroidFlag = true; } } } void Decoding_Message(char message []) { char *commande = 0; int valeur = 0; sscanf(message,"%s %d",&commande , &valeur); if ((char)commande == 'T') { set_time(valeur); } else if ((char)commande == 'I') { servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f; #ifdef DELTA_MODE float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f; if(Sf >= 0 && Sf <= 90) servo_fuite = Sf; #endif printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f); } 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 float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f; if(Sf >= 0 && Sf <= 90) 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 { sensors.cozirSend(message); } //wait_ms(100); strcpy(param," "); indexParam = 0; newParamFlag = false; } void Decoding_Message_Android(char message []) { char *commande = 0; int valeur = 0; sscanf(message,"%s %d",&commande , &valeur); if ((char)commande == 'T') { set_time(valeur); } else if ((char)commande == 'I') { servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f; #ifdef DELTA_MODE float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f; if(Sf >= 0 && Sf <= 90) servo_fuite = Sf; #endif printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f); } 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 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 ///////////////////////////////////////// } else if ((char)commande == 'R') { NVIC_SystemReset(); } #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; 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"); #ifdef DELTA_MODE //Position initial delta servo_poumon = 0.5; servo_fuite = 0.5 + remap(delta, 0, 90, 0, 100) / 100.f; #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); android.attach(&callbackAndroid, Serial::RxIrq); serialMonit.printf(" Demarrage...\r\n\r\n Entrez les comandes COZIR si besoin :\r\n"); 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 while (true) { //Démarrage du Timer mesurant le temps d'éxecution du code REAL_RATE.start(); Affichage_moniteur(); if (newParamFlag) { serialMonit.printf("Param = %s\r\n", param); Decoding_Message(param); } if (newAndroidFlag) { serialMonit.printf("Android = %s\r\n", Android); Decoding_Message_Android(Android); } //Vers Android if (NEED_ANDROID_OUTPUT == 1) { 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 = (Ref_Time - RATE) * 1000; wait_ms(RATE_TRUE); } }