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@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?

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 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 }