![](/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@11:b2feed92584a, 2017-11-05 (annotated)
- Committer:
- POTLESS_2
- Date:
- Sun Nov 05 16:03:02 2017 +0000
- Revision:
- 11:b2feed92584a
- Parent:
- 10:aca745a66d51
- Child:
- 12:7f3aadd79f89
Version avec delta mode et android qui marche...
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:d84250f67dec | 5 | |
POTLESS_2 | 10:aca745a66d51 | 6 | //Commandes des servos |
POTLESS_2 | 10:aca745a66d51 | 7 | #define PWM_SERVO_POUMON PB_15 |
POTLESS_2 | 10:aca745a66d51 | 8 | #define PWM_SERVO_FUITE PB_1 |
POTLESS_2 | 10:aca745a66d51 | 9 | //Retour des servos |
POTLESS_2 | 10:aca745a66d51 | 10 | #define FEED_BACK_SERVO_POUMON PC_2 |
POTLESS_2 | 10:aca745a66d51 | 11 | #define FEED_BACK_SERVO_FUITE PC_3 |
POTLESS_2 | 9:04bfdfc029cb | 12 | |
POTLESS_2 | 9:04bfdfc029cb | 13 | //Ecrit dans le moniteur série de la tablette à 9600 bds si sur 1, penser à mettre NEED_CONSOLE_OUTPUT à 0 |
POTLESS_2 | 9:04bfdfc029cb | 14 | #define NEED_ANDROID_OUTPUT 1 |
POTLESS_2 | 9:04bfdfc029cb | 15 | |
POTLESS_2 | 10:aca745a66d51 | 16 | //Sortie en mode VT100, à commenter si on veut une sortie type Arduino |
POTLESS_2 | 10:aca745a66d51 | 17 | //#define VT100 |
POTLESS_2 | 9:04bfdfc029cb | 18 | |
POTLESS_2 | 11:b2feed92584a | 19 | //Mode Delta constant entre les servos à commenter si on en veut pas |
POTLESS_2 | 11:b2feed92584a | 20 | //#define DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 21 | float delta = 0.0f; |
POTLESS_2 | 11:b2feed92584a | 22 | |
POTLESS_2 | 11:b2feed92584a | 23 | #ifndef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 24 | int DELTA_FLAG = 0; |
POTLESS_2 | 11:b2feed92584a | 25 | #endif |
POTLESS_2 | 11:b2feed92584a | 26 | |
POTLESS_2 | 11:b2feed92584a | 27 | #ifdef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 28 | int DELTA_FLAG = 1; |
POTLESS_2 | 11:b2feed92584a | 29 | #endif |
POTLESS_2 | 11:b2feed92584a | 30 | |
POTLESS_2 | 9:04bfdfc029cb | 31 | #if NEED_ANDROID_OUTPUT |
POTLESS_2 | 9:04bfdfc029cb | 32 | #define ANDROID(...) { android.printf(__VA_ARGS__); } |
POTLESS_2 | 9:04bfdfc029cb | 33 | #else |
POTLESS_2 | 9:04bfdfc029cb | 34 | #define ANDROID(...) |
POTLESS_2 | 9:04bfdfc029cb | 35 | #endif |
POTLESS_2 | 9:04bfdfc029cb | 36 | |
POTLESS_2 | 7:ad15c386e960 | 37 | //Moniteur série |
POTLESS_2 | 9:04bfdfc029cb | 38 | Serial serialMonit(USBTX,USBRX,9600); |
POTLESS_2 | 10:aca745a66d51 | 39 | |
POTLESS_2 | 9:04bfdfc029cb | 40 | //COM Série vers Android, Serial 3 |
POTLESS_2 | 9:04bfdfc029cb | 41 | Serial android(PC_10,PC_11,9600); |
POTLESS_2 | 4:d84250f67dec | 42 | |
POTLESS_2 | 7:ad15c386e960 | 43 | //Init de la lib ARNSRS; |
POTLESS_2 | 7:ad15c386e960 | 44 | SENSOR_HEAD_REV_B sensors; |
potless | 2:4a8bf1d53439 | 45 | |
POTLESS_2 | 6:ebed9093d661 | 46 | //pour Param Cozir |
POTLESS_2 | 9:04bfdfc029cb | 47 | const int sizeParam = 20; |
POTLESS_2 | 11:b2feed92584a | 48 | char param[sizeParam]; |
POTLESS_2 | 11:b2feed92584a | 49 | volatile int indexParam = 0; |
POTLESS_2 | 6:ebed9093d661 | 50 | bool newParamFlag = false; |
potless | 2:4a8bf1d53439 | 51 | |
POTLESS_2 | 9:04bfdfc029cb | 52 | //pour Commandes Android |
POTLESS_2 | 9:04bfdfc029cb | 53 | const int sizeAndroid = 20; |
POTLESS_2 | 11:b2feed92584a | 54 | char Android[sizeAndroid]; |
POTLESS_2 | 11:b2feed92584a | 55 | volatile int indexAndroid = 0; |
POTLESS_2 | 9:04bfdfc029cb | 56 | bool newAndroidFlag = false; |
POTLESS_2 | 9:04bfdfc029cb | 57 | |
POTLESS_2 | 7:ad15c386e960 | 58 | //Variables de stockage des infos capteurs |
POTLESS_2 | 7:ad15c386e960 | 59 | int co2 = 0; |
POTLESS_2 | 7:ad15c386e960 | 60 | float pression = 0; |
POTLESS_2 | 9:04bfdfc029cb | 61 | float Temp1 = 0; |
POTLESS_2 | 7:ad15c386e960 | 62 | int ppO2 = 0; |
POTLESS_2 | 7:ad15c386e960 | 63 | int CellO2_1 = 0; |
POTLESS_2 | 7:ad15c386e960 | 64 | int CellO2_2 = 0; |
POTLESS_2 | 7:ad15c386e960 | 65 | |
POTLESS_2 | 7:ad15c386e960 | 66 | //Mesure du tempsd'éxecution du loop |
POTLESS_2 | 7:ad15c386e960 | 67 | Timer REAL_RATE; |
POTLESS_2 | 7:ad15c386e960 | 68 | float RATE = 0; |
POTLESS_2 | 11:b2feed92584a | 69 | float RATE_TRUE = 0; |
POTLESS_2 | 7:ad15c386e960 | 70 | |
POTLESS_2 | 8:a750d531b381 | 71 | //HTU21D |
POTLESS_2 | 8:a750d531b381 | 72 | HTU21D temphumid(PB_9, PB_8); //Temp humid sensor || SDA, SCL |
POTLESS_2 | 9:04bfdfc029cb | 73 | float Temp2; |
POTLESS_2 | 9:04bfdfc029cb | 74 | int Humid; |
POTLESS_2 | 9:04bfdfc029cb | 75 | |
POTLESS_2 | 9:04bfdfc029cb | 76 | //Data LOG |
POTLESS_2 | 9:04bfdfc029cb | 77 | char to_store[50]; |
POTLESS_2 | 9:04bfdfc029cb | 78 | time_t seconds; |
POTLESS_2 | 8:a750d531b381 | 79 | |
POTLESS_2 | 8:a750d531b381 | 80 | //VT100 |
POTLESS_2 | 8:a750d531b381 | 81 | static const char CLS[] = "\x1B[2J"; |
POTLESS_2 | 8:a750d531b381 | 82 | static const char HOME[] = "\x1B[H"; |
POTLESS_2 | 8:a750d531b381 | 83 | |
POTLESS_2 | 7:ad15c386e960 | 84 | //Thread d'intérogation des capteurs |
POTLESS_2 | 7:ad15c386e960 | 85 | Thread thread; |
POTLESS_2 | 7:ad15c386e960 | 86 | |
POTLESS_2 | 10:aca745a66d51 | 87 | //Contrôle des servos |
POTLESS_2 | 10:aca745a66d51 | 88 | PwmOut servo_poumon(PWM_SERVO_POUMON); |
POTLESS_2 | 10:aca745a66d51 | 89 | AnalogIn FeedBack_Servo_Poumon(FEED_BACK_SERVO_POUMON); |
POTLESS_2 | 10:aca745a66d51 | 90 | float volet_poumon_Position; |
POTLESS_2 | 10:aca745a66d51 | 91 | float Limit_min_Servo_Poumon, Limit_max_Servo_Poumon; |
POTLESS_2 | 10:aca745a66d51 | 92 | float Delta_FB_1; |
POTLESS_2 | 10:aca745a66d51 | 93 | |
POTLESS_2 | 10:aca745a66d51 | 94 | PwmOut servo_fuite(PWM_SERVO_FUITE); |
POTLESS_2 | 10:aca745a66d51 | 95 | AnalogIn FeedBack_Servo_Fuite(FEED_BACK_SERVO_FUITE); |
POTLESS_2 | 10:aca745a66d51 | 96 | float volet_fuite_Position; |
POTLESS_2 | 10:aca745a66d51 | 97 | float Limit_min_Servo_Fuite, Limit_max_Servo_Fuite; |
POTLESS_2 | 10:aca745a66d51 | 98 | float Delta_FB_2; |
POTLESS_2 | 10:aca745a66d51 | 99 | |
POTLESS_2 | 10:aca745a66d51 | 100 | bool FLAG_COMMAND_SERVO = false; |
POTLESS_2 | 10:aca745a66d51 | 101 | |
POTLESS_2 | 10:aca745a66d51 | 102 | //Buffer pour Commandes servo |
POTLESS_2 | 10:aca745a66d51 | 103 | char commande_servo[50]; |
POTLESS_2 | 10:aca745a66d51 | 104 | int indexCommande; |
POTLESS_2 | 10:aca745a66d51 | 105 | |
POTLESS_2 | 10:aca745a66d51 | 106 | //Remap d'une valeur |
POTLESS_2 | 10:aca745a66d51 | 107 | float remap(float x, float in_min, float in_max, float out_min, float out_max) |
POTLESS_2 | 10:aca745a66d51 | 108 | { |
POTLESS_2 | 10:aca745a66d51 | 109 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
POTLESS_2 | 10:aca745a66d51 | 110 | } |
POTLESS_2 | 10:aca745a66d51 | 111 | |
POTLESS_2 | 10:aca745a66d51 | 112 | |
POTLESS_2 | 10:aca745a66d51 | 113 | //Thread d'intérogation des capteurs, positions servo et constitution de la chaine |
POTLESS_2 | 10:aca745a66d51 | 114 | void Get_Info_thread() |
POTLESS_2 | 6:ebed9093d661 | 115 | { |
POTLESS_2 | 7:ad15c386e960 | 116 | while (true) { |
POTLESS_2 | 7:ad15c386e960 | 117 | |
POTLESS_2 | 7:ad15c386e960 | 118 | //CO2 sur Cozir |
POTLESS_2 | 7:ad15c386e960 | 119 | co2 = sensors.requestCO2(); |
POTLESS_2 | 7:ad15c386e960 | 120 | //P / T sur MS5837 |
POTLESS_2 | 7:ad15c386e960 | 121 | pression = sensors.requestPress(); |
POTLESS_2 | 9:04bfdfc029cb | 122 | Temp1 = sensors.requestTemp(); |
POTLESS_2 | 7:ad15c386e960 | 123 | //PPO2 sur ADS1015 |
POTLESS_2 | 7:ad15c386e960 | 124 | ppO2 = sensors.requestPpO2(); |
POTLESS_2 | 7:ad15c386e960 | 125 | //Cell O2 en mV |
POTLESS_2 | 7:ad15c386e960 | 126 | CellO2_1 = sensors.requestCellO2_1(); |
POTLESS_2 | 7:ad15c386e960 | 127 | CellO2_2 = sensors.requestCellO2_2(); |
POTLESS_2 | 9:04bfdfc029cb | 128 | |
POTLESS_2 | 8:a750d531b381 | 129 | //HTU21D |
POTLESS_2 | 9:04bfdfc029cb | 130 | Temp2 = temphumid.sample_ctemp(); |
POTLESS_2 | 9:04bfdfc029cb | 131 | Humid = temphumid.sample_humid(); |
POTLESS_2 | 9:04bfdfc029cb | 132 | |
POTLESS_2 | 10:aca745a66d51 | 133 | //Retour position des servos |
POTLESS_2 | 10:aca745a66d51 | 134 | volet_poumon_Position = 90 - remap(FeedBack_Servo_Poumon, Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90); |
POTLESS_2 | 10:aca745a66d51 | 135 | volet_fuite_Position = remap(FeedBack_Servo_Fuite, Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90); |
POTLESS_2 | 10:aca745a66d51 | 136 | |
POTLESS_2 | 9:04bfdfc029cb | 137 | //Fabrication de la chaine Date / heure |
POTLESS_2 | 9:04bfdfc029cb | 138 | seconds = time(NULL); |
POTLESS_2 | 9:04bfdfc029cb | 139 | char Time_buf[32]; |
POTLESS_2 | 9:04bfdfc029cb | 140 | strftime(Time_buf, 32, "%D %I-%M-%S ", localtime(&seconds)); |
POTLESS_2 | 9:04bfdfc029cb | 141 | |
POTLESS_2 | 9:04bfdfc029cb | 142 | //Fabrication de la chaine à enregistrer |
POTLESS_2 | 11:b2feed92584a | 143 | sprintf(to_store,"<%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%3.2f:%3.2f:%d>", |
POTLESS_2 | 9:04bfdfc029cb | 144 | Time_buf, |
POTLESS_2 | 9:04bfdfc029cb | 145 | co2, |
POTLESS_2 | 9:04bfdfc029cb | 146 | ppO2, |
POTLESS_2 | 9:04bfdfc029cb | 147 | pression, |
POTLESS_2 | 9:04bfdfc029cb | 148 | Temp1, |
POTLESS_2 | 9:04bfdfc029cb | 149 | Temp2, |
POTLESS_2 | 9:04bfdfc029cb | 150 | Humid, |
POTLESS_2 | 9:04bfdfc029cb | 151 | CellO2_1, |
POTLESS_2 | 10:aca745a66d51 | 152 | CellO2_2, |
POTLESS_2 | 10:aca745a66d51 | 153 | volet_poumon_Position, |
POTLESS_2 | 11:b2feed92584a | 154 | volet_fuite_Position, |
POTLESS_2 | 11:b2feed92584a | 155 | DELTA_FLAG); |
POTLESS_2 | 10:aca745a66d51 | 156 | } |
potless | 2:4a8bf1d53439 | 157 | } |
POTLESS_2 | 4:d84250f67dec | 158 | |
POTLESS_2 | 7:ad15c386e960 | 159 | void Affichage_moniteur() |
POTLESS_2 | 4:d84250f67dec | 160 | { |
POTLESS_2 | 10:aca745a66d51 | 161 | #ifndef VT100 |
POTLESS_2 | 9:04bfdfc029cb | 162 | printf("\r\n"); |
POTLESS_2 | 9:04bfdfc029cb | 163 | printf(" CO2 = %d ppm\r\n" , co2); |
POTLESS_2 | 9:04bfdfc029cb | 164 | printf(" PPO2 = %d mb\r\n", ppO2); |
POTLESS_2 | 9:04bfdfc029cb | 165 | printf(" Pression = %f msw\r\n", pression); |
POTLESS_2 | 10:aca745a66d51 | 166 | printf(" Temp MS5837 = %f C\r\n", Temp1); |
POTLESS_2 | 10:aca745a66d51 | 167 | printf(" Temp HTU21D = %f C\n\r", Temp2); |
POTLESS_2 | 9:04bfdfc029cb | 168 | printf(" Humidity = %d %%\n\r", Humid); |
POTLESS_2 | 8:a750d531b381 | 169 | printf("\n\r"); |
POTLESS_2 | 8:a750d531b381 | 170 | printf(" Cell O2 n 1 = %d\r\n" , CellO2_1); |
POTLESS_2 | 8:a750d531b381 | 171 | printf(" Cell O2 n 2 = %d\r\n" , CellO2_2); |
POTLESS_2 | 7:ad15c386e960 | 172 | printf("\r\n"); |
POTLESS_2 | 9:04bfdfc029cb | 173 | printf("\n"); |
POTLESS_2 | 10:aca745a66d51 | 174 | printf(" Volet Poumon = %3.2f%\r\n" , volet_poumon_Position); |
POTLESS_2 | 10:aca745a66d51 | 175 | printf(" Volet Fuite = %3.2f%\r\n" , volet_fuite_Position); |
POTLESS_2 | 10:aca745a66d51 | 176 | printf("\r\n"); |
POTLESS_2 | 11:b2feed92584a | 177 | printf("Temps d execution de la boucle = %f secondes\n", (RATE + RATE_TRUE) / 1000); |
POTLESS_2 | 9:04bfdfc029cb | 178 | printf("\r\n", ""); |
POTLESS_2 | 9:04bfdfc029cb | 179 | printf("A enregistrer = %s\n", to_store); |
POTLESS_2 | 9:04bfdfc029cb | 180 | printf("\r\n"); |
POTLESS_2 | 10:aca745a66d51 | 181 | #endif |
POTLESS_2 | 9:04bfdfc029cb | 182 | |
POTLESS_2 | 10:aca745a66d51 | 183 | #ifdef VT100 |
POTLESS_2 | 8:a750d531b381 | 184 | printf(HOME); |
POTLESS_2 | 8:a750d531b381 | 185 | printf("\x1b[30m"); |
POTLESS_2 | 9:04bfdfc029cb | 186 | printf("\x1b[0m\r CO2 = \x1b[1m\x1b[K%d ppm\n", co2); |
POTLESS_2 | 9:04bfdfc029cb | 187 | printf("\x1b[0m\r PPO2 = \x1b[1m\x1b[K%d mb\n", ppO2); |
POTLESS_2 | 9:04bfdfc029cb | 188 | printf("\n"); |
POTLESS_2 | 9:04bfdfc029cb | 189 | printf("\x1b[0m\r Pression = \x1b[1m\x1b[K%.2f msw\n", pression); |
POTLESS_2 | 9:04bfdfc029cb | 190 | printf("\n"); |
POTLESS_2 | 10:aca745a66d51 | 191 | printf("\x1b[0m\r Temp MS5837 = \x1b[1m\x1b[K%.2f C\n", Temp1); |
POTLESS_2 | 10:aca745a66d51 | 192 | printf("\x1b[0m\r Temp HTU21D = \x1b[1m\x1b[K%.2f C\n", Temp2); |
POTLESS_2 | 9:04bfdfc029cb | 193 | printf("\n"); |
POTLESS_2 | 9:04bfdfc029cb | 194 | printf("\x1b[0m\r Humidity = \x1b[1m\x1b[K%d %\n", Humid); |
POTLESS_2 | 9:04bfdfc029cb | 195 | printf("\n"); |
POTLESS_2 | 8:a750d531b381 | 196 | printf("\x1b[0m\r Cell O2 n 1 = \x1b[1m\x1b[K%d\n", CellO2_1); |
POTLESS_2 | 8:a750d531b381 | 197 | printf("\x1b[0m\r Cell O2 n 2 = \x1b[1m\x1b[K%d\n", CellO2_2); |
POTLESS_2 | 8:a750d531b381 | 198 | printf("\n"); |
POTLESS_2 | 10:aca745a66d51 | 199 | printf("\x1b[0m\r Volet Poumon = \x1b[1m\x1b[K%3.2f%\n", volet_poumon_Position); |
POTLESS_2 | 10:aca745a66d51 | 200 | printf("\x1b[0m\r Volet Fuite = \x1b[1m\x1b[K%3.2f%\n", volet_fuite_Position); |
POTLESS_2 | 10:aca745a66d51 | 201 | printf("\n"); |
POTLESS_2 | 11:b2feed92584a | 202 | printf("\x1b[0m\r Temps d execution de la boucle = \x1b[1m\x1b[K%f secondes\n", (RATE + RATE_TRUE) / 1000); |
POTLESS_2 | 9:04bfdfc029cb | 203 | printf("\r\n", ""); |
POTLESS_2 | 9:04bfdfc029cb | 204 | printf("\x1b[0m\r A enregistrer = \x1b[1m\x1b[K%s\n", to_store); |
POTLESS_2 | 9:04bfdfc029cb | 205 | printf("\r\n", ""); |
POTLESS_2 | 10:aca745a66d51 | 206 | #endif |
POTLESS_2 | 4:d84250f67dec | 207 | } |
POTLESS_2 | 4:d84250f67dec | 208 | |
POTLESS_2 | 11:b2feed92584a | 209 | //Callback de l'intérruption des envois de commandes depuis le terminal |
POTLESS_2 | 6:ebed9093d661 | 210 | void callbackParam() |
POTLESS_2 | 4:d84250f67dec | 211 | { |
POTLESS_2 | 6:ebed9093d661 | 212 | while(serialMonit.readable()) { |
POTLESS_2 | 6:ebed9093d661 | 213 | if (indexParam == sizeParam) //éviter la saturation du buffer |
POTLESS_2 | 6:ebed9093d661 | 214 | serialMonit.getc(); |
POTLESS_2 | 6:ebed9093d661 | 215 | else |
POTLESS_2 | 6:ebed9093d661 | 216 | param [indexParam++] = serialMonit.getc();//chargement du buffer dans le message |
POTLESS_2 | 6:ebed9093d661 | 217 | if ((indexParam == sizeParam) || (param[indexParam -1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut... |
POTLESS_2 | 6:ebed9093d661 | 218 | param[indexParam] = 0; |
POTLESS_2 | 6:ebed9093d661 | 219 | newParamFlag = true; |
POTLESS_2 | 6:ebed9093d661 | 220 | } |
POTLESS_2 | 4:d84250f67dec | 221 | } |
POTLESS_2 | 6:ebed9093d661 | 222 | } |
POTLESS_2 | 4:d84250f67dec | 223 | |
POTLESS_2 | 9:04bfdfc029cb | 224 | //Callback de l'intérruption des envois de commandes depuis Android |
POTLESS_2 | 9:04bfdfc029cb | 225 | void callbackAndroid() |
POTLESS_2 | 9:04bfdfc029cb | 226 | { |
POTLESS_2 | 9:04bfdfc029cb | 227 | while(android.readable()) { |
POTLESS_2 | 9:04bfdfc029cb | 228 | if (indexAndroid == sizeAndroid) //éviter la saturation du buffer |
POTLESS_2 | 9:04bfdfc029cb | 229 | android.getc(); |
POTLESS_2 | 9:04bfdfc029cb | 230 | else |
POTLESS_2 | 9:04bfdfc029cb | 231 | Android [indexAndroid++] = android.getc();//chargement du buffer dans le message |
POTLESS_2 | 9:04bfdfc029cb | 232 | if ((indexAndroid == sizeAndroid) || (Android[indexAndroid -1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut... |
POTLESS_2 | 9:04bfdfc029cb | 233 | Android[indexAndroid] = 0; |
POTLESS_2 | 9:04bfdfc029cb | 234 | newAndroidFlag = true; |
POTLESS_2 | 9:04bfdfc029cb | 235 | } |
POTLESS_2 | 9:04bfdfc029cb | 236 | } |
POTLESS_2 | 9:04bfdfc029cb | 237 | } |
POTLESS_2 | 11:b2feed92584a | 238 | |
POTLESS_2 | 9:04bfdfc029cb | 239 | void Decoding_Message(char message []) |
POTLESS_2 | 9:04bfdfc029cb | 240 | { |
POTLESS_2 | 9:04bfdfc029cb | 241 | char *commande = 0; |
POTLESS_2 | 9:04bfdfc029cb | 242 | int valeur = 0; |
POTLESS_2 | 9:04bfdfc029cb | 243 | |
POTLESS_2 | 9:04bfdfc029cb | 244 | sscanf(message,"%s %d",&commande , &valeur); |
POTLESS_2 | 9:04bfdfc029cb | 245 | |
POTLESS_2 | 9:04bfdfc029cb | 246 | if ((char)commande == 'T') { |
POTLESS_2 | 9:04bfdfc029cb | 247 | set_time(valeur); |
POTLESS_2 | 11:b2feed92584a | 248 | } else if ((char)commande == 'I'){ |
POTLESS_2 | 10:aca745a66d51 | 249 | servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 250 | #ifdef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 251 | float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 252 | if(Sf >= 0 && Sf <= 90) |
POTLESS_2 | 11:b2feed92584a | 253 | servo_fuite = Sf; |
POTLESS_2 | 11:b2feed92584a | 254 | #endif |
POTLESS_2 | 10:aca745a66d51 | 255 | printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f); |
POTLESS_2 | 11:b2feed92584a | 256 | } else if ((char)commande == 'O'){ |
POTLESS_2 | 10:aca745a66d51 | 257 | servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 10:aca745a66d51 | 258 | printf(" Servo Fuite = %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f); |
POTLESS_2 | 11:b2feed92584a | 259 | } else if ((char)commande == 'D') { |
POTLESS_2 | 11:b2feed92584a | 260 | delta = valeur; |
POTLESS_2 | 11:b2feed92584a | 261 | #ifdef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 262 | float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 263 | if(Sf >= 0 && Sf <= 90) |
POTLESS_2 | 11:b2feed92584a | 264 | servo_fuite = Sf; |
POTLESS_2 | 11:b2feed92584a | 265 | #endif |
POTLESS_2 | 10:aca745a66d51 | 266 | } else if ((char)commande == 'R') { |
POTLESS_2 | 10:aca745a66d51 | 267 | NVIC_SystemReset(); |
POTLESS_2 | 11:b2feed92584a | 268 | ///////////////////////////////////////// |
POTLESS_2 | 11:b2feed92584a | 269 | //Pour rajouter une commande |
POTLESS_2 | 11:b2feed92584a | 270 | //} else if ((char)commande == 'X') { |
POTLESS_2 | 11:b2feed92584a | 271 | // attribuer à une VARIABLE = valeur; |
POTLESS_2 | 11:b2feed92584a | 272 | // ou une action, avec ou sans valeur |
POTLESS_2 | 11:b2feed92584a | 273 | ///////////////////////////////////////// |
POTLESS_2 | 10:aca745a66d51 | 274 | }else { |
POTLESS_2 | 9:04bfdfc029cb | 275 | sensors.cozirSend(message); |
POTLESS_2 | 9:04bfdfc029cb | 276 | } |
POTLESS_2 | 9:04bfdfc029cb | 277 | |
POTLESS_2 | 11:b2feed92584a | 278 | //wait_ms(100); |
POTLESS_2 | 9:04bfdfc029cb | 279 | strcpy(param," "); |
POTLESS_2 | 9:04bfdfc029cb | 280 | indexParam = 0; |
POTLESS_2 | 9:04bfdfc029cb | 281 | newParamFlag = false; |
POTLESS_2 | 9:04bfdfc029cb | 282 | } |
POTLESS_2 | 9:04bfdfc029cb | 283 | |
POTLESS_2 | 9:04bfdfc029cb | 284 | void Decoding_Message_Android(char message []) |
POTLESS_2 | 9:04bfdfc029cb | 285 | { |
POTLESS_2 | 9:04bfdfc029cb | 286 | char *commande = 0; |
POTLESS_2 | 9:04bfdfc029cb | 287 | int valeur = 0; |
POTLESS_2 | 9:04bfdfc029cb | 288 | |
POTLESS_2 | 9:04bfdfc029cb | 289 | sscanf(message,"%s %d",&commande , &valeur); |
POTLESS_2 | 9:04bfdfc029cb | 290 | |
POTLESS_2 | 9:04bfdfc029cb | 291 | if ((char)commande == 'T') { |
POTLESS_2 | 9:04bfdfc029cb | 292 | set_time(valeur); |
POTLESS_2 | 11:b2feed92584a | 293 | } else if ((char)commande == 'I'){ |
POTLESS_2 | 10:aca745a66d51 | 294 | servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 295 | #ifdef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 296 | float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 297 | if(Sf >= 0 && Sf <= 90) |
POTLESS_2 | 11:b2feed92584a | 298 | servo_fuite = Sf; |
POTLESS_2 | 11:b2feed92584a | 299 | #endif |
POTLESS_2 | 10:aca745a66d51 | 300 | printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f); |
POTLESS_2 | 11:b2feed92584a | 301 | } else if ((char)commande == 'O'){ |
POTLESS_2 | 10:aca745a66d51 | 302 | servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 303 | printf(" Servo Fuite = %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f); |
POTLESS_2 | 11:b2feed92584a | 304 | } else if ((char)commande == 'D') { |
POTLESS_2 | 11:b2feed92584a | 305 | delta = valeur; |
POTLESS_2 | 11:b2feed92584a | 306 | #ifdef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 307 | float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 308 | if(Sf >= 0 && Sf <= 90) |
POTLESS_2 | 11:b2feed92584a | 309 | servo_fuite = Sf; |
POTLESS_2 | 11:b2feed92584a | 310 | #endif |
POTLESS_2 | 11:b2feed92584a | 311 | ///////////////////////////////////////// |
POTLESS_2 | 11:b2feed92584a | 312 | //Pour rajouter une commande |
POTLESS_2 | 11:b2feed92584a | 313 | //} else if ((char)commande == 'X') { |
POTLESS_2 | 11:b2feed92584a | 314 | // attribuer à une VARIABLE = valeur; |
POTLESS_2 | 11:b2feed92584a | 315 | // ou une action, avec ou sans valeur |
POTLESS_2 | 11:b2feed92584a | 316 | ///////////////////////////////////////// |
POTLESS_2 | 9:04bfdfc029cb | 317 | } else if ((char)commande == 'R') { |
POTLESS_2 | 9:04bfdfc029cb | 318 | NVIC_SystemReset(); |
POTLESS_2 | 11:b2feed92584a | 319 | } else { |
POTLESS_2 | 11:b2feed92584a | 320 | sensors.cozirSend(message); |
POTLESS_2 | 9:04bfdfc029cb | 321 | } |
POTLESS_2 | 9:04bfdfc029cb | 322 | |
POTLESS_2 | 11:b2feed92584a | 323 | //wait_ms(100); |
POTLESS_2 | 9:04bfdfc029cb | 324 | strcpy(Android," "); |
POTLESS_2 | 9:04bfdfc029cb | 325 | indexAndroid = 0; |
POTLESS_2 | 9:04bfdfc029cb | 326 | newAndroidFlag = false; |
POTLESS_2 | 9:04bfdfc029cb | 327 | } |
POTLESS_2 | 9:04bfdfc029cb | 328 | |
POTLESS_2 | 10:aca745a66d51 | 329 | //Calibration des limites de feedback des servos |
POTLESS_2 | 10:aca745a66d51 | 330 | void Calibration_servo() |
POTLESS_2 | 10:aca745a66d51 | 331 | { |
POTLESS_2 | 10:aca745a66d51 | 332 | //Servos, calibration du feedback |
POTLESS_2 | 10:aca745a66d51 | 333 | printf(" Check Servos :\r\n"); |
POTLESS_2 | 10:aca745a66d51 | 334 | printf("\r\n"); |
POTLESS_2 | 10:aca745a66d51 | 335 | 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 | 336 | servo_fuite.period(0.001); |
POTLESS_2 | 10:aca745a66d51 | 337 | //Rentrer les servos à fond |
POTLESS_2 | 10:aca745a66d51 | 338 | servo_poumon = 0; |
POTLESS_2 | 10:aca745a66d51 | 339 | servo_fuite = 0; |
POTLESS_2 | 10:aca745a66d51 | 340 | wait(4); |
POTLESS_2 | 10:aca745a66d51 | 341 | |
POTLESS_2 | 10:aca745a66d51 | 342 | Limit_min_Servo_Poumon = FeedBack_Servo_Poumon.read(); |
POTLESS_2 | 10:aca745a66d51 | 343 | wait_ms(100); |
POTLESS_2 | 10:aca745a66d51 | 344 | Limit_min_Servo_Fuite = FeedBack_Servo_Fuite.read(); |
POTLESS_2 | 10:aca745a66d51 | 345 | |
POTLESS_2 | 10:aca745a66d51 | 346 | printf(" FeedBack Servo POUMON min = %f\n", Limit_min_Servo_Poumon); |
POTLESS_2 | 10:aca745a66d51 | 347 | printf(" FeedBack Servo FUITE min = %f\n", Limit_min_Servo_Fuite); |
POTLESS_2 | 10:aca745a66d51 | 348 | printf("\r\n"); |
POTLESS_2 | 10:aca745a66d51 | 349 | |
POTLESS_2 | 10:aca745a66d51 | 350 | //Sortir les servos à fond |
POTLESS_2 | 10:aca745a66d51 | 351 | servo_poumon = 1; |
POTLESS_2 | 10:aca745a66d51 | 352 | servo_fuite = 1; |
POTLESS_2 | 10:aca745a66d51 | 353 | wait(4); |
POTLESS_2 | 10:aca745a66d51 | 354 | |
POTLESS_2 | 10:aca745a66d51 | 355 | Limit_max_Servo_Poumon = FeedBack_Servo_Poumon.read(); |
POTLESS_2 | 10:aca745a66d51 | 356 | wait_ms(100); |
POTLESS_2 | 10:aca745a66d51 | 357 | Limit_max_Servo_Fuite = FeedBack_Servo_Fuite.read(); |
POTLESS_2 | 10:aca745a66d51 | 358 | |
POTLESS_2 | 10:aca745a66d51 | 359 | printf(" FeedBack Servo POUMON Max = %f\n", Limit_max_Servo_Poumon); |
POTLESS_2 | 10:aca745a66d51 | 360 | printf(" FeedBack Servo FUITE Max = %f\n", Limit_max_Servo_Fuite); |
POTLESS_2 | 10:aca745a66d51 | 361 | printf("\r\n"); |
POTLESS_2 | 10:aca745a66d51 | 362 | |
POTLESS_2 | 10:aca745a66d51 | 363 | //Position milieu |
POTLESS_2 | 10:aca745a66d51 | 364 | servo_poumon = 0.5; |
POTLESS_2 | 10:aca745a66d51 | 365 | servo_fuite = 0.5; |
POTLESS_2 | 10:aca745a66d51 | 366 | wait(4); |
POTLESS_2 | 10:aca745a66d51 | 367 | |
POTLESS_2 | 10:aca745a66d51 | 368 | //Mesure du delta consigne / feedback |
POTLESS_2 | 10:aca745a66d51 | 369 | Delta_FB_1 = abs(remap(FeedBack_Servo_Poumon.read(), Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90) - 45); |
POTLESS_2 | 10:aca745a66d51 | 370 | Delta_FB_2 = abs(remap(FeedBack_Servo_Fuite.read(), Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90) - 45); |
POTLESS_2 | 10:aca745a66d51 | 371 | |
POTLESS_2 | 10:aca745a66d51 | 372 | printf(" Delta Servo 1 = %f\n", Delta_FB_1); |
POTLESS_2 | 10:aca745a66d51 | 373 | printf(" Delta Servo 2 = %f\n", Delta_FB_2); |
POTLESS_2 | 10:aca745a66d51 | 374 | |
POTLESS_2 | 10:aca745a66d51 | 375 | if(Delta_FB_1 > 10 || Delta_FB_2 > 10) printf(" Delta Servos non satisfaisant...\r\n\r\n"); |
POTLESS_2 | 11:b2feed92584a | 376 | |
POTLESS_2 | 11:b2feed92584a | 377 | #ifdef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 378 | //Position initial delta |
POTLESS_2 | 11:b2feed92584a | 379 | servo_poumon = 0.5; |
POTLESS_2 | 11:b2feed92584a | 380 | servo_fuite = 0.5 + remap(delta, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 381 | #endif |
POTLESS_2 | 10:aca745a66d51 | 382 | } |
POTLESS_2 | 10:aca745a66d51 | 383 | |
POTLESS_2 | 4:d84250f67dec | 384 | int main() |
POTLESS_2 | 11:b2feed92584a | 385 | { |
POTLESS_2 | 11:b2feed92584a | 386 | //UNIX TIMESTAMP depuis le erminal MAC = date +%s + 7200 pour heure d'été..... |
POTLESS_2 | 10:aca745a66d51 | 387 | |
POTLESS_2 | 10:aca745a66d51 | 388 | Calibration_servo(); |
POTLESS_2 | 10:aca745a66d51 | 389 | |
POTLESS_2 | 11:b2feed92584a | 390 | sensors.Sensors_INIT(false, true, 5, SPOOLING, DIGI_FILTER32, CALIB_AIR); |
POTLESS_2 | 6:ebed9093d661 | 391 | |
POTLESS_2 | 6:ebed9093d661 | 392 | serialMonit.attach(&callbackParam, Serial::RxIrq); |
POTLESS_2 | 6:ebed9093d661 | 393 | |
POTLESS_2 | 9:04bfdfc029cb | 394 | android.attach(&callbackAndroid, Serial::RxIrq); |
POTLESS_2 | 9:04bfdfc029cb | 395 | |
POTLESS_2 | 7:ad15c386e960 | 396 | serialMonit.printf(" Demarrage...\r\n\r\n Entrez les comandes COZIR si besoin :\r\n"); |
POTLESS_2 | 6:ebed9093d661 | 397 | |
POTLESS_2 | 10:aca745a66d51 | 398 | thread.start(Get_Info_thread); |
POTLESS_2 | 9:04bfdfc029cb | 399 | |
POTLESS_2 | 7:ad15c386e960 | 400 | thread.set_priority(osPriorityRealtime); |
POTLESS_2 | 11:b2feed92584a | 401 | |
POTLESS_2 | 11:b2feed92584a | 402 | #ifdef VT100 |
POTLESS_2 | 8:a750d531b381 | 403 | printf(CLS); |
POTLESS_2 | 11:b2feed92584a | 404 | #endif |
POTLESS_2 | 9:04bfdfc029cb | 405 | |
POTLESS_2 | 7:ad15c386e960 | 406 | while (true) { |
POTLESS_2 | 6:ebed9093d661 | 407 | |
POTLESS_2 | 7:ad15c386e960 | 408 | //Démarrage du Timer mesurant le temps d'éxecution du code |
POTLESS_2 | 7:ad15c386e960 | 409 | REAL_RATE.start(); |
POTLESS_2 | 7:ad15c386e960 | 410 | |
POTLESS_2 | 7:ad15c386e960 | 411 | Affichage_moniteur(); |
POTLESS_2 | 9:04bfdfc029cb | 412 | |
POTLESS_2 | 6:ebed9093d661 | 413 | if (newParamFlag) { |
POTLESS_2 | 6:ebed9093d661 | 414 | serialMonit.printf("Param = %s\r\n", param); |
POTLESS_2 | 9:04bfdfc029cb | 415 | Decoding_Message(param); |
POTLESS_2 | 9:04bfdfc029cb | 416 | } |
POTLESS_2 | 9:04bfdfc029cb | 417 | |
POTLESS_2 | 9:04bfdfc029cb | 418 | if (newAndroidFlag) { |
POTLESS_2 | 9:04bfdfc029cb | 419 | serialMonit.printf("Android = %s\r\n", Android); |
POTLESS_2 | 9:04bfdfc029cb | 420 | Decoding_Message_Android(Android); |
POTLESS_2 | 6:ebed9093d661 | 421 | } |
POTLESS_2 | 7:ad15c386e960 | 422 | |
POTLESS_2 | 9:04bfdfc029cb | 423 | //Vers Android |
POTLESS_2 | 9:04bfdfc029cb | 424 | if (NEED_ANDROID_OUTPUT == 1) { |
POTLESS_2 | 9:04bfdfc029cb | 425 | ANDROID(to_store); |
POTLESS_2 | 9:04bfdfc029cb | 426 | } |
POTLESS_2 | 9:04bfdfc029cb | 427 | |
POTLESS_2 | 9:04bfdfc029cb | 428 | sensors.Write_SD((string)to_store); |
POTLESS_2 | 9:04bfdfc029cb | 429 | |
POTLESS_2 | 7:ad15c386e960 | 430 | //Arrêt du Timer mesurant le temps d'éxecution du code |
POTLESS_2 | 7:ad15c386e960 | 431 | REAL_RATE.stop(); |
POTLESS_2 | 7:ad15c386e960 | 432 | //Définition de la nouvelle valeur du temps d'échantillonage du PID. |
POTLESS_2 | 7:ad15c386e960 | 433 | RATE = REAL_RATE.read(); |
POTLESS_2 | 7:ad15c386e960 | 434 | //Reset du Timer |
POTLESS_2 | 7:ad15c386e960 | 435 | REAL_RATE.reset(); |
POTLESS_2 | 11:b2feed92584a | 436 | |
POTLESS_2 | 11:b2feed92584a | 437 | //Pour ralentir le code à 1 seconde fixe quelque soit les intéruptions du loop.... |
POTLESS_2 | 11:b2feed92584a | 438 | RATE_TRUE = (1000 - (RATE / 1000)); |
POTLESS_2 | 11:b2feed92584a | 439 | wait_ms(RATE_TRUE); |
POTLESS_2 | 4:d84250f67dec | 440 | } |
POTLESS_2 | 4:d84250f67dec | 441 | } |