/******************************************************************************/
/* Files to Include                                                           */
/******************************************************************************/
#include "Process.h"


void RegulationDoseur(void)
{
float           pente;
float           offset;
float           interf;
float           consigne;
float           echelon;
unsigned long   interL;
float           debit;

// Retour en volumetrique si on n'est plus avec la regulation
if ((!PAR.ModeRegul) && (EXP.PosCycleRegulation == 20))
    EXP.PosCycleRegulation = 10;

// Choix de la consigne
switch (PAR.TypeConsigne)
{
    case 0 : PAR.Consigne = PAR.ConsigneAff; PAR.Consigne /= 1000; break;       // Clavier
    case 1 : PAR.Consigne = PAR.ConsigneAff; PAR.Consigne /= 1000; break;       // Clavier
    case 2 : PAR.Consigne = PAR.ConsigneAff; PAR.Consigne /= 1000; break;       // Liaison série
    case 3 : PAR.Consigne = PAR.ConsigneAna; PAR.Consigne /= 1000; break;       // Analogique
}
// Mise en forme consigne 
switch (PAR.NbChiffreDecimaleDebit)
{
    case 0 : echelon = 1; break;
    case 1 : echelon = 0.1; break;
    case 2 : echelon = 0.01; break;
    case 3 : echelon = 0.001; break;
}
if (PAR.Consigne > 0)
    consigne = PAR.Consigne + (echelon/2);
else
    consigne = PAR.Consigne - (echelon/2);
consigne = consigne / echelon;
interL = consigne;
consigne = interL * echelon;
PAR.Consigne = consigne;

if ((PAR.OP_Doseur) 
        && (PAR.Consigne*1000 >= PAR.LimiteConsigneNulle)
        && (EXP.PosCycleRegulation == 0))
{
    if (((ValEntTOR(DI_ModeDistant,0)) && ValEntTOR(DI_DCY, 0) && ValEntTOR(DI_BdC_RMarcheMoteurVis, 0))
        || (!(ValEntTOR(DI_ModeDistant,0)) && (EXP.Command1 & 0x0001) && (EXP.Command1 & 0x4000)))
    {
    EXP.PosCycleRegulation = 10;
    T100[timer_Volumetrique] = PAR.TempsFonctVolumetrique;
    }
}

if ((PAR.OP_Doseur) 
        && ((!ValEntTOR(DI_DCY, 0) && (!ValEntTOR(DI_ModeDistant,0))) 
            || ((EXP.Command1 & 0x0004) && (ValEntTOR(DI_ModeDistant,0))) 
            || (PAR.Consigne*1000 < PAR.LimiteConsigneNulle)) && (EXP.PosCycleRegulation > 0))
    EXP.PosCycleRegulation = 0;

interf = PAR.ValeurMaxRegul; interf /= 1000;
pente = (PAR.Calage100-PAR.Calage10) / (interf - interf/10);
offset = PAR.Calage100 - (interf * pente);
        
if (EXP.PosCycleRegulation == 0)                                                // Fonctionnement en volumétrique
        EXP.SortieRegul = 0;

if (EXP.PosCycleRegulation == 10)                                               // Fonctionnement en volumétrique
{
    if (PAR.MemorisationSortieAna)
        EXP.SortieRegul = EXP.SortieRegulMem;
    else
        EXP.SortieRegul = PAR.Consigne * pente + offset;
    EXP.PID_ITerm = EXP.SortieRegul;
}   

// Gestion défaut déviation
debit = EXP.DebitMoyen;                                                         // Régulation normale
if (PAR.ValSortieANA[1] == 34)                                                  // Régulation A-B
    debit = EXP.DebitBascule2/1000 - EXP.DebitMoyen;
if (PAR.ValSortieANA[1] == 35)                                                  // Régulation B-A
    debit = EXP.DebitMoyen - EXP.DebitBascule2/1000;
    
interf = 1000 - PAR.ToleranceMoins; interf = interf / 1000;
if ((debit < PAR.Consigne * interf)                                             // Défaut Débit < Tolerance moins
        && (PAR.ActivationDefaut2 & 0x0001)
        && (EXP.PosCycleRegulation == 20))                                      // Doseur en régulation
{
    if (!T100[timer_DefautDeviationMoins])
    {
        EXP.Defaut2 = EXP.Defaut2 | 0x0001;
        sortiesTOR(DO_DefDeviationPlus, 1);                                     // Défaut déviation +
    }
}
else
{
    T100[timer_DefautDeviationMoins] = PAR.TempoDefaut*10*((PAR.AffectTempoDefaut2&0x0001) == 0x0001);
    if (PAR.AcqAutoDefauts)
    {
        EXP.Defaut2 = EXP.Defaut2 & 0xFFFE;
        sortiesTOR(DO_DefDeviationMoins, 0);                                    // Défaut déviation +
    }
}

interf = 1000 + PAR.TolerancePlus; interf = interf / 1000;
if ((debit > PAR.Consigne * interf)                                             // Défaut Débit > Tolerance plus
        && (PAR.ActivationDefaut2 & 0x0002)
        && (EXP.PosCycleRegulation == 20))                                      // Doseur en régulation
{
    if (!T100[timer_DefautDeviationPlus])
    {
        EXP.Defaut2 = EXP.Defaut2 | 0x0002;
        sortiesTOR(DO_DefDeviationPlus, 1);                                     // Défaut déviation +
    }
}
else
{
    T100[timer_DefautDeviationPlus] = PAR.TempoDefaut*10*((PAR.AffectTempoDefaut2&0x0002) == 0x0002);
    if (PAR.AcqAutoDefauts)
    {
        EXP.Defaut2 = EXP.Defaut2 & 0xFFFD;
        sortiesTOR(DO_DefDeviationPlus, 0);                                     // Défaut déviation +
    }
}

if ((!T100[timer_RegulationPID]) && (EXP.PosCycleRegulation == 20))             // Fonctionnement en régulation
{
    EXP.SortieRegul = RegulationPID();
    EXP.SortieRegulMem = EXP.SortieRegul;
}

switch (EXP.PosCycleRegulation)
{
    case  0 :                                                                   // Doseur à l'arret
                EXP.Etat1 = EXP.Etat1 & 0xBFFF;                                 // Fonctionnement en volumetrique
                EXP.Etat1 = EXP.Etat1 & 0xDFFF;                                 // Fonctionnement en régulation
                break;

    case 10 :   EXP.Etat1 = EXP.Etat1 | 0x4000;                                 // Fonctionnement en volumetrique
                if ((!T100[timer_Volumetrique])                                 // Doseur en volumétrique
                        && (PAR.ModeRegul))                                     // Fonctionnement avec régulation
                    EXP.PosCycleRegulation = 20;
                break;

    case 20 :   EXP.Etat1 = EXP.Etat1 & 0xBFFF;                                 // Fonctionnement en volumetrique
                EXP.Etat1 = EXP.Etat1 | 0x2000;                                 // Fonctionnement en régulation
                break;
                
    default :   EXP.PosCycleRegulation = 0;
}
}


