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:
- 17:bef8abc445f2
- Parent:
- 16:917656586772
- Child:
- 18:bfd78c05b589
diff -r 917656586772 -r bef8abc445f2 main.cpp --- a/main.cpp Mon Nov 27 16:33:13 2017 +0000 +++ b/main.cpp Mon Dec 04 16:14:19 2017 +0000 @@ -1,37 +1,112 @@ +/* + +Câblage complet STM32L476RG : + +- Port SPI carte SD : + +MOSI -> PA_7 +MISO -> PA_6 +SCK -> PA_5 +CS -> PB_6 + +- Sensor Head : + +Alim en 3.3v + +SCL -> PB_8 +SDA -> PB_9 +TX -> PC_12 +RX -> PD_2 +AlertRdy -> PA_11 + + +- HC_06 : + +Alim en 3.3v ou 5v + +TX -> PC_10 +RX -> PC_11 + +- STM32L476RG sur H-Bridge L293 : + +Reférence L293 : http://www.ti.com/lit/ds/symlink/l293.pdf + +Volet Poumon : +PWM -> PB_5 -> 1 +FWD -> PB_4 -> 2 +REV -> PB_10 -> 7 + +Volet Fuite : +PWM -> PB_13 -> 9 +FWD -> PB_14 -> 10 +REV -> PB_15 -> 15 + +Potentiomètres retour Position : + +Alim en 3.3v + +Feedback Volet Poumon -> PC_2 +Feedback Volet Fuite -> PC_3 + +- Câblage complet du H-Bridge L293 : + +1 -> PB_5 +2 -> PB_4 +3 -> Borne moteur +4 -> non connectée +5 -> GND +6 -> Borne moteur +7 -> PB_10 +8 -> V+ Alimentation moteurs 4.5v - 36v + +9 -> PB_13 +10 -> PB_14 +11 -> Borne moteur +12 -> GND +13 -> non connectée +14 -> borne moteur +15 -> PB_15 +16 -> V+ Alimentation logic 5v (alim moteur si tension compatible....) + +*/ + #include "mbed.h" #include <string> #include "Sensor_head_revB.h" #include "HTU21D.h" #include "PID.h" +#include "Actuonix.h" -//Commandes des servos -#define PWM_SERVO_POUMON PB_15 -#define PWM_SERVO_FUITE PB_1 -//Retour des servos +//Commandes des servos +#define PWM_SERVO_POUMON PB_5 +#define FWD_SERVO_POUMON PB_4 +#define REV_SERVO_POUMON PB_10 #define FEED_BACK_SERVO_POUMON PC_2 + +#define PWM_SERVO_FUITE PB_13 +#define FWD_SERVO_FUITE PB_14 +#define REV_SERVO_FUITE PB_15 #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 +//Mapping des valeur. +#define MAX_MAP 100//Mapping en pourcentage +//#define MAX_MAP 90//Mapping en en degrés sur 90 degrés + + +//Ecrit dans le moniteur série de la tablette à 115200 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; +//Mode PID, STD à commenter / décommenter +#define STD_MODE +//#define PID_MODE #ifdef STD_MODE int MODE_FLAG = 0; #endif -#ifdef DELTA_MODE -int MODE_FLAG = 1; -#endif - #ifdef PID_MODE int MODE_FLAG = 2; #endif @@ -42,11 +117,15 @@ #define ANDROID(...) #endif -//Moniteur série -Serial serialMonit(USBTX,USBRX,9600); +Actuonix Servo_Poumon(FEED_BACK_SERVO_POUMON, PWM_SERVO_POUMON, FWD_SERVO_POUMON, REV_SERVO_POUMON, 1, MAX_MAP); + +Actuonix Servo_Fuite(FEED_BACK_SERVO_FUITE, PWM_SERVO_FUITE, FWD_SERVO_FUITE, REV_SERVO_FUITE, 1, MAX_MAP); + +//Moniteur série, Serial 2 +Serial serialMonit(USBTX,USBRX,115200); //COM Série vers Android, Serial 3 -Serial android(PC_10,PC_11,9600); +Serial android(PC_10,PC_11,115200); //Init de la lib ARNSRS; SENSOR_HEAD_REV_B sensors; @@ -72,13 +151,13 @@ int CellO2_1 = 0; int CellO2_2 = 0; -//Mesure du tempsd'éxecution du loop +//Mesure du temps d'é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 sur l'I2C HTU21D temphumid(PB_9, PB_8); //Temp humid sensor || SDA, SCL float Temp2; int Humid; @@ -91,21 +170,15 @@ static const char CLS[] = "\x1B[2J"; static const char HOME[] = "\x1B[H"; -//Thread d'intérogation des capteurs +//Thread d'intérogation des capteurs et de positionnement des volets Thread thread; //Contrôle des servos -PwmOut servo_poumon(PWM_SERVO_POUMON); -AnalogIn FeedBack_Servo_Poumon(FEED_BACK_SERVO_POUMON); +float Consigne_poumon = 50; 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 Consigne_fuite = 50; float volet_fuite_Position; -float Limit_min_Servo_Fuite, Limit_max_Servo_Fuite; -float Delta_FB_2; #ifdef PID_MODE //Paramètre du PID @@ -117,21 +190,15 @@ float consigne = 210; float Max_Input = 1000; float Min_Input = 80; -float Max_Output = 1;//Vérifier la valeur pour angle à laisser ouvert... -float Min_Output = 0; +float Max_Output = MAX_MAP - 10;//Vérifier la valeur pour angle à laisser ouvert... +float Min_Output = 10; //Init PID -PID control_Servo(Kc, Ti, Td, RATE_PID);//Kc, Ti, Td, interval +PID control_Servo(Kc, Ti, Td, RATE_PID); #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 +//Thread d'intérogation des capteurs, positions servo void Get_Info_thread() { while (true) { @@ -152,9 +219,12 @@ 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); + volet_poumon_Position = Servo_Poumon.Get_Position(); + volet_fuite_Position = Servo_Fuite.Get_Position(); + //Position des servo mise à jour en permanence dans le thread + Servo_Poumon.Go_To_Prop(Consigne_poumon); + Servo_Fuite.Go_To_Prop(Consigne_fuite); } } @@ -173,11 +243,11 @@ 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(" Volet Poumon = %f\r\n" , volet_poumon_Position); + printf(" Volet Fuite = %f\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("\r\n"); printf("A enregistrer = %s\n", to_store); printf("\r\n"); #endif @@ -198,13 +268,13 @@ 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("\x1b[0m\r Volet Poumon = \x1b[1m\x1b[K%.2f\n", volet_poumon_Position); + printf("\x1b[0m\r Volet Fuite = \x1b[1m\x1b[K%.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("\r\n"); printf("\x1b[0m\r A enregistrer = \x1b[1m\x1b[K%s\n", to_store); - printf("\r\n", ""); + printf("\r\n"); #endif } @@ -248,23 +318,11 @@ 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)//Pas bon, à retravailler.... - servo_fuite = Sf; -#endif - printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f); + Consigne_poumon = (float)valeur; + printf(" Servo Poumon = %f\r\n", Consigne_poumon); } 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)//Pas bon, à retravailler.... - servo_fuite = Sf; -#endif + Consigne_fuite = (float)valeur; + printf(" Servo Fuite = %f\r\n", Consigne_fuite); } else if ((char)commande == 'R') { NVIC_SystemReset(); ///////////////////////////////////////// @@ -277,7 +335,6 @@ sensors.cozirSend(message); } - //wait_ms(100); strcpy(param," "); indexParam = 0; newParamFlag = false; @@ -293,23 +350,11 @@ 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)//Pas bon, à retravailler.... - servo_fuite = Sf; -#endif - printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f); + Consigne_poumon = (float)valeur; + printf(" Servo Poumon = %f\r\n", Consigne_poumon); } 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)//Pas bon, à retravailler.... - servo_fuite = Sf; -#endif + Consigne_fuite = (float)valeur; + printf(" Servo Fuite = %f\r\n", Consigne_fuite); ///////////////////////////////////////// //Pour rajouter une commande //} else if ((char)commande == 'X') { @@ -341,72 +386,17 @@ printf(" UPDATE CONSIGNE PID --> Consigne = %d\r\n\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(); + Servo_Poumon.Calibrate(); + Servo_Fuite.Calibrate(); /* Par défaut les valeur en cas de calibration sur true sont les suivant @@ -464,7 +454,6 @@ control_Servo.setMode(AUTO_MODE); //Consigne à x mb control_Servo.setSetPoint(consigne); - #endif #ifdef VT100 @@ -491,8 +480,9 @@ char Time_buf[32]; strftime(Time_buf, 32, "%D %I-%M-%S ", localtime(&seconds)); +#ifdef PID_MODE //Fabrication de la chaine à enregistrer - sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%3.2f:%3.2f:%d:%.3f:%.3f:%.3f", + sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%.2f:%.2f:%d:%.3f:%.3f:%.3f:%d", Time_buf, co2, ppO2, @@ -507,7 +497,31 @@ MODE_FLAG, Kc, Ti, - Td); + Td, + (int)consigne + ); +#endif +#ifndef PID_MODE + //Fabrication de la chaine à enregistrer sans les variables du PID + sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%.2f:%.2f:%d:%.3f:%.3f:%.3f:%d", + Time_buf, + co2, + ppO2, + pression, + Temp1, + Temp2, + Humid, + CellO2_1, + CellO2_2, + volet_poumon_Position, + volet_fuite_Position, + MODE_FLAG, + 0, + 0, + 0, + 0 + ); +#endif //Enregistrement de la chaine sensors.Write_SD((string)to_store); @@ -525,10 +539,8 @@ #ifdef PID_MODE //Update du PID control_Servo.setProcessValue(ppO2); - //Nouvelle sortie servo - Commande_PID = control_Servo.compute(); - //Appliquer la consigne - servo_fuite = Commande_PID; + //Nouvelle sortie servo fuite + Consigne_fuite = control_Servo.compute(); #endif //Arrêt du Timer mesurant le temps d'éxecution du code