WebSVN - shark - Rev 1668 - /unsupported/trunk/arezzo/2004/lupo/lupo.c

/*
* Project: S.Ha.R.K.
*
* Coordinators:
* Giorgio Buttazzo <giorgio@sssup.it>
* Paolo Gai <pj@gandalf.sssup.it>
*
* Authors :
* Paolo Gai <pj@gandalf.sssup.it>
* (see the web pages for full authors list)
*
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
*
* http://www.sssup.it
* http://retis.sssup.it
* http://shark.sssup.it
*/

/**
------------
CVS : $Id: lupo.c,v 1.1 2004-08-05 11:34:36 pj Exp $

File: $File$
Revision: $Revision: 1.1 $
Last update: $Date: 2004-08-05 11:34:36 $
------------
**/

/*
* Copyright (C) 2000 Paolo Gai and Giorgio Buttazzo
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/

/*--------------------------------------------------------------*/
/* SIMULATION OF RANDOM FLIES */
/*--------------------------------------------------------------*/

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

#define YMENU 10 /* menu level */
#define XMIN 50
#define XMAX 600
#define YMIN 100
#define YMAX 450
#define VEL 5 /* Velocità della pecora (def. = 5)*/
#define VELWOLF 4 /* Velocità del lupo(def. = 4) */
#define ANG 30    /* angolo massimo sterzata (30) */
#define D 3 /* raggio pecore e lupi */
#define ESC 27    /* ASCII code of ESCAPE key */
#define MAXPECORE 4 /* Massimo numero di pecore */
#define MAXLUPI 3 /* Massimo numero di lupi */
#define WOLFGROUP 1
#define SHEEPGROUP 1
#define HUNTGROUP 1

/* Variabili globali utilizzate nel programma*/
double tick = 1.0; /* system tick = 1 ms */
int hunt_period = 40000;    /* Periodo del task cacciatore */
int sheep_period = 40000;    /* Periodo del task lupo */
int wolf_period = 40000; /* Periodo del task pecora */
int hunt_wcet = 100; /* wcet del task cacciatore */
int sheep_wcet = 100; /* wcet del task pecora */
int wolf_wcet = 100; /* wcet del task lupo */
int num_p = 0; /* numero iniziale di pecore */
int num_l = 0; /* numero iniziale di lupi */
int num_c = 0; /* numero iniziale di cacciatori */
PID pidp,pidl,pidc; /* etichetta creata dal S.O. attribuita ad ogni task*/
sem_t mutex; /* semaforo di mutua */

/*--------------------------------------------------------------*/
/*Definizione della struttura posizione*/
struct pos {
int x , y;    /*Posizione nel piano di ogni singolo task*/
sem_t priv; /*Semaforo relativo alla posizione di ogni singolo task*/
int death; /*Flag di vita o di morte di ogni singolo task*/
};

/*--------------------------------------------------------------*/
/*Generazione di vettori di struttura posizione per ogni task*/
struct pos pecora[MAXPECORE];
struct pos lupo[MAXLUPI];
struct pos caccia[1];

/*--------------------------------------------------------------*/
/*Funzione per disegnare a video lupi e pecore*/

void draw_obj(int x, int y, int c)
{
sem_wait(&mutex); /*Semaforo di mutua per proteggere la scrittura a video*/
grx_disc(x, y, D, c);
sem_post(&mutex);
}

/*--------------------------------------------------------------*/
/*Funzione di inizializzazione per tutti le variabili di ogni task*/
void init_sem()
{
int i;
sem_init(&mutex,0,1);

/*Inizializzazione del vettore di struttura posizione pecora*/

for (i=0;i<MAXPECORE;i++){
sem_init(&pecora[i].priv,0,1);
pecora[i].x = 0;
pecora[i].y = 0;
pecora[i].death = 0;
}

/*Inizializzazione del vettore di struttura posizione lupi*/

for (i=0;i<MAXLUPI;i++){
sem_init(&lupo[i].priv,0,1);
lupo[i].x = 0;
lupo[i].y = 0;
lupo[i].death = 0;
}

/*Inizializzazione del vettore di struttura posizione cacciatore*/

sem_init(&caccia[0].priv,0,1);
caccia[0].x = 0;
caccia[0].y = 0;
caccia[0].death = 0;
}

