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@15:efd3b3bf3f37, 2017-11-25 (annotated)

Committer:
POTLESS_2
Date:
Sat Nov 25 16:22:38 2017 +0000
Revision:
15:efd3b3bf3f37
Parent:
14:b5e0e882205e
Child:
16:917656586772
A tester...; ; Modification du thread pour enlever la fabrication des chaines a enregistrer et pour android.; En commentaire les r?glages possible de priorit? des thread ? tester.; Ajout de l'enregistrement des param du PID.;

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 12:7f3aadd79f89 325 control_Servo.setTunings(Kc, Ti, Td);
POTLESS_2 14:b5e0e882205e 326 printf(" UPDATE PID --> Kc = %f Ti = %f Td = %f\r\n\n", Kc, Ti, Td);
POTLESS_2 15:efd3b3bf3f37 327 } else if ((char)commande == 'i') {
POTLESS_2 12:7f3aadd79f89 328 Ti = (float)valeur;
POTLESS_2 12:7f3aadd79f89 329 control_Servo.setTunings(Kc, Ti, Td);
POTLESS_2 14:b5e0e882205e 330 printf(" UPDATE PID --> Kc = %f Ti = %f Td = %f\r\n\n", Kc, Ti, Td);
POTLESS_2 15:efd3b3bf3f37 331 } else if ((char)commande == 'd') {
POTLESS_2 12:7f3aadd79f89 332 Td = (float)valeur;
POTLESS_2 12:7f3aadd79f89 333 control_Servo.setTunings(Kc, Ti, Td);
POTLESS_2 14:b5e0e882205e 334 printf(" UPDATE PID --> Kc = %f Ti = %f Td = %f\r\n\n", Kc, Ti, Td);
POTLESS_2 15:efd3b3bf3f37 335 } else if ((char)commande == 'c') {
POTLESS_2 12:7f3aadd79f89 336 consigne = valeur;
POTLESS_2 15:efd3b3bf3f37 337 control_Servo.setSetPoint(consigne);
POTLESS_2 14:b5e0e882205e 338 printf(" UPDATE CONSIGNE PID --> Consigne = %d\r\n\n", consigne);
POTLESS_2 12:7f3aadd79f89 339 }
POTLESS_2 15:efd3b3bf3f37 340 #endif
POTLESS_2 11:b2feed92584a 341 //wait_ms(100);
POTLESS_2 9:04bfdfc029cb 342 strcpy(Android," ");
POTLESS_2 9:04bfdfc029cb 343 indexAndroid = 0;
POTLESS_2 9:04bfdfc029cb 344 newAndroidFlag = false;
POTLESS_2 9:04bfdfc029cb 345 }
POTLESS_2 9:04bfdfc029cb 346
POTLESS_2 10:aca745a66d51 347 //Calibration des limites de feedback des servos
POTLESS_2 10:aca745a66d51 348 void Calibration_servo()
POTLESS_2 10:aca745a66d51 349 {
POTLESS_2 10:aca745a66d51 350 //Servos, calibration du feedback
POTLESS_2 10:aca745a66d51 351 printf(" Check Servos :\r\n");
POTLESS_2 10:aca745a66d51 352 printf("\r\n");
POTLESS_2 10:aca745a66d51 353 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 354 servo_fuite.period(0.001);
POTLESS_2 10:aca745a66d51 355 //Rentrer les servos à fond
POTLESS_2 10:aca745a66d51 356 servo_poumon = 0;
POTLESS_2 10:aca745a66d51 357 servo_fuite = 0;
POTLESS_2 10:aca745a66d51 358 wait(4);
POTLESS_2 10:aca745a66d51 359
POTLESS_2 10:aca745a66d51 360 Limit_min_Servo_Poumon = FeedBack_Servo_Poumon.read();
POTLESS_2 10:aca745a66d51 361 wait_ms(100);
POTLESS_2 10:aca745a66d51 362 Limit_min_Servo_Fuite = FeedBack_Servo_Fuite.read();
POTLESS_2 10:aca745a66d51 363
POTLESS_2 10:aca745a66d51 364 printf(" FeedBack Servo POUMON min = %f\n", Limit_min_Servo_Poumon);
POTLESS_2 10:aca745a66d51 365 printf(" FeedBack Servo FUITE min = %f\n", Limit_min_Servo_Fuite);
POTLESS_2 10:aca745a66d51 366 printf("\r\n");
POTLESS_2 10:aca745a66d51 367
POTLESS_2 10:aca745a66d51 368 //Sortir les servos à fond
POTLESS_2 10:aca745a66d51 369 servo_poumon = 1;
POTLESS_2 10:aca745a66d51 370 servo_fuite = 1;
POTLESS_2 10:aca745a66d51 371 wait(4);
POTLESS_2 10:aca745a66d51 372
POTLESS_2 10:aca745a66d51 373 Limit_max_Servo_Poumon = FeedBack_Servo_Poumon.read();
POTLESS_2 10:aca745a66d51 374 wait_ms(100);
POTLESS_2 10:aca745a66d51 375 Limit_max_Servo_Fuite = FeedBack_Servo_Fuite.read();
POTLESS_2 10:aca745a66d51 376
POTLESS_2 10:aca745a66d51 377 printf(" FeedBack Servo POUMON Max = %f\n", Limit_max_Servo_Poumon);
POTLESS_2 10:aca745a66d51 378 printf(" FeedBack Servo FUITE Max = %f\n", Limit_max_Servo_Fuite);
POTLESS_2 10:aca745a66d51 379 printf("\r\n");
POTLESS_2 10:aca745a66d51 380
POTLESS_2 10:aca745a66d51 381 //Position milieu
POTLESS_2 10:aca745a66d51 382 servo_poumon = 0.5;
POTLESS_2 10:aca745a66d51 383 servo_fuite = 0.5;
POTLESS_2 10:aca745a66d51 384 wait(4);
POTLESS_2 10:aca745a66d51 385
POTLESS_2 10:aca745a66d51 386 //Mesure du delta consigne / feedback
POTLESS_2 10:aca745a66d51 387 Delta_FB_1 = abs(remap(FeedBack_Servo_Poumon.read(), Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90) - 45);
POTLESS_2 10:aca745a66d51 388 Delta_FB_2 = abs(remap(FeedBack_Servo_Fuite.read(), Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90) - 45);
POTLESS_2 10:aca745a66d51 389
POTLESS_2 10:aca745a66d51 390 printf(" Delta Servo 1 = %f\n", Delta_FB_1);
POTLESS_2 10:aca745a66d51 391 printf(" Delta Servo 2 = %f\n", Delta_FB_2);
POTLESS_2 10:aca745a66d51 392
POTLESS_2 10:aca745a66d51 393 if(Delta_FB_1 > 10 || Delta_FB_2 > 10) printf(" Delta Servos non satisfaisant...\r\n\r\n");
POTLESS_2 12:7f3aadd79f89 394
POTLESS_2 11:b2feed92584a 395 #ifdef DELTA_MODE
POTLESS_2 11:b2feed92584a 396 //Position initial delta
POTLESS_2 11:b2feed92584a 397 servo_poumon = 0.5;
POTLESS_2 11:b2feed92584a 398 servo_fuite = 0.5 + remap(delta, 0, 90, 0, 100) / 100.f;
POTLESS_2 12:7f3aadd79f89 399 #endif
POTLESS_2 10:aca745a66d51 400 }
POTLESS_2 10:aca745a66d51 401
POTLESS_2 4:d84250f67dec 402 int main()
POTLESS_2 12:7f3aadd79f89 403 {
POTLESS_2 11:b2feed92584a 404 //UNIX TIMESTAMP depuis le erminal MAC = date +%s + 7200 pour heure d'été.....
POTLESS_2 12:7f3aadd79f89 405
POTLESS_2 10:aca745a66d51 406 Calibration_servo();
POTLESS_2 12:7f3aadd79f89 407
POTLESS_2 11:b2feed92584a 408 sensors.Sensors_INIT(false, true, 5, SPOOLING, DIGI_FILTER32, CALIB_AIR);
POTLESS_2 6:ebed9093d661 409
POTLESS_2 6:ebed9093d661 410 serialMonit.attach(&callbackParam, Serial::RxIrq);
POTLESS_2 6:ebed9093d661 411
POTLESS_2 9:04bfdfc029cb 412 android.attach(&callbackAndroid, Serial::RxIrq);
POTLESS_2 9:04bfdfc029cb 413
POTLESS_2 7:ad15c386e960 414 serialMonit.printf(" Demarrage...\r\n\r\n Entrez les comandes COZIR si besoin :\r\n");
POTLESS_2 6:ebed9093d661 415
POTLESS_2 15:efd3b3bf3f37 416 /*
POTLESS_2 15:efd3b3bf3f37 417
POTLESS_2 15:efd3b3bf3f37 418 Pour mémoire, les réglage de priorité des thread
POTLESS_2 15:efd3b3bf3f37 419
POTLESS_2 15:efd3b3bf3f37 420 osPriorityIdle = -3, ///< priority: idle (lowest)
POTLESS_2 15:efd3b3bf3f37 421 osPriorityLow = -2, ///< priority: low
POTLESS_2 15:efd3b3bf3f37 422 osPriorityBelowNormal = -1, ///< priority: below normal
POTLESS_2 15:efd3b3bf3f37 423 osPriorityNormal = 0, ///< priority: normal (default)
POTLESS_2 15:efd3b3bf3f37 424 osPriorityAboveNormal = +1, ///< priority: above normal
POTLESS_2 15:efd3b3bf3f37 425 osPriorityHigh = +2, ///< priority: high
POTLESS_2 15:efd3b3bf3f37 426 osPriorityRealtime = +3, ///< priority: realtime (highest)
POTLESS_2 15:efd3b3bf3f37 427 osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
POTLESS_2 15:efd3b3bf3f37 428 */
POTLESS_2 15:efd3b3bf3f37 429
POTLESS_2 10:aca745a66d51 430 thread.start(Get_Info_thread);
POTLESS_2 9:04bfdfc029cb 431
POTLESS_2 7:ad15c386e960 432 thread.set_priority(osPriorityRealtime);
POTLESS_2 12:7f3aadd79f89 433
POTLESS_2 12:7f3aadd79f89 434 #ifdef PID_MODE
POTLESS_2 12:7f3aadd79f89 435 //Init PID
POTLESS_2 12:7f3aadd79f89 436 //Entrée PPO2 entre 100 et 1000 mb
POTLESS_2 12:7f3aadd79f89 437 control_Servo.setInputLimits(Min_Input, Max_Input);
POTLESS_2 12:7f3aadd79f89 438 //Sortie servo entre 0 et 100 %
POTLESS_2 12:7f3aadd79f89 439 control_Servo.setOutputLimits(Min_Output, Max_Output);
POTLESS_2 15:efd3b3bf3f37 440 //Mode auto au démarrage
POTLESS_2 12:7f3aadd79f89 441 control_Servo.setMode(AUTO_MODE);
POTLESS_2 12:7f3aadd79f89 442 //Consigne à x mb
POTLESS_2 12:7f3aadd79f89 443 control_Servo.setSetPoint(consigne);
POTLESS_2 12:7f3aadd79f89 444
POTLESS_2 15:efd3b3bf3f37 445 #endif
POTLESS_2 15:efd3b3bf3f37 446
POTLESS_2 11:b2feed92584a 447 #ifdef VT100
POTLESS_2 8:a750d531b381 448 printf(CLS);
POTLESS_2 11:b2feed92584a 449 #endif
POTLESS_2 9:04bfdfc029cb 450
POTLESS_2 7:ad15c386e960 451 while (true) {
POTLESS_2 6:ebed9093d661 452
POTLESS_2 7:ad15c386e960 453 //Démarrage du Timer mesurant le temps d'éxecution du code
POTLESS_2 7:ad15c386e960 454 REAL_RATE.start();
POTLESS_2 7:ad15c386e960 455
POTLESS_2 6:ebed9093d661 456 if (newParamFlag) {
POTLESS_2 6:ebed9093d661 457 serialMonit.printf("Param = %s\r\n", param);
POTLESS_2 9:04bfdfc029cb 458 Decoding_Message(param);
POTLESS_2 9:04bfdfc029cb 459 }
POTLESS_2 9:04bfdfc029cb 460
POTLESS_2 9:04bfdfc029cb 461 if (newAndroidFlag) {
POTLESS_2 9:04bfdfc029cb 462 serialMonit.printf("Android = %s\r\n", Android);
POTLESS_2 9:04bfdfc029cb 463 Decoding_Message_Android(Android);
POTLESS_2 6:ebed9093d661 464 }
POTLESS_2 7:ad15c386e960 465
POTLESS_2 15:efd3b3bf3f37 466 //Fabrication de la chaine Date / heure
POTLESS_2 15:efd3b3bf3f37 467 seconds = time(NULL);
POTLESS_2 15:efd3b3bf3f37 468 char Time_buf[32];
POTLESS_2 15:efd3b3bf3f37 469 strftime(Time_buf, 32, "%D %I-%M-%S ", localtime(&seconds));
POTLESS_2 15:efd3b3bf3f37 470
POTLESS_2 15:efd3b3bf3f37 471 //Fabrication de la chaine à enregistrer
POTLESS_2 15:efd3b3bf3f37 472 sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%3.2f:%3.2f:%d:%f:%f:%f",
POTLESS_2 15:efd3b3bf3f37 473 Time_buf,
POTLESS_2 15:efd3b3bf3f37 474 co2,
POTLESS_2 15:efd3b3bf3f37 475 ppO2,
POTLESS_2 15:efd3b3bf3f37 476 pression,
POTLESS_2 15:efd3b3bf3f37 477 Temp1,
POTLESS_2 15:efd3b3bf3f37 478 Temp2,
POTLESS_2 15:efd3b3bf3f37 479 Humid,
POTLESS_2 15:efd3b3bf3f37 480 CellO2_1,
POTLESS_2 15:efd3b3bf3f37 481 CellO2_2,
POTLESS_2 15:efd3b3bf3f37 482 volet_poumon_Position,
POTLESS_2 15:efd3b3bf3f37 483 volet_fuite_Position,
POTLESS_2 15:efd3b3bf3f37 484 MODE_FLAG,
POTLESS_2 15:efd3b3bf3f37 485 Kc,
POTLESS_2 15:efd3b3bf3f37 486 Ti,
POTLESS_2 15:efd3b3bf3f37 487 Td);
POTLESS_2 15:efd3b3bf3f37 488
POTLESS_2 15:efd3b3bf3f37 489 //Enregistrement de la chaine
POTLESS_2 15:efd3b3bf3f37 490 sensors.Write_SD((string)to_store);
POTLESS_2 15:efd3b3bf3f37 491
POTLESS_2 15:efd3b3bf3f37 492 //Pour Android on ajoute < et > pour décoder l'arrivée du message
POTLESS_2 9:04bfdfc029cb 493 if (NEED_ANDROID_OUTPUT == 1) {
POTLESS_2 15:efd3b3bf3f37 494 sprintf(to_android,"<%s>",to_store);
POTLESS_2 12:7f3aadd79f89 495 ANDROID(to_android);
POTLESS_2 9:04bfdfc029cb 496 }
POTLESS_2 15:efd3b3bf3f37 497
POTLESS_2 13:22a96dc67e85 498 //Vers le moniteur dérie
POTLESS_2 13:22a96dc67e85 499 Affichage_moniteur();
POTLESS_2 15:efd3b3bf3f37 500
POTLESS_2 15:efd3b3bf3f37 501
POTLESS_2 15:efd3b3bf3f37 502 #ifdef PID_MODE
POTLESS_2 15:efd3b3bf3f37 503 //Update du PID
POTLESS_2 15:efd3b3bf3f37 504 control_Servo.setProcessValue(ppO2);
POTLESS_2 15:efd3b3bf3f37 505 //Nouvelle sortie servo
POTLESS_2 15:efd3b3bf3f37 506 Commande_PID = control_Servo.compute();
POTLESS_2 15:efd3b3bf3f37 507 //Appliquer la consigne
POTLESS_2 15:efd3b3bf3f37 508 servo_fuite = Commande_PID;
POTLESS_2 12:7f3aadd79f89 509 #endif
POTLESS_2 15:efd3b3bf3f37 510
POTLESS_2 7:ad15c386e960 511 //Arrêt du Timer mesurant le temps d'éxecution du code
POTLESS_2 7:ad15c386e960 512 REAL_RATE.stop();
POTLESS_2 7:ad15c386e960 513 //Définition de la nouvelle valeur du temps d'échantillonage du PID.
POTLESS_2 7:ad15c386e960 514 RATE = REAL_RATE.read();
POTLESS_2 7:ad15c386e960 515 //Reset du Timer
POTLESS_2 7:ad15c386e960 516 REAL_RATE.reset();
POTLESS_2 12:7f3aadd79f89 517
POTLESS_2 11:b2feed92584a 518 //Pour ralentir le code à 1 seconde fixe quelque soit les intéruptions du loop....
POTLESS_2 12:7f3aadd79f89 519 RATE_TRUE = (Ref_Time - RATE) * 1000;
POTLESS_2 15:efd3b3bf3f37 520
POTLESS_2 11:b2feed92584a 521 wait_ms(RATE_TRUE);
POTLESS_2 4:d84250f67dec 522 }
POTLESS_2 4:d84250f67dec 523 }