Subversion Repositories shark

Rev

Rev 1655 | Blame | Compare with Previous | Last modification | View Log | RSS feed

/***************************************************************************
  **************************************************************************
  ***                        Universita' di Pavia                        ***
  ***                 Corso  :  Informatoca Industriale                  ***
  **************************************************************************
  ***                Progetto : MONITORAGGIO AUTOSTRADA                  ***
  **************************************************************************
  ***   Realizzato da : Nino Verzellesi e Quadrubbi Giacomo              ***
  **************************************************************************
  ***************************************************************************/



/* ----------------------------------------------------------------------------- */

#include <kernel/kern.h>
#include <drivers/glib.h>
#include <drivers/keyb.h>
#include <semaphore.h>
#include <stdlib.h>
#include <math.h>

/* ----------------------------------------------------------------------------- */


/********************* DEFINIZIONE DELLE COSTANTI **************************/

#define ESC      27             /* codice ASCII  del tasto ESCAPE */
#define MAX_V    35             /* massimo numero di veicoli */
#define GRUPPO   1
#define LUNGCAMION  40          /* lunghezza del camion */
#define LARGCAMION  10          /* larghezza del camion */
#define LUNGLENTA  16           /* lunghezza della macchina lenta */
#define LARGLENTA  8            /* larghezza della macchina lenta */
#define LUNGVELOCE  20          /* lunghezza della macchina veloce */
#define LARGVELOCE  8           /* larghezza della macchina veloce */
#define DT 0.04                 /* passo del campionamento */
#define CENTROCORSIA1 477       /* coordinata del centro della corsia 1 */
#define CENTROCORSIA2 459       /* coordinata del centro della corsia 2 */
#define CENTROCARREGGIATA 469   /* coordinata del centro della carreggiata */


/* ----------------------------------------------------------------------------- */


/****************** DEFINIZIONE DELLE VARIABILI GLOBALI ********************/

double  tick = 1.0;             /* system tick = 1 ms */
int     periodo = 40000;        /* periodo */
int     wcet = 1000;            /* deadline */
PID     pid;                    
sem_t   mutex;                  /* semaforo per la grafica */
sem_t   strada;                 /* semaforo per le variabili corsia1 e corsia2 */
int MAX_X;                      /* dimensione x della modalita' di visualizzazione dello schermo */
int MAX_Y;                      /* dimensione y della modalita' di visualizzazione dello schermo */
short corsia1[40000];           /* vettore della corsia1 */
short corsia2[40000];           /* vettore della corsia2 */
int xelic;                      /* cordinata dell'elicottero */
char c;                         /* carattere da tastiera */


/* ----------------------------------------------------------------------------- */


/****************** FUNZIONE CHE DISEGNA I VEICOLI *************************/

void    draw_veicolo(int x, int y, int colore,int lunghezza,int larghezza)
{
        int u;          /* indice di ciclo */
        int v;          /* variabile ausiliaria per la ricostruzione della segnaletica */
        int xreal;      /* coordinata di fine veicolo */

        xreal=x-lunghezza;      /* calcola la coordinata di fine veicolo */
        if (xreal<0)            /* la pone a zero nel caso in cui sia minore di zero (veicolo non ancora interamente entrato in autostrada) */
                xreal=0;

        /* disegna il veicolo nella posizione indicata e con il colore indicato */
        sem_wait(&mutex);
        grx_box(xreal,(int)(y-larghezza/2),x,(int)(y+larghezza/2),colore);
        sem_post(&mutex);

        /* ricostruisce la segnaletica orrizzontale della strada nel caso in cui si sta' cancellando il veicolo dalla posizione precedente (ridisegna solo quella parte che e' stata cancellata) */
        if (colore==0)
                for (u=xreal;u<x;u++)
                {
                        v=u%8;
                        if ((v==0) || (v==1))
                        {
                                sem_wait(&mutex);
                                grx_plot(u,CENTROCARREGGIATA,rgb16(255,255,255));
                                sem_post(&mutex);
                        }
                }
}


/* ----------------------------------------------------------------------------- */


/** FUNZIONE CHE CALCOLA LA DISTANZA DAL VEICOLO CHE PRECEDE SULLA CORSIA **/

int dist_ant(int pos_x,int pos_y,int distsic)
{
        int libero;   /* variabile che indica se l'attuale posizione puntata dal veicolo e' libera */
        int distant;  /* distanza dal veicolo che ci precede */
        int fine;     /* variabile ausiliaria per capire se sto' arrivando alla fine della strada */

        /* inizializzazione delle variabili */
        libero=0;
        distant=0;

        /* calcola la distanza da un eventuale veicolo che ci precede */
        sem_wait(&strada);
        if (pos_y<CENTROCARREGGIATA)                    /* controlla se il veicolo e' nella prima corsia, altrimenti e' in seconda corsia */
                while (libero==0 && distant<distsic)    /* il calcolo della distanza da un veicolo che ci precede termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */
                {
                        fine=pos_x+distant;             /* nel caso in cui il veicolo si giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia preceduto da nessun veicolo */
                        if (fine>40000)
                                distant=distsic;
                        else
                                if (corsia2[fine]!=0)   /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */
                                        libero=1;
                                else
                                        distant++;
                }
        else                                            /* il veicolo e' nella seconda corsia */
                while (libero==0 && distant<distsic)    /* il calcolo della distanza da un veicolo che ci precede termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */
                {
                        fine=pos_x+distant;             /* nel caso in cui il veicolo si giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia preceduto da nessun veicolo */
                        if (fine>40000)
                                distant=distsic;
                        else
                                if (corsia1[fine]!=0)   /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */
                                        libero=1;
                                else
                                        distant++;
                }
        sem_post(&strada);

        return(distant);        /* ritorna il valore della distanza misurata dal sensore , se non ha trovato nessun veicolo tale distanza e' pari alla massima lunghezza a cui puo' arrivare il sensore */
}


