File content as of revision 11:b2feed92584a:

#include "mbed.h"
#include <string>
#include "Sensor_head_revB.h"
#include "HTU21D.h"

//Commandes des servos
#define PWM_SERVO_POUMON PB_15
#define PWM_SERVO_FUITE PB_1
//Retour des servos
#define FEED_BACK_SERVO_POUMON PC_2
#define FEED_BACK_SERVO_FUITE PC_3

//Ecrit dans le moniteur série de la tablette à 9600 bds si sur 1, penser à mettre NEED_CONSOLE_OUTPUT à 0
#define NEED_ANDROID_OUTPUT 1

//Sortie en mode VT100, à commenter si on veut une sortie type Arduino
//#define VT100

//Mode Delta constant entre les servos à commenter si on en veut pas
//#define DELTA_MODE
float delta = 0.0f;

#ifndef DELTA_MODE
int DELTA_FLAG = 0;
#endif

#ifdef DELTA_MODE
int DELTA_FLAG = 1;
#endif

#if NEED_ANDROID_OUTPUT
#define ANDROID(...) { android.printf(__VA_ARGS__); }
#else
#define ANDROID(...)
#endif

//Moniteur série
Serial serialMonit(USBTX,USBRX,9600);

//COM Série vers Android, Serial 3
Serial android(PC_10,PC_11,9600);

//Init de la lib ARNSRS;
SENSOR_HEAD_REV_B sensors;

//pour Param Cozir
const int sizeParam = 20;
char  param[sizeParam];
volatile int indexParam = 0;
bool newParamFlag = false;

//pour Commandes Android
const int sizeAndroid = 20;
char  Android[sizeAndroid];
volatile int indexAndroid = 0;
bool newAndroidFlag = false;

//Variables de stockage des infos capteurs
int co2 = 0;
float pression = 0;
float Temp1 = 0;
int ppO2 = 0;
int CellO2_1 = 0;
int CellO2_2 = 0;

//Mesure du tempsd'éxecution du loop
Timer REAL_RATE;
float RATE = 0;
float RATE_TRUE = 0;

//HTU21D
HTU21D temphumid(PB_9, PB_8); //Temp humid sensor || SDA, SCL
float Temp2;
int Humid;

//Data LOG
char to_store[50];
time_t seconds;

//VT100
static const char CLS[] = "\x1B[2J";
static const char HOME[] = "\x1B[H";

//Thread d'intérogation des capteurs
Thread thread;

//Contrôle des servos
PwmOut servo_poumon(PWM_SERVO_POUMON);
AnalogIn FeedBack_Servo_Poumon(FEED_BACK_SERVO_POUMON);
float volet_poumon_Position;
float Limit_min_Servo_Poumon, Limit_max_Servo_Poumon;
float Delta_FB_1;

PwmOut servo_fuite(PWM_SERVO_FUITE);
AnalogIn FeedBack_Servo_Fuite(FEED_BACK_SERVO_FUITE);
float volet_fuite_Position;
float Limit_min_Servo_Fuite, Limit_max_Servo_Fuite;
float Delta_FB_2;

bool FLAG_COMMAND_SERVO = false;

//Buffer pour Commandes servo
char commande_servo[50];
int indexCommande;

