Subversion Repositories shark

/*************************************************************
 *                                                           *
 *           Stabilizzazione di un pendolo inverso           *
 *                                                           *
 *                                                           *
 *Written by: Gabriele Bolognini - Scuola S.Anna -- Pisa     *
 *                                                           *
 *************************************************************/
#include "demo.h"
#include "pclab.h"
#include "cost.h"   /* Contiene le costanti del task DISEGNA  */



#ifdef SOFTPROG
long int PERIOD_CARRELLO={1200}, PERIOD_TRACKING={12000};
long int WCET_CARRELLO={240},  WCET_TRACKING={7200};
#else
long int PERIOD_CARRELLO={1800}, PERIOD_TRACKING={22200}; 
#endif

#define LUNGH 35.0
#define THETAMAX 45.0
#define VDANG 2.4
#define NMAX 300
#define SEC_ID 7.0
#define MAXRUN_ID 9 //16
int start_id=0;
//   dati da scrivere nei files

static long int contat_id=-1, num_id=0;
#ifdef DATAPLANT
struct DATA_FILE {
  TIME  time;
  float xist ;
  float thist ;
  float par1 ;
  float par2 ;
  float par3 ;
  float par4 ;
} dati[NMAX];
long int numdat=0; // numero dati scritti su DATI[NMAX]
#endif

#define NSAMPLES  500
#define NTASKS  10
static float timetab[NSAMPLES][NTASKS];
static int index[NTASKS];
static float somma[MAXRUN_ID][2]; // valore dello indice di prestazione
mutex_t mutex,mutexadc;

#define MAX_TEMPI 15
static char  stringa[MAX_TEMPI][250];
// **- ACHTUNG!!!: param. iniz. del guadagno, deadline, ecc.-----*******  /
#define INIPARAMS {{ 2.3 , 300 , 0.5 , 15 },1.,1.0,20.,20., 0.1,  1 , 1, 0.}
#define MODPARAMS {{ 1. , 300 , 0.9 , 34 },0.74 ,1.,20.,20., 0.2,  1 , 1, 0.}
#define PARAM2 {{ 1. , 300 , 0.79 , 23 },0.70 ,2.5 ,20.,20., 0.1,  1 , 1, 0.}
/*
struct Parametri{
  float GUAD[4];
  float COST;
  float NOISE;
  float WCUT;
  float WCUT1;
  float OFFSVAL;
  int DEADLINE;
  int SCAN;
  float XTRASL;
};
struct  Data_cab1 {
        float   rif;
        int     imp;
};
struct  Data_cab2 {
        float   x;
        float   y;
} ;
*/
struct Parametri prm=PARAM2, prmbase=INIPARAMS, prmmod=MODPARAMS, prm2=PARAM2;



#define DEADSECX(x) ( x /*PERIOD_TRACKING*/ * 1.0e-6 )  // mit task TRACK in sec
#define DEADSECTH(x) (x /*PERIOD_CARRELLO*/ * 1.0e-6 ) // mit task CARRELLO in sec


CAB     cab1, cab2; 

float vmax, vmin, vmaxth, vminth;

float vin=0, vout, yout, thout;



void    graphframe(void);
void    my_end(void);
void    scritt(void);
void    scritt1(void);
void    scritt2(void);
void    scritt3(void);
void nuovi_tempi(long int*);
void    initial(void);

void scenario_jetcontrol(void);
void init_jetcontrol(void);
TASK jetslide_task(void *);
TASK jetctrl_task(void *);
TASK jetdummy_task(void *);


void framegrabber_close(void *);
TASK camera_task(void *);
TASK tracking_task(void *);

extern int white, black, red, gray, green, blue;
float integrale(float y[], float u, int flag)
{
  static float sum=0, oldcoeff=0;
  float coeff=0 ;
  static float R=1 , Q[2][2]={1,0,0,1.5} ;

  if(flag) sum=0; //reset
  coeff=Q[0][0]*y[0]*y[0]+Q[0][1]*y[0]*y[1]+Q[1][0]*y[1]*y[0]+Q[1][1]*y[1]*y[1]+R*u*u;
  sum += (coeff+oldcoeff)*DEADSECTH(PERIOD_CARRELLO) / 2.0 ;
  oldcoeff=coeff;
  return sum;
}
void zerofun(KEY_EVT *k)
{
  int i;
  for (i=0; i<MAX_PROC; i++) jet_delstat(i);
}


