![](/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
Diff: main.cpp
- Revision:
- 10:aca745a66d51
- Parent:
- 9:04bfdfc029cb
- Child:
- 11:b2feed92584a
diff -r 04bfdfc029cb -r aca745a66d51 main.cpp --- 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);