/* ----------------------------------------------------------------------------- */


/** FUNZIONE CHE CALCOLA LA DISTANZA ANTERIORE DESTRA DI UN VEICOLO CHE PRECEDE **/

int dist_ant_dx(int pos_x,int pos_y,int distsicdx)
{
        int distantdx;  /* distanza dal veicolo che ci precede a destra */
        int libero;     /* variabile che indica se l'attuale posizione puntata dal veicolo e' libera */
        int fine;       /* variabile ausiliaria per capire se sto' arrivando alla fine della strada */

        /* inizializzazione delle variabili */
        libero=0;
        distantdx=0;

        sem_wait(&strada);
        while (libero==0 && distantdx<distsicdx)        /* il calcolo della distanza da un veicolo che ci precede a destra termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */
        {
                fine=pos_x+distantdx;           /* nel caso in cui il veicolo sia giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia preceduto da nessun veicolo */
                if (fine>40000)
                        distantdx=distsicdx;
                else
                        if (corsia1[fine]!=0)   /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */
                                libero=1;
                        else
                                distantdx++;
        }
        sem_post(&strada);

        return(distantdx);      /* ritorna il valore della distanza misurata dal sensore , se non ha trovato nessun veicolo tale distanza e' pari alla massima lunghezza a cui puo' arrivare il sensore */
}


/* ----------------------------------------------------------------------------- */


/** FUNZIONE CHE CALCOLA LA DISTANZA ANTERIORE SINISTRA DI UN VEICOLO CHE PRECEDE **/

int dist_ant_sx(int pos_x,int pos_y,int distsicsx)
{
        int distantsx;  /* distanza dal veicolo che ci precede a sinistra */
        int libero;     /* variabile che indica se l'attuale posizione puntata dal veicolo e' libera */
        int fine;       /* variabile ausiliaria per capire se sto' arrivando alla fine della strada */

        /* inizializzazione delle variabili */
        libero=0;
        distantsx=0;

        sem_wait(&strada);
        while (libero==0 && distantsx<distsicsx)        /* il calcolo della distanza da un veicolo che ci precede a sinistra termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */
        {
                fine=pos_x+distantsx;           /* nel caso in cui il veicolo si giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia preceduto da nessun veicolo */
                if (fine>40000)
                        distantsx=distsicsx;
                else
                        if (corsia2[fine]!=0)   /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */
                                libero=1;
                        else
                                distantsx++;
        }
        sem_post(&strada);

        return(distantsx);      /* ritorna il valore della distanza misurata dal sensore , se non ha trovato nessun veicolo tale distanza e' pari alla massima lunghezza a cui puo' arrivare il sensore */
}


/* ----------------------------------------------------------------------------- */


/** FUNZIONE CHE CALCOLA LA DISTANZA POSTERIORE SINISTRA DI UN VEICOLO CHE INSEGUE **/

int dist_postsx (int pos_x,int pos_y,int distsorp)
{
        int distpostsx; /* distanza dal veicolo che ci insegue a sinistra */
        int libero;     /* variabile che indica se l'attuale posizione puntata dal veicolo e' libera */
        int inizio;     /* variabile ausiliaria per capire se sono all'inizio della strada */

        /* inizializzazione delle variabili */
        distpostsx=0;
        libero=0;

        sem_wait(&strada);
        while (libero==0 && distpostsx<distsorp)        /* il calcolo della distanza da un veicolo che ci insegue a sinistra termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */
        {
                inizio=pos_x-distpostsx;        /* nel caso in cui il veicolo si giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia insuguito da nessun veicolo */
                if (inizio<0)
                        distpostsx=distsorp;
                else
                        if (corsia2[inizio]!=0) /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */
                                libero=1;
                        else
                                distpostsx++;
        }
        sem_post(&strada);

        return(distpostsx);     /* ritorna il valore della distanza misurata dal sensore , se non ha trovato nessun veicolo tale distanza e' pari alla massima lunghezza a cui puo' arrivare il sensore */
}


/* ----------------------------------------------------------------------------- */


/** FUNZIONE CHE CALCOLA LA DISTANZA POSTERIORE DESTRA DI UN VEICOLO CHE INSEGUE **/

int dist_postdx (int pos_x,int pos_y,int distrientro)
{
        int distpostdx; /* distanza dal veicolo che ci insegue a destra */
        int libero;     /* variabile che indica se l'attuale posizione puntata dal veicolo e' libera */
        int inizio;     /* variabile ausiliaria per capire se sono all'inizio della strada */

        /* inizializzazione delle variabili */
        libero=0;
        distpostdx=0;

        sem_wait(&strada);
        while (libero==0 && distpostdx<distrientro)     /* il calcolo della distanza da un veicolo che ci insegue a destra termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */
        {
                inizio=pos_x-distpostdx;        /* nel caso in cui il veicolo si giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia inseguito da nessun veicolo */
                if (inizio<0)
                        distpostdx=distrientro;
                else
                        if (corsia1[inizio]!=0) /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */
                                libero=1;
                        else
                                distpostdx++;
        }
        sem_post(&strada);

        return(distpostdx);     /* ritorna il valore della distanza misurata dal sensore , se non ha trovato nessun veicolo tale distanza e' pari alla massima lunghezza a cui puo' arrivare il sensore */
}


/* ----------------------------------------------------------------------------- */


/**************************** TASK AUTO LENTA ******************************/