int da_motor(float v)
{
  da_conv( 2.5 , 2 );
  da_conv( v + 2.5 , 1 );
  return(0);
}
float v2x(float v)
{ /* trasforma i  volt in x */
  float x;
  x= LUNGH * (v-(vmax+vmin)/2 ) / (vmax-vmin);
  return x;
}
float v2theta(float v)
{ /* trasforma i  volt in angoli */
  float theta;
  theta=2.0*(THETAMAX/FCA) * (v-(vmaxth+vminth)/2 ) / (vmaxth-vminth);
  return theta;
}
float bass1(float u)
{
  float y;
  static float oldy=0;
  y=(oldy + prm.WCUT * DEADSECX(PERIOD_TRACKING) *u)/(1+ prm.WCUT * DEADSECX(PERIOD_TRACKING));
  oldy=y;
  return y;
}
float bass2(float u)
{
  float y;
  static float oldy=0;
  y=(oldy + prm.WCUT1 * DEADSECX(PERIOD_TRACKING) *u)/(1+ prm.WCUT1 * DEADSECX(PERIOD_TRACKING));
  oldy=y;
  return y;
}
float bass3(float u)
{
  float y;
  static float oldy=0;
  y=(oldy + prm.WCUT * DEADSECTH(PERIOD_CARRELLO) *u)/(1+ prm.WCUT * DEADSECTH(PERIOD_CARRELLO));
  oldy=y;
  return y;
}
float bass4(float u)
{
  float y;
  static float oldy=0;
  y=(oldy + prm.WCUT1 * DEADSECTH(PERIOD_CARRELLO) *u)/(1+ prm.WCUT1 * DEADSECTH(PERIOD_CARRELLO));
  oldy=y;
  return y;
}
float  dx(float u)
{
  static float oldu=0;
  float y;
  y=(u-oldu)/DEADSECX(PERIOD_TRACKING);
  oldu=u;
  return y;
}
float  dth(float u)
{
  static float oldu=0;
  float y;
  y=(u-oldu)/DEADSECTH(PERIOD_CARRELLO);
  oldu=u;
  return y;
}

