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

}