TASK    auto_lenta(void *arg)
{
        int x;                  /* posizione x assunta dal veicolo */
        int y;                  /* posizione y assunta dal veicolo */
        int oxelic;             /* posizione vecchia dell'elicottero */
        int ox;                 /* posizione vecchia x dall'auto lenta */
        int oy;                 /* posizione vecchia y dall'auto lenta */
        int k;                  /* indice di ciclo */
        int estremo;            /* distanza massima a cui ci possiamo spostare dall'elicottero (sia a destra che a sinistra) */
        int sorpasso;           /* indica se il veicolo e' in fase di sorpasso */
        int rientro;            /* indica se il veicolo e' in fase di rientro */
        int precedentedritto;   /* distanza anteriore da un veicolo */
        int od;                 /* vecchia distanza anteriore da un veicolo */
        int tot;                /* distanza massima raggiungibile del sensore posteriore sinistro */
        int tot3;               /* distanza massima raggiungibile del sensore anteriore destro */
        int tot2;               /* distanza massima raggiungibile del sensore posteriore destro */
        int i = (int)arg;      
        int sensore_ant=200;    /* distanza massima raggiungibile del sensore anteriore */
        char stri[22];          /* vettore di caratteri */
        char ostri[22];         /* vettore di caratteri */
        float vcrociera;        /* velocita' desiderata */
        float vmax2=35.0;       /* velocita' massima raggiungibile in seconda corsia */
        float vmax1=25.0;       /* velocita' massima raggiungibile in prima corsia */
        float frenata=-7.0;     /* valore massimo della frenata */
        float amax=10.0;        /* valore massimo dell'accellerazione */
        float v;                /* velocita' attuale */
        float a;                /* accellerazione attuale */
        float ov;               /* velocita' precedente */
        float oa;               /* accellerazione precedente */

        /* inizializzazione delle variabili */
        v=5.0;
        a=0.0;
        x=LUNGLENTA;
        y=CENTROCORSIA1;
        sorpasso=0;
        rientro=0;
        tot=100;
        tot2=150;
        tot3=100;
        vcrociera=vmax1;
        oa=a;
        ov=v;
        od=0;
        oxelic=xelic;
        estremo=(int)((MAX_X/2)-1);

        while (1)
        {
                /* cancella il veicolo se era presente sull'autostrada vista dall'elicottero */
                if (abs(oxelic-ox)<estremo)
                        draw_veicolo(ox-oxelic+estremo, oy, 0,LUNGLENTA,LARGLENTA);

                /* salva le vecchie coordinate */
                ox = x;
                oy = y;
               
                /* cancella il veicolo dalla vecchia posizione sulla corsia , andando a cancellarlo nel vettore della corsia in cui si trovava */
                sem_wait(&strada);
                for (k=0;k<LUNGLENTA;k++)
                {
                        if ((oy<CENTROCORSIA1) && (oy>CENTROCORSIA2))
                        {
                                corsia1[ox-k]=0;
                                corsia2[ox-k]=0;
                        }
                        else
                                if (oy==CENTROCORSIA1)
                                        corsia1[ox-k]=0;
                                else
                                        if (oy==CENTROCORSIA2)
                                                corsia2[ox-k]=0;
                }
                sem_post(&strada);

                /* cancella tutte le vecchie informazioni che compaiono sullo schermo */
                sprintf(stri,"posizione %d",(int)(ox*0.25));
                sem_wait(&mutex);
                grx_text(stri,70,50+i*10,rgb16(0,0,0),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"a %nf",oa);
                sem_wait(&mutex);
                grx_text(ostri,300,50+i*10,rgb16(0,0,0),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"v %nf",ov);
                sem_wait(&mutex);
                grx_text(ostri,200,50+i*10,rgb16(0,0,0),rgb16(0,0,0));
                sem_post(&mutex);

                /* legge la distanza dal sensore anteriore e nel caso in cui sia attaccato alla macchina che precede vengo eliminato */
                precedentedritto=dist_ant(x,y,sensore_ant);
                if (precedentedritto==1)
                        task_abort(i);

                /* disegna il veicolo nella nuova posizione */
                if (abs(xelic-x)<estremo)
                        draw_veicolo(x-xelic+estremo, y, rgb16(255,0,0),LUNGLENTA,LARGLENTA);

                /* varia la massima distanza vista dal sensore posteriore sinistro a seconda della velocita' */
                tot=(int)(v*4);
                if (tot<50)
                        tot=50;
           
                if (precedentedritto<100)       /* ci siamo avvicinando troppo alla macchina che ci precede */
                        if ((sorpasso==0) && (dist_postsx(x,y,200)>tot)&&(dist_ant_sx(x,y,50)>40))
                        {
                                /* c'e' uno davanti e non sopraggiunge nessuno sulla corsia di sorpasso */
                                sorpasso=1;
                                rientro=0;
                                vcrociera=vmax2;
                        }
                        else
                                a=frenata;      /* c'e qualcuno davanti ,ma non possiamo sorpassare */
                else            /* siamo lontani da un veicolo che ci precede o non ci precede nessuno */
                        if (v<vcrociera)        /* accelleriamo gradualmente fino a portarci alla velocita' di crociera desiderata */
                                a=amax-(amax/vcrociera)*v;
                        else
                                if(v>vcrociera)         /* freniamo per portarci al valore della velocita'di crociera */
                                        a=frenata/3;
                                else
                                        a=0.0;          /* siamo alla velocita' di crociera */

                /* controlla se e' possibile la manovra di rientro dal sorpasso, aggiustando tutti i parametri nel modo corretto */
                if ((dist_postdx(x,y,200)>tot2) && (dist_ant_dx(x,y,200)>tot3) && (sorpasso==1))
                {
                        rientro=1;
                        sorpasso=0;
                        vcrociera=vmax1;
                }
               
                /* aggiusta la cordinata y per far visualizzare il sorpasso o il rientro della macchina in modo graduale */
                if(y>CENTROCORSIA2 && sorpasso==1)
                        y=y-1;

                if(y<CENTROCORSIA1 && rientro==1)
                        y=y+1;

                /* calcola la velocita' e nel caso di v elocita' negative la pone a zero (non sono permesse le retromarcie) */
                v=v+a*DT;
                if (v<0)
                        v=0.0;

                /* calcola la cordinata x a cui si trova la macchina */
                x=x+(int)((v*DT)/0.25);

                /* scrive a video i nuovi parametri appena calcolati */
                sprintf(stri,"posizione %d",(int)(x*0.25));
                sem_wait(&mutex);
                grx_text(stri,70,50+i*10,rgb16(255,255,255),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"v %nf",v);
                sem_wait(&mutex);
                grx_text(ostri,200,50+i*10,rgb16(255,255,255),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"a %nf",a);
                sem_wait(&mutex);
                grx_text(ostri,300,50+i*10,rgb16(255,255,255),rgb16(0,0,0));
                sem_post(&mutex);
     
                /* quando il veicolo arriva alla fine dell'autostrada viene eliminato */
                if (x>=40000)
                        task_abort(i);

                /* salva la nuova posizione del veicolo */
                sem_wait(&strada);
                for (k=0;k<LUNGLENTA;k++)
                {
                        if ((y<CENTROCORSIA1) && (y>CENTROCORSIA2))
                        {
                                corsia2[x-k]=1;
                                corsia1[x-k]=1;
                        }
                        else
                                if (y==CENTROCORSIA1)
                                        corsia1[x-k]=1;
                                else
                                        if (y==CENTROCORSIA2)
                                                corsia2[x-k]=1;
                }
                sem_post(&strada);

                /* salvo i parametri che occorrono per il prossimo ciclo */
                oxelic=xelic;
                od=precedentedritto;
                ov=v;
                oa=a;

                task_endcycle();        /* termina le operazioni che il task deve eseguire */
        }
}