TASK    eval(void)
{
  int x,y;
  char str[100];
        mutex_lock(&mutex);
        
        grx_rect(10,325,XTI - 20,479,blue);
        x = 20;
        y = 350;
        grx_text("PARAMETRI DI CONTROLLO ( R , T ) ", x+60,  y-18 , red, 0);
        grx_text("Guad=",  x, y,green, 0);y+=18;
        grx_text("Cost=",  x, y,green, 0); y+=18;

        grx_text("WCUT=",  x, y,green, 0); y+=18;
        grx_text("WCUT1=",  x, y,green, 0); y+=18;
        grx_text("NOISE=",  x, y,green, 0); y+=18;
        grx_text("DEADL=",  x, y,green, 0);y+=18;
        grx_text("OFFS=",  x, y,green, 0);y+=18;

        {  /* legenda */
          int x=280, y=350;
          grx_text("Guad  1,2  3,4  5,6  7,8",  x, y,green, 0);y+=18;
          grx_text("Cost  [ , ]",  x, y,green, 0); y+=18;
          grx_text("WCUT  o , p", x , y,green, 0); y+=18;
          grx_text("WCUT1 k , l",  x, y,green, 0); y+=18;
          grx_text("NOISE n , m",  x, y,green, 0); y+=18;
          grx_text("DEADL - , +",  x, y,green, 0);y+=18;
          grx_text("OFFS  / , *",  x, y,green, 0);y+=18;
        }
        {  /* frame dei valori rilevati dai sensori */
          int x=15, y=YTI+10;
          grx_rect(10,YTI,XTI - 20,YTI+70,red);
          grx_text("V_com =    ",  x, y,green, 0);y+=14;
          grx_text("V+offs =   ",  x, y,green, 0);y+=14;
          grx_text("V_sens_x = ",  x, y,green, 0);y+=14;
          grx_text("SCAN = ",  x, y,green, 0);y+=14;

        }

        mutex_unlock(&mutex);
  while(1){
        x = 70;
        y = 350;

        mutex_lock(&mutex);
        
        grx_text("                         ",  x, y, 12, 0);
        sprintf(str, "%6.2f  %6.2f  %6.2f  %6.2f ", prm.GUAD[0],prm.GUAD[1],prm.GUAD[2],prm.GUAD[3]);
        grx_text(str, x, y, blue, 0);y+=18;

        grx_text("                ",  x, y, 12, 0);
        sprintf(str, "%6.2f ", prm.COST);
        grx_text(str, x, y, blue, 0); y += 18;

        grx_text("               ",  x, y, 12, 0);
        sprintf(str, "%6.2f  Hz", prm.WCUT);
        grx_text(str, x, y, blue, 0); y += 18;

        grx_text("              ",  x, y, 12, 0);
        sprintf(str, "%6.2f Hz ", prm.WCUT1);
        grx_text(str, x, y, blue, 0); y += 18;

        grx_text("              ",  x, y, 12, 0);
        sprintf(str, "%6.2f Volt ", prm.NOISE);
        grx_text(str, x, y, blue, 0); y += 18;

        grx_text("               ",  x, y, 12, 0);
        sprintf(str, "%d ticks ", prm.DEADLINE);
        grx_text(str, x, y, blue, 0); y += 18;

        grx_text("               ",  x, y, 12, 0);
        sprintf(str, "%6.2f Volt ", prm.OFFSVAL);
        grx_text(str, x, y, blue, 0); y += 18;

        mutex_unlock(&mutex);
        

        
        /* scrivo vin , vout (vin+offs) , SCAN e PARAMETRIBASE*/
        { int x=95, y= YTI + 10;

          mutex_lock(&mutex);
                
          grx_text("              ", x, y,  0,  0);
          sprintf(str, "%7.4f ", vin);
          grx_text(str, x, y, blue, 0);y += 14;

          grx_text("               ", x, y,  0,  0);
          sprintf(str, "%7.4f",  vout);
          grx_text(str, x, y, blue, 0);y += 14;

          grx_text("         ", x, y,  0,  0);
          sprintf(str, "%7.3f", yout);
          grx_text(str, x,  y , blue, 0); y += 14;

          grx_text("         ", x, y,  0,  0);
          sprintf(str, "%3.2d  c , v", prm.SCAN);
          grx_text(str, x,  y , blue, 0); y += 14;
          
          mutex_unlock(&mutex);
        }



    task_endcycle();
  }
}
/*------------------------------------------------------------------------
  Il task CARRELLO controlla la posizione del carrello

        y = posizione angolare dell'asta (non usata)
        x = posizione lineare del carrello

-----------------------------------------------------------------------*/
float xrif=0, orif=1;
TASK    carrello(void)
{
  float   thist=0, y[2]={0,0}, yp[2]={0,0};  /* valori attuali   */
  PORT    pa, pb;                          /* communication ports  */
  struct  Data_cab1   data;
  struct  Data_cab2   tmp;

  char *pun, *pun2;
  int i;
  float offset;

  long int k=0;
  long int stime =0 ;
  /* Si creano 2 per mandare al task DISEGNA le var.di stato x1 e x3 */
  
  pa = port_create("porta1",sizeof(float),1,STICK,WRITE);
  pb = port_create("porta2",sizeof(float),1,STICK,WRITE);

  while (1) {

    /* prendo i dati sul riferimento in volt   per la x */
        pun = cab_getmes(cab1);
        memcpy(&data, pun, sizeof(struct Data_cab1));
        cab_unget(cab1, pun);

    /* prendo i dati sulle posizioni dal trasduttore ADC  */


        task_nopreempt();
        thout=ad_conv(10);
        task_preempt();
     
              /* taglio brusco dei livelli di rumore sopra il NOISE */
        {
#define AVR 10
          static float  othout[AVR]; 
          float  avth;
          static int index=0, flag=1;

          if(flag==1) {
            for(i=0; i<AVR;++i) {  othout[i] = thout;}
            flag=0;
          }
          avth=0; 
          for(i=0;i<AVR;++i) { avth += othout[i]; }
          avth /= AVR ; 

        
          if(fabs(thout-avth)>=prm.NOISE) { thout=avth;} 

           othout[index]=thout; 
          index = (index+1) % AVR ;
        }

        /* converto i valori dei sensori in posizioni */
        /* calcolo le velocita' lineari e angolari opportunamente filtrate */


        thist=v2theta(thout);
        y[1]=bass3(thist);
        yp[1] = bass4(dth(y[1]));


        /* a questo punto dovrei prendere i valori dall'altro task CARR2  */

        
        pun2 = cab_getmes(cab2);    
        memcpy(&tmp , pun2, sizeof(struct Data_cab2));
        cab_unget(cab2, pun2);
        
        y[0]=yout=tmp.x;
        yp[0]=tmp.y;
        /* PARAMETRI del controllore :1 */
          vin = prm.COST * (prm.GUAD[0] * (y[0]-v2x(data.rif+prm.XTRASL))+ prm.GUAD[1] * thist + prm.GUAD[2] * yp[0] + prm.GUAD[3] * yp[1]) ;


        /* metto un offset che mi elimina l'attrito */
        if(  vin >= 0) offset=prm.OFFSVAL;
        else offset=-prm.OFFSVAL;

        vout=vin+offset;


        if(vout >= VDANG ) vout = VDANG ;
        if(vout <= -VDANG ) vout = -VDANG ;
        /* la applico   */
        da_motor(vout);


        /* mando i dati sulle posizioni al task DISEGNA */
        port_send(pa, &y[1], NON_BLOCK);
        port_send(pb, &y[0], NON_BLOCK);

        {
          char str[5];
          static long int contatwait=-1;
//        extern float xrif;
          if(start_id == 1) {
            start_id=0;
            contat_id=(int) (SEC_ID / DEADSECTH(PERIOD_CARRELLO));
            num_id++; 
            somma[num_id-1][1]=contat_id;

            integrale(y,vout,1);
            contatwait=(long int)  (1.5 * SEC_ID / DEADSECTH(PERIOD_CARRELLO));

          }
          if(contat_id > 0) {
            xrif=0.0;    //pone la xrif a zero
            somma[num_id-1][0]=integrale(y,vout,0);
            somma[num_id-1][1]= --contat_id;
          }
          if(contat_id == 0) {
            if(num_id>= (MAXRUN_ID-1))   nuovi_tempi(&contatwait); //sys_end();
            xrif=-10.0;
            contatwait--; if(contatwait==0) start_id=1;//attiva la racc. dati 
          }
          if(contat_id == -1 )  ;  //  vero solo allo inizio del programma
        }
        /* aggiorno i riferimenti utilizzati in questo task, piu' in alto*/
        data.rif=(vmax+vmin)/2. + xrif * (vmax-vmin)/LUNGH;
        data.imp=0;
        
        pun = cab_reserve(cab1);
        memcpy(pun, &data, sizeof(struct Data_cab1));
        cab_putmes(cab1, pun);
        /* scrivo  i dati istantanei (fino a  NMAX) nella struttura 'dati' */
#ifdef DATAPLANT        
        if((contat_id>0)&&(k<(NMAX-1))) {
          dati[k].time=sys_gettime(NULL)/1000;
          dati[k].xist=y[0];
          dati[k].thist=thist;
          dati[k].par1=vout;
          dati[k].par2=yp[0];
          dati[k].par3=yp[1];
          dati[k].par4=thist*FCA;
          numdat=k++;
        }
#endif

        task_endcycle();
  }
  /* Eliminazione porte di comunicazione */
  port_delete(pa);
  port_delete(pb);

}   /* FINE DEL TASK CARRELLO */


