Subversion Repositories shark

/*************************************************************
 *                                                           *
 *           Stabilizzazione di un pendolo inverso           *
 *                                                           *
 *                                                           *
 *Written by: Gabriele Bolognini - Scuola S.Anna -- Pisa     *
 *                                                           *
 *************************************************************/

#include "demo.h"
#include "kernel/func.h"

#include "kernel/kern.h"
#include <stdlib.h>

#include "drivers/keyb.h"
#include "drivers/glib.h"
#include "ll/i386/x-dos.h"
#include "modules/cabs.h"
#include "modules/hartport.h"
#include "string.h"

#include <math.h>
#include "pclab.h"
#include "modules/pi.h"
#include "modules/nop.h"

#include "cost.h"   /* Contiene le costanti del task DISEGNA  */

#define NSAMPLES  500
#define NTASKS  10


TIME timetab[NSAMPLES][NTASKS];
static int index[MAX_PROC];
char dest[10000];



mutex_t mutex;
void app_mutex_init(mutex_t *);

#define LUNGH 35
#define THETAMAX 45
#define VDANG 2.4
#define NMAX 500

/*----- ACHTUNG!!!: param. iniz. del guadagno, deadline, ecc.----------- */
#define INIPARAMS {{ 2.3 , 300 , 0.5 , 15 },1.,1.0,20.,20., 0.1,  1 , 1}
#define MODPARAMS {{ 1. , 300 , 0.9 , 34 },0.74 ,1.,20.,20., 0.2,  1 , 1}
struct Parametri{
  float GUAD[4];
  float COST;
  float NOISE;
  float WCUT;
  float WCUT1;
  float OFFSVAL;
  int DEADLINE;
  int SCAN;
} prm=INIPARAMS, prmbase=INIPARAMS, prmmod=MODPARAMS;
#define DEADSECX (prm.DEADLINE * 0.001 * prm.SCAN)/*mit task CONTROLLO in sec*/
#define DEADSECTH (prm.DEADLINE * 0.001 ) /* mit task CONTROLLO in sec*/

struct  Data_cab1 {
        float   rif;
        int     imp;
};

struct  Data_cab2 {
        float   x;
        float   y;
};

CAB     cab1, cab2;

float vmax, vmin, vmaxth, vminth;


float vin=0, vout, yout, thout;


//int sysid=0, startid=0; /* usate nell'identificazione del sistema */



void    graphframe(void);
void    my_end(void);
void    scritt(void);
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;
     


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 *u)/(1+ prm.WCUT * DEADSECX);
  oldy=y;
  return y;
}
float bass2(float u)
{

  float y;
  static float oldy=0;
  y=(oldy + prm.WCUT1 * DEADSECX *u)/(1+ prm.WCUT1 * DEADSECX);
  oldy=y;
  return y;
}
float bass3(float u)
{

  float y;
  static float oldy=0;
  y=(oldy + prm.WCUT * DEADSECTH *u)/(1+ prm.WCUT * DEADSECTH);
  oldy=y;
  return y;
}
float bass4(float u)
{

  float y;
  static float oldy=0;
  y=(oldy + prm.WCUT1 * DEADSECTH *u)/(1+ prm.WCUT1 * DEADSECTH);
  oldy=y;
  return y;
}