/* ----------------------------------------------------------------------------- */


/************************** TASK AUTO VELOCE *******************************/

TASK    auto_veloce(void *arg)
{
        int x;                  /* posizione x assunta dal veicolo */
        int y;                  /* posizione y assunta dal veicolo */
        int oxelic;             /* posizione vecchia dell'elicottero */
        int ox;                 /* posizione vecchia x dall'auto veloce */
        int oy;                 /* posizione vecchia y dall'auto veloce */
        int k;                  /* indice di ciclo */
        int estremo;            /* distanza massima a cui ci possiamo spostare dall'elicottero (sia a destra che a sinistra) */
        int sorpasso;           /* indica se il veicolo e' in fase di sorpasso */
        int rientro;            /* indica se il veicolo e' in fase di rientro */
        int precedentedritto;   /* distanza anteriore da un veicolo */
        int od;                 /* vecchia distanza anteriore da un veicolo */
        int tot;                /* distanza massima raggiungibile del sensore posteriore sinistro */
        int tot3;               /* distanza massima raggiungibile del sensore anteriore destro */
        int tot2;               /* distanza massima raggiungibile del sensore posteriore destro */
        int i = (int)arg;
        int sensore_ant=200;    /* distanza massima raggiungibile del sensore anteriore */
        char stri[22];          /* vettore di caratteri */
        char ostri[22];         /* vettore di caratteri */
        float vcrociera;        /* velocita' desiderata */
        float vmax1=35.0;       /* velocita' massima raggiungibile in prima corsia */
        float vmax2=40.0;       /* velocita' massima raggiungibile in seconda corsia */
        float frenata=-7.0;     /* valore massimo della frenata */
        float amax=15.0;        /* valore massimo del'accellerazione */
        float v;                /* velocita' attuale */
        float a;                /* accellerazione attuale */
        float ov;               /* velocita' precedente */
        float oa;               /* accellerazione precedente */

        /* inizializzazione delle variabili */
        v=5.0;
        a=0.0;
        x=LUNGVELOCE;
        y=CENTROCORSIA1;
        sorpasso=0;
        rientro=0;
        tot=100;
        tot2=150;
        tot3=100;
        oa=a;
        vcrociera=vmax1;
        ov=v;
        od=0;
        oxelic=xelic;
        estremo=(int)((MAX_X/2)-1);

        while (1)
        {
                /* cancella il veicolo se era presente sull'autostrada vista dall'elicottero */
                if (abs(oxelic-ox)<estremo)
                        draw_veicolo(ox-oxelic+estremo, oy, 0,LUNGVELOCE,LARGVELOCE);

                /* salva le vecchie coordinate */
                ox = x;
                oy = y;
               
                /* cancella il veicolo dalla vecchia posizione sulla corsia , andando a cancellarlo nel vettore della corsia in cui si trovava */
                sem_wait(&strada);
                for (k=0;k<LUNGVELOCE;k++)
                {
                        if ((oy<CENTROCORSIA1) && (oy>CENTROCORSIA2))
                        {
                                corsia1[ox-k]=0;
                                corsia2[ox-k]=0;
                        }
                        else
                                if (oy==CENTROCORSIA1)
                                        corsia1[ox-k]=0;
                                else
                                        if (oy==CENTROCORSIA2)
                                                corsia2[ox-k]=0;
                }
                sem_post(&strada);

                /* cancella tutte le vecchie informazioni che compaiono sullo schermo */
                sprintf(stri,"posizione %d",(int)(ox*0.25));
                sem_wait(&mutex);
                grx_text(stri,70,50+i*10,rgb16(0,0,0),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"v %nf",ov);
                sem_wait(&mutex);
                grx_text(ostri,200,50+i*10,rgb16(0,0,0),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"a %nf",oa);
                sem_wait(&mutex);
                grx_text(ostri,300,50+i*10,rgb16(0,0,0),rgb16(0,0,0));
                sem_post(&mutex);

                /* legge la distanza dal sensore anteriore e nel caso in cui sia attaccato alla macchina che precede vengo eliminato */
                precedentedritto=dist_ant(x,y,sensore_ant);
                if (precedentedritto==1)
                        task_abort(i);

                /* disegna il veicolo nella nuova posizione */
                if (abs(xelic-x)<estremo)
                        draw_veicolo(x-xelic+estremo, y, rgb16(0,255,0),LUNGVELOCE,LARGVELOCE);


                /* varia la massima distanza vista dal sensore posteriore sinistro a seconda della velocita' */
                tot=(int)(v*4);
                if (tot<50)
                        tot=50;

                if (precedentedritto<150)       /* ci siamo avvicinando troppo alla macchina che ci precede */
                        if ((sorpasso==0) && (dist_postsx(x,y,200)>tot)&&(dist_ant_sx(x,y,50)>40))
                        {
                                /* c'e' uno davanti e non sopraggiunge nessuno sulla corsia di sorpasso */
                                sorpasso=1;
                                rientro=0;
                                vcrociera=vmax2;
                        }
                        else    /* c'e qualcuno davanti ,ma non possiamo sorpassare */
                                a=frenata;
                else            /* siamo lontani da un veicolo che ci precede o non ci precede nessuno */
                        if (v<vcrociera)        /* accelleriamo gradualmente fino a portarci alla velocita' di crociera desiderata */
                                a=amax-(amax/vcrociera)*v;
                        else
                                if(v>vcrociera)         /* freniamo per portarci al valore della velocita'di crociera */
                                        a=frenata/3;
                                else
                                        a=0.0;          /* siamo alla velocita' di crociera */

                /* controlla se e' possibile la manovra di rientro dal sorpasso, aggiustando tutti i parametri nel modo corretto */
                if ((dist_postdx(x,y,200)>tot2) && (dist_ant_dx(x,y,200)>tot3) && (sorpasso==1))
                {
                        rientro=1;
                        sorpasso=0;
                        vcrociera=vmax1;
                }

                /* aggiusta la cordinata y per far visualizzare il sorpasso o il rientro della macchina in modo graduale */
                if(y>CENTROCORSIA2 && sorpasso==1)
                        y=y-1;

                if(y<CENTROCORSIA1 && rientro==1)
                        y=y+1;

                /* calcola la velocita' e nel caso di v elocita' negative la pone a zero (non sono permesse le retromarcie) */
                v=v+a*DT;
                if (v<0)
                        v=0.0;

                /* calcola la cordinata x a cui si trova la macchina */
                x=x+(int)((v*DT)/0.25);

                /* scrive a video i nuovi parametri appena calcolati */
                sprintf(stri,"posizione %d",(int)(x*0.25));
                sem_wait(&mutex);
                grx_text(stri,70,50+i*10,rgb16(255,255,255),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"v %nf",v);
                sem_wait(&mutex);
                grx_text(ostri,200,50+i*10,rgb16(255,255,255),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"a %nf",a);
                sem_wait(&mutex);
                grx_text(ostri,300,50+i*10,rgb16(255,255,255),rgb16(0,0,0));
                sem_post(&mutex);
     
                /* quando il veicolo arriva alla fine dell'autostrada viene eliminato */
                if (x>=40000)
                        task_abort(i);

                /* salva la nuova posizione del veicolo */
                sem_wait(&strada);
                for (k=0;k<LUNGVELOCE;k++)
                {
                        if ((y<CENTROCORSIA1) && (y>CENTROCORSIA2))
                        {
                                corsia2[x-k]=1;
                                corsia1[x-k]=1;
                        }
                        else
                                if (y==CENTROCORSIA1)
                                        corsia1[x-k]=1;
                                else
                                        if (y==CENTROCORSIA2)
                                                corsia2[x-k]=1;
                }
                sem_post(&strada);

                /* salvo i parametri che occorrono per il prossimo ciclo */
                oxelic=xelic;
                od=precedentedritto;
                ov=v;
                oa=a;

                task_endcycle();        /* termina le operazioni che il task deve eseguire */
        }
}