/*--------------------------------------------------------------*/
/* Il task DISEGNA riceve dal task CONTROLLO i valori delle */
/* variabili x1 e x3 e disegna il pendolo corrispondente    */
/*--------------------------------------------------------------*/

TASK    diseg(void)
{
WORD xp, yp, oxp, oyp;   /* coordinate carrello  */
WORD xa, ya, oxa, oya;   /* coordinate asta  */
float   x1, x3;         /* variabili di stato   */
char    str[100];
PORT    pra, prb, prc;       /* communication ports  */
int delta;          /* dimensione finestre  */


        /* Connessione alle porte create dal task CONTROLLO */
        pra = port_connect("porta1",sizeof(float),STICK,READ);
        prb = port_connect("porta2",sizeof(float),STICK,READ);


        delta = 120;

        /* Inizializzazione variabili carrello e asta    */
        xp = oxp = 100;
        xa = oxa = 100;
        yp = oyp = 105 + delta - H;
        ya = oya = 105 + delta - H;


        graphframe();
        while (1) {
          
          /*if(contat_id==0)*/ {
            char str1[15];
            mutex_lock(&mutex);
            grx_text("     ", XTI, YTI+174,  0,  0);
            sprintf(str1, "%d ", contat_id);
            grx_text(str1, XTI, YTI+174, red, 0);
            grx_text("     ", XTI, YTI+188,  0,  0);
            sprintf(str1, "%d ", num_id);
            grx_text(str1, XTI, YTI+188,green , 0);
            grx_text("     ", XTI, YTI+202,  0,  0);
            sprintf(str1, "%f ", somma[num_id-1][0]);
            grx_text(str1, XTI, YTI+202,green , 0);
            
            grx_text("     ", XTI, YTI+216,  0,  0);
            sprintf(str1, "carr= %d %d %f", PERIOD_CARRELLO, WCET_CARRELLO, ((float)  WCET_CARRELLO) / PERIOD_CARRELLO );
            grx_text(str1, XTI, YTI+216,green , 0);
            
            grx_text("     ", XTI, YTI+230,  0,  0);
            sprintf(str1, "track= %d %d %f", PERIOD_TRACKING, WCET_TRACKING, ((float)  WCET_TRACKING) / PERIOD_TRACKING );
            grx_text(str1, XTI, YTI+230,green , 0);

            mutex_unlock(&mutex);
          }
          
          /* disegna il pendolo */
          
          port_receive(pra, &x1, BLOCK);
          port_receive(prb, &x3, BLOCK);

                /* scrivo posizione del carrello e angolo dell'asta */
                mutex_lock(&mutex);
                
                grx_text("       ", XTI+45, YTI+28,  0,  0);
                sprintf(str, "%5.2f", x3);
                grx_text(str, XTI+45, YTI+28, blue, 0);

                grx_text("     ", XTI+50, YTI+42,  0,  0);
                sprintf(str, "%4.3f", x1 * FCA );
                grx_text(str, XTI+50, YTI+42, blue, 0);
                
                mutex_unlock(&mutex);

                /* calcolo ascissa del punto P del carrello */
                xp = XMED + x3 * SGX;

                /* calcolo ascissa e ordinata dell'estremo dell'asta */
                xa = xp + LA * sin((double)(x1 * SGA));
                ya = yp - LA * cos((double)(x1 * SGA));

                mutex_lock(&mutex);
                
                /* cancella e disegna il carrello */
                grx_box(oxp-LC,oyp,oxp+LC,oyp+H,0);
                grx_box(xp-LC,yp,xp+LC,yp+H,blue);

                /* cancella e disegna la ruota sinistra del carrello */
                grx_circle(oxp-LC+8,oyp+H+4,RAGGIO+2,0);
                grx_circle(xp-LC+8,yp+H+4,RAGGIO+2,green);

                /* cancella e disegna la ruota destra del carrello */
                grx_circle(oxp+LC-8,oyp+H+4,RAGGIO+2,0);
                grx_circle(xp+LC-8,yp+H+4,RAGGIO+2,green);

                /* cancella e disegna l'asta */
                grx_line(oxp,oyp,oxa,oya,0);
                grx_line(xp,yp,xa,ya,red);

                /* cancella e disegna la pallina sull'asta */
                grx_circle(oxa,oya,RAGGIO,0);
                grx_circle(xa,ya,RAGGIO,blue);
                
                mutex_unlock(&mutex);

                /* aggiornamento variabili */
                oxp = xp;
                oyp = yp;
                oxa = xa;
                oya = ya;

                task_endcycle();

        }

        port_disconnect(pra);
        port_disconnect(prb);
        port_disconnect(prc);


} /* FINE TASK DISEGNA */


