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@12:7f3aadd79f89, 2017-11-09 (annotated)
- Committer:
- POTLESS_2
- Date:
- Thu Nov 09 15:47:49 2017 +0000
- Revision:
- 12:7f3aadd79f89
- Parent:
- 11:b2feed92584a
- Child:
- 13:22a96dc67e85
Version avec contr?le PID en plus...a tester...
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 | 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 | 12:7f3aadd79f89 | 117 | int consigne = 210; |
POTLESS_2 | 12:7f3aadd79f89 | 118 | int Max_Input = 1000; |
POTLESS_2 | 12:7f3aadd79f89 | 119 | int Min_Input = 100; |
POTLESS_2 | 12:7f3aadd79f89 | 120 | int Max_Output = 100; |
POTLESS_2 | 12:7f3aadd79f89 | 121 | int 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 | 9:04bfdfc029cb | 158 | //Fabrication de la chaine Date / heure |
POTLESS_2 | 9:04bfdfc029cb | 159 | seconds = time(NULL); |
POTLESS_2 | 9:04bfdfc029cb | 160 | char Time_buf[32]; |
POTLESS_2 | 9:04bfdfc029cb | 161 | strftime(Time_buf, 32, "%D %I-%M-%S ", localtime(&seconds)); |
POTLESS_2 | 9:04bfdfc029cb | 162 | |
POTLESS_2 | 9:04bfdfc029cb | 163 | //Fabrication de la chaine à enregistrer |
POTLESS_2 | 12:7f3aadd79f89 | 164 | sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%3.2f:%3.2f:%d", |
POTLESS_2 | 9:04bfdfc029cb | 165 | Time_buf, |
POTLESS_2 | 9:04bfdfc029cb | 166 | co2, |
POTLESS_2 | 9:04bfdfc029cb | 167 | ppO2, |
POTLESS_2 | 9:04bfdfc029cb | 168 | pression, |
POTLESS_2 | 9:04bfdfc029cb | 169 | Temp1, |
POTLESS_2 | 9:04bfdfc029cb | 170 | Temp2, |
POTLESS_2 | 9:04bfdfc029cb | 171 | Humid, |
POTLESS_2 | 9:04bfdfc029cb | 172 | CellO2_1, |
POTLESS_2 | 10:aca745a66d51 | 173 | CellO2_2, |
POTLESS_2 | 10:aca745a66d51 | 174 | volet_poumon_Position, |
POTLESS_2 | 11:b2feed92584a | 175 | volet_fuite_Position, |
POTLESS_2 | 12:7f3aadd79f89 | 176 | MODE_FLAG); |
POTLESS_2 | 12:7f3aadd79f89 | 177 | |
POTLESS_2 | 12:7f3aadd79f89 | 178 | if (NEED_ANDROID_OUTPUT == 1) { |
POTLESS_2 | 12:7f3aadd79f89 | 179 | sprintf(to_android,"<%s>",to_store); |
POTLESS_2 | 12:7f3aadd79f89 | 180 | } |
POTLESS_2 | 12:7f3aadd79f89 | 181 | |
POTLESS_2 | 12:7f3aadd79f89 | 182 | } |
potless | 2:4a8bf1d53439 | 183 | } |
POTLESS_2 | 4:d84250f67dec | 184 | |
POTLESS_2 | 7:ad15c386e960 | 185 | void Affichage_moniteur() |
POTLESS_2 | 4:d84250f67dec | 186 | { |
POTLESS_2 | 12:7f3aadd79f89 | 187 | #ifndef VT100 |
POTLESS_2 | 9:04bfdfc029cb | 188 | printf("\r\n"); |
POTLESS_2 | 9:04bfdfc029cb | 189 | printf(" CO2 = %d ppm\r\n" , co2); |
POTLESS_2 | 9:04bfdfc029cb | 190 | printf(" PPO2 = %d mb\r\n", ppO2); |
POTLESS_2 | 9:04bfdfc029cb | 191 | printf(" Pression = %f msw\r\n", pression); |
POTLESS_2 | 10:aca745a66d51 | 192 | printf(" Temp MS5837 = %f C\r\n", Temp1); |
POTLESS_2 | 10:aca745a66d51 | 193 | printf(" Temp HTU21D = %f C\n\r", Temp2); |
POTLESS_2 | 9:04bfdfc029cb | 194 | printf(" Humidity = %d %%\n\r", Humid); |
POTLESS_2 | 8:a750d531b381 | 195 | printf("\n\r"); |
POTLESS_2 | 8:a750d531b381 | 196 | printf(" Cell O2 n 1 = %d\r\n" , CellO2_1); |
POTLESS_2 | 8:a750d531b381 | 197 | printf(" Cell O2 n 2 = %d\r\n" , CellO2_2); |
POTLESS_2 | 7:ad15c386e960 | 198 | printf("\r\n"); |
POTLESS_2 | 9:04bfdfc029cb | 199 | printf("\n"); |
POTLESS_2 | 10:aca745a66d51 | 200 | printf(" Volet Poumon = %3.2f%\r\n" , volet_poumon_Position); |
POTLESS_2 | 10:aca745a66d51 | 201 | printf(" Volet Fuite = %3.2f%\r\n" , volet_fuite_Position); |
POTLESS_2 | 10:aca745a66d51 | 202 | printf("\r\n"); |
POTLESS_2 | 12:7f3aadd79f89 | 203 | printf("Temps d execution de la boucle = %f seconde(s)\n", (RATE + RATE_TRUE / 1000)); |
POTLESS_2 | 9:04bfdfc029cb | 204 | printf("\r\n", ""); |
POTLESS_2 | 9:04bfdfc029cb | 205 | printf("A enregistrer = %s\n", to_store); |
POTLESS_2 | 9:04bfdfc029cb | 206 | printf("\r\n"); |
POTLESS_2 | 10:aca745a66d51 | 207 | #endif |
POTLESS_2 | 9:04bfdfc029cb | 208 | |
POTLESS_2 | 12:7f3aadd79f89 | 209 | #ifdef VT100 |
POTLESS_2 | 8:a750d531b381 | 210 | printf(HOME); |
POTLESS_2 | 8:a750d531b381 | 211 | printf("\x1b[30m"); |
POTLESS_2 | 9:04bfdfc029cb | 212 | printf("\x1b[0m\r CO2 = \x1b[1m\x1b[K%d ppm\n", co2); |
POTLESS_2 | 9:04bfdfc029cb | 213 | printf("\x1b[0m\r PPO2 = \x1b[1m\x1b[K%d mb\n", ppO2); |
POTLESS_2 | 9:04bfdfc029cb | 214 | printf("\n"); |
POTLESS_2 | 9:04bfdfc029cb | 215 | printf("\x1b[0m\r Pression = \x1b[1m\x1b[K%.2f msw\n", pression); |
POTLESS_2 | 9:04bfdfc029cb | 216 | printf("\n"); |
POTLESS_2 | 10:aca745a66d51 | 217 | printf("\x1b[0m\r Temp MS5837 = \x1b[1m\x1b[K%.2f C\n", Temp1); |
POTLESS_2 | 10:aca745a66d51 | 218 | printf("\x1b[0m\r Temp HTU21D = \x1b[1m\x1b[K%.2f C\n", Temp2); |
POTLESS_2 | 9:04bfdfc029cb | 219 | printf("\n"); |
POTLESS_2 | 9:04bfdfc029cb | 220 | printf("\x1b[0m\r Humidity = \x1b[1m\x1b[K%d %\n", Humid); |
POTLESS_2 | 9:04bfdfc029cb | 221 | printf("\n"); |
POTLESS_2 | 8:a750d531b381 | 222 | printf("\x1b[0m\r Cell O2 n 1 = \x1b[1m\x1b[K%d\n", CellO2_1); |
POTLESS_2 | 8:a750d531b381 | 223 | printf("\x1b[0m\r Cell O2 n 2 = \x1b[1m\x1b[K%d\n", CellO2_2); |
POTLESS_2 | 8:a750d531b381 | 224 | printf("\n"); |
POTLESS_2 | 10:aca745a66d51 | 225 | printf("\x1b[0m\r Volet Poumon = \x1b[1m\x1b[K%3.2f%\n", volet_poumon_Position); |
POTLESS_2 | 10:aca745a66d51 | 226 | printf("\x1b[0m\r Volet Fuite = \x1b[1m\x1b[K%3.2f%\n", volet_fuite_Position); |
POTLESS_2 | 10:aca745a66d51 | 227 | printf("\n"); |
POTLESS_2 | 12:7f3aadd79f89 | 228 | 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 | 229 | printf("\r\n", ""); |
POTLESS_2 | 9:04bfdfc029cb | 230 | printf("\x1b[0m\r A enregistrer = \x1b[1m\x1b[K%s\n", to_store); |
POTLESS_2 | 9:04bfdfc029cb | 231 | printf("\r\n", ""); |
POTLESS_2 | 12:7f3aadd79f89 | 232 | #endif |
POTLESS_2 | 4:d84250f67dec | 233 | } |
POTLESS_2 | 4:d84250f67dec | 234 | |
POTLESS_2 | 11:b2feed92584a | 235 | //Callback de l'intérruption des envois de commandes depuis le terminal |
POTLESS_2 | 6:ebed9093d661 | 236 | void callbackParam() |
POTLESS_2 | 4:d84250f67dec | 237 | { |
POTLESS_2 | 6:ebed9093d661 | 238 | while(serialMonit.readable()) { |
POTLESS_2 | 6:ebed9093d661 | 239 | if (indexParam == sizeParam) //éviter la saturation du buffer |
POTLESS_2 | 6:ebed9093d661 | 240 | serialMonit.getc(); |
POTLESS_2 | 6:ebed9093d661 | 241 | else |
POTLESS_2 | 6:ebed9093d661 | 242 | param [indexParam++] = serialMonit.getc();//chargement du buffer dans le message |
POTLESS_2 | 6:ebed9093d661 | 243 | if ((indexParam == sizeParam) || (param[indexParam -1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut... |
POTLESS_2 | 6:ebed9093d661 | 244 | param[indexParam] = 0; |
POTLESS_2 | 6:ebed9093d661 | 245 | newParamFlag = true; |
POTLESS_2 | 6:ebed9093d661 | 246 | } |
POTLESS_2 | 4:d84250f67dec | 247 | } |
POTLESS_2 | 6:ebed9093d661 | 248 | } |
POTLESS_2 | 4:d84250f67dec | 249 | |
POTLESS_2 | 9:04bfdfc029cb | 250 | //Callback de l'intérruption des envois de commandes depuis Android |
POTLESS_2 | 9:04bfdfc029cb | 251 | void callbackAndroid() |
POTLESS_2 | 9:04bfdfc029cb | 252 | { |
POTLESS_2 | 9:04bfdfc029cb | 253 | while(android.readable()) { |
POTLESS_2 | 9:04bfdfc029cb | 254 | if (indexAndroid == sizeAndroid) //éviter la saturation du buffer |
POTLESS_2 | 9:04bfdfc029cb | 255 | android.getc(); |
POTLESS_2 | 9:04bfdfc029cb | 256 | else |
POTLESS_2 | 9:04bfdfc029cb | 257 | Android [indexAndroid++] = android.getc();//chargement du buffer dans le message |
POTLESS_2 | 9:04bfdfc029cb | 258 | if ((indexAndroid == sizeAndroid) || (Android[indexAndroid -1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut... |
POTLESS_2 | 9:04bfdfc029cb | 259 | Android[indexAndroid] = 0; |
POTLESS_2 | 9:04bfdfc029cb | 260 | newAndroidFlag = true; |
POTLESS_2 | 9:04bfdfc029cb | 261 | } |
POTLESS_2 | 9:04bfdfc029cb | 262 | } |
POTLESS_2 | 9:04bfdfc029cb | 263 | } |
POTLESS_2 | 11:b2feed92584a | 264 | |
POTLESS_2 | 9:04bfdfc029cb | 265 | void Decoding_Message(char message []) |
POTLESS_2 | 9:04bfdfc029cb | 266 | { |
POTLESS_2 | 9:04bfdfc029cb | 267 | char *commande = 0; |
POTLESS_2 | 9:04bfdfc029cb | 268 | int valeur = 0; |
POTLESS_2 | 9:04bfdfc029cb | 269 | |
POTLESS_2 | 9:04bfdfc029cb | 270 | sscanf(message,"%s %d",&commande , &valeur); |
POTLESS_2 | 9:04bfdfc029cb | 271 | |
POTLESS_2 | 9:04bfdfc029cb | 272 | if ((char)commande == 'T') { |
POTLESS_2 | 9:04bfdfc029cb | 273 | set_time(valeur); |
POTLESS_2 | 12:7f3aadd79f89 | 274 | } else if ((char)commande == 'I') { |
POTLESS_2 | 10:aca745a66d51 | 275 | servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 12:7f3aadd79f89 | 276 | #ifdef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 277 | float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 278 | if(Sf >= 0 && Sf <= 90) |
POTLESS_2 | 12:7f3aadd79f89 | 279 | servo_fuite = Sf; |
POTLESS_2 | 12:7f3aadd79f89 | 280 | #endif |
POTLESS_2 | 10:aca745a66d51 | 281 | printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f); |
POTLESS_2 | 12:7f3aadd79f89 | 282 | } else if ((char)commande == 'O') { |
POTLESS_2 | 10:aca745a66d51 | 283 | servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 12:7f3aadd79f89 | 284 | printf(" Servo Fuite = %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f); |
POTLESS_2 | 11:b2feed92584a | 285 | } else if ((char)commande == 'D') { |
POTLESS_2 | 11:b2feed92584a | 286 | delta = valeur; |
POTLESS_2 | 12:7f3aadd79f89 | 287 | #ifdef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 288 | float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 289 | if(Sf >= 0 && Sf <= 90) |
POTLESS_2 | 12:7f3aadd79f89 | 290 | servo_fuite = Sf; |
POTLESS_2 | 12:7f3aadd79f89 | 291 | #endif |
POTLESS_2 | 10:aca745a66d51 | 292 | } else if ((char)commande == 'R') { |
POTLESS_2 | 10:aca745a66d51 | 293 | NVIC_SystemReset(); |
POTLESS_2 | 12:7f3aadd79f89 | 294 | ///////////////////////////////////////// |
POTLESS_2 | 12:7f3aadd79f89 | 295 | //Pour rajouter une commande |
POTLESS_2 | 12:7f3aadd79f89 | 296 | //} else if ((char)commande == 'X') { |
POTLESS_2 | 12:7f3aadd79f89 | 297 | // attribuer à une VARIABLE = valeur; |
POTLESS_2 | 12:7f3aadd79f89 | 298 | // ou une action, avec ou sans valeur |
POTLESS_2 | 12:7f3aadd79f89 | 299 | ///////////////////////////////////////// |
POTLESS_2 | 12:7f3aadd79f89 | 300 | } else { |
POTLESS_2 | 9:04bfdfc029cb | 301 | sensors.cozirSend(message); |
POTLESS_2 | 9:04bfdfc029cb | 302 | } |
POTLESS_2 | 9:04bfdfc029cb | 303 | |
POTLESS_2 | 11:b2feed92584a | 304 | //wait_ms(100); |
POTLESS_2 | 9:04bfdfc029cb | 305 | strcpy(param," "); |
POTLESS_2 | 9:04bfdfc029cb | 306 | indexParam = 0; |
POTLESS_2 | 9:04bfdfc029cb | 307 | newParamFlag = false; |
POTLESS_2 | 9:04bfdfc029cb | 308 | } |
POTLESS_2 | 9:04bfdfc029cb | 309 | |
POTLESS_2 | 9:04bfdfc029cb | 310 | void Decoding_Message_Android(char message []) |
POTLESS_2 | 9:04bfdfc029cb | 311 | { |
POTLESS_2 | 9:04bfdfc029cb | 312 | char *commande = 0; |
POTLESS_2 | 9:04bfdfc029cb | 313 | int valeur = 0; |
POTLESS_2 | 9:04bfdfc029cb | 314 | |
POTLESS_2 | 9:04bfdfc029cb | 315 | sscanf(message,"%s %d",&commande , &valeur); |
POTLESS_2 | 9:04bfdfc029cb | 316 | |
POTLESS_2 | 9:04bfdfc029cb | 317 | if ((char)commande == 'T') { |
POTLESS_2 | 9:04bfdfc029cb | 318 | set_time(valeur); |
POTLESS_2 | 12:7f3aadd79f89 | 319 | } else if ((char)commande == 'I') { |
POTLESS_2 | 10:aca745a66d51 | 320 | servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 12:7f3aadd79f89 | 321 | #ifdef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 322 | float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 323 | if(Sf >= 0 && Sf <= 90) |
POTLESS_2 | 12:7f3aadd79f89 | 324 | servo_fuite = Sf; |
POTLESS_2 | 12:7f3aadd79f89 | 325 | #endif |
POTLESS_2 | 10:aca745a66d51 | 326 | printf(" Servo Poumon = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f); |
POTLESS_2 | 12:7f3aadd79f89 | 327 | } else if ((char)commande == 'O') { |
POTLESS_2 | 10:aca745a66d51 | 328 | servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 329 | printf(" Servo Fuite = %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f); |
POTLESS_2 | 11:b2feed92584a | 330 | } else if ((char)commande == 'D') { |
POTLESS_2 | 11:b2feed92584a | 331 | delta = valeur; |
POTLESS_2 | 12:7f3aadd79f89 | 332 | #ifdef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 333 | float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 11:b2feed92584a | 334 | if(Sf >= 0 && Sf <= 90) |
POTLESS_2 | 12:7f3aadd79f89 | 335 | servo_fuite = Sf; |
POTLESS_2 | 12:7f3aadd79f89 | 336 | #endif |
POTLESS_2 | 12:7f3aadd79f89 | 337 | ///////////////////////////////////////// |
POTLESS_2 | 12:7f3aadd79f89 | 338 | //Pour rajouter une commande |
POTLESS_2 | 12:7f3aadd79f89 | 339 | //} else if ((char)commande == 'X') { |
POTLESS_2 | 12:7f3aadd79f89 | 340 | // attribuer à une VARIABLE = valeur; |
POTLESS_2 | 12:7f3aadd79f89 | 341 | // ou une action, avec ou sans valeur |
POTLESS_2 | 12:7f3aadd79f89 | 342 | ///////////////////////////////////////// |
POTLESS_2 | 9:04bfdfc029cb | 343 | } else if ((char)commande == 'R') { |
POTLESS_2 | 9:04bfdfc029cb | 344 | NVIC_SystemReset(); |
POTLESS_2 | 9:04bfdfc029cb | 345 | } |
POTLESS_2 | 12:7f3aadd79f89 | 346 | #ifdef PID_MODE |
POTLESS_2 | 12:7f3aadd79f89 | 347 | else if ((char)commande == 'p') { |
POTLESS_2 | 12:7f3aadd79f89 | 348 | Kc = (float)valeur; |
POTLESS_2 | 12:7f3aadd79f89 | 349 | control_Servo.setTunings(Kc, Ti, Td); |
POTLESS_2 | 12:7f3aadd79f89 | 350 | printf(" UPDATE PID --> Kc = %f Ti = %f Td = %f\r\n", Kc, Ti, Td); |
POTLESS_2 | 12:7f3aadd79f89 | 351 | }else if ((char)commande == 'i') { |
POTLESS_2 | 12:7f3aadd79f89 | 352 | Ti = (float)valeur; |
POTLESS_2 | 12:7f3aadd79f89 | 353 | control_Servo.setTunings(Kc, Ti, Td); |
POTLESS_2 | 12:7f3aadd79f89 | 354 | printf(" UPDATE PID --> Kc = %f Ti = %f Td = %f\r\n", Kc, Ti, Td); |
POTLESS_2 | 12:7f3aadd79f89 | 355 | }else if ((char)commande == 'd') { |
POTLESS_2 | 12:7f3aadd79f89 | 356 | Td = (float)valeur; |
POTLESS_2 | 12:7f3aadd79f89 | 357 | control_Servo.setTunings(Kc, Ti, Td); |
POTLESS_2 | 12:7f3aadd79f89 | 358 | printf(" UPDATE PID --> Kc = %f Ti = %f Td = %f\r\n", Kc, Ti, Td); |
POTLESS_2 | 12:7f3aadd79f89 | 359 | }else if ((char)commande == 'c') { |
POTLESS_2 | 12:7f3aadd79f89 | 360 | consigne = valeur; |
POTLESS_2 | 12:7f3aadd79f89 | 361 | printf(" UPDATE CONSIGNE PID --> Consigne = %d\r\n", consigne); |
POTLESS_2 | 12:7f3aadd79f89 | 362 | } |
POTLESS_2 | 12:7f3aadd79f89 | 363 | #endif |
POTLESS_2 | 11:b2feed92584a | 364 | //wait_ms(100); |
POTLESS_2 | 9:04bfdfc029cb | 365 | strcpy(Android," "); |
POTLESS_2 | 9:04bfdfc029cb | 366 | indexAndroid = 0; |
POTLESS_2 | 9:04bfdfc029cb | 367 | newAndroidFlag = false; |
POTLESS_2 | 9:04bfdfc029cb | 368 | } |
POTLESS_2 | 9:04bfdfc029cb | 369 | |
POTLESS_2 | 10:aca745a66d51 | 370 | //Calibration des limites de feedback des servos |
POTLESS_2 | 10:aca745a66d51 | 371 | void Calibration_servo() |
POTLESS_2 | 10:aca745a66d51 | 372 | { |
POTLESS_2 | 10:aca745a66d51 | 373 | //Servos, calibration du feedback |
POTLESS_2 | 10:aca745a66d51 | 374 | printf(" Check Servos :\r\n"); |
POTLESS_2 | 10:aca745a66d51 | 375 | printf("\r\n"); |
POTLESS_2 | 10:aca745a66d51 | 376 | 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 | 377 | servo_fuite.period(0.001); |
POTLESS_2 | 10:aca745a66d51 | 378 | //Rentrer les servos à fond |
POTLESS_2 | 10:aca745a66d51 | 379 | servo_poumon = 0; |
POTLESS_2 | 10:aca745a66d51 | 380 | servo_fuite = 0; |
POTLESS_2 | 10:aca745a66d51 | 381 | wait(4); |
POTLESS_2 | 10:aca745a66d51 | 382 | |
POTLESS_2 | 10:aca745a66d51 | 383 | Limit_min_Servo_Poumon = FeedBack_Servo_Poumon.read(); |
POTLESS_2 | 10:aca745a66d51 | 384 | wait_ms(100); |
POTLESS_2 | 10:aca745a66d51 | 385 | Limit_min_Servo_Fuite = FeedBack_Servo_Fuite.read(); |
POTLESS_2 | 10:aca745a66d51 | 386 | |
POTLESS_2 | 10:aca745a66d51 | 387 | printf(" FeedBack Servo POUMON min = %f\n", Limit_min_Servo_Poumon); |
POTLESS_2 | 10:aca745a66d51 | 388 | printf(" FeedBack Servo FUITE min = %f\n", Limit_min_Servo_Fuite); |
POTLESS_2 | 10:aca745a66d51 | 389 | printf("\r\n"); |
POTLESS_2 | 10:aca745a66d51 | 390 | |
POTLESS_2 | 10:aca745a66d51 | 391 | //Sortir les servos à fond |
POTLESS_2 | 10:aca745a66d51 | 392 | servo_poumon = 1; |
POTLESS_2 | 10:aca745a66d51 | 393 | servo_fuite = 1; |
POTLESS_2 | 10:aca745a66d51 | 394 | wait(4); |
POTLESS_2 | 10:aca745a66d51 | 395 | |
POTLESS_2 | 10:aca745a66d51 | 396 | Limit_max_Servo_Poumon = FeedBack_Servo_Poumon.read(); |
POTLESS_2 | 10:aca745a66d51 | 397 | wait_ms(100); |
POTLESS_2 | 10:aca745a66d51 | 398 | Limit_max_Servo_Fuite = FeedBack_Servo_Fuite.read(); |
POTLESS_2 | 10:aca745a66d51 | 399 | |
POTLESS_2 | 10:aca745a66d51 | 400 | printf(" FeedBack Servo POUMON Max = %f\n", Limit_max_Servo_Poumon); |
POTLESS_2 | 10:aca745a66d51 | 401 | printf(" FeedBack Servo FUITE Max = %f\n", Limit_max_Servo_Fuite); |
POTLESS_2 | 10:aca745a66d51 | 402 | printf("\r\n"); |
POTLESS_2 | 10:aca745a66d51 | 403 | |
POTLESS_2 | 10:aca745a66d51 | 404 | //Position milieu |
POTLESS_2 | 10:aca745a66d51 | 405 | servo_poumon = 0.5; |
POTLESS_2 | 10:aca745a66d51 | 406 | servo_fuite = 0.5; |
POTLESS_2 | 10:aca745a66d51 | 407 | wait(4); |
POTLESS_2 | 10:aca745a66d51 | 408 | |
POTLESS_2 | 10:aca745a66d51 | 409 | //Mesure du delta consigne / feedback |
POTLESS_2 | 10:aca745a66d51 | 410 | Delta_FB_1 = abs(remap(FeedBack_Servo_Poumon.read(), Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90) - 45); |
POTLESS_2 | 10:aca745a66d51 | 411 | Delta_FB_2 = abs(remap(FeedBack_Servo_Fuite.read(), Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90) - 45); |
POTLESS_2 | 10:aca745a66d51 | 412 | |
POTLESS_2 | 10:aca745a66d51 | 413 | printf(" Delta Servo 1 = %f\n", Delta_FB_1); |
POTLESS_2 | 10:aca745a66d51 | 414 | printf(" Delta Servo 2 = %f\n", Delta_FB_2); |
POTLESS_2 | 10:aca745a66d51 | 415 | |
POTLESS_2 | 10:aca745a66d51 | 416 | if(Delta_FB_1 > 10 || Delta_FB_2 > 10) printf(" Delta Servos non satisfaisant...\r\n\r\n"); |
POTLESS_2 | 12:7f3aadd79f89 | 417 | |
POTLESS_2 | 11:b2feed92584a | 418 | #ifdef DELTA_MODE |
POTLESS_2 | 11:b2feed92584a | 419 | //Position initial delta |
POTLESS_2 | 11:b2feed92584a | 420 | servo_poumon = 0.5; |
POTLESS_2 | 11:b2feed92584a | 421 | servo_fuite = 0.5 + remap(delta, 0, 90, 0, 100) / 100.f; |
POTLESS_2 | 12:7f3aadd79f89 | 422 | #endif |
POTLESS_2 | 10:aca745a66d51 | 423 | } |
POTLESS_2 | 10:aca745a66d51 | 424 | |
POTLESS_2 | 4:d84250f67dec | 425 | int main() |
POTLESS_2 | 12:7f3aadd79f89 | 426 | { |
POTLESS_2 | 11:b2feed92584a | 427 | //UNIX TIMESTAMP depuis le erminal MAC = date +%s + 7200 pour heure d'été..... |
POTLESS_2 | 12:7f3aadd79f89 | 428 | |
POTLESS_2 | 10:aca745a66d51 | 429 | Calibration_servo(); |
POTLESS_2 | 12:7f3aadd79f89 | 430 | |
POTLESS_2 | 11:b2feed92584a | 431 | sensors.Sensors_INIT(false, true, 5, SPOOLING, DIGI_FILTER32, CALIB_AIR); |
POTLESS_2 | 6:ebed9093d661 | 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 | 10:aca745a66d51 | 439 | thread.start(Get_Info_thread); |
POTLESS_2 | 9:04bfdfc029cb | 440 | |
POTLESS_2 | 7:ad15c386e960 | 441 | thread.set_priority(osPriorityRealtime); |
POTLESS_2 | 12:7f3aadd79f89 | 442 | |
POTLESS_2 | 12:7f3aadd79f89 | 443 | #ifdef PID_MODE |
POTLESS_2 | 12:7f3aadd79f89 | 444 | //Init PID |
POTLESS_2 | 12:7f3aadd79f89 | 445 | //Entrée PPO2 entre 100 et 1000 mb |
POTLESS_2 | 12:7f3aadd79f89 | 446 | control_Servo.setInputLimits(Min_Input, Max_Input); |
POTLESS_2 | 12:7f3aadd79f89 | 447 | //Sortie servo entre 0 et 100 % |
POTLESS_2 | 12:7f3aadd79f89 | 448 | control_Servo.setOutputLimits(Min_Output, Max_Output); |
POTLESS_2 | 12:7f3aadd79f89 | 449 | //Mode manuel au démarrage |
POTLESS_2 | 12:7f3aadd79f89 | 450 | control_Servo.setMode(AUTO_MODE); |
POTLESS_2 | 12:7f3aadd79f89 | 451 | //Consigne à x mb |
POTLESS_2 | 12:7f3aadd79f89 | 452 | control_Servo.setSetPoint(consigne); |
POTLESS_2 | 12:7f3aadd79f89 | 453 | |
POTLESS_2 | 12:7f3aadd79f89 | 454 | #endif |
POTLESS_2 | 12:7f3aadd79f89 | 455 | |
POTLESS_2 | 11:b2feed92584a | 456 | #ifdef VT100 |
POTLESS_2 | 8:a750d531b381 | 457 | printf(CLS); |
POTLESS_2 | 11:b2feed92584a | 458 | #endif |
POTLESS_2 | 9:04bfdfc029cb | 459 | |
POTLESS_2 | 7:ad15c386e960 | 460 | while (true) { |
POTLESS_2 | 6:ebed9093d661 | 461 | |
POTLESS_2 | 7:ad15c386e960 | 462 | //Démarrage du Timer mesurant le temps d'éxecution du code |
POTLESS_2 | 7:ad15c386e960 | 463 | REAL_RATE.start(); |
POTLESS_2 | 7:ad15c386e960 | 464 | |
POTLESS_2 | 7:ad15c386e960 | 465 | Affichage_moniteur(); |
POTLESS_2 | 9:04bfdfc029cb | 466 | |
POTLESS_2 | 6:ebed9093d661 | 467 | if (newParamFlag) { |
POTLESS_2 | 6:ebed9093d661 | 468 | serialMonit.printf("Param = %s\r\n", param); |
POTLESS_2 | 9:04bfdfc029cb | 469 | Decoding_Message(param); |
POTLESS_2 | 9:04bfdfc029cb | 470 | } |
POTLESS_2 | 9:04bfdfc029cb | 471 | |
POTLESS_2 | 9:04bfdfc029cb | 472 | if (newAndroidFlag) { |
POTLESS_2 | 9:04bfdfc029cb | 473 | serialMonit.printf("Android = %s\r\n", Android); |
POTLESS_2 | 9:04bfdfc029cb | 474 | Decoding_Message_Android(Android); |
POTLESS_2 | 6:ebed9093d661 | 475 | } |
POTLESS_2 | 7:ad15c386e960 | 476 | |
POTLESS_2 | 9:04bfdfc029cb | 477 | //Vers Android |
POTLESS_2 | 9:04bfdfc029cb | 478 | if (NEED_ANDROID_OUTPUT == 1) { |
POTLESS_2 | 12:7f3aadd79f89 | 479 | ANDROID(to_android); |
POTLESS_2 | 9:04bfdfc029cb | 480 | } |
POTLESS_2 | 12:7f3aadd79f89 | 481 | |
POTLESS_2 | 9:04bfdfc029cb | 482 | sensors.Write_SD((string)to_store); |
POTLESS_2 | 12:7f3aadd79f89 | 483 | |
POTLESS_2 | 12:7f3aadd79f89 | 484 | #ifdef PID_MODE |
POTLESS_2 | 12:7f3aadd79f89 | 485 | //Adéquation du RATE du PID avec la contrainte du temps du loop |
POTLESS_2 | 12:7f3aadd79f89 | 486 | control_Servo.setInterval(RATE_PID); |
POTLESS_2 | 12:7f3aadd79f89 | 487 | //Update du PID |
POTLESS_2 | 12:7f3aadd79f89 | 488 | control_Servo.setProcessValue(ppO2); |
POTLESS_2 | 12:7f3aadd79f89 | 489 | //Nouvelle sortie servo |
POTLESS_2 | 12:7f3aadd79f89 | 490 | Commande_PID = control_Servo.compute(); |
POTLESS_2 | 12:7f3aadd79f89 | 491 | //Appliquer la consigne |
POTLESS_2 | 12:7f3aadd79f89 | 492 | servo_fuite = Commande_PID / 100.0f; |
POTLESS_2 | 12:7f3aadd79f89 | 493 | #endif |
POTLESS_2 | 12:7f3aadd79f89 | 494 | |
POTLESS_2 | 7:ad15c386e960 | 495 | //Arrêt du Timer mesurant le temps d'éxecution du code |
POTLESS_2 | 7:ad15c386e960 | 496 | REAL_RATE.stop(); |
POTLESS_2 | 7:ad15c386e960 | 497 | //Définition de la nouvelle valeur du temps d'échantillonage du PID. |
POTLESS_2 | 7:ad15c386e960 | 498 | RATE = REAL_RATE.read(); |
POTLESS_2 | 7:ad15c386e960 | 499 | //Reset du Timer |
POTLESS_2 | 7:ad15c386e960 | 500 | REAL_RATE.reset(); |
POTLESS_2 | 12:7f3aadd79f89 | 501 | |
POTLESS_2 | 11:b2feed92584a | 502 | //Pour ralentir le code à 1 seconde fixe quelque soit les intéruptions du loop.... |
POTLESS_2 | 12:7f3aadd79f89 | 503 | RATE_TRUE = (Ref_Time - RATE) * 1000; |
POTLESS_2 | 11:b2feed92584a | 504 | wait_ms(RATE_TRUE); |
POTLESS_2 | 4:d84250f67dec | 505 | } |
POTLESS_2 | 4:d84250f67dec | 506 | } |