int RegulationPID(void)
{
float           erreur;
float           dInput;
float           output;
int             valeur;
float           debit;
float           interf;
float           interf2;
float           pente;
float           offset;
float           derivee;
float           integrale;

T100[timer_RegulationPID] = 3;

PAR.PID_Kd = 0.0;

debit = EXP.DebitMoyen;                                                         // Régulation normale
if (PAR.ValSortieANA[1] == 34)                                                  // Régulation A-B
    debit = EXP.DebitBascule2/1000 - EXP.DebitMoyen;
if (PAR.ValSortieANA[1] == 35)                                                  // Régulation B-A
    debit = EXP.DebitMoyen - EXP.DebitBascule2/1000;

// Calcul de l'erreur
erreur = PAR.Consigne - debit;

// Calcul de la derivee
derivee = erreur - EXP.ErreurMem;
EXP.ErreurMem = erreur;

// Calcul de l'integrale
EXP.PID_ITerm += ((PAR.PID_Ki/10000) * erreur);

//dInput = (debit - EXP.PID_DebitMoyenMem);
//EXP.PID_DebitMoyenMem = debit;

output = (PAR.PID_Kp/1000) * erreur + EXP.PID_ITerm; // - (PAR.PID_Kd/1000) * dInput;

if (output > 4095) EXP.PID_ITerm = 4095 - (PAR.PID_Kp/1000) * erreur;
if (output < 0) EXP.PID_ITerm = -(PAR.PID_Kp/1000) * erreur;
output = (PAR.PID_Kp/1000) * erreur + EXP.PID_ITerm; // - (PAR.PID_Kd/1000) * dInput;

// Vérification par rapport aux seuil haut et seuil bas
interf2 = PAR.ValeurMaxRegul; interf2 /= 1000;
pente = (PAR.Calage100-PAR.Calage10) / (interf2 - interf2/10);
offset = PAR.Calage100 - (interf2 * pente);
interf = PAR.Consigne * pente + offset;
if (output > interf * (1 + (float)PAR.SeuilHautRegul/1000))
    output = interf * (1 + (float)PAR.SeuilHautRegul/1000);
if (output < interf * (1 - (float)PAR.SeuilBasRegul/1000))
    output = interf * (1 - (float)PAR.SeuilBasRegul/1000);

if (output < 0)
{
    output = 0;
    EXP.PID_ITerm = -(PAR.PID_Kp/10000);
}
if (output > 4095) output = 4095;

valeur = output;
return(valeur);
}