TASK    query(void)
{
char c, str[100];
int     count = 0;
char *pun;
struct Data_cab1    data={0,0}; 

  while(1)
    {
      
       do {
          c = keyb_getch(BLOCK);


          switch (c) {
            case 'd':   xrif += 1; break;
            case 's':   xrif -= 1; break;
            case 'D':    xrif=7; break;
            case 'S':    xrif=-7; break;
            case '0':   xrif =0 ; break;



            case '1':   prm.GUAD[0] -= 0.1 ; break;
            case '2':   prm.GUAD[0] += 0.1; break;
            case '3':   prm.GUAD[1] -= 10 ; break;
            case '4':   prm.GUAD[1] += 10 ; break;
            case '5':   prm.GUAD[2] -= 0.05; break;
            case '6':   prm.GUAD[2] += 0.05; break;
            case '7':   prm.GUAD[3] -= 0.5; break;
            case '8':   prm.GUAD[3] += 0.5; break;

            case 'o':   prm.WCUT -= 2. ; break;
            case 'p':   prm.WCUT += 2. ; break;
            case 'k':   prm.WCUT1 -= 2. ; break;
            case 'l':   prm.WCUT1 += 2. ; break;
            case 'n':   prm.NOISE -= 0.1; break;
            case 'm':   prm.NOISE += 0.1; break;

            case '/':   prm.OFFSVAL -= 0.1; break;
            case '*':   prm.OFFSVAL += 0.1; break;

            case '[':   prm.COST -= 0.02; break;
            case ']':   prm.COST += 0.02; break;

            case 'c':   prm.SCAN -= 1; break;
            case 'v':   prm.SCAN += 1; break;

            case 'R':   prm = prmbase; break;
            case 'T':   prm = prmmod; break;
            case 'U':   prm = prm2; break;

            case '<':   prm.XTRASL -= .05; break;
            case '>':   prm.XTRASL += .05; break;
              
            case '!':   start_id=1;  break;
            case '@':   if(num_id>0) num_id--; break;
            default:    break;
          }
          
          data.rif=(vmax+vmin)/2. + xrif * (vmax-vmin)/LUNGH;

          /* controllo che il riferim non vada fuori scala massima */
          if (data.rif >  vmax) data.rif =  vmax;
          if (data.rif <  vmin) data.rif =  vmin;

          /* controllo l'impulso */
          if (count>0) count--;
          if (data.imp != 0 && count==0) data.imp = 0;

          /* scrivo sul cab1 i riferim ( rif + imp) per il carrello */
          pun = cab_reserve(cab1);
          memcpy(pun, &data, sizeof(struct Data_cab1));
          cab_putmes(cab1, pun);
          


          /* si visualizza il riferimento del carrello. */

          if (xrif != orif) {
            orif = xrif;
            mutex_lock(&mutex);
            grx_text("       ", XTI+45, YTI+14, 0, 0);
            sprintf(str, "%2.1f cm.", xrif);
            grx_text(str, XTI+55, YTI+14, green, 0);
            mutex_unlock(&mutex);
          }

        } while (c != 27);

        sys_end();
        
        task_endcycle();
    }
}

int printglob;
TASK timejet(void *arg)
{
  TIME table[JET_TABLE_DIM];
  int k, nistan;
  PID i;

  int printed;
  
  while(1) {
    for (i=2, printed=0; i<NTASKS  ; i++) {     
      if ( (nistan=jet_gettable(i,(TIME *) table, -1)) != -1) {
        for(k=0;k<nistan;k++){
          if(index[i] >= NSAMPLES) break;
          if( i>=NTASKS ) {grx_close();perror("Ntask too high for jet");sys_end();}
          timetab[ index[i] ][i] = (float) table[k];
          index[i]++;
        }
        printed++;
      }      
    }
    printglob=printed;
    task_endcycle();
  }
}


/******************   *   MAIN   *    ******************************/


