WebSVN - shark - Rev 1171 - /demos/trunk/autostr/autostr.c

/***************************************************************************
**************************************************************************
*** 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;
}

Subversion Repositories shark

(root)/demos/trunk/autostr/autostr.c @ 1289 - Rev 1171