File content as of revision 18:bfd78c05b589:

/*

Câblage complet STM32L476RG :

- Port SPI carte SD :

MOSI        ->      PA_7
MISO        ->      PA_6
SCK         ->      PA_5
CS          ->      PB_6

- Sensor Head :

Alim en 3.3v

SCL         ->      PB_8
SDA         ->      PB_9
TX          ->      PC_12
RX          ->      PD_2
AlertRdy    ->      PA_11


- HC_06 :

Alim en 3.3v ou 5v

TX          ->      PC_10
RX          ->      PC_11

- STM32L476RG sur H-Bridge L293 :

Reférence L293 : http://www.ti.com/lit/ds/symlink/l293.pdf

Volet Poumon :
PWM     ->      PB_5        ->      1
FWD     ->      PB_4        ->      2
REV     ->      PB_10       ->      7

Volet Fuite :
PWM     ->      PB_13       ->      9
FWD     ->      PB_14       ->      10
REV     ->      PB_15       ->      15

Potentiomètres retour Position :

Alim en 3.3v

Feedback Volet Poumon    ->      PC_2
Feedback Volet Fuite     ->      PC_3

- Câblage complet du H-Bridge L293 :

1       ->      PB_5
2       ->      PB_4
3       ->      Borne moteur
4       ->      non connectée
5       ->      GND
6       ->      Borne moteur
7       ->      PB_10
8       ->      V+ Alimentation moteurs 4.5v - 36v

9       ->      PB_13
10      ->      PB_14
11      ->      Borne moteur
12      ->      GND
13      ->      non connectée
14      ->      borne moteur
15      ->      PB_15
16      ->      V+ Alimentation logic  5v (alim moteur si tension compatible....)

*/

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

//Commandes  des servos
#define PWM_SERVO_POUMON PB_5
#define FWD_SERVO_POUMON PB_4
#define REV_SERVO_POUMON PB_10
#define FEED_BACK_SERVO_POUMON PC_2

#define PWM_SERVO_FUITE PB_13
#define FWD_SERVO_FUITE PB_14
#define REV_SERVO_FUITE PB_15
#define FEED_BACK_SERVO_FUITE PC_3

//Mapping des valeur.
#define MAX_MAP 100//Mapping en pourcentage
//#define MAX_MAP 90//Mapping en en degrés sur 90 degrés

//Ecrit dans le moniteur série de la tablette à 115200 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 PID, STD à commenter / décommenter
#define STD_MODE
//#define PID_MODE

#ifdef STD_MODE
int MODE_FLAG = 0;
#endif

#ifdef PID_MODE
int MODE_FLAG = 2;
#endif

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

Actuonix Servo_Poumon(FEED_BACK_SERVO_POUMON, PWM_SERVO_POUMON, FWD_SERVO_POUMON, REV_SERVO_POUMON, 1, MAX_MAP);

Actuonix Servo_Fuite(FEED_BACK_SERVO_FUITE, PWM_SERVO_FUITE, FWD_SERVO_FUITE, REV_SERVO_FUITE, 1, MAX_MAP);

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

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

//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;
char to_android[100];

//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;

//Variables et constantes OTU
float OTU = 0;
float COEF_OTU = 0.83;

//Mesure du temps d'éxecution du loop
Timer REAL_RATE;
float RATE = 0;
float RATE_TRUE = 0;
float Ref_Time = 1.0; //La durée de la boucle désirée...

//HTU21D sur l'I2C
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 et de positionnement des volets
Thread thread;

//Contrôle des servos
float Consigne_poumon = 50;
float volet_poumon_Position;

float Consigne_fuite = 50;
float volet_fuite_Position;

#ifdef PID_MODE
//Paramètre du PID
float Kc = 40;
float Ti = 0;
float Td = 0;
float RATE_PID = Ref_Time;
float Commande_PID;
float consigne = 210;
float Max_Input = 1000;
float Min_Input = 80;
float Max_Output = MAX_MAP - 5;//Vérifier la valeur pour angle à laisser ouvert...
float Min_Output = 0;

//Init PID
PID control_Servo(Kc, Ti, Td, RATE_PID);
#endif

//Boolean du status de l'appareil, en mode secours ou nominal
bool EN_MODE_SCOURS = false;