PID         pc,pc2, pd, pe, pq, pt;
PID camera_PID;
PID tracking_PID;
void    main(int argc, char **argv)
{
 
int     modenum;


HARD_TASK_MODEL    m;
SOFT_TASK_MODEL    ms, md, mt;

  SOFT_TASK_MODEL m1, m2;
  HARD_TASK_MODEL m3;



white = rgb16(255,255,255);
black = rgb16(0,0,0);
red   = rgb16(255,0,0);
gray  = rgb16(128,128,128);
green = rgb16(0,255,0);
blue  = rgb16(0,0,255);

#ifdef SOFTPROG
if(argc!=4) {perror("numero di parametri errato. Rilancia");keyb_getchar();sys_end();}
    PERIOD_TRACKING=atol(argv[1]);
    PERIOD_CARRELLO=atol(argv[2]);
    WCET_TRACKING =(long int) PERIOD_TRACKING *( 1.0 - (atof(argv[3])) - 0.2) ;
    WCET_CARRELLO =(long int) PERIOD_CARRELLO * ( atof(argv[3]));
#else
    if(argc!=3) {perror("numero di parametri errato. Rilancia");keyb_getchar();sys_end();}
    PERIOD_TRACKING=atol(argv[1]);
    PERIOD_CARRELLO=atol(argv[2]);
#endif



    
        sys_atrunlevel((void *) my_end,NULL, RUNLEVEL_BEFORE_EXIT);
#ifdef TIMEJET
        sys_atrunlevel((void *) scritt,NULL, RUNLEVEL_AFTER_EXIT);
#endif
#ifdef DATAPLANT
        sys_atrunlevel((void *) scritt1,NULL, RUNLEVEL_AFTER_EXIT);
#endif

        sys_atrunlevel((void *) scritt2,NULL, RUNLEVEL_AFTER_EXIT);
        sys_atrunlevel((void *) scritt3,NULL, RUNLEVEL_AFTER_EXIT);
        
        modenum = grx_getmode(800, 600, 16);
        if (modenum == -1) {
        cprintf("Errore in grx_mode");
        sys_end();
        exit(2);
        }
        if (grx_setmode(modenum) == -1) {
        cprintf("Non posso andare in modo grafico");
        sys_end();
        exit(3);
        }

        init_framegrabber();

        { char str[50];

        sprintf(str, "CARRELLO = %d TRACK = %d ", PERIOD_CARRELLO, PERIOD_TRACKING);
        grx_text(str, 20 , 20 , green, 0);
        grx_text("Premi un tasto per continuare", 20 , 30 , green, 0);
        
        keyb_getchar();
        grx_text("                                 ", 20 , 20 , black, 0);
        grx_text("                                 ", 20 , 30 , black, 0);
        }
        
        initial();

        app_mutex_init(&mutex);

        scenario_jetcontrol();  

        {
          KEY_EVT k;
          k.flag = ALTL_BIT;
          k.scan = KEY_C;
          k.ascii = 'c';
          keyb_hook(k,zerofun);
        }


        /* Creazione dei CAB   */
        cab1 = cab_create("cab1", sizeof(struct Data_cab1), 3);
        cab2 = cab_create("cab2", sizeof(struct Data_cab2), 3);
        /* Creazione dei task */



        


#ifdef SOFTPROG
        soft_task_default_model(m1);
        soft_task_def_level(m1,1);
        soft_task_def_met(m1,WCET_TRACKING);
        soft_task_def_usemath(m1);
        
        soft_task_def_period(m1,(PERIOD_TRACKING));
        soft_task_def_group(m1,1);
        soft_task_def_ctrl_jet(m1);
        //  soft_task_def_skip_arrivals(m1);
        tracking_PID = task_create("track", tracking_task, &m1, NULL);
        if (tracking_PID == -1) {
          sys_end();
          exit(4);
  }
#else
        hard_task_default_model(m3);
        //hard_task_def_level(m3,1);
        hard_task_def_wcet(m3,WCET_TRACKING);
        hard_task_def_mit(m3,(PERIOD_TRACKING));
        hard_task_def_usemath(m3);
        
        hard_task_def_group(m3,1);
        hard_task_def_ctrl_jet(m3);
        tracking_PID = task_create("track", tracking_task, &m3, NULL);
        if (tracking_PID == -1) {
          sys_end();
          exit(4);
        }
#endif

        
        soft_task_default_model(m2);
        soft_task_def_level(m2,1);
        soft_task_def_met(m2,WCET_CAMERA);
        soft_task_def_usemath(m2);
        
        soft_task_def_period(m2,PERIOD_CAMERA);
        soft_task_def_group(m2,1);
        soft_task_def_ctrl_jet(m2);
        
        camera_PID = task_create("cam", camera_task, &m2, NULL);
        if (camera_PID == -1) {
          sys_end();
          exit(4);
        }




        
        hard_task_default_model(m);
        hard_task_def_mit(m, PERIOD_CARRELLO  );
        hard_task_def_wcet(m,WCET_CARRELLO );
        hard_task_def_group(m,2);
        hard_task_def_ctrl_jet(m);
        hard_task_def_usemath(m);

        soft_task_default_model(ms);
        soft_task_def_period(ms, PERIOD_CARRELLO  );
        soft_task_def_met(ms,WCET_CARRELLO );
        soft_task_def_group(ms,2);
        soft_task_def_ctrl_jet(ms);
        soft_task_def_usemath(ms);
                
        soft_task_default_model(md);
        soft_task_def_level(md,1);
        soft_task_def_period(md,PERIOD_DESIGN);
        soft_task_def_met(md, WCET_DESIGN);
        soft_task_def_group(md, 2);
        soft_task_def_ctrl_jet(md);
        soft_task_def_usemath(md);

#ifdef TIMEJET
        soft_task_default_model(mt);
        soft_task_def_level(mt,1);
        soft_task_def_period(mt,PERIOD_TIMEJET);
        soft_task_def_met(mt, WCET_TIMEJET);
        soft_task_def_group(mt, 2);
        soft_task_def_ctrl_jet(mt); 
        soft_task_def_usemath(mt);
#endif





#ifndef TIMEJET
        init_jetcontrol();
#else
        pt = task_create("timejet",(TASK) timejet,&md,NULL);
        if (pt == -1) {
                sys_end();
                exit(4);
        }
#endif
#ifdef SOFTPROG
        pc = task_create("carrello",(TASK) carrello, &ms, NULL);
        if (pc == -1) {
                sys_end();
                exit(4);
        }
#else
        pc = task_create("carrello",(TASK) carrello, &m, NULL);
        if (pc == -1) {
                sys_end();
                exit(4);
        }
#endif


        
        pe = task_create("eval",(TASK) eval,&md, NULL);
        if (pe == -1) {
                sys_end();
                exit(4);
        }
        

        pd = task_create("diseg",(TASK) diseg,&md,NULL);
        if (pd == -1) {
                sys_end();
                exit(4);
        }

        pq = task_create("query",(TASK) query,&md,NULL);
        if (pq == -1) {
                sys_end();
                exit(4);
        }



        /* Attivo i task per controllare e disegnare il carrello */

#ifdef TIMEJET
        task_activate(pt);
#endif
//      task_activate(pc2);

        task_activate(pd);
        task_activate(pc);
        task_activate(pe);

        task_activate(pq);

        
        group_activate(1);


        
}