/******************************************************************/
/*Task pecora*/
TASK sheep(void *arg)
{
int x, y; /*Posizione futura*/
int ox, oy; /*Posizione attuale*/
int dx, dy, da; /*Spostamento lungo x e y e angolare*/
int teta, col; /*Angolo di direzione dello spostamento e colore pecora*/
int outx, outy,i_libera,i; /*Limiti perimetrali*/
double r; /*Angolo di direzione dello spostamento in radianti*/

i_libera = 0; /*Inizializzazione della prima posizione libera nel vettore di    struttura pecora*/

for(i=0;i<MAXPECORE;i++){ /*Ciclo per cercare la prima posizione libera*/
sem_wait(&pecora[i].priv); /*Proteggiamo la lettura di ogni singola posizione del vettore */
if (!pecora[i].x) i_libera = i; /*Le posizioni libere si contraddistinguono per x=0 e y=0*/
sem_post(&pecora[i].priv);
}

num_p++; /*Incremento del numero di pecore*/

i = i_libera; /*La posizione nel vettore attribuita al task è la prima libera trovata*/

sem_wait(&pecora[i].priv); /*Proteggiamo l'attribuzione della posizione di origine della pecora*/
pecora[i].x = ox = x = XMAX - 20;
pecora[i].y = oy = y = YMAX - 20;
//pecora[i].pid = pidp;
sem_post(&pecora[i].priv);

teta = 0; /*Inizializzazione dell' angolo di direzione dello spostamento*/
col = 29; /* colore della pecora*/

while (1) { /*Ciclo infinito del task*/

/*Calcolo casuale della posizione della pecora*/
da = rand()%(2*ANG) - ANG;    /* da = [-ANG,ANG] */
teta += da;

if (teta > 360) teta -= 360;
if (teta < 0) teta += 360;
r = (double)teta * PI / 180.;

dx = (float)(VEL * cos(r));
dy = (float)(VEL * sin(r));
x += dx;
y += dy;

outx = (x >= XMAX) || (x <= XMIN); /*Controllo posizione all'interno del perimetro*/
outy = (y >= YMAX) || (y <= YMIN);

if (outx || outy) { /*In caso di uscita dal perimetro voluto ricalcolo dx e dy
in modo che la pecora non esca da esso*/
x = x - dx;
y = y - dy;
if (outx) teta = 180 - teta;
if (outy) teta = -teta;
if (teta > 360) teta -= 360;
if (teta < 0) teta += 360;
r = (double)teta * PI / 180.;

dx = (float)(VEL * cos(r));
dy = (float)(VEL * sin(r));

x += dx;
y += dy;
}

sem_wait(&pecora[i].priv);
if (pecora[i].death == 1) { /*Controllo sul flag di vita o di morte del task pecora.*/
pecora[i].x = 0; /* Se il flag è uguale a 1 la pecora "deve morire". */
pecora[i].y = 0;    /*Questo lo indichiamo mettendo le sue posizioni in 0*/
pecora[i].death = 0; /*Rimettiamo il flag di vita o morte a 0*/
num_p--; /*Decremento del numero di pecore*/
draw_obj(ox, oy, 0); /*Chiamata della funzione per disegnare a video una pecora    nera=morta*/
sem_post(&pecora[i].priv);
return 0;
}
pecora[i].x = x; /*Attribuzione delle nuove posizioni*/
pecora[i].y = y;
sem_post(&pecora[i].priv);

draw_obj(ox, oy, 0); /*Disegno della nuova posizione cancellando precedentemante quella    vecchia*/
draw_obj(x, y, col);
ox = x; oy = y; /*Nuove posizioni attuali*/

task_endcycle();
}
}