//Passage en mode secours
void Mode_Secours()
{
#ifdef PID_MODE    
    //Mise du PID en mode manuel (desactivation...)
    control_Servo.setMode(MANUAL_MODE);
#endif
    Consigne_poumon = MAX_MAP;
    Consigne_fuite = MAX_MAP;
    printf("-------------- Appareil en mode secours ---------------");
    EN_MODE_SCOURS = true;
}

//Contrôle du status de l'appareil / des constantes
bool Check()
{
    if (ppO2 > 100)
    return true;//Situation OK
    else
    return false;//Situation dégradée
}

//Calcul des OTU
float Calcul_OTU()
{
    /*
    La formule suivante permet de calculer la quantité d' OTU accumulée
    OTU = T * (2* PpO2 -1)0,83
    avec :
    T = temps de plongée en minutes
    PpO2 = pression partielle d’ oxygène en bars
    */

    if (ppO2 > 500){
        float val = (2 * (float)ppO2/1000 - 1);//je divise par 1000 car la PP est en mb...
        OTU += Ref_Time * pow(val, COEF_OTU);
        } 
}

//Thread d'intérogation des capteurs, positions servo
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 = Servo_Poumon.Get_Position();
        volet_fuite_Position = Servo_Fuite.Get_Position();

        //Position des servo mise à jour en permanence dans le thread
        Servo_Poumon.Go_To_Prop(Consigne_poumon);
        Servo_Fuite.Go_To_Prop(Consigne_fuite);
        
        if(!Check()) Mode_Secours();
    }
}

void Affichage_moniteur()
{
#ifndef VT100
    printf("\r\n");
    printf("  CO2             = %d ppm\r\n"  , co2);
    printf("  PPO2            = %d mb\r\n", ppO2);
    printf("  OTU             = %d \r\n", OTU);
    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    = %f\r\n"  , volet_poumon_Position);
    printf("  Volet Fuite     = %f\r\n"  , volet_fuite_Position);
    printf("\r\n");
    printf("Temps d execution de la boucle = %f seconde(s)\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  OTU           = \x1b[1m\x1b[K%d mb\n", OTU);
    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%.2f\n", volet_poumon_Position);
    printf("\x1b[0m\r  Volet Fuite  = \x1b[1m\x1b[K%.2f\n", volet_fuite_Position);
    printf("\n");
    printf("\x1b[0m\r  Temps d execution de la boucle = \x1b[1m\x1b[K%f seconde(s)\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;
    float valeur = 0;

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

    if ((char)commande == 'T') {
        set_time(valeur);
    } else if ((char)commande == 'I') {
        Consigne_poumon = (float)valeur;
        printf("  Servo Poumon    = %f\r\n", Consigne_poumon);
    } else if ((char)commande == 'O') {
        Consigne_fuite = (float)valeur;
        printf("  Servo Fuite     =  %f\r\n", Consigne_fuite);
    } 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);
    }

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

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

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

    if ((char)commande == 'T') {
        set_time(valeur);
    } else if ((char)commande == 'I') {
        Consigne_poumon = (float)valeur;
        printf("  Servo Poumon    = %f\r\n", Consigne_poumon);
    } else if ((char)commande == 'O') {
        Consigne_fuite = (float)valeur;
        printf("  Servo Fuite     =  %f\r\n", Consigne_fuite);
        /////////////////////////////////////////
        //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();
    }
#ifdef PID_MODE
    else if ((char)commande == 'p') {
        Kc = (float)valeur;
        control_Servo.reset();
        control_Servo.setTunings(Kc, Ti, Td);
        printf("  UPDATE PID -->  Kc = %f   Ti = %f   Td = %f\r\n\n", Kc, Ti, Td);
    } else if ((char)commande == 'i') {
        Ti = (float)valeur;
        control_Servo.reset();
        control_Servo.setTunings(Kc, Ti, Td);
        printf("  UPDATE PID -->  Kc = %f   Ti = %f   Td = %f\r\n\n", Kc, Ti, Td);
    } else if ((char)commande == 'd') {
        Td = (float)valeur;
        control_Servo.reset();
        control_Servo.setTunings(Kc, Ti, Td);
        printf("  UPDATE PID -->  Kc = %f   Ti = %f   Td = %f\r\n\n", Kc, Ti, Td);
    } else if ((char)commande == 'c') {
        consigne = valeur;
        control_Servo.setSetPoint(consigne);
        printf("  UPDATE CONSIGNE PID -->  Consigne = %d\r\n\n", consigne);
    }
#endif

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

