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: fly2.c,v 1.1 2004-03-30 06:46:08 pj Exp $

 File:        $File$

 Revision:    $Revision: 1.1 $

 Last update: $Date: 2004-03-30 06:46:08 $

 ------------

**/

/*

 * 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 (with death of flies)         */

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

#include <kernel/kern.h>

#include <drivers/glib.h>

#include <drivers/keyb.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              /* linear velocity (def. = 5)   */

#define ANG      30             /* angolo massimo sterzata (30) */

#define D        3              /* raggio mosca                 */

#define ESC      27             /* ASCII code of ESCAPE key     */

#define MAX_P    35             /* max number of flies          */

#define FLYGROUP 1

double  tick = 1.0;             /* system tick = 1 ms           */

int     fly_period = 40000;     /* task period                  */

int     fly_wcet = 1000;        /* task wcet                    */

PID     pid;

sem_t   mutex;

sem_t   mutex_i;        /* mutex to protect i */

int  i = 0;             /* number of tasks created      */

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

void    draw_fly(int x, int y, int c)

{

        sem_wait(&mutex);

        grx_disc(x, y, D, c);

        sem_post(&mutex);

}

void    draw_i(void)

{

        char s[50];

        sem_wait(&mutex_i);

        sprintf(s,"i=%d  ",i);

        sem_post(&mutex_i);

        sem_wait(&mutex);

        grx_text(s, XMIN+200, YMENU+30, 12, 0);

        sem_post(&mutex);

}

int get_i(void)

{

    int j;

    sem_wait(&mutex_i);

    j = i;

    sem_post(&mutex_i);

    return j;

}

void i_minus_1(void)

{

    sem_wait(&mutex_i);

    i--;

    sem_post(&mutex_i);

}

void i_plus_1(void)

{

    sem_wait(&mutex_i);

    i++;

    sem_post(&mutex_i);

}

/******************************************************************/

TASK    fly(void *arg)

{

int     x, y;

int     ox, oy;

int     dx, dy, da;

int     teta, col;

int     outx, outy;

double  r;

int     local_i = (int)arg;

int     number_of_iterations;

        x = ox = (XMIN+XMAX)/2;

        y = oy = (YMIN+YMAX)/2;

        teta = 0;

        col = 2 + local_i;                    /* colore fly           */

        number_of_iterations = 200;

        while (number_of_iterations) {

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

                outy = (y >= YMAX) || (y <= YMIN);

                if (outx || outy) {

                        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;

                }

                draw_fly(ox, oy, 0);

                draw_fly(x, y, col);

                ox = x; oy = y;

                number_of_iterations--;

                task_endcycle();

        }

        draw_fly(x, y, 0);

        i_minus_1();

        draw_i();

        return 0;

}

/****************************************************************/

/* This function is called when the system exits */

void byebye(void *arg)

{

  grx_close();

  cprintf("Bye Bye!\n");

}

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

int main(int argc, char **argv)

{

    HARD_TASK_MODEL m;

    char c;             /* character from keyboard      */

    TIME seme;          /* used to init the random seed */

    int local_i;        /* local copy of i */

    /* Set the closing function */

    sys_atrunlevel(byebye, NULL, RUNLEVEL_BEFORE_EXIT);

    sem_init(&mutex,0,1);

    sem_init(&mutex_i,0,1);

    /* 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("Simulation of Random Flies", XMIN, YMENU+10, 13, 0);

    grx_text("SPACE create a fly"        , XMIN, YMENU+20, 12, 0);

    grx_text("ESC   exit to DOS"         , XMIN, YMENU+30, 12, 0);

    draw_i();

    /* The program waits a space to create a fly */

    c = keyb_getch(BLOCK);

    /* randomize!!!! */

    seme = sys_gettime(NULL);

    srand(seme);

    do {

        local_i = get_i();

        if ((c == ' ') && (local_i < MAX_P)) {

            hard_task_default_model(m);

            hard_task_def_ctrl_jet (m);

            hard_task_def_arg      (m, (void *)local_i);

            hard_task_def_wcet     (m, fly_wcet);

            hard_task_def_mit      (m, fly_period);

            hard_task_def_group    (m, FLYGROUP);

            hard_task_def_usemath  (m);

            pid = task_create("fly", fly, &m, NULL);

            if (pid == NIL) {

              grx_close();

              perror("Could not create task <fly>");

              sys_abort(1);

            }

            task_activate(pid);

            i_plus_1();

            draw_i();

        }

        c = keyb_getch(BLOCK);

    } while (c != ESC);

    sys_end();

    return 0;

}

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