/****************************************************************/
/* initial                                                     */
/****************************************************************/

void    initial(void)
{
int x, y;


        grx_rect(1,   1, 639, 243, blue);  /* dialog box pendolo  */
        da_motor(0.);

        x = 10;
        y = YTI;
        grx_text("INIZIALIZZ.",  x, y, blue, 0); y += 42;
        grx_text("Carr a sx e premi un tasto", x, y, green, 0);y += 28;
        keyb_getchar();
        vmin=ad_conv(11);

        grx_text("Carr a dx e premi un tasto",x,y,green,0); y += 28;
        keyb_getchar();
        vmax=ad_conv(11);

        grx_text("Asta a sx e premi un tasto", x, y, green, 0);y += 28;
        keyb_getchar();
        vminth=ad_conv(10);

        grx_text("Asta a dx e premi un tasto", x, y, green, 0);y += 28;
        keyb_getchar();
        vmaxth=ad_conv(10);

        grx_text("Premi 'y' per l' identificazione ", x, y, green, 0);y += 28;
        keyb_getchar();

        grx_clear(0);


}

/****************************************************************/
/* graphframe                                                    */
/****************************************************************/
void    graphframe(void)
{
int x, y;

        mutex_lock(&mutex);
        grx_rect(XTI-15,245,639,479,blue);  /* finestra di stato    */
        grx_rect(1,   1, 450 /*639*/, 243, blue);  /* dialog box pendolo  */


        x = XTI;
        y = YTI;
        grx_text("PENDOLO",         x, y, red, 0); y += 14;
        grx_text("Rif.:",           x, y, green, 0); y += 14;

        grx_text("X =",             x,    y,  green,  0); y += 14;
        grx_text("Ang =",           x,    y,  green,  0); y += 14;


        grx_text("s sx, d dx",      x,    y,  green,  0); y += 14;
        grx_text("S: xrif-> -7 ",   x,    y,  green,  0); y += 14;
        grx_text("D: xrif-> +7 ",   x,    y,  green,  0); y += 14;
        grx_text("0: xrif->  0 ",   x,    y,  green,  0); y += 14;

        grx_text("S imp-,D imp+",   x,    y,  green,  0); y += 14;
        grx_text("CALIBRAZ: < , >",   x,    y,  green,  0); y += 14;
        grx_text("Dati Start: !",   x,    y,  green,  0); y += 14;
        grx_text("Cancel Last: @",   x,    y,  green,  0); y += 14;
        
        mutex_unlock(&mutex);
}

/*--------------------------------------------------------------*/
/*      This function is called at system termination       */
/*--------------------------------------------------------------*/
void     my_end(void)
{
  da_motor(0.0);
  grx_close();
  
//  sys_status(CLOCK_STATUS|SCHED_STATUS); 

  sys_end();
}
/*--------------------------------------------------------------*/
/*     This function writes the data in the file DATA           */
/*--------------------------------------------------------------*/


void    scritt(void)
{
  DOS_FILE *file;
  long int i , k;
  char  str[500];

    file = DOS_fopen("data","w");
  
#if 1
  for(i=2;i<NTASKS;i++) {
    sprintf(str,"%s ",proc_table[i].name);
    cprintf("%s", str);
      DOS_fwrite(str,1,strlen(str) ,file);
  }

      DOS_fwrite("\n",1,1,file);
      cprintf("%s", "\n");
#endif

  for(k=0;k<NSAMPLES;k++){

    for(i=2;i<NTASKS;i++) {
      sprintf(str,"%f  ",timetab[k][i]);
      cprintf("%s", str);
//      keyb_getchar();
      DOS_fwrite(str,1,strlen(str) ,file);
    }
    
    DOS_fwrite("\n",1,1,file);
    cprintf("%s", "\n");

  }
  DOS_fclose(file);
}