int main()
{

    Servo_Poumon.Calibrate();
    Servo_Fuite.Calibrate();

    /*
    Par défaut les valeur en cas de calibration sur true sont les suivant

        nbCalibO2 = 5
        Mode = SPOOLING
        Filtre = DIGI_FILTER32
        CalibrationCO2 = "CALIB_AIR"

        Parfois la calibration du Cozir coince...faire reset et relancer...

        Pour calibrer avec ces paramètres :

        sensors.Sensors_INIT(true, true);

        Pour changer utiliser la syntaxe suivante :

        sensors.Sensors_INIT(true, true, 5, SPOOLING, DIGI_FILTER32, CALIB_AIR);

     */

    sensors.Sensors_INIT(false, true);

    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");

    /*

    Pour mémoire, les réglage de priorité des thread

          osPriorityIdle          = -3,          ///< priority: idle (lowest)
          osPriorityLow           = -2,          ///< priority: low
          osPriorityBelowNormal   = -1,          ///< priority: below normal
          osPriorityNormal        =  0,          ///< priority: normal (default)
          osPriorityAboveNormal   = +1,          ///< priority: above normal
          osPriorityHigh          = +2,          ///< priority: high
          osPriorityRealtime      = +3,          ///< priority: realtime (highest)
          osPriorityError         =  0x84        ///< system cannot determine priority or thread has illegal priority
    */

    thread.start(Get_Info_thread);

    thread.set_priority(osPriorityNormal);

#ifdef PID_MODE
    //Init PID
    //Entrée PPO2 entre 100 et 1000 mb
    control_Servo.setInputLimits(Min_Input, Max_Input);
    //Sortie servo entre 0 et 100 %
    control_Servo.setOutputLimits(Min_Output, Max_Output);
    //Mode auto au démarrage
    control_Servo.setMode(AUTO_MODE);
    //Consigne à x mb
    control_Servo.setSetPoint(consigne);
#endif

#ifdef VT100
    printf(CLS);
#endif

    while (true) {

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

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

        //Fabrication de la chaine Date / heure
        seconds = time(NULL);
        char Time_buf[32];
        strftime(Time_buf, 32, "%D %I-%M-%S ", localtime(&seconds));

#ifdef PID_MODE
        //Fabrication de la chaine à enregistrer
        sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%.2f:%.2f:%d:%.3f:%.3f:%.3f:%d",
                Time_buf,
                co2,
                ppO2,
                pression,
                Temp1,
                Temp2,
                Humid,
                CellO2_1,
                CellO2_2,
                volet_poumon_Position,
                volet_fuite_Position,
                MODE_FLAG,
                Kc,
                Ti,
                Td,
                (int)consigne
               );
#endif
#ifndef PID_MODE
        //Fabrication de la chaine à enregistrer sans les variables du PID
        sprintf(to_store,"%s:%d:%d:%.2f:%.2f:%.2f:%d:%d:%d:%.2f:%.2f:%d:%.3f:%.3f:%.3f:%d",
                Time_buf,
                co2,
                ppO2,
                pression,
                Temp1,
                Temp2,
                Humid,
                CellO2_1,
                CellO2_2,
                volet_poumon_Position,
                volet_fuite_Position,
                MODE_FLAG,
                0,
                0,
                0,
                0
               );
#endif

        //Enregistrement de la chaine
        sensors.Write_SD((string)to_store);

        //Pour Android on ajoute < et > pour décoder l'arrivée du message
        if (NEED_ANDROID_OUTPUT == 1) {
            sprintf(to_android,"<%s>",to_store);
            ANDROID(to_android);
        }

        //Calcul des OTU
        Calcul_OTU();
        
        //Vers le moniteur dérie
        Affichage_moniteur();


#ifdef PID_MODE
        //Update du PID
        control_Servo.setProcessValue(ppO2);
        //Nouvelle sortie servo fuite si on est pas en mode secours
        if(!EN_MODE_SCOURS) Consigne_fuite = control_Servo.compute();
#endif

        //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 à Ref_Time seconde fixe quelque soit les intéruptions du loop....
        if (Ref_Time > RATE) {
            RATE_TRUE = (Ref_Time - RATE) * 1000;
        } else {
            RATE_TRUE = 0;
#ifdef PID_MODE
            control_Servo.setInterval(RATE);
#endif
            printf("Pour ralentir le code, Ref_Time doit être supérieur à %f seconde(s)\r\n\n", RATE);
        }

        wait_ms(RATE_TRUE);
    }
}