Subversion Repositories shark

/*

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

}

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