//Remap d'une valeur
float remap(float x, float in_min, float in_max, float out_min, float out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


//Thread d'intérogation des capteurs, positions servo et constitution de la chaine 
void Get_Info_thread()
{
    while (true) {

        //CO2 sur Cozir
        co2 = sensors.requestCO2();
        //P / T sur MS5837
        pression = sensors.requestPress();
        Temp1 =  sensors.requestTemp();
        //PPO2 sur ADS1015
        ppO2 = sensors.requestPpO2();
        //Cell O2 en mV
        CellO2_1 = sensors.requestCellO2_1();
        CellO2_2 = sensors.requestCellO2_2();

        //HTU21D
        Temp2 = temphumid.sample_ctemp();
        Humid = temphumid.sample_humid();

        //Retour position des servos
        volet_poumon_Position = 90 - remap(FeedBack_Servo_Poumon, Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90);
        volet_fuite_Position = remap(FeedBack_Servo_Fuite, Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90);
        
        //Fabrication de la chaine Date / heure
        seconds = time(NULL);
        char Time_buf[32];
        strftime(Time_buf, 32, "%D %I-%M-%S ", localtime(&seconds));

        //Fabrication de la chaine à enregistrer
        sprintf(to_store,"<%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%3.2f:%3.2f:%d>",
                Time_buf,
                co2,
                ppO2,
                pression,
                Temp1,
                Temp2,
                Humid,
                CellO2_1,
                CellO2_2,
                volet_poumon_Position,
                volet_fuite_Position,
                DELTA_FLAG);
    }    
}

void Affichage_moniteur()
{
#ifndef VT100    
    printf("\r\n");
    printf("  CO2             = %d ppm\r\n"  , co2);
    printf("  PPO2            = %d mb\r\n", ppO2);
    printf("  Pression        = %f msw\r\n", pression);
    printf("  Temp MS5837     = %f C\r\n", Temp1);
    printf("  Temp HTU21D     = %f C\n\r", Temp2);
    printf("  Humidity        = %d %%\n\r", Humid);
    printf("\n\r");
    printf("  Cell O2 n 1     = %d\r\n"  , CellO2_1);
    printf("  Cell O2 n 2     = %d\r\n"  , CellO2_2);
    printf("\r\n");
    printf("\n");
    printf("  Volet Poumon    = %3.2f%\r\n"  , volet_poumon_Position);
    printf("  Volet Fuite     = %3.2f%\r\n"  , volet_fuite_Position);
    printf("\r\n");
    printf("Temps d execution de la boucle = %f secondes\n", (RATE + RATE_TRUE) / 1000);
    printf("\r\n", "");
    printf("A enregistrer = %s\n", to_store);
    printf("\r\n");
#endif

#ifdef VT100   
    printf(HOME);
    printf("\x1b[30m");
    printf("\x1b[0m\r  CO2           = \x1b[1m\x1b[K%d ppm\n", co2);
    printf("\x1b[0m\r  PPO2          = \x1b[1m\x1b[K%d mb\n", ppO2);
    printf("\n");
    printf("\x1b[0m\r  Pression      = \x1b[1m\x1b[K%.2f msw\n", pression);
    printf("\n");
    printf("\x1b[0m\r  Temp MS5837   = \x1b[1m\x1b[K%.2f C\n", Temp1);
    printf("\x1b[0m\r  Temp HTU21D   = \x1b[1m\x1b[K%.2f C\n", Temp2);
    printf("\n");
    printf("\x1b[0m\r  Humidity      = \x1b[1m\x1b[K%d %\n", Humid);
    printf("\n");
    printf("\x1b[0m\r  Cell O2 n 1  = \x1b[1m\x1b[K%d\n", CellO2_1);
    printf("\x1b[0m\r  Cell O2 n 2  = \x1b[1m\x1b[K%d\n", CellO2_2);
    printf("\n");
    printf("\x1b[0m\r  Volet Poumon = \x1b[1m\x1b[K%3.2f%\n", volet_poumon_Position);
    printf("\x1b[0m\r  Volet Fuite  = \x1b[1m\x1b[K%3.2f%\n", volet_fuite_Position);
    printf("\n");
    printf("\x1b[0m\r  Temps d execution de la boucle = \x1b[1m\x1b[K%f secondes\n", (RATE + RATE_TRUE) / 1000);
    printf("\r\n", "");
    printf("\x1b[0m\r  A enregistrer = \x1b[1m\x1b[K%s\n", to_store);
    printf("\r\n", "");
#endif    
}

//Callback de l'intérruption des envois de commandes depuis le terminal
void callbackParam()
{
    while(serialMonit.readable()) {
        if (indexParam  == sizeParam) //éviter la saturation du buffer
            serialMonit.getc();
        else
            param [indexParam++] = serialMonit.getc();//chargement du buffer dans le message
        if ((indexParam == sizeParam) || (param[indexParam -1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut...
            param[indexParam] = 0;
            newParamFlag  = true;
        }
    }
}

//Callback de l'intérruption des envois de commandes depuis Android
void callbackAndroid()
{
    while(android.readable()) {
        if (indexAndroid  == sizeAndroid) //éviter la saturation du buffer
            android.getc();
        else
            Android [indexAndroid++] = android.getc();//chargement du buffer dans le message
        if ((indexAndroid == sizeAndroid) || (Android[indexAndroid -1] == '\n')) {//le message est complet ou nouvelle ligne ou autre si on veut...
            Android[indexAndroid] = 0;
            newAndroidFlag  = true;
        }
    }
}

void Decoding_Message(char message [])
{
    char *commande = 0;
    int valeur = 0;

    sscanf(message,"%s %d",&commande , &valeur);

    if ((char)commande == 'T') {
        set_time(valeur);
    } else if ((char)commande == 'I'){
        servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f;
        #ifdef DELTA_MODE
        float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
        if(Sf >= 0 && Sf <= 90)
        servo_fuite = Sf;
        #endif
        printf("  Servo Poumon    = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f);
    } else if ((char)commande == 'O'){
        servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f;
        printf("  Servo Fuite     =  %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f);    
    } else if ((char)commande == 'D') {
        delta = valeur;
        #ifdef DELTA_MODE
        float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
        if(Sf >= 0 && Sf <= 90)
        servo_fuite = Sf;
        #endif
    } else if ((char)commande == 'R') {
        NVIC_SystemReset();
    /////////////////////////////////////////    
    //Pour rajouter une commande    
    //} else if ((char)commande == 'X') {
    //  attribuer à une VARIABLE = valeur;
    //  ou une action, avec ou sans valeur
    /////////////////////////////////////////   
    }else {
        sensors.cozirSend(message);
    }

    //wait_ms(100);
    strcpy(param," ");
    indexParam = 0;
    newParamFlag = false;
}

void Decoding_Message_Android(char message [])
{
    char *commande = 0;
    int valeur = 0;

    sscanf(message,"%s %d",&commande , &valeur);

    if ((char)commande == 'T') {
        set_time(valeur);
    } else if ((char)commande == 'I'){
        servo_poumon = remap(valeur, 0, 90, 0, 100) / 100.f;
        #ifdef DELTA_MODE
        float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
        if(Sf >= 0 && Sf <= 90)
        servo_fuite = Sf;
        #endif
        printf("  Servo Poumon    = %f\r\n", remap(valeur, 0, 90, 0, 100) / 100.f);
    } else if ((char)commande == 'O'){
        servo_fuite = 1 - remap(valeur, 0, 90, 0, 100) / 100.f;
        printf("  Servo Fuite     =  %f\r\n", 1 - remap(valeur, 0, 90, 0, 100) / 100.f);
    } else if ((char)commande == 'D') {
        delta = valeur;
        #ifdef DELTA_MODE
        float Sf = servo_poumon + remap(delta, 0, 90, 0, 100) / 100.f;
        if(Sf >= 0 && Sf <= 90)
        servo_fuite = Sf;
        #endif
    /////////////////////////////////////////    
    //Pour rajouter une commande    
    //} else if ((char)commande == 'X') {
    //  attribuer à une VARIABLE = valeur;
    //  ou une action, avec ou sans valeur
    /////////////////////////////////////////          
    } else if ((char)commande == 'R') {
        NVIC_SystemReset();
    } else {
        sensors.cozirSend(message);
    }

    //wait_ms(100);
    strcpy(Android," ");
    indexAndroid = 0;
    newAndroidFlag = false;
}

//Calibration des limites de feedback des servos
void Calibration_servo()
{
    //Servos, calibration du feedback
    printf("  Check Servos :\r\n");
    printf("\r\n");
    servo_poumon.period(0.001); // à mettre dans le setup si cette calib n'est pas appelée (je l ai trouvée commentée)
    servo_fuite.period(0.001);
    //Rentrer les servos à fond
    servo_poumon = 0;
    servo_fuite = 0;
    wait(4);

    Limit_min_Servo_Poumon = FeedBack_Servo_Poumon.read();
    wait_ms(100);
    Limit_min_Servo_Fuite = FeedBack_Servo_Fuite.read();

    printf("  FeedBack Servo POUMON min = %f\n", Limit_min_Servo_Poumon);
    printf("  FeedBack Servo FUITE  min = %f\n", Limit_min_Servo_Fuite);
    printf("\r\n");

    //Sortir les servos à fond
    servo_poumon = 1;
    servo_fuite = 1;
    wait(4);

    Limit_max_Servo_Poumon = FeedBack_Servo_Poumon.read();
    wait_ms(100);
    Limit_max_Servo_Fuite = FeedBack_Servo_Fuite.read();

    printf("  FeedBack Servo POUMON Max = %f\n", Limit_max_Servo_Poumon);
    printf("  FeedBack Servo FUITE  Max = %f\n", Limit_max_Servo_Fuite);
    printf("\r\n");

    //Position milieu
    servo_poumon = 0.5;
    servo_fuite = 0.5;
    wait(4);

    //Mesure du delta consigne / feedback
    Delta_FB_1 = abs(remap(FeedBack_Servo_Poumon.read(), Limit_max_Servo_Poumon, Limit_min_Servo_Poumon, 0, 90) - 45);
    Delta_FB_2 = abs(remap(FeedBack_Servo_Fuite.read(), Limit_max_Servo_Fuite, Limit_min_Servo_Fuite, 0, 90) - 45);

    printf("  Delta Servo 1 = %f\n", Delta_FB_1);
    printf("  Delta Servo 2 = %f\n", Delta_FB_2);

    if(Delta_FB_1 > 10 || Delta_FB_2 > 10)  printf("  Delta Servos non satisfaisant...\r\n\r\n");
    
#ifdef DELTA_MODE
    //Position initial delta
    servo_poumon = 0.5;
    servo_fuite = 0.5 + remap(delta, 0, 90, 0, 100) / 100.f;
#endif        
}

int main()
{   
    //UNIX TIMESTAMP depuis le erminal MAC =       date +%s         + 7200 pour heure d'été.....
    
    Calibration_servo();
    
    sensors.Sensors_INIT(false, true, 5, SPOOLING, DIGI_FILTER32, CALIB_AIR);

    serialMonit.attach(&callbackParam, Serial::RxIrq);

    android.attach(&callbackAndroid, Serial::RxIrq);

    serialMonit.printf("  Demarrage...\r\n\r\n  Entrez les comandes COZIR si besoin :\r\n");

    thread.start(Get_Info_thread);

    thread.set_priority(osPriorityRealtime);
    
#ifdef VT100
    printf(CLS);
#endif

    while (true) {

        //Démarrage du Timer mesurant le temps d'éxecution du code
        REAL_RATE.start();

        Affichage_moniteur();

        if (newParamFlag) {
            serialMonit.printf("Param = %s\r\n", param);
            Decoding_Message(param);
        }

        if (newAndroidFlag) {
            serialMonit.printf("Android = %s\r\n", Android);
            Decoding_Message_Android(Android);
        }

        //Vers Android
        if (NEED_ANDROID_OUTPUT == 1) {
            ANDROID(to_store);
        }

        sensors.Write_SD((string)to_store);

        //Arrêt du Timer mesurant le temps d'éxecution du code
        REAL_RATE.stop();
        //Définition de la nouvelle valeur du temps d'échantillonage du PID.
        RATE = REAL_RATE.read();
        //Reset du Timer
        REAL_RATE.reset();
        
        //Pour ralentir le code à 1 seconde fixe quelque soit les intéruptions du loop....
        RATE_TRUE = (1000 - (RATE / 1000));
        wait_ms(RATE_TRUE);
    }
}