/****************************************************************/
/*Task lupo*/
TASK wolf(void *arg)
{
int x, y; /*Posizione futura*/
int ox, oy; /*Posizione attuale*/
int dx, dy; /*Spostamento lungo x e y*/
int deltax,deltay; /*Distanza lupo pecora lungo l'asse x e lungo l'asse y*/
int ipo,ipo_min; /*Ipotenusa e ipotenusa minima di distanza tra lupo e pecora*/
int i_min,i,j_libera,j; /*indici della pecora più vicina, della prima posizione libera sul    vettore*/
int teta, col; /*Angolo di direzione dello spostamento e colore lupo*/
int outx, outy; /*Limiti perimetrali*/
double r=0; /*Angolo di direzione dello spostamento in radianti*/

j_libera = 0; /*Inizializzazione della prima posizione libera nel vettore di    struttura lupo*/

for(j=0;j<MAXLUPI;j++){ /*Ciclo per cercare la prima posizione libera*/
sem_wait(&lupo[j].priv); /*Proteggiamo la lettura di ogni singola posizione del vettore */
if (!lupo[j].x) j_libera = j; /*Le posizioni libere si contraddistinguono per x=0 e y=0*/
sem_post(&lupo[j].priv);
}

num_l++; /*Incremento del numero di lupi*/

j = j_libera; /*La posizione nel vettore attribuita al task è la prima libera trovata*/

sem_wait(&lupo[j].priv); /*Proteggiamo l'attribuzione della posizione di origine del lupo */
lupo[j].x = ox = x = XMIN + 20;
lupo[j].y = oy = y = YMIN + 20;
sem_post(&lupo[j].priv);

teta = 0; /*Inizializzazione dell' angolo di direzione dello spostamento*/
col = 4; /* colore lupo*/

while (1) { /*Ciclo infinito del task*/

/*controllo posizione cacciatore*/

sem_wait(&caccia[0].priv);
deltax = caccia[0].x-ox; /*Calcolo della distanza tra lupo e cacciatore*/
deltay = caccia[0].y-oy;
sem_post(&caccia[0].priv);

if (abs(deltax)<30 && abs(deltay)<30){ /*Se la distanza è minore di 30 pixel il lupo deve scappare dal cacciatore*/
if (deltax==0){ /*calcolo degli angoli di spostamento per fuggire dal cacciatore*/
if (deltay>0) teta=-45;
if (deltay<0) teta=45;
}
else{
teta=(180/PI)*atan(deltay/deltax);
if (deltax<0) teta=45+teta;
if (deltax>0) teta=-(teta+90);
}

}else
{

/*controllo pecora piu vicina*/
ipo_min=6000; /*ipotenusa in modo da averne sempre una minore*/
i_min=0; /*Inizializzazione dell' indice della pecora più vicina*/
for(i=0;i<MAXPECORE;i++){ /*Per ogni pecora controllo la distanza lupo-pecora*/
sem_wait(&pecora[i].priv);
if (pecora[i].x){ /*la posizione della pecora è 0 quando è morta*/
deltax=pecora[i].x-ox; /*calcolo distanza lupo-pecora lungo x*/
deltay=pecora[i].y-oy; /*calcolo distanza lupo-pecora lungo y*/
sem_post(&pecora[i].priv);
ipo=sqrt(deltax*deltax+deltay*deltay); /*Calcolo dell'ipotenusa=distanza tra lupo e pecora*/
if (ipo<ipo_min) {ipo_min=ipo;i_min=i;} /*Calcolo l'ipotenusa più piccola*/
}else
sem_post(&pecora[i].priv);
}
/*calcolo la distanza della pecora piu vicina*/
sem_wait(&pecora[i_min].priv);
deltax=pecora[i_min].x-ox;
deltay=pecora[i_min].y-oy;
if (deltax==0){ /*Calcolo angoli per far inseguire al lupo la pecora più vicina*/
if (deltay>0) teta=90;
if (deltay<0) teta=-90;
}
else{
teta=(180/PI)*atan(deltay/deltax);
if (deltax<0) teta=180+teta;
}

//controllo se posso uccidere la pecora
if ((abs(deltax)<2)&&(abs(deltay)<2)) pecora[i_min].death = 1; /*Se il lupo è sopra la pecora,questa deve    morire*/

sem_post(&pecora[i_min].priv);
}
r = (double)teta * PI / 180.; /*Calcolo l'angolo in radianti*/
dx = (float)((VELWOLF+j) * cos(r)); /*Una volta calcolato l'angolo di spostamento,*/
dy = (float)((VELWOLF+j) * sin(r)); /*calcolo i relativi spostamenti lungo x e y*/

x += dx; /*calcolo le nuove posizioni*/
y += dy;

outx = (x >= XMAX) || (x <= XMIN); /*Controllo posizione all'interno del perimetro*/
outy = (y >= YMAX) || (y <= YMIN);

if (outx || outy) { /*In caso di uscita dal perimetro voluto ricalcolo dx e dy
in modo che il lupo non esca da esso*/
x = x - dx;
y = y - dy;
if (outx) teta = 180 - teta;
if (outy) teta = -teta;
if (teta > 360) teta -= 360;
if (teta < 0) teta += 360;
r = (double)teta * PI / 180.;

dx = (float)(VEL * cos(r));
dy = (float)(VEL * sin(r));

x += dx;
y += dy;
}

sem_wait(&lupo[j].priv);

if (lupo[j].death == 1) { /*Controllo sul flag di vita o di morte del task lupo*/
lupo[j].x = 0; /* Se il flag è uguale a 1 il lupo "deve morire". */
lupo[j].y = 0; /*Questo lo indichiamo mettendo le sue posizioni in 0*/
lupo[j].death = 0; /*Rimettiamo il flag di vita o morte a 0*/
num_l--; /*Decremento del numero di lupi*/
draw_obj(ox, oy, 0); /*Chiamata della funzione per disegnare a video un lupo nero=morto*/
sem_post(&lupo[j].priv);
return 0;
}
lupo[j].x = x ; /*Attribuzione delle nuove posizioni*/
lupo[j].y = y ;
sem_post(&lupo[j].priv);

draw_obj(ox, oy, 0); /*Disegno della nuova posizione cancellando precedentemante quella    vecchia*/
draw_obj(x, y, col);
ox = x; oy = y; /*Nuove posizioni attuali*/

task_endcycle();
}
}