/* ----------------------------------------------------------------------------- */


/***************************** TASK AUTOCARRO ******************************/

TASK    auto_carro(void *arg)
{
        int x;                  /* posizione x assunta dal veicolo */
        int y;                  /* posizione y assunta dal veicolo */
        int oxelic;             /* posizione vecchia dell'elicottero */
        int ox;                 /* posizione vecchia x assunta dal camion */
        int oy;                 /* posizione vecchia y assunta dal camion */
        int k;                  /* indice di ciclo */
        int estremo;            /* distanza massima a cui ci possiamo spostare dall'elicottero (sia a destra che a sinistra) */
        int sorpasso;           /* indica se il veicolo e' in fase di sorpasso */
        int rientro;            /* indica se il veicolo e' in fase di rientro */
        int precedentedritto;   /* distanza anteriore da un veicolo */
        int od;                 /* vecchia distanza anteriore da un veicolo */
        int tot;                /* distanza massima raggiungibile del sensore posteriore sinistro */
        int tot3;               /* distanza massima raggiungibile del sensore anteriore destro */
        int tot2;               /* distanza massima raggiungibile del sensore posteriore destro */
        int i = (int)arg;
        int sensore_ant=200;    /* distanza massima raggiungibile del sensore anteriore */
        char stri[22];          /* vettore di caratteri */
        char ostri[22];         /* vettore di caratteri */
        float vcrociera;        /* velocita' desiderata */
        float vmax1=15.0;       /* velocita' massima raggiungibile in prima corsia */
        float vmax2=20.0;       /* velocita' massima raggiungibile in seconda corsia */
        float frenata=-5.0;     /* valore massimo della frenata */
        float amax=3.0;         /* valore massimo del'accellerazione */
        float v;                /* velocita' attuale */
        float a;                /* accellerazione attuale */
        float ov;               /* velocita' precedente */
        float oa;               /* accellerazione precedente */

        /* inizializzazione delle variabili */
        v=5.0;
        a=0.0;
        x=LUNGCAMION;
        y=CENTROCORSIA1;
        sorpasso=0;
        rientro=0;
        tot=100;
        tot2=150;
        tot3=100;
        oa=a;
        vcrociera=vmax1;
        ov=v;
        od=0;
        oxelic=xelic;
        estremo=(int)((MAX_X/2)-1);

        while (1)
        {
                /* cancella il veicolo se era presente sull'autostrada vista dall'elicottero */
                if (abs(oxelic-ox)<estremo)
                        draw_veicolo(ox-oxelic+estremo, oy, 0,LUNGCAMION,LARGCAMION);

                /* salva le vecchie coordinate */
                ox = x;
                oy = y;

                /* cancella il veicolo dalla vecchia posizione sulla corsia , andando a cancellarlo nel vettore della corsia in cui si trovava */
                sem_wait(&strada);
                for (k=0;k<LUNGCAMION;k++)
                {
                        if ((oy<CENTROCORSIA1) && (oy>CENTROCORSIA2))
                        {
                                corsia1[ox-k]=0;
                                corsia2[ox-k]=0;
                        }
                        else
                                if (oy==CENTROCORSIA1)
                                        corsia1[ox-k]=0;
                                else
                                        if (oy==CENTROCORSIA2)
                                                corsia2[ox-k]=0;
                }
                sem_post(&strada);

                /* cancella tutte le vecchie informazioni che compaiono sullo schermo */
                sprintf(stri,"posizione %d",(int)(ox*0.25));
                sem_wait(&mutex);
                grx_text(stri,70,50+i*10,rgb16(0,0,0),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"v %nf",ov);
                sem_wait(&mutex);
                grx_text(ostri,200,50+i*10,rgb16(0,0,0),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"a %nf",oa);
                sem_wait(&mutex);
                grx_text(ostri,300,50+i*10,rgb16(0,0,0),rgb16(0,0,0));
                sem_post(&mutex);

                /* legge la distanza dal sensore anteriore e nel caso in cui sia attaccato alla macchina che precede vengo eliminato */
                precedentedritto=dist_ant(x,y,sensore_ant);
                if (precedentedritto==1)
                        task_abort(i);

                /* disegna il veicolo nella nuova posizione */
                if (abs(xelic-x)<estremo)
                        draw_veicolo(x-xelic+estremo, y, rgb16(0,0,255),LUNGCAMION,LARGCAMION);

                /* varia la massima distanza vista dal sensore posteriore sinistro a seconda della velocita' */
                tot=(int)(v*4);
                if (tot<50)
                        tot=50;

                if (precedentedritto<100)       /* ci siamo avvicinando troppo alla macchina che ci precede */
                        if ((sorpasso==0) && (dist_postsx(x,y,200)>tot)&&(dist_ant_sx(x,y,50)>40))
                        {
                                /* c'e' uno davanti e non sopraggiunge nessuno sulla corsia di sorpasso */
                                sorpasso=1;
                                rientro=0;
                                vcrociera=vmax2;
                        }
                        else    /* c'e qualcuno davanti ,ma non possiamo sorpassare */
                                a=frenata;
                else            /* siamo lontani da un veicolo che ci precede o non ci precede nessuno */
                        if (v<vcrociera)        /* accelleriamo gradualmente fino a portarci alla velocita' di crociera desiderata */
                                a=amax-(amax/vcrociera)*v;
                        else
                                if(v>vcrociera)         /* freniamo per portarci al valore della velocita'di crociera */
                                        a=frenata/3;
                                else
                                        a=0.0;          /* siamo alla velocita' di crociera */

                /* controlla se e' possibile la manovra di rientro dal sorpasso, aggiustando tutti i parametri nel modo corretto */
                if ((dist_postdx(x,y,200)>tot2) && (dist_ant_dx(x,y,200)>tot3) && (sorpasso==1))
                {
                        rientro=1;
                        sorpasso=0;
                        vcrociera=vmax1;
                }

                /* aggiusta la cordinata y per far visualizzare il sorpasso o il rientro della macchina in modo graduale */
                if(y>CENTROCORSIA2 && sorpasso==1)
                        y=y-1;

                if(y<CENTROCORSIA1 && rientro==1)
                        y=y+1;

                /* calcola la velocita' e nel caso di v elocita' negative la pone a zero (non sono permesse le retromarcie) */
                v=v+a*DT;
                if (v<0)
                        v=0.0;

                /* calcola la cordinata x a cui si trova la macchina */
                x=x+(int)((v*DT)/0.25);

                /* scrive a video i nuovi parametri appena calcolati */
                sprintf(stri,"posizione %d",(int)(x*0.25));
                sem_wait(&mutex);
                grx_text(stri,70,50+i*10,rgb16(255,255,255),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"v %nf",v);
                sem_wait(&mutex);
                grx_text(ostri,200,50+i*10,rgb16(255,255,255),rgb16(0,0,0));
                sem_post(&mutex);

                sprintf(ostri,"a %nf",a);
                sem_wait(&mutex);
                grx_text(ostri,300,50+i*10,rgb16(255,255,255),rgb16(0,0,0));
                sem_post(&mutex);
     
                /* quando il veicolo arriva alla fine dell'autostrada viene eliminato */
                if (x>=40000)
                        task_abort(i);

                /* salva la nuova posizione del veicolo */
                sem_wait(&strada);
                for (k=0;k<LUNGCAMION;k++)
                {
                        if ((y<CENTROCORSIA1) && (y>CENTROCORSIA2))
                        {
                                corsia2[x-k]=1;
                                corsia1[x-k]=1;
                        }
                        else
                                if (y==CENTROCORSIA1)
                                        corsia1[x-k]=1;
                                else
                                        if (y==CENTROCORSIA2)
                                                corsia2[x-k]=1;
                }
                sem_post(&strada);

                /* salvo i parametri che occorrono per il prossimo ciclo */
                oxelic=xelic;
                od=precedentedritto;
                ov=v;
                oa=a;

                task_endcycle();        /* termina le operazioni che il task deve eseguire */
        }
}