#ifdef DATAPLANT
void    scritt1(void)
{
  DOS_FILE *file;
  char  str[300];
  long int i;
  float pippy=3.14;

  file = DOS_fopen("dataplant","w");
  for(i=0;i<numdat;i++) {
      sprintf(str,"%lu  %f  %f  %f  %f  %f  %f \n",dati[i].time , dati[i].xist,dati[i].thist,dati[i].par1, dati[i].par2, dati[i].par3, dati[i].par4 );
      cprintf("%lu  %f  %f  %f  %f  %f  %f \n",dati[i].time , dati[i].xist,dati[i].thist,dati[i].par1, dati[i].par2, dati[i].par3, dati[i].par4 );
    if(DOS_fwrite(str,1,strlen(str),file)==0) perror("Errore in write");
  }
  DOS_fclose(file); 
}
#endif

void    scritt2(void)
{
  DOS_FILE *file;
  char tmp[15]={0}, str[250];
  int i;
  float parz=0.0 , average=0.0, sumsq=0.0, confid_int=0.0;
  float t_stud[20]={0.0, 6.314,2.920,2.353,2.132,2.015,1.943,1.895,1.860,\
  1.833,1.812,1.796,1.782,1.771,1.761,1.753,1.746,1.740,1.734,1.729};
  
  file = DOS_fopen("integrale","wa");
  for(i=0;i<num_id;i++) {
    sprintf(tmp,"%f %f \n",somma[i][0],somma[i][1]);
    strcat(str,tmp);
  }
  
    for(i=0;i<num_id;i++) parz +=  somma[i][0];
    average=parz /num_id;
    for(i=0;i<num_id;i++) sumsq +=(average-somma[i][0])*(average-somma[i][0]);
        confid_int= t_stud[num_id-1]*sqrt((double) sumsq/((float) num_id*(num_id-1)));
    sprintf(tmp,"n.run=%d aver=%f confid_int=%f \n",num_id,average,confid_int);
    strcat(str,tmp);
   if(DOS_fwrite(str,1,strlen(str),file)==0) perror("Errore in write");
  DOS_fclose(file);

  file = DOS_fopen("intdata","wa");

  sprintf(str,"%d %d %f %f %d %d %d \n",PERIOD_CARRELLO, PERIOD_TRACKING,average,confid_int, num_id,WCET_CARRELLO,  WCET_TRACKING );
//  strcat(str,tmp);
  if(DOS_fwrite(str,1,strlen(str),file)==0) perror("Errore in write");

  DOS_fclose(file);
}



void nuovi_tempi(long int * contatwait)
{

  int i;
  float parz=0.0 , average=0.0, sumsq=0.0, confid_int=0.0;
  float t_stud[20]={0.0, 6.314,2.920,2.353,2.132,2.015,1.943,1.895,1.860,\
  1.833,1.812,1.796,1.782,1.771,1.761,1.753,1.746,1.740,1.734,1.729};

  static int num_tempi=0;
  // calcolo la media e lo interv di confidenza
  for(i=0;i<num_id;i++) parz +=  somma[i][0];
  average=parz /num_id;
  for(i=0;i<num_id;i++) sumsq +=(average-somma[i][0])*(average-somma[i][0]);
  confid_int= t_stud[num_id-1]*sqrt((double) sumsq/((float) num_id*(num_id-1)));
  
  
  // scrivo i dati relativi alrun in una matrice globale
  sprintf(stringa[num_tempi],"%d %d %f %f %d %d %d \n",PERIOD_CARRELLO, PERIOD_TRACKING,average,confid_int, num_id,WCET_CARRELLO,  WCET_TRACKING );
  num_tempi++;
  if(num_tempi >= MAX_TEMPI) sys_end();


  // cambio i periodi-banda dei tasks 
    {
      double band;
      band=((float) WCET_CARRELLO)/PERIOD_CARRELLO;
      
      PERIOD_CARRELLO = PERIOD_CARRELLO; // + 500;
      PERIOD_TRACKING = PERIOD_TRACKING;  // +2000;
      band+=0.1;
      WCET_CARRELLO = band * PERIOD_CARRELLO;
      WCET_TRACKING = ( 0.8 - band) * PERIOD_TRACKING;

    }
        
  CBS_modify_bandwidth(pc,(TIME) PERIOD_CARRELLO,(int) WCET_CARRELLO);
  CBS_modify_bandwidth(tracking_PID,(TIME) PERIOD_TRACKING,(int) WCET_TRACKING);   

  // resetto le variabili per il run successivo 
  num_id=0;
  *contatwait=(long int)  (1.5 * SEC_ID / DEADSECTH(PERIOD_CARRELLO));
}

void    scritt3(void)
{
  DOS_FILE *file;
  char tmp[1000]={0};
  int i;
  
  file = DOS_fopen("totaldata","wa");

  for(i=0;i<MAX_TEMPI;i++) {
    
    if(DOS_fwrite(stringa[i],1,strlen(stringa[i]),file)==0) perror("Errore in write");

    
//    strcat(tmp, stringa[i][0]);
  }
//    if(DOS_fwrite(tmp,1,strlen(tmp),file)==0) perror("Errore in write");
  DOS_fclose(file);
}