/****************************************************************/
void my_mouse_handler(MOUSE_EVT *m)
{
grx_box(caccia[0].x-3,caccia[0].y-3,caccia[0].x+3,caccia[0].y+3,0); /*Cancella la posizione precedente del    cacciatore disegnando un rettangolo nero*/
grx_box(m->x-3,m->y-3,m->x+3,m->y+3,2); /*disegno del cursore a forma di    rettangolino attorno alla posizione attuale*/

sem_wait(&caccia[0].priv);
caccia[0].x = m->x; /*Attribuisco al cacciatore le nuove coordinate del mouse*/
caccia[0].y = m->y;
sem_post(&caccia[0].priv);
}

/****************************************************************/
TASK cacciatore(void *arg)
{
int i = 0;
int deltax,deltay; /*Distanza cacciatore-lupo lungo l'asse x e lungo l'asse y*/

num_c++; /*incremento del numero di cacciatori*/

mouse_on(); /*attivazione e visualizzazione del mouse*/
mouse_hook(my_mouse_handler); /*chiamata all'hook*/

while(1){ /*controllo infinito per vedere se il lupo è da ammazzare*/

if (i == MAXLUPI) i = 0; /*Se ha controllato tutti i lupi ricomincia dal primo nel vettore*/

sem_wait(&lupo[i].priv);

if (lupo[i].x){ /*la posizione del lupo è 0 solo quando è morto*/
deltax=lupo[i].x-caccia[0].x; /*calcolo la distanza tra lupo e cacciatore*/
deltay=lupo[i].y-caccia[0].y;
if (abs(deltax)<4 && abs(deltay)<4) lupo[i].death=1; /*Se il cacciatore è vicino al lupo questo    deve morire*/
}
sem_post(&lupo[i].priv);

i++; /*Incrementa i per controllare il lupo successivo*/

task_endcycle();
}
return 0;
}

