Olivier Nastorg / Mbed OS ARNSRS_testDFU

mbed-os github

Dependencies:   ADS1015 Faulhaber HTU21D_mod MS5837_potless Sensor_Head_RevB_3 USBDevice_dfu Utilsdfu beep

Fork of ARNSRS_testDFU by POTLESS

main.cpp@16:917656586772, 2017-11-27 (annotated)

Committer:
POTLESS_2
Date:
Mon Nov 27 16:33:13 2017 +0000
Revision:
16:917656586772
Parent:
15:efd3b3bf3f37
Child:
17:bef8abc445f2
Reprise du thread, enregistrement des valeurs du PID, changement init de la lib sensor, warning emp?chant un loop trop court, reset du PID quand on change une valeur...et je me rappel plus le reste....

Who changed what in which revision?

UserRevisionLine numberNew contents of line
potless 1:bef7856b5c0a 1 #include "mbed.h"
potless 2:4a8bf1d53439 2 #include <string>
POTLESS_2 7:ad15c386e960 3 #include "Sensor_head_revB.h"
POTLESS_2 8:a750d531b381 4 #include "HTU21D.h"
POTLESS_2 12:7f3aadd79f89 5 #include "PID.h"
POTLESS_2 4:d84250f67dec 6
POTLESS_2 10:aca745a66d51 7 //Commandes des servos
POTLESS_2 10:aca745a66d51 8 #define PWM_SERVO_POUMON PB_15
POTLESS_2 10:aca745a66d51 9 #define PWM_SERVO_FUITE PB_1
POTLESS_2 10:aca745a66d51 10 //Retour des servos
POTLESS_2 10:aca745a66d51 11 #define FEED_BACK_SERVO_POUMON PC_2
POTLESS_2 10:aca745a66d51 12 #define FEED_BACK_SERVO_FUITE PC_3
POTLESS_2 9:04bfdfc029cb 13
POTLESS_2 9:04bfdfc029cb 14 //Ecrit dans le moniteur série de la tablette à 9600 bds si sur 1, penser à mettre NEED_CONSOLE_OUTPUT à 0
POTLESS_2 9:04bfdfc029cb 15 #define NEED_ANDROID_OUTPUT 1
POTLESS_2 9:04bfdfc029cb 16
POTLESS_2 10:aca745a66d51 17 //Sortie en mode VT100, à commenter si on veut une sortie type Arduino
POTLESS_2 10:aca745a66d51 18 //#define VT100
POTLESS_2 9:04bfdfc029cb 19
POTLESS_2 12:7f3aadd79f89 20 //Mode PID, STD et DELTA, à commenter / décommenter
POTLESS_2 12:7f3aadd79f89 21 //#define STD_MODE
POTLESS_2 12:7f3aadd79f89 22 #define PID_MODE
POTLESS_2 11:b2feed92584a 23 //#define DELTA_MODE
POTLESS_2 12:7f3aadd79f89 24
POTLESS_2 11:b2feed92584a 25 float delta = 0.0f;
POTLESS_2 11:b2feed92584a 26
POTLESS_2 12:7f3aadd79f89 27 #ifdef STD_MODE
POTLESS_2 12:7f3aadd79f89 28 int MODE_FLAG = 0;
POTLESS_2 11:b2feed92584a 29 #endif
POTLESS_2 11:b2feed92584a 30
POTLESS_2 11:b2feed92584a 31 #ifdef DELTA_MODE
POTLESS_2 12:7f3aadd79f89 32 int MODE_FLAG = 1;
POTLESS_2 12:7f3aadd79f89 33 #endif
POTLESS_2 12:7f3aadd79f89 34
POTLESS_2 12:7f3aadd79f89 35 #ifdef PID_MODE
POTLESS_2 12:7f3aadd79f89 36 int MODE_FLAG = 2;
POTLESS_2 11:b2feed92584a 37 #endif
POTLESS_2 11:b2feed92584a 38
POTLESS_2 9:04bfdfc029cb 39 #if NEED_ANDROID_OUTPUT
POTLESS_2 9:04bfdfc029cb 40 #define ANDROID(...) { android.printf(__VA_ARGS__); }
POTLESS_2 9:04bfdfc029cb 41 #else
POTLESS_2 9:04bfdfc029cb 42 #define ANDROID(...)
POTLESS_2 9:04bfdfc029cb 43 #endif
POTLESS_2 9:04bfdfc029cb 44
POTLESS_2 7:ad15c386e960 45 //Moniteur série
POTLESS_2 9:04bfdfc029cb 46 Serial serialMonit(USBTX,USBRX,9600);
POTLESS_2 10:aca745a66d51 47
POTLESS_2 9:04bfdfc029cb 48 //COM Série vers Android, Serial 3
POTLESS_2 9:04bfdfc029cb 49 Serial android(PC_10,PC_11,9600);
POTLESS_2 4:d84250f67dec 50
POTLESS_2 7:ad15c386e960 51 //Init de la lib ARNSRS;
POTLESS_2 7:ad15c386e960 52 SENSOR_HEAD_REV_B sensors;
potless 2:4a8bf1d53439 53
POTLESS_2 6:ebed9093d661 54 //pour Param Cozir
POTLESS_2 9:04bfdfc029cb 55 const int sizeParam = 20;
POTLESS_2 11:b2feed92584a 56 char param[sizeParam];
POTLESS_2 11:b2feed92584a 57 volatile int indexParam = 0;
POTLESS_2 6:ebed9093d661 58 bool newParamFlag = false;
potless 2:4a8bf1d53439 59
POTLESS_2 9:04bfdfc029cb 60 //pour Commandes Android
POTLESS_2 9:04bfdfc029cb 61 const int sizeAndroid = 20;
POTLESS_2 11:b2feed92584a 62 char Android[sizeAndroid];
POTLESS_2 11:b2feed92584a 63 volatile int indexAndroid = 0;
POTLESS_2 9:04bfdfc029cb 64 bool newAndroidFlag = false;
POTLESS_2 12:7f3aadd79f89 65 char to_android[100];
POTLESS_2 9:04bfdfc029cb 66
POTLESS_2 7:ad15c386e960 67 //Variables de stockage des infos capteurs
POTLESS_2 7:ad15c386e960 68 int co2 = 0;
POTLESS_2 7:ad15c386e960 69 float pression = 0;
POTLESS_2 9:04bfdfc029cb 70 float Temp1 = 0;
POTLESS_2 7:ad15c386e960 71 int ppO2 = 0;
POTLESS_2 7:ad15c386e960 72 int CellO2_1 = 0;
POTLESS_2 7:ad15c386e960 73 int CellO2_2 = 0;
POTLESS_2 7:ad15c386e960 74
POTLESS_2 7:ad15c386e960 75 //Mesure du tempsd'éxecution du loop
POTLESS_2 7:ad15c386e960 76 Timer REAL_RATE;
POTLESS_2 7:ad15c386e960 77 float RATE = 0;
POTLESS_2 11:b2feed92584a 78 float RATE_TRUE = 0;
POTLESS_2 12:7f3aadd79f89 79 float Ref_Time = 1.0; //La durée de la boucle désirée...
POTLESS_2 7:ad15c386e960 80
POTLESS_2 8:a750d531b381 81 //HTU21D
POTLESS_2 8:a750d531b381 82 HTU21D temphumid(PB_9, PB_8); //Temp humid sensor || SDA, SCL
POTLESS_2 9:04bfdfc029cb 83 float Temp2;
POTLESS_2 9:04bfdfc029cb 84 int Humid;
POTLESS_2 9:04bfdfc029cb 85
POTLESS_2 9:04bfdfc029cb 86 //Data LOG
POTLESS_2 9:04bfdfc029cb 87 char to_store[50];
POTLESS_2 9:04bfdfc029cb 88 time_t seconds;
POTLESS_2 8:a750d531b381 89
POTLESS_2 8:a750d531b381 90 //VT100
POTLESS_2 8:a750d531b381 91 static const char CLS[] = "\x1B[2J";
POTLESS_2 8:a750d531b381 92 static const char HOME[] = "\x1B[H";
POTLESS_2 8:a750d531b381 93
POTLESS_2 7:ad15c386e960 94 //Thread d'intérogation des capteurs
POTLESS_2 7:ad15c386e960 95 Thread thread;
POTLESS_2 7:ad15c386e960 96
POTLESS_2 10:aca745a66d51 97 //Contrôle des servos
POTLESS_2 10:aca745a66d51 98 PwmOut servo_poumon(PWM_SERVO_POUMON);
POTLESS_2 10:aca745a66d51 99 AnalogIn FeedBack_Servo_Poumon(FEED_BACK_SERVO_POUMON);
POTLESS_2 10:aca745a66d51 100 float volet_poumon_Position;
POTLESS_2 10:aca745a66d51 101 float Limit_min_Servo_Poumon, Limit_max_Servo_Poumon;
POTLESS_2 10:aca745a66d51 102 float Delta_FB_1;
POTLESS_2 10:aca745a66d51 103
POTLESS_2 10:aca745a66d51 104 PwmOut servo_fuite(PWM_SERVO_FUITE);
POTLESS_2 10:aca745a66d51 105 AnalogIn FeedBack_Servo_Fuite(FEED_BACK_SERVO_FUITE);
POTLESS_2 10:aca745a66d51 106 float volet_fuite_Position;
POTLESS_2 10:aca745a66d51 107 float Limit_min_Servo_Fuite, Limit_max_Servo_Fuite;
POTLESS_2 10:aca745a66d51 108 float Delta_FB_2;
POTLESS_2 10:aca745a66d51 109
POTLESS_2 12:7f3aadd79f89 110 #ifdef PID_MODE
POTLESS_2 12:7f3aadd79f89 111 //Paramètre du PID
POTLESS_2 12:7f3aadd79f89 112 float Kc = 40;
POTLESS_2 12:7f3aadd79f89 113 float Ti = 0;
POTLESS_2 12:7f3aadd79f89 114 float Td = 0;
POTLESS_2 12:7f3aadd79f89 115 float RATE_PID = Ref_Time;
POTLESS_2 12:7f3aadd79f89 116 float Commande_PID;
POTLESS_2 15:efd3b3bf3f37 117 float consigne = 210;
POTLESS_2 15:efd3b3bf3f37 118 float Max_Input = 1000;
POTLESS_2 15:efd3b3bf3f37 119 float Min_Input = 80;
POTLESS_2 15:efd3b3bf3f37 120 float Max_Output = 1;//Vérifier la valeur pour angle à laisser ouvert...
POTLESS_2 15:efd3b3bf3f37 121 float Min_Output = 0;
POTLESS_2 10:aca745a66d51 122
POTLESS_2 12:7f3aadd79f89 123 //Init PID
POTLESS_2 12:7f3aadd79f89 124 PID control_Servo(Kc, Ti, Td, RATE_PID);//Kc, Ti, Td, interval
POTLESS_2 12:7f3aadd79f89 125 #endif
POTLESS_2 10:aca745a66d51 126
POTLESS_2 10:aca745a66d51 127 //Remap d'une valeur
POTLESS_2 10:aca745a66d51 128 float remap(float x, float in_min, float in_max, float out_min, float out_max)
POTLESS_2 10:aca745a66d51 129 {
POTLESS_2 10:aca745a66d51 130 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
POTLESS_2 10:aca745a66d51 131 }
POTLESS_2 10:aca745a66d51 132
POTLESS_2 10:aca745a66d51 133
POTLESS_2 12:7f3aadd79f89 134 //Thread d'intérogation des capteurs, positions servo et constitution de la chaine
POTLESS_2 10:aca745a66d51 135 void Get_Info_thread()
POTLESS_2 6:ebed9093d661 136 {
POTLESS_2 7:ad15c386e960 137 while (true) {
POTLESS_2 7:ad15c386e960 138
POTLESS_2 7:ad15c386e960 139 //CO2 sur Cozir
POTLESS_2 7:ad15c386e960 140 co2 = sensors.requestCO2();
POTLESS_2 7:ad15c386e960 141 //P / T sur MS5837
POTLESS_2 7:ad15c386e960 142 pression = sensors.requestPress();
POTLESS_2 9:04bfdfc029cb 143 Temp1 = sensors.requestTemp();
POTLESS_2 7:ad15c386e960 144 //PPO2 sur ADS1015
POTLESS_2 7:ad15c386e960 145 ppO2 = sensors.requestPpO2();
POTLESS_2 7:ad15c386e960 146 //Cell O2 en mV
POTLESS_2 7:ad15c386e960 147 CellO2_1 = sensors.requestCellO2_1();
POTLESS_2 7:ad15c386e960 148 CellO2_2 = sensors.requestCellO2_2();
POTLESS_2 9:04bfdfc029cb 149
POTLESS_2 8:a750d531b381 150 //HTU21D
POTLESS_2 9:04bfdfc029cb 151 Temp2 = temphumid.sample_ctemp();
POTLESS_2 9:04bfdfc029cb 152 Humid = temphumid.sample_humid();
POTLESS_2 9:04bfdfc029cb 153
POTLESS_2 10:aca745a66d51 154 //Retour position des servos
POTLESS_2 10:aca745a66d51 155 volet_poumon_Position = 90 - remap(FeedBack_Servo_Poumon, Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90);
POTLESS_2 10:aca745a66d51 156 volet_fuite_Position = remap(FeedBack_Servo_Fuite, Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90);
POTLESS_2 12:7f3aadd79f89 157
POTLESS_2 15:efd3b3bf3f37 158 }
potless 2:4a8bf1d53439 159 }
POTLESS_2 4:d84250f67dec 160
POTLESS_2 7:ad15c386e960 161 void Affichage_moniteur()
POTLESS_2 4:d84250f67dec 162 {
POTLESS_2 12:7f3aadd79f89 163 #ifndef VT100
POTLESS_2 9:04bfdfc029cb 164 printf("\r\n");
POTLESS_2 9:04bfdfc029cb 165 printf(" CO2 = %d ppm\r\n" , co2);
POTLESS_2 9:04bfdfc029cb 166 printf(" PPO2 = %d mb\r\n", ppO2);
POTLESS_2 9:04bfdfc029cb 167 printf(" Pression = %f msw\r\n", pression);
POTLESS_2 10:aca745a66d51 168 printf(" Temp MS5837 = %f C\r\n", Temp1);
POTLESS_2 10:aca745a66d51 169 printf(" Temp HTU21D = %f C\n\r", Temp2);
POTLESS_2 9:04bfdfc029cb 170 printf(" Humidity = %d %%\n\r", Humid);
POTLESS_2 8:a750d531b381 171 printf("\n\r");
POTLESS_2 8:a750d531b381 172 printf(" Cell O2 n 1 = %d\r\n" , CellO2_1);
POTLESS_2 8:a750d531b381 173 printf(" Cell O2 n 2 = %d\r\n" , CellO2_2);
POTLESS_2 7:ad15c386e960 174 printf("\r\n");
POTLESS_2 9:04bfdfc029cb 175 printf("\n");
POTLESS_2 10:aca745a66d51 176 printf(" Volet Poumon = %3.2f%\r\n" , volet_poumon_Position);
POTLESS_2 10:aca745a66d51 177 printf(" Volet Fuite = %3.2f%\r\n" , volet_fuite_Position);
POTLESS_2 10:aca745a66d51 178 printf("\r\n");
POTLESS_2 12:7f3aadd79f89 179 printf("Temps d execution de la boucle = %f seconde(s)\n", (RATE + RATE_TRUE / 1000));
POTLESS_2 9:04bfdfc029cb 180 printf("\r\n", "");
POTLESS_2 9:04bfdfc029cb 181 printf("A enregistrer = %s\n", to_store);
POTLESS_2 9:04bfdfc029cb 182 printf("\r\n");
POTLESS_2 10:aca745a66d51 183 #endif
POTLESS_2 9:04bfdfc029cb 184
POTLESS_2 12:7f3aadd79f89 185 #ifdef VT100
POTLESS_2 8:a750d531b381 186 printf(HOME);
POTLESS_2 8:a750d531b381 187 printf("\x1b[30m");
POTLESS_2 9:04bfdfc029cb 188 printf("\x1b[0m\r CO2 = \x1b[1m\x1b[K%d ppm\n", co2);
POTLESS_2 9:04bfdfc029cb 189 printf("\x1b[0m\r PPO2 = \x1b[1m\x1b[K%d mb\n", ppO2);
POTLESS_2 9:04bfdfc029cb 190 printf("\n");
POTLESS_2 9:04bfdfc029cb 191 printf("\x1b[0m\r Pression = \x1b[1m\x1b[K%.2f msw\n", pression);
POTLESS_2 9:04bfdfc029cb 192 printf("\n");
POTLESS_2 10:aca745a66d51 193 printf("\x1b[0m\r Temp MS5837 = \x1b[1m\x1b[K%.2f C\n", Temp1);
POTLESS_2 10:aca745a66d51 194 printf("\x1b[0m\r Temp HTU21D = \x1b[1m\x1b[K%.2f C\n", Temp2);
POTLESS_2 9:04bfdfc029cb 195 printf("\n");
POTLESS_2 9:04bfdfc029cb 196 printf("\x1b[0m\r Humidity = \x1b[1m\x1b[K%d %\n", Humid);
POTLESS_2 9:04bfdfc029cb 197 printf("\n");
POTLESS_2 8:a750d531b381 198 printf("\x1b[0m\r Cell O2 n 1 = \x1b[1m\x1b[K%d\n", CellO2_1);
POTLESS_2 8:a750d531b381 199 printf("\x1b[0m\r Cell O2 n 2 = \x1b[1m\x1b[K%d\n", CellO2_2);
POTLESS_2 8:a750d531b381 200 printf("\n");
POTLESS_2 10:aca745a66d51 201 printf("\x1b[0m\r Volet Poumon = \x1b[1m\x1b[K%3.2f%\n", volet_poumon_Position);
POTLESS_2 10:aca745a66d51 202 printf("\x1b[0m\r Volet Fuite = \x1b[1m\x1b[K%3.2f%\n", volet_fuite_Position);
POTLESS_2 10:aca745a66d51 203 printf("\n");
POTLESS_2 13:22a96dc67e85 204 printf("\x1b[0m\r Temps d execution de la boucle = \x1b[1m\x1b[K%f seconde(s)\n", (RATE + RATE_TRUE / 1000));
POTLESS_2 9:04bfdfc029cb 205 printf("\r\n", "");
POTLESS_2 9:04bfdfc029cb 206 printf("\x1b[0m\r A enregistrer = \x1b[1m\x1b[K%s\n", to_store);
POTLESS_2 9:04bfdfc029cb 207 printf("\r\n", "");
POTLESS_2 12:7f3aadd79f89 208 #endif
POTLESS_2 4:d84250f67dec 209 }
POTLESS_2 4:d84250f67dec 210
POTLESS_2 11:b2feed92584a 211 //Callback de l'intérruption des envois de commandes depuis le terminal
POTLESS_2 6:ebed9093d661 212 void callbackParam()
POTLESS_2 4:d84250f67dec 213 {
POTLESS_2 6:ebed9093d661 214 while(serialMonit.readable()) {
POTLESS_2 6:ebed9093d661 215 if (indexParam == sizeParam) //éviter la saturation du buffer
POTLESS_2 6:ebed9093d661 216 serialMonit.getc();
POTLESS_2 6:ebed9093d661 217 else
POTLESS_2 6:ebed9093d661 218 param [indexParam++] = serialMonit.getc();//chargement du buffer dans le message
POTLESS_2 6:ebed9093d661 219 if ((indexParam == sizeParam) || (param[indexParam -1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut...
POTLESS_2 6:ebed9093d661 220 param[indexParam] = 0;
POTLESS_2 6:ebed9093d661 221 newParamFlag = true;
POTLESS_2 6:ebed9093d661 222 }
POTLESS_2 4:d84250f67dec 223 }
POTLESS_2 6:ebed9093d661 224 }
POTLESS_2 4:d84250f67dec 225
POTLESS_2 9:04bfdfc029cb 226 //Callback de l'intérruption des envois de commandes depuis Android
POTLESS_2 9:04bfdfc029cb 227 void callbackAndroid()
POTLESS_2 9:04bfdfc029cb 228 {
POTLESS_2 9:04bfdfc029cb 229 while(android.readable()) {
POTLESS_2 9:04bfdfc029cb 230 if (indexAndroid == sizeAndroid) //éviter la saturation du buffer
POTLESS_2 9:04bfdfc029cb 231 android.getc();
POTLESS_2 9:04bfdfc029cb 232 else
POTLESS_2 9:04bfdfc029cb 233 Android [indexAndroid++] = android.getc();//chargement du buffer dans le message
POTLESS_2 9:04bfdfc029cb 234 if ((indexAndroid == sizeAndroid) || (Android[indexAndroid -1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut...
POTLESS_2 9:04bfdfc029cb 235 Android[indexAndroid] = 0;
POTLESS_2 9:04bfdfc029cb 236 newAndroidFlag = true;
POTLESS_2 9:04bfdfc029cb 237 }
POTLESS_2 9:04bfdfc029cb 238 }
POTLESS_2 9:04bfdfc029cb 239 }
POTLESS_2 11:b2feed92584a 240
POTLESS_2 9:04bfdfc029cb 241 void Decoding_Message(char message [])
POTLESS_2 9:04bfdfc029cb 242 {
POTLESS_2 9:04bfdfc029cb 243 char *commande = 0;
POTLESS_2 14:b5e0e882205e 244 float valeur = 0;
POTLESS_2 9:04bfdfc029cb 245
POTLESS_2 14:b5e0e882205e 246 sscanf(message,"%s %f",&commande , &valeur);
POTLESS_2 9:04bfdfc029cb 247
POTLESS_2 9:04bfdfc029cb 248 if ((char)commande == 'T') {
POTLESS_2 9:04bfdfc029cb 249 set_time(valeur);
POTLESS_2 12:7f3aadd79f89 250 } else if ((char)commande == 'I') {
POTLESS_2 10:aca745a66d51 251 servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f;
POTLESS_2 12:7f3aadd79f89 252 #ifdef DELTA_MODE
POTLESS_2 11:b2feed92584a 253 float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
POTLESS_2 13:22a96dc67e85 254 if(Sf >= 0 && Sf <= 90)//Pas bon, à retravailler....
POTLESS_2 12:7f3aadd79f89 255 servo_fuite = Sf;
POTLESS_2 12:7f3aadd79f89 256 #endif
POTLESS_2 10:aca745a66d51 257 printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f);
POTLESS_2 12:7f3aadd79f89 258 } else if ((char)commande == 'O') {
POTLESS_2 10:aca745a66d51 259 servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f;
POTLESS_2 12:7f3aadd79f89 260 printf(" Servo Fuite = %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f);
POTLESS_2 11:b2feed92584a 261 } else if ((char)commande == 'D') {
POTLESS_2 11:b2feed92584a 262 delta = valeur;
POTLESS_2 12:7f3aadd79f89 263 #ifdef DELTA_MODE
POTLESS_2 11:b2feed92584a 264 float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
POTLESS_2 13:22a96dc67e85 265 if(Sf >= 0 && Sf <= 90)//Pas bon, à retravailler....
POTLESS_2 12:7f3aadd79f89 266 servo_fuite = Sf;
POTLESS_2 12:7f3aadd79f89 267 #endif
POTLESS_2 10:aca745a66d51 268 } else if ((char)commande == 'R') {
POTLESS_2 10:aca745a66d51 269 NVIC_SystemReset();
POTLESS_2 12:7f3aadd79f89 270 /////////////////////////////////////////
POTLESS_2 12:7f3aadd79f89 271 //Pour rajouter une commande
POTLESS_2 12:7f3aadd79f89 272 //} else if ((char)commande == 'X') {
POTLESS_2 12:7f3aadd79f89 273 // attribuer à une VARIABLE = valeur;
POTLESS_2 12:7f3aadd79f89 274 // ou une action, avec ou sans valeur
POTLESS_2 12:7f3aadd79f89 275 /////////////////////////////////////////
POTLESS_2 12:7f3aadd79f89 276 } else {
POTLESS_2 9:04bfdfc029cb 277 sensors.cozirSend(message);
POTLESS_2 9:04bfdfc029cb 278 }
POTLESS_2 9:04bfdfc029cb 279
POTLESS_2 11:b2feed92584a 280 //wait_ms(100);
POTLESS_2 9:04bfdfc029cb 281 strcpy(param," ");
POTLESS_2 9:04bfdfc029cb 282 indexParam = 0;
POTLESS_2 9:04bfdfc029cb 283 newParamFlag = false;
POTLESS_2 9:04bfdfc029cb 284 }
POTLESS_2 9:04bfdfc029cb 285
POTLESS_2 9:04bfdfc029cb 286 void Decoding_Message_Android(char message [])
POTLESS_2 9:04bfdfc029cb 287 {
POTLESS_2 9:04bfdfc029cb 288 char *commande = 0;
POTLESS_2 14:b5e0e882205e 289 float valeur = 0;
POTLESS_2 9:04bfdfc029cb 290
POTLESS_2 14:b5e0e882205e 291 sscanf(message,"%s %f",&commande , &valeur);
POTLESS_2 9:04bfdfc029cb 292
POTLESS_2 9:04bfdfc029cb 293 if ((char)commande == 'T') {
POTLESS_2 9:04bfdfc029cb 294 set_time(valeur);
POTLESS_2 12:7f3aadd79f89 295 } else if ((char)commande == 'I') {
POTLESS_2 10:aca745a66d51 296 servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f;
POTLESS_2 12:7f3aadd79f89 297 #ifdef DELTA_MODE
POTLESS_2 11:b2feed92584a 298 float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
POTLESS_2 13:22a96dc67e85 299 if(Sf >= 0 && Sf <= 90)//Pas bon, à retravailler....
POTLESS_2 12:7f3aadd79f89 300 servo_fuite = Sf;
POTLESS_2 12:7f3aadd79f89 301 #endif
POTLESS_2 10:aca745a66d51 302 printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f);
POTLESS_2 12:7f3aadd79f89 303 } else if ((char)commande == 'O') {
POTLESS_2 10:aca745a66d51 304 servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f;
POTLESS_2 11:b2feed92584a 305 printf(" Servo Fuite = %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f);
POTLESS_2 11:b2feed92584a 306 } else if ((char)commande == 'D') {
POTLESS_2 11:b2feed92584a 307 delta = valeur;
POTLESS_2 12:7f3aadd79f89 308 #ifdef DELTA_MODE
POTLESS_2 11:b2feed92584a 309 float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
POTLESS_2 13:22a96dc67e85 310 if(Sf >= 0 && Sf <= 90)//Pas bon, à retravailler....
POTLESS_2 12:7f3aadd79f89 311 servo_fuite = Sf;
POTLESS_2 12:7f3aadd79f89 312 #endif
POTLESS_2 12:7f3aadd79f89 313 /////////////////////////////////////////
POTLESS_2 12:7f3aadd79f89 314 //Pour rajouter une commande
POTLESS_2 12:7f3aadd79f89 315 //} else if ((char)commande == 'X') {
POTLESS_2 12:7f3aadd79f89 316 // attribuer à une VARIABLE = valeur;
POTLESS_2 12:7f3aadd79f89 317 // ou une action, avec ou sans valeur
POTLESS_2 12:7f3aadd79f89 318 /////////////////////////////////////////
POTLESS_2 9:04bfdfc029cb 319 } else if ((char)commande == 'R') {
POTLESS_2 9:04bfdfc029cb 320 NVIC_SystemReset();
POTLESS_2 9:04bfdfc029cb 321 }
POTLESS_2 12:7f3aadd79f89 322 #ifdef PID_MODE
POTLESS_2 12:7f3aadd79f89 323 else if ((char)commande == 'p') {
POTLESS_2 12:7f3aadd79f89 324 Kc = (float)valeur;
POTLESS_2 16:917656586772 325 control_Servo.reset();
POTLESS_2 12:7f3aadd79f89 326 control_Servo.setTunings(Kc, Ti, Td);
POTLESS_2 14:b5e0e882205e 327 printf(" UPDATE PID --> Kc = %f Ti = %f Td = %f\r\n\n", Kc, Ti, Td);
POTLESS_2 15:efd3b3bf3f37 328 } else if ((char)commande == 'i') {
POTLESS_2 12:7f3aadd79f89 329 Ti = (float)valeur;
POTLESS_2 16:917656586772 330 control_Servo.reset();
POTLESS_2 12:7f3aadd79f89 331 control_Servo.setTunings(Kc, Ti, Td);
POTLESS_2 14:b5e0e882205e 332 printf(" UPDATE PID --> Kc = %f Ti = %f Td = %f\r\n\n", Kc, Ti, Td);
POTLESS_2 15:efd3b3bf3f37 333 } else if ((char)commande == 'd') {
POTLESS_2 12:7f3aadd79f89 334 Td = (float)valeur;
POTLESS_2 16:917656586772 335 control_Servo.reset();
POTLESS_2 12:7f3aadd79f89 336 control_Servo.setTunings(Kc, Ti, Td);
POTLESS_2 14:b5e0e882205e 337 printf(" UPDATE PID --> Kc = %f Ti = %f Td = %f\r\n\n", Kc, Ti, Td);
POTLESS_2 15:efd3b3bf3f37 338 } else if ((char)commande == 'c') {
POTLESS_2 12:7f3aadd79f89 339 consigne = valeur;
POTLESS_2 15:efd3b3bf3f37 340 control_Servo.setSetPoint(consigne);
POTLESS_2 14:b5e0e882205e 341 printf(" UPDATE CONSIGNE PID --> Consigne = %d\r\n\n", consigne);
POTLESS_2 12:7f3aadd79f89 342 }
POTLESS_2 15:efd3b3bf3f37 343 #endif
POTLESS_2 11:b2feed92584a 344 //wait_ms(100);
POTLESS_2 9:04bfdfc029cb 345 strcpy(Android," ");
POTLESS_2 9:04bfdfc029cb 346 indexAndroid = 0;
POTLESS_2 9:04bfdfc029cb 347 newAndroidFlag = false;
POTLESS_2 9:04bfdfc029cb 348 }
POTLESS_2 9:04bfdfc029cb 349
POTLESS_2 10:aca745a66d51 350 //Calibration des limites de feedback des servos
POTLESS_2 10:aca745a66d51 351 void Calibration_servo()
POTLESS_2 10:aca745a66d51 352 {
POTLESS_2 10:aca745a66d51 353 //Servos, calibration du feedback
POTLESS_2 10:aca745a66d51 354 printf(" Check Servos :\r\n");
POTLESS_2 10:aca745a66d51 355 printf("\r\n");
POTLESS_2 10:aca745a66d51 356 servo_poumon.period(0.001); // à mettre dans le setup si cette calib n'est pas appelée (je l ai trouvée commentée)
POTLESS_2 10:aca745a66d51 357 servo_fuite.period(0.001);
POTLESS_2 10:aca745a66d51 358 //Rentrer les servos à fond
POTLESS_2 10:aca745a66d51 359 servo_poumon = 0;
POTLESS_2 10:aca745a66d51 360 servo_fuite = 0;
POTLESS_2 10:aca745a66d51 361 wait(4);
POTLESS_2 10:aca745a66d51 362
POTLESS_2 10:aca745a66d51 363 Limit_min_Servo_Poumon = FeedBack_Servo_Poumon.read();
POTLESS_2 10:aca745a66d51 364 wait_ms(100);
POTLESS_2 10:aca745a66d51 365 Limit_min_Servo_Fuite = FeedBack_Servo_Fuite.read();
POTLESS_2 10:aca745a66d51 366
POTLESS_2 10:aca745a66d51 367 printf(" FeedBack Servo POUMON min = %f\n", Limit_min_Servo_Poumon);
POTLESS_2 10:aca745a66d51 368 printf(" FeedBack Servo FUITE min = %f\n", Limit_min_Servo_Fuite);
POTLESS_2 10:aca745a66d51 369 printf("\r\n");
POTLESS_2 10:aca745a66d51 370
POTLESS_2 10:aca745a66d51 371 //Sortir les servos à fond
POTLESS_2 10:aca745a66d51 372 servo_poumon = 1;
POTLESS_2 10:aca745a66d51 373 servo_fuite = 1;
POTLESS_2 10:aca745a66d51 374 wait(4);
POTLESS_2 10:aca745a66d51 375
POTLESS_2 10:aca745a66d51 376 Limit_max_Servo_Poumon = FeedBack_Servo_Poumon.read();
POTLESS_2 10:aca745a66d51 377 wait_ms(100);
POTLESS_2 10:aca745a66d51 378 Limit_max_Servo_Fuite = FeedBack_Servo_Fuite.read();
POTLESS_2 10:aca745a66d51 379
POTLESS_2 10:aca745a66d51 380 printf(" FeedBack Servo POUMON Max = %f\n", Limit_max_Servo_Poumon);
POTLESS_2 10:aca745a66d51 381 printf(" FeedBack Servo FUITE Max = %f\n", Limit_max_Servo_Fuite);
POTLESS_2 10:aca745a66d51 382 printf("\r\n");
POTLESS_2 10:aca745a66d51 383
POTLESS_2 10:aca745a66d51 384 //Position milieu
POTLESS_2 10:aca745a66d51 385 servo_poumon = 0.5;
POTLESS_2 10:aca745a66d51 386 servo_fuite = 0.5;
POTLESS_2 10:aca745a66d51 387 wait(4);
POTLESS_2 10:aca745a66d51 388
POTLESS_2 10:aca745a66d51 389 //Mesure du delta consigne / feedback
POTLESS_2 10:aca745a66d51 390 Delta_FB_1 = abs(remap(FeedBack_Servo_Poumon.read(), Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90) - 45);
POTLESS_2 10:aca745a66d51 391 Delta_FB_2 = abs(remap(FeedBack_Servo_Fuite.read(), Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90) - 45);
POTLESS_2 10:aca745a66d51 392
POTLESS_2 10:aca745a66d51 393 printf(" Delta Servo 1 = %f\n", Delta_FB_1);
POTLESS_2 10:aca745a66d51 394 printf(" Delta Servo 2 = %f\n", Delta_FB_2);
POTLESS_2 10:aca745a66d51 395
POTLESS_2 10:aca745a66d51 396 if(Delta_FB_1 > 10 || Delta_FB_2 > 10) printf(" Delta Servos non satisfaisant...\r\n\r\n");
POTLESS_2 12:7f3aadd79f89 397
POTLESS_2 11:b2feed92584a 398 #ifdef DELTA_MODE
POTLESS_2 11:b2feed92584a 399 //Position initial delta
POTLESS_2 11:b2feed92584a 400 servo_poumon = 0.5;
POTLESS_2 11:b2feed92584a 401 servo_fuite = 0.5 + remap(delta, 0, 90, 0, 100) / 100.f;
POTLESS_2 12:7f3aadd79f89 402 #endif
POTLESS_2 10:aca745a66d51 403 }
POTLESS_2 10:aca745a66d51 404
POTLESS_2 4:d84250f67dec 405 int main()
POTLESS_2 12:7f3aadd79f89 406 {
POTLESS_2 11:b2feed92584a 407 //UNIX TIMESTAMP depuis le erminal MAC = date +%s + 7200 pour heure d'été.....
POTLESS_2 12:7f3aadd79f89 408
POTLESS_2 10:aca745a66d51 409 Calibration_servo();
POTLESS_2 12:7f3aadd79f89 410
POTLESS_2 16:917656586772 411 /*
POTLESS_2 16:917656586772 412 Par défaut les valeur en cas de calibration sur true sont les suivant
POTLESS_2 6:ebed9093d661 413
POTLESS_2 16:917656586772 414 nbCalibO2 = 5
POTLESS_2 16:917656586772 415 Mode = SPOOLING
POTLESS_2 16:917656586772 416 Filtre = DIGI_FILTER32
POTLESS_2 16:917656586772 417 CalibrationCO2 = "CALIB_AIR"
POTLESS_2 16:917656586772 418
POTLESS_2 16:917656586772 419 Parfois la calibration du Cozir coince...faire reset et relancer...
POTLESS_2 16:917656586772 420
POTLESS_2 16:917656586772 421 Pour calibrer avec ces paramètres :
POTLESS_2 16:917656586772 422
POTLESS_2 16:917656586772 423 sensors.Sensors_INIT(true, true);
POTLESS_2 16:917656586772 424
POTLESS_2 16:917656586772 425 Pour changer utiliser la syntaxe suivante :
POTLESS_2 16:917656586772 426
POTLESS_2 16:917656586772 427 sensors.Sensors_INIT(true, true, 5, SPOOLING, DIGI_FILTER32, CALIB_AIR);
POTLESS_2 16:917656586772 428
POTLESS_2 16:917656586772 429 */
POTLESS_2 16:917656586772 430
POTLESS_2 16:917656586772 431 sensors.Sensors_INIT(false, true);
POTLESS_2 16:917656586772 432
POTLESS_2 6:ebed9093d661 433 serialMonit.attach(&callbackParam, Serial::RxIrq);
POTLESS_2 6:ebed9093d661 434
POTLESS_2 9:04bfdfc029cb 435 android.attach(&callbackAndroid, Serial::RxIrq);
POTLESS_2 9:04bfdfc029cb 436
POTLESS_2 7:ad15c386e960 437 serialMonit.printf(" Demarrage...\r\n\r\n Entrez les comandes COZIR si besoin :\r\n");
POTLESS_2 6:ebed9093d661 438
POTLESS_2 15:efd3b3bf3f37 439 /*
POTLESS_2 15:efd3b3bf3f37 440
POTLESS_2 15:efd3b3bf3f37 441 Pour mémoire, les réglage de priorité des thread
POTLESS_2 15:efd3b3bf3f37 442
POTLESS_2 15:efd3b3bf3f37 443 osPriorityIdle = -3, ///< priority: idle (lowest)
POTLESS_2 15:efd3b3bf3f37 444 osPriorityLow = -2, ///< priority: low
POTLESS_2 15:efd3b3bf3f37 445 osPriorityBelowNormal = -1, ///< priority: below normal
POTLESS_2 15:efd3b3bf3f37 446 osPriorityNormal = 0, ///< priority: normal (default)
POTLESS_2 15:efd3b3bf3f37 447 osPriorityAboveNormal = +1, ///< priority: above normal
POTLESS_2 15:efd3b3bf3f37 448 osPriorityHigh = +2, ///< priority: high
POTLESS_2 15:efd3b3bf3f37 449 osPriorityRealtime = +3, ///< priority: realtime (highest)
POTLESS_2 15:efd3b3bf3f37 450 osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
POTLESS_2 15:efd3b3bf3f37 451 */
POTLESS_2 15:efd3b3bf3f37 452
POTLESS_2 10:aca745a66d51 453 thread.start(Get_Info_thread);
POTLESS_2 9:04bfdfc029cb 454
POTLESS_2 16:917656586772 455 thread.set_priority(osPriorityNormal);
POTLESS_2 12:7f3aadd79f89 456
POTLESS_2 12:7f3aadd79f89 457 #ifdef PID_MODE
POTLESS_2 12:7f3aadd79f89 458 //Init PID
POTLESS_2 12:7f3aadd79f89 459 //Entrée PPO2 entre 100 et 1000 mb
POTLESS_2 12:7f3aadd79f89 460 control_Servo.setInputLimits(Min_Input, Max_Input);
POTLESS_2 12:7f3aadd79f89 461 //Sortie servo entre 0 et 100 %
POTLESS_2 12:7f3aadd79f89 462 control_Servo.setOutputLimits(Min_Output, Max_Output);
POTLESS_2 15:efd3b3bf3f37 463 //Mode auto au démarrage
POTLESS_2 12:7f3aadd79f89 464 control_Servo.setMode(AUTO_MODE);
POTLESS_2 12:7f3aadd79f89 465 //Consigne à x mb
POTLESS_2 12:7f3aadd79f89 466 control_Servo.setSetPoint(consigne);
POTLESS_2 12:7f3aadd79f89 467
POTLESS_2 15:efd3b3bf3f37 468 #endif
POTLESS_2 15:efd3b3bf3f37 469
POTLESS_2 11:b2feed92584a 470 #ifdef VT100
POTLESS_2 8:a750d531b381 471 printf(CLS);
POTLESS_2 11:b2feed92584a 472 #endif
POTLESS_2 9:04bfdfc029cb 473
POTLESS_2 7:ad15c386e960 474 while (true) {
POTLESS_2 6:ebed9093d661 475
POTLESS_2 7:ad15c386e960 476 //Démarrage du Timer mesurant le temps d'éxecution du code
POTLESS_2 7:ad15c386e960 477 REAL_RATE.start();
POTLESS_2 7:ad15c386e960 478
POTLESS_2 6:ebed9093d661 479 if (newParamFlag) {
POTLESS_2 6:ebed9093d661 480 serialMonit.printf("Param = %s\r\n", param);
POTLESS_2 9:04bfdfc029cb 481 Decoding_Message(param);
POTLESS_2 9:04bfdfc029cb 482 }
POTLESS_2 9:04bfdfc029cb 483
POTLESS_2 9:04bfdfc029cb 484 if (newAndroidFlag) {
POTLESS_2 9:04bfdfc029cb 485 serialMonit.printf("Android = %s\r\n", Android);
POTLESS_2 9:04bfdfc029cb 486 Decoding_Message_Android(Android);
POTLESS_2 6:ebed9093d661 487 }
POTLESS_2 7:ad15c386e960 488
POTLESS_2 15:efd3b3bf3f37 489 //Fabrication de la chaine Date / heure
POTLESS_2 15:efd3b3bf3f37 490 seconds = time(NULL);
POTLESS_2 15:efd3b3bf3f37 491 char Time_buf[32];
POTLESS_2 15:efd3b3bf3f37 492 strftime(Time_buf, 32, "%D %I-%M-%S ", localtime(&seconds));
POTLESS_2 15:efd3b3bf3f37 493
POTLESS_2 15:efd3b3bf3f37 494 //Fabrication de la chaine à enregistrer
POTLESS_2 16:917656586772 495 sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%3.2f:%3.2f:%d:%.3f:%.3f:%.3f",
POTLESS_2 15:efd3b3bf3f37 496 Time_buf,
POTLESS_2 15:efd3b3bf3f37 497 co2,
POTLESS_2 15:efd3b3bf3f37 498 ppO2,
POTLESS_2 15:efd3b3bf3f37 499 pression,
POTLESS_2 15:efd3b3bf3f37 500 Temp1,
POTLESS_2 15:efd3b3bf3f37 501 Temp2,
POTLESS_2 15:efd3b3bf3f37 502 Humid,
POTLESS_2 15:efd3b3bf3f37 503 CellO2_1,
POTLESS_2 15:efd3b3bf3f37 504 CellO2_2,
POTLESS_2 15:efd3b3bf3f37 505 volet_poumon_Position,
POTLESS_2 15:efd3b3bf3f37 506 volet_fuite_Position,
POTLESS_2 15:efd3b3bf3f37 507 MODE_FLAG,
POTLESS_2 15:efd3b3bf3f37 508 Kc,
POTLESS_2 15:efd3b3bf3f37 509 Ti,
POTLESS_2 15:efd3b3bf3f37 510 Td);
POTLESS_2 15:efd3b3bf3f37 511
POTLESS_2 15:efd3b3bf3f37 512 //Enregistrement de la chaine
POTLESS_2 15:efd3b3bf3f37 513 sensors.Write_SD((string)to_store);
POTLESS_2 15:efd3b3bf3f37 514
POTLESS_2 15:efd3b3bf3f37 515 //Pour Android on ajoute < et > pour décoder l'arrivée du message
POTLESS_2 9:04bfdfc029cb 516 if (NEED_ANDROID_OUTPUT == 1) {
POTLESS_2 15:efd3b3bf3f37 517 sprintf(to_android,"<%s>",to_store);
POTLESS_2 12:7f3aadd79f89 518 ANDROID(to_android);
POTLESS_2 9:04bfdfc029cb 519 }
POTLESS_2 15:efd3b3bf3f37 520
POTLESS_2 13:22a96dc67e85 521 //Vers le moniteur dérie
POTLESS_2 13:22a96dc67e85 522 Affichage_moniteur();
POTLESS_2 15:efd3b3bf3f37 523
POTLESS_2 15:efd3b3bf3f37 524
POTLESS_2 15:efd3b3bf3f37 525 #ifdef PID_MODE
POTLESS_2 15:efd3b3bf3f37 526 //Update du PID
POTLESS_2 15:efd3b3bf3f37 527 control_Servo.setProcessValue(ppO2);
POTLESS_2 15:efd3b3bf3f37 528 //Nouvelle sortie servo
POTLESS_2 15:efd3b3bf3f37 529 Commande_PID = control_Servo.compute();
POTLESS_2 15:efd3b3bf3f37 530 //Appliquer la consigne
POTLESS_2 15:efd3b3bf3f37 531 servo_fuite = Commande_PID;
POTLESS_2 12:7f3aadd79f89 532 #endif
POTLESS_2 15:efd3b3bf3f37 533
POTLESS_2 7:ad15c386e960 534 //Arrêt du Timer mesurant le temps d'éxecution du code
POTLESS_2 7:ad15c386e960 535 REAL_RATE.stop();
POTLESS_2 7:ad15c386e960 536 //Définition de la nouvelle valeur du temps d'échantillonage du PID.
POTLESS_2 7:ad15c386e960 537 RATE = REAL_RATE.read();
POTLESS_2 7:ad15c386e960 538 //Reset du Timer
POTLESS_2 7:ad15c386e960 539 REAL_RATE.reset();
POTLESS_2 12:7f3aadd79f89 540
POTLESS_2 16:917656586772 541 //Pour ralentir le code à Ref_Time seconde fixe quelque soit les intéruptions du loop....
POTLESS_2 16:917656586772 542 if (Ref_Time > RATE){
POTLESS_2 12:7f3aadd79f89 543 RATE_TRUE = (Ref_Time - RATE) * 1000;
POTLESS_2 16:917656586772 544 }else{
POTLESS_2 16:917656586772 545 RATE_TRUE = 0;
POTLESS_2 16:917656586772 546 #ifdef PID_MODE
POTLESS_2 16:917656586772 547 control_Servo.setInterval(RATE);
POTLESS_2 16:917656586772 548 #endif
POTLESS_2 16:917656586772 549 printf("Pour ralentir le code, Ref_Time doit être supérieur à %f seconde(s)\r\n\n", RATE);
POTLESS_2 16:917656586772 550 }
POTLESS_2 16:917656586772 551
POTLESS_2 11:b2feed92584a 552 wait_ms(RATE_TRUE);
POTLESS_2 4:d84250f67dec 553 }
POTLESS_2 4:d84250f67dec 554 }