source: Sophya/trunk/Poubelle/archediab.old/archediab.sources/c/tache_regul.c@ 3670

#include "manip.h"
#include "choix_acquisition.h"
#include "archeops.h"
#include "choix_param.h"
#include "structure.h"
#include "carte_acqui.h"
#include "carte_pci.h"
#include "tm.h"
#include "tache.h"
#include "bolo.h"


//*****************************************************************************************
//                                                                                      ***
#ifdef _diabolo //---------   pour  Diabolo    ------------------------------   ***
//                                                                                      ***
//*****************************************************************************************





double  deglitch_regul(double x);
double  deglitch_regul(double x)
{
double dx;
static int debut=1;
static double X,DX;
//return(x);                    //  ligne a retirer pour deglitcher

if(debut) {     X=0;DX=1000;    debut=0;        }

dx=x-X; if(dx<0)  dx=-dx;
if( dx > DX ) 
                {
                x=X;
                DX *=1.1;
                }
else    {
                DX=0.9 * DX + 0.5 * dx +1 ; 
                X=x;
                }               // en moyenne, 5 fois l'interval moyen
return(x);
}


void  tache_regul(tmtc* tt)
{
block_tc  btc;
btc=tt->tc.btc[tt->tc.pos_lit];
tt->vi.reg.pid=* ( (regul_p_i_d*)    &btc );
}


void    regul(tmtc* tt)
{
int j,cc;
double i,ecart;


if(tt->vi.reg.pid.nb_mes<1) return;             // pas de regul si repetition <1
tt->vi.reg.compteur++;
if( (tt->vi.reg.compteur>0) && (tt->vi.reg.compteur< (tt->vi.reg.pid.nb_mes*4+4) ) )    return;

ecart=-deglitch_regul(tt->vi.reg.valeur);
ecart/=4*tt->vi.reg.pid.nb_mes;         //  valeur mesurée et moyennée apres repetition
ecart=ecart/(long)tt->ds.dsnorme;
ecart=((1e5*(double)ecart)/(65536.*tt->reglage.gain[tt->vi.reg.pid.num_bolo-1]));
tt->vi.reg.valeur=(long)ecart;
i=tt->vi.reg.tint;                                                      
i=i+ecart*tt->vi.reg.pid.i;                                     //      terme intergral
if(i>4000) i=4000;
if(i<0) i=0;
cc=tt->vi.reg.pid.p*ecart+i+(ecart-tt->vi.reg.anc_ecart)*tt->vi.reg.pid.d;      //      calcul de la commande
if(cc>4000) cc=4000;
if(cc<0) cc=0;
if(!tt->vi.reg.pid.chauf) cc=0; 
//cc=tt->vi.reg.pid.p;
tt->vi.reg.cc=cc;
cc=( 0x0fff - cc ) & 0x0fff;            //  12 bits  avec inverser la polarité 
if( (acquisition_PCI) && ( tt->vi.flag_ecriture_data>2 ) )              //    rien en attente d'ecriture: j'ecris direct en interruption                
        {
        ecrit_carte(tc_regul );                 // ecrit bolo 251  -->  ecrit regulation
        ecrit_carte( ( (tt->vi.reg.pid.chauf&3 ) <<6 ) |  (cc>>6)  );           // ecrit relais (2 bits) + 6 bits de data 
        ecrit_carte( (cc<<2) |  2  );                                                           // ecrit  6 bits de data + 1  +  0      
        for(j=0;j<7;j++)        ecrit_carte(0);                         // reste 7 mots à ecrire 
        tt->vi.flag_ecriture_data=0;
        }
                
tt->vi.reg.compteur=0;
tt->vi.reg.valeur=0;
tt->vi.reg.anc_ecart=(int)ecart;
tt->vi.reg.tint=i;
}

void    raz_periodique(tmtc* tt)
{
/*****************************  ne  fait  rien  !!!!!!!!
int j;
if(tt->reg.per_raz<1) return;           // pas de raz si periode demandee = 0
tt->reg.nb_raz++;
if( (tt->reg.nb_raz>0) && (tt->reg.nb_raz< (tt->reg.per_raz) ) )        return;


if( tt->vi.flag_ecriture_data>2 )       //   j'ecris direct en interruption sans regrader si autre chose est en cours d'ecriture                
        {
        ecrit_carte(23 );                       // ecrit bolo 23  -->  bebo A
        ecrit_carte( 16);                       // ecrit a 
        for(j=0;j<8;j++)        ecrit_carte(0);                         // reste 8 mots à ecrire 
        }

tt->tb.synchro[2][tt->tb.pos_ecrit]=999;                
tt->reg.nb_raz=0;
*/
}


//*****************************************************************************************
//                                                                                      ***
#endif          //--------------------------------------------------------------------  ***
//                                                                                      ***
//*****************************************************************************************