/* ----------------------------------------------------------------------------- */


/***************************** TASK ELICOTTERO *****************************/

TASK eli_cottero(void *arg)
{
        int i = (int)arg;
        int oxelic;             /* vecchia posizione dell'elicottero */
        char stri[22];          /* vettore di caratteri */
        char ostri[22];         /* vettore di caratteri */

        /* disegna le scritte per l'indicatore a barra e finestra della posizione dell'elicottero */
        sprintf(stri,"0 Km");
        sem_wait(&mutex);
        grx_text(stri,100,560,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(stri,"10 Km");
        sem_wait(&mutex);
        grx_text(stri,700,560,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        while (1)
        {
                oxelic=xelic;   /* salva la posizione precedente dell'elicottero */

                if (c == '+')   /* sposta l'elicottero a destra (verso la fine dell'autostrada */
                {
                        xelic=xelic+50;
                        if (xelic>40000-(int)((MAX_X/2)-1))
                                xelic=40000-(int)((MAX_X/2)-1);
                }
                else
                        if (c =='-')    /* sposta l'elicottero a sinistra (verso l'inizio dell'autostrada */
                        {
                                xelic=xelic-30;
                                if (xelic<(int)((MAX_X/2)-1))
                                        xelic=(int)((MAX_X/2)-1);
                        }
           
                /* disegna le scritte innerenti all'elicottero e il valore della sua posizione (mediante scritta ed indicatore a barra con una finestra scorrevole) */
                sprintf(stri,"- <- xelic %d -> +",(int)(xelic*0.25));
                sprintf(ostri,"- <- xelic %d -> +",(int)(oxelic*0.25));

                sem_wait(&mutex);
                grx_text(ostri,MAX_X/2-50,500,rgb16(0,0,0),rgb16(0,0,0));
                grx_text(stri,MAX_X/2-50,500,rgb16(255,255,255),rgb16(0,0,0));
                grx_line(150,560,650,560,rgb16(255,255,255));
                grx_rect((int)((oxelic-(MAX_X/2))/80+150),540,(int)((oxelic+(MAX_X/2))/80+150) ,580 ,rgb16(0,0,0));
                grx_rect((int)((xelic-(MAX_X/2))/80+150),540,(int)((xelic+(MAX_X/2))/80+150) ,580 ,rgb16(255,255,255));
                sem_post(&mutex);

                c=' ';          /* setta il carattere c */

                task_endcycle();        /* termina le operazioni che il task deve eseguire */
        }
}


/* -----------------------------------------------------------------------------*/


/************* FUNZIONE DI USCITA DAL SISTEMA ******************************/

void byebye(void *arg)
{                       /* questa funzione e' chiamata quando il sistema esce */
        grx_close();                    /* chiude la grafica */
        kern_printf("Ciao Ciao ");      /* scrive il messaggio indicato sul terminale */
}


/* -----------------------------------------------------------------------------*/


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

int main(int argc, char **argv)
{
        int  n_task = 0;                /* numero di task creati */
        int u;
        int v;
        char introduzione[100];         /* vettore di caratteri */
        HARD_TASK_MODEL autolenta;      /* task auto lenta */
        HARD_TASK_MODEL autoveloce;     /* task auto veloce */
        HARD_TASK_MODEL autocarro;      /* task camion */
        HARD_TASK_MODEL elicottero;     /* task elicottero */


        /* inizializza le corsie dell'autostrada */
        sem_wait(&strada);
        for (u=0;u<=40000;u++)
        {
                corsia1[u]=0;
                corsia2[u]=0;
        }
        sem_post(&strada);

        /* Set the exception handler */
        //set_exchandler_grx();

        /* Set the closing function */
        sys_atrunlevel(byebye, NULL, RUNLEVEL_BEFORE_EXIT);

        /* inizializzazione grafica */
        if (grx_init() < 1)
                sys_abort(1);

        /* scelta automatica della risoluzione applicabile con quella scheda video (scegliendola tra 800*600 e 1024*768 ) */
        if(grx_getmode(1024,768,16)==-1)
        {
                if (grx_open(800, 600, 16) < 0)
                {
                        kern_printf("GRX Err\n");
                        sys_abort(1);
                }
                MAX_X=800;
                MAX_Y=600;
        }
        else
        {
                if (grx_open(1024,768, 16) < 0)
                {
                        kern_printf("GRX Err\n");
                        sys_abort(1);
                }
                MAX_X=1024;
                MAX_Y=768;
        }

        kern_printf("La scheda video va'!!\n");

        /* posizione iniziale elicottero */
        xelic=(int)((MAX_X/2)-1);

        /* disegna lo scenario della strada ed il menu */
        sprintf(introduzione,"Monitoraggio dei mezzi che transitano su una autostrada.");
        sem_wait(&mutex);
        grx_text(introduzione,50,10,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(introduzione,"Sviluppato da: Verzellesi Quadrubbi");
        sem_wait(&mutex);
        grx_text(introduzione,50,20,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(introduzione,"MENU");
        sem_wait(&mutex);
        grx_text(introduzione,480,70,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(introduzione,"s = macchina sportiva");
        sem_wait(&mutex);
        grx_text(introduzione,480,80,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(introduzione,"c = mezzo pesante");
        sem_wait(&mutex);
        grx_text(introduzione,480,90,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(introduzione,"l = macchina lenta");
        sem_wait(&mutex);
        grx_text(introduzione,480,100,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(introduzione,"+ = sposta l'elicottero verso destra");
        sem_wait(&mutex);
        grx_text(introduzione,480,110,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(introduzione,"- = sposta l'elicottero verso sinistra");
        sem_wait(&mutex);
        grx_text(introduzione,480,120,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(introduzione,"esc = uscita");
        sem_wait(&mutex);
        grx_text(introduzione,480,130,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(introduzione,"NOTA");
        sem_wait(&mutex);
        grx_text(introduzione,480,140,rgb16(255,0,0),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(introduzione,"Se i veicoli tamponano ");
        sem_wait(&mutex);
        grx_text(introduzione,480,150,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        sprintf(introduzione,"   vengono eliminati");
        sem_wait(&mutex);
        grx_text(introduzione,480,160,rgb16(255,255,255),rgb16(0,0,0));
        sem_post(&mutex);

        grx_line(1,450,MAX_X,450,rgb16(255,255,255));
        grx_line(1,486,MAX_X,486,rgb16(255,255,255));

        for (u=0;u<MAX_X;u++)
        {
                v=u%8;
                if ((v==0) || (v==1))
                {
                        sem_wait(&mutex);
                        grx_plot(u,CENTROCARREGGIATA,rgb16(255,255,255));
                        sem_post(&mutex);
                }
        }

        n_task=0;       /* inizializzazione del numero dei task */

        /* definisce e crea il task elicottero */
        kern_printf("elicottero");
        hard_task_default_model (elicottero);
        hard_task_def_ctrl_jet (elicottero);
        hard_task_def_arg (elicottero, (void *)n_task);
        hard_task_def_wcet (elicottero,wcet);
        hard_task_def_mit (elicottero, periodo);
        hard_task_def_group (elicottero, GRUPPO);
        hard_task_def_usemath (elicottero);
        pid = task_create ("elicottero",eli_cottero, &elicottero, NULL);
        if (pid == NIL)
        {
                grx_close();
                perror("Non si puo' creare il task");
                sys_abort(1);
        }
        task_activate(pid);

        n_task=1;       /* incremente il numero dei task (ha creato l'elicottero */
   
        /*Attesa di un carattere per creare un veicolo */
        c = keyb_getch(BLOCK);
        do {
                if (((c == 'c')||(c=='s')||(c=='l')) && (n_task < MAX_V))       /* in base al tasto premuto crea il task opportuno */
                {
                        if (c == 'l')           /* definisce e crea il task autolenta */
                        {    
                                kern_printf("lenta");
                                hard_task_default_model (autolenta);
                                hard_task_def_ctrl_jet (autolenta);
                                hard_task_def_arg (autolenta, (void *)n_task);
                                hard_task_def_wcet (autolenta, wcet);
                                hard_task_def_mit (autolenta, periodo);
                                hard_task_def_group (autolenta, GRUPPO);
                                hard_task_def_usemath (autolenta);
                                pid = task_create ("autolenta",auto_lenta, &autolenta, NULL);
                        }
                        else
                                if (c == 's')           /* definisce e crea il task autoveloce */
                                {
                                        hard_task_default_model (autoveloce);
                                        hard_task_def_ctrl_jet (autoveloce);
                                        hard_task_def_arg (autoveloce, (void *)n_task);
                                        hard_task_def_wcet (autoveloce, wcet);
                                        hard_task_def_mit (autoveloce, periodo);
                                        hard_task_def_group (autoveloce, GRUPPO);
                                        hard_task_def_usemath (autoveloce);
                                        pid = task_create ("autoveloce",auto_veloce, &autoveloce, NULL);
                                }
                                else
                                        if (c == 'c')           /* definisce e crea il task autocarro */
                                        {
                                                hard_task_default_model (autocarro);
                                                hard_task_def_ctrl_jet (autocarro);
                                                hard_task_def_arg (autocarro, (void *)n_task);
                                                hard_task_def_wcet (autocarro, wcet);
                                                hard_task_def_mit (autocarro, periodo);
                                                hard_task_def_group (autocarro, GRUPPO);
                                                hard_task_def_usemath (autocarro);
                                                pid = task_create ("camion",auto_carro, &autocarro, NULL);
                                        }

                        if (pid == NIL)         /* nel caso in non si possano creare dei task chiude la grafica e con un messaggio segnala l'errore */
                        {
                                grx_close();
                                perror("Non si puo' creare il task");
                                sys_abort(1);
                        }

                        task_activate(pid);             /* attiva i task */
                        n_task++;                       /* incrementa il numero dei task creati */
                }

                c = keyb_getch(BLOCK);

        } while (c != ESC);     /* termino il tutto solo quando e' stato premuto il tasto esc */

        sys_end();      /* esco dal sistema */

        return 0;
}