/****************************************************************/
/* This function is called when the system exits */
void byebye(void *arg)
{
mouse_end();
grx_close();
cprintf("Bye Bye!\n");
}

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

int main(int argc, char **argv)
{
HARD_TASK_MODEL m;

char c; /* caratere dalla tastiera */
int i = 0; /* numero di tasks creati */
TIME seme; /* used to init the random seed */

// Inizializzazione del mouse
MOUSE_PARMS mouse = BASE_MOUSE;

mouse_def_ps2(mouse);
mouse_init(&mouse);
mouse_limit(XMIN,YMIN,XMAX,YMAX); /* limiti del mouse all'interno del rettangolo di gioco*/
mouse_position(XMIN+20,YMAX-20); /* posizione di partenza cacciatore*/
mouse_threshold(10); /* sensibilità del mouse */

// Inizializzazione dei vettori di strutture
init_sem();

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

/* graphic card Initialization */
if (grx_init() < 1) {
sys_abort(1);
}

if (grx_open(640, 480, 8) < 0) {
cprintf("GRX Err\n");
sys_abort(1);
}

/* The scenario */
grx_rect(XMIN-D-1, YMIN-D-1, XMAX+D+1, YMAX+D+1, 14);
grx_text("Simulazione di caccia al Lupo", XMIN, YMENU+10, 13, 0);
grx_text("p crea una pecora" , XMIN, YMENU+20, 12, 0);
grx_text("l crea un lupo" , XMIN, YMENU+30, 12, 0);
grx_text("c crea un cacciatore " , XMIN, YMENU+40, 12, 0);
grx_text("ESC exit to DOS" , XMIN, YMENU+50, 12, 0);

/* The program waits a space to create a fly */
c = keyb_getch(BLOCK);

/* randomize!!!! */
seme = sys_gettime(NULL);
srand(seme);

do {
if ((c == 'p')&&(num_p<MAXPECORE)) { /*se il numero di pecore max è raggiunto non crearne altre*/
hard_task_default_model(m);
hard_task_def_ctrl_jet (m);
hard_task_def_arg (m, (void *)i);
hard_task_def_wcet (m, sheep_wcet);
hard_task_def_mit (m, sheep_period);
hard_task_def_group (m, SHEEPGROUP);
hard_task_def_usemath (m);
pidp = task_create("sheep", sheep, &m, NULL);
if (pidp == NIL) {
grx_close();
perror("Could not create task <sheep>");
sys_abort(1);
}
task_activate(pidp);

}
if ((c == 'l')&&(num_l<MAXLUPI)) { /*se il numero di lupi max è raggiunto non crearne altri*/
hard_task_default_model(m);
hard_task_def_ctrl_jet (m);
hard_task_def_arg (m, (void *)i);
hard_task_def_wcet (m, wolf_wcet);
hard_task_def_mit (m, wolf_period);
hard_task_def_group (m, WOLFGROUP);
hard_task_def_usemath (m);
pidl = task_create("wolf", wolf, &m, NULL);
if (pidl == NIL) {
grx_close();
perror("Could not create task <wolf>");
sys_abort(1);
}
task_activate(pidl);

}
if ((c == 'c') && (num_c == 0)) { /*se il caccaiatore è già stato creato non crearne altri*/
hard_task_default_model(m);
hard_task_def_ctrl_jet (m);
hard_task_def_arg (m, (void *)i);
hard_task_def_wcet (m, hunt_wcet);
hard_task_def_mit (m, hunt_period);
hard_task_def_group (m, HUNTGROUP);
hard_task_def_usemath (m);
pidc = task_create("cacciatore", cacciatore, &m, NULL);
if (pidc == NIL) {
grx_close();
perror("Could not create task <cacciatore>");
sys_abort(1);
}
task_activate(pidc);

}

c = keyb_getch(BLOCK);

} while (c != ESC);

sys_end();

return 0;
}

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

Subversion Repositories shark

(root)/unsupported/trunk/arezzo/2004/lupo/lupo.c @ 1669 - Rev 1668