void RegulationPdP(void)
{
float           pente;
float           offset;
float           interf;
float           consigne;
float           echelon;
unsigned long   interL;
float           debit;

// Retour en volumetrique si on n'est plus avec la regulation
if ((!PAR.ModeRegul) && (EXP.PosCycleRegulation == 20))
    EXP.PosCycleRegulation = 10;

// Choix de la consigne
switch (PAR.TypeConsigne)
{
    case 0 : PAR.Consigne = PAR.ConsigneAff; PAR.Consigne /= 1000; break;       // Clavier
    case 1 : PAR.Consigne = PAR.ConsigneAff; PAR.Consigne /= 1000; break;       // Clavier
    case 2 : PAR.Consigne = PAR.ConsigneAff; PAR.Consigne /= 1000; break;       // Liaison série
    case 3 : PAR.Consigne = PAR.ConsigneAna; PAR.Consigne /= 1000; break;       // Analogique
}

// Mise en forme consigne 
switch (PAR.NbChiffreDecimaleDebit)
{
    case 0 : echelon = 1; break;
    case 1 : echelon = 0.1; break;
    case 2 : echelon = 0.01; break;
    case 3 : echelon = 0.001; break;
}
if (PAR.Consigne > 0)
    consigne = PAR.Consigne + (echelon/2);
else
    consigne = PAR.Consigne - (echelon/2);
consigne = consigne / echelon;
interL = consigne; consigne = interL * echelon;
PAR.Consigne = consigne;

interf = PAR.ValeurMaxRegul; interf /= 1000;
pente = (PAR.Calage100-PAR.Calage10) / (interf - interf/10);
offset = PAR.Calage100 - (interf * pente);
        
if (EXP.PosCycleRegulation == 0)                                                // Fonctionnement en volumétrique
        EXP.SortieRegul = 0;

if (EXP.PosCycleRegulation == 10)                                               // Fonctionnement en volumétrique
{
    if (PAR.MemorisationSortieAna)
        EXP.SortieRegul = EXP.SortieRegulMem;
    else
        EXP.SortieRegul = PAR.Consigne * pente + offset;
    EXP.PID_ITerm = EXP.SortieRegul;
}   

// Gestion défaut déviation
debit = EXP.DebitMoyen;                                                         // Régulation normale
if (PAR.ValSortieANA[1] == 34)                                                  // Régulation A-B
    debit = EXP.DebitBascule2/1000 - EXP.DebitMoyen;
if (PAR.ValSortieANA[1] == 35)                                                  // Régulation B-A
    debit = EXP.DebitMoyen - EXP.DebitBascule2/1000;
    
interf = 1000 - PAR.ToleranceMoins; interf = interf / 1000;
if ((debit < PAR.Consigne * interf)                                             // Défaut Débit < Tolerance moins
        && (PAR.ActivationDefaut2 & 0x0001)
        && (EXP.PosCycleRegulation == 20))                                      // Doseur en régulation
{
    if (!T100[timer_DefautDeviationMoins])
    {
        EXP.Defaut2 = EXP.Defaut2 | 0x0001;
        sortiesTOR(DO_DefDeviationPlus, 1);                                     // Défaut déviation +
    }
}
else
{
    T100[timer_DefautDeviationMoins] = PAR.TempoDefaut*10*((PAR.AffectTempoDefaut2&0x0001) == 0x0001);
    if (PAR.AcqAutoDefauts)
    {
        EXP.Defaut2 = EXP.Defaut2 & 0xFFFE;
        sortiesTOR(DO_DefDeviationMoins, 0);                                    // Défaut déviation +
    }
}

interf = 1000 + PAR.TolerancePlus; interf = interf / 1000;
if ((debit > PAR.Consigne * interf)                                             // Défaut Débit > Tolerance plus
        && (PAR.ActivationDefaut2 & 0x0002)
        && (EXP.PosCycleRegulation == 20))                                      // Doseur en régulation
{
    if (!T100[timer_DefautDeviationPlus])
    {
        EXP.Defaut2 = EXP.Defaut2 | 0x0002;
        sortiesTOR(DO_DefDeviationPlus, 1);                                     // Défaut déviation +
    }
}
else
{
    T100[timer_DefautDeviationPlus] = PAR.TempoDefaut*10*((PAR.AffectTempoDefaut2&0x0002) == 0x0002);
    if (PAR.AcqAutoDefauts)
    {
        EXP.Defaut2 = EXP.Defaut2 & 0xFFFD;
        sortiesTOR(DO_DefDeviationPlus, 0);                                     // Défaut déviation +
    }
}

if ((!T100[timer_RegulationPID]) && (EXP.PosCycleRegulation == 20))             // Fonctionnement en régulation
{
    EXP.SortieRegul = RegulationPID();
    EXP.SortieRegulMem = EXP.SortieRegul;
}

switch (EXP.PosCycleRegulation)
{
    case  0 :                                                                   // Doseur à l'arret
                EXP.Etat1 = EXP.Etat1 & 0xBFFF;                                 // Fonctionnement en volumetrique
                EXP.Etat1 = EXP.Etat1 & 0xDFFF;                                 // Fonctionnement en régulation
                break;

    case 10 :   EXP.Etat1 = EXP.Etat1 | 0x4000;                                 // Fonctionnement en volumetrique
                if ((!T100[timer_Volumetrique])                                 // Doseur en volumétrique
                        && (PAR.ModeRegul))                                     // Fonctionnement avec régulation
                    EXP.PosCycleRegulation = 20;
                break;

    case 20 :   EXP.Etat1 = EXP.Etat1 & 0xBFFF;                                 // Fonctionnement en volumetrique
                EXP.Etat1 = EXP.Etat1 | 0x2000;                                 // Fonctionnement en régulation
                break;
                
    default :   EXP.PosCycleRegulation = 0;
}
}