float  dx(float u)
{
  static float oldu=0;
  float y;
  y=(u-oldu)/DEADSECX;
  oldu=u;
  return y;
}
float  dth(float u)
{
  static float oldu=0;
  float y;
  y=(u-oldu)/DEADSECTH;
  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

-----------------------------------------------------------------------*/
TASK    carr2(void)
{
  float   thist, y[2], yp[2],x3ist=0;    
  while(1) {

        if(contat%prm.SCAN==0) yout=ad_conv(11);

        /* converto i valori dei sensori in posizioni */
        /* calcolo le velocita' lineari e angolari opportunamente filtrate */
        if(contat%prm.SCAN == 0) {
          x3ist=v2x(yout);
          y[0]=bass1(x3ist);
          yp[0] = bass2(dx(y[0]));
        }

    
    task_endcycle();
  }
}
TASK    carrello(void)
{
  float   thist, y[2], yp[2],x3ist=0;          /* valori attuali   */
  PORT    pa, pb;                          /* communication ports  */
  struct  Data_cab1   data;
  char *pun;
  int i;
  float offset;

  long int k=0, contat=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*/
        pun = cab_getmes(cab1);
        memcpy(&data, pun, sizeof(struct Data_cab1));
        cab_unget(cab1, pun);

    /* prendo i dati sulle posizioni dal trasduttore ADC  */
//      if(contat%prm.SCAN==0) yout=ad_conv(11);
        thout=ad_conv(10);

        /* taglio brusco dei livelli di rumore sopra il NOISE */
        {
#define AVR 10
          static float oyout[AVR], othout[AVR];
          float avy, avth;
          static int index=0, flag=1;

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

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

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

        /* converto i valori dei sensori in posizioni */
        /* calcolo le velocita' lineari e angolari opportunamente filtrate */
//      if(contat%prm.SCAN == 0) {
//        x3ist=v2x(yout);
//        y[0]=bass1(x3ist);
//        yp[0] = bass2(dx(y[0]));
//      }
        thist=v2theta(thout);
        y[1]=bass3(thist);
        yp[1] = bass4(dth(y[1]));

        /* PARAMETRI del controllore :1 */
          vin = prm.COST * (prm.GUAD[0] * (x3ist-v2x(data.rif))+ 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;

        /* !!!! se si e' scelta la procedura di identificazione */
        /*      if(sysid != 0){
          if(startid == 0) {vin=0.0; vout=0.0;}
          else if(startid == 1 ) {
            if(stime==0 ) stime = sys_gettime(NULL);
            if((sys_gettime(NULL)-stime)/1000 <= 400 )  {vin= -1.7; vout=-2.4;}
            else  {vin= 0.0; vout=0.0;}
          }
        }
        */
        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);


        contat++;
        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) {


        /* 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}; 
float xrif=0, orif=1;
  while(1)
    {
      
       do {
          c = keyb_getch(BLOCK);


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

//          case 'z':   startid = 1; 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.1; break;
            case '6':   prm.GUAD[2] += 0.1; break;
            case '7':   prm.GUAD[3] -= 0.5; break;
            case '8':   prm.GUAD[3] += 0.5; break;

            case 'o':   prm.WCUT -= 5. ; break;
            case 'p':   prm.WCUT += 5. ; break;
            case 'k':   prm.WCUT1 -= 5. ; break;
            case 'l':   prm.WCUT1 += 5. ; 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.05; break;
            case ']':   prm.COST += 0.05; break;

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



            case 'R':   prm = prmbase; break;
            case 'T':   prm = prmmod; 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;
  
  for (;;) {
    for (i=2, printed=0; i<MAX_PROC && printed<JET_NTASK; i++) {     
      if ( (nistan=jet_gettable(i,(TIME *) &table, -1)) != -1) {
        for(k=0;k<nistan;k++){
          if(index[i] >= NSAMPLES) break;
          if( i>NTASKS ) sys_end();
          timetab[ index[i] ][i] = table[k];
          index[i]++;
        }
        printed++;
      }      
    }
    printglob=printed;
    task_endcycle();
  }
}


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



void    main(int argc, char **argv)
{
 
int     modenum;


HARD_TASK_MODEL    m;
SOFT_TASK_MODEL    md, mt;
PID         pc,pd, pe, pq, pt;



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

        sys_atrunlevel((void *) my_end,NULL, RUNLEVEL_BEFORE_EXIT);
        sys_atrunlevel((void *) scritt,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);
        }

        
        initial();

        app_mutex_init(&mutex);

        scenario_jetcontrol();  


        init_framegrabber();



        /* Creazione dei CAB   */
        cab1 = cab_create("cab1", sizeof(struct Data_cab1), 3);
        cab2 = cab_create("cab2", sizeof(struct Data_cab2), 3);
        /* Creazione dei task */
        
        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(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);

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

        init_jetcontrol();
        
        pc = task_create("carrello",(TASK) carrello, &m, NULL);
        if (pc == -1) {
                sys_end();
                exit(4);
        }

        
        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);
        }
        pt = task_create("timejet",(TASK) timejet,&md,NULL);
        if (pt == -1) {
                sys_end();
                exit(4);
        }



                /* Attivo i task per controllare e disegnare il carrello */
        task_activate(pt);
        task_activate(pc);
        task_activate(pd);
        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;
        if(keyb_getchar() == 'y' ) sysid=1;
        else sysid=0 ;
        
        if(sysid==1) {
        grx_text("MODALITA' IDENTIFICAZIONE!!!!", x, y, green, 0);y += 14;
        grx_text("Premi un tasto per iniziare", x, y, green, 0);y += 14;
        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("q: xrif-> -7 ",   x,    y,  green,  0); y += 14;
        grx_text("p: 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;
        mutex_unlock(&mutex);
}

/*--------------------------------------------------------------*/
/*      This function is called at system termination       */
/*--------------------------------------------------------------*/
void     my_end(void)
{
  da_motor(0.0);
  grx_close();
  sys_end();
}
/*--------------------------------------------------------------*/
/*     This function writes the data in the file DATA           */
/*--------------------------------------------------------------*/


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

  
  for(i=2;i<11;i++) {
    sprintf(str,"%s ",proc_table[i].name);
    strcat(dest,str);
  }
  strcat(dest,"\n");

  
  for(k=0;k<500;k++){  
    for(i=2;i<11;i++) {
      sprintf(str,"%ld ",timetab[k][i]);
      strcat(dest,str);
    }
    strcat(dest,"\n");
    }
  file = DOS_fopen("data","w");    
  DOS_fwrite(dest,1,strlen(dest),file);
  DOS_fclose(file);
 
}