Subversion Repositories shark

/*

 * Project: S.Ha.R.K.

 *

 * Coordinators:

 *   Giorgio Buttazzo    <giorgio@sssup.it>

 *   Paolo Gai           <pj@gandalf.sssup.it>

 *

 * Authors     :

 *              Bera Marco              mbera@libero.it

 *              Varasio Gabriele        varasio@odino.unipv.it

 *

 * Universita' degli studi di Pavia

 *

 * http://www.sssup.it

 * http://retis.sssup.it

 * http://shark.sssup.it

 */

/**

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

 CVS :        $Id: tel.c,v 1.1 2003-06-04 09:41:01 giacomo Exp $

 File:        $File$

 Revision:    $Revision: 1.1 $

 Last update: $Date: 2003-06-04 09:41:01 $

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

**/

/*

 * 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

 *

 */

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

/*      PERIODIC PROCESS TEST                                   */

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

#include <ll/i386/x-dos.h>

#include <kernel/kern.h>

#include <modules/cabs.h>

#include <drivers/glib.h>

#include <drivers/keyb.h>

#include <math.h>

#include <semaphore.h>

#include <stdlib.h>

#include "moon.h"

#define N_MAX_TELESCOPI 5

#define X0      10

#define pi      3.14

#define ex       2.71

#define NCAB    N_MAX_TELESCOPI+1

#define dim_msg 8

#define dim_x 49

#define dim_y 49

#define radius 9

#define Y_TELESCOPI 300

/* task periods */

#define PERIOD_T1  80000

#define PERIOD_T4 100000

#define PERIOD_T5 100000

#define WCET_T 5000

int period_t1=0;

#define SOGLIA 8

char    *cname[NCAB] = {"cab1", "cab2", "cab3", "cab4","cab5","cab6"};

// posizioni dei telescopi e rispettivi poli

int     x[5] = {100, 200, 300, 400,500};

//double  poli[5]={-50,-30,-1, -20, -5};

double  poli[5];

// guadagno proporzionale

double kp=0.0;

// flag per rilevare la pressione del tasto spazio

int numero_telescopi=0;

int flag=0;

KEY_EVT         move_tasto;

// Immagini ausiliarie per il disegno della luna

BYTE lunabkg[900];

BYTE lunadest[900];

/* A semaphore used to access Video Cards in mutual exclusion */

sem_t   mutex;

sem_t   mutex_tasto;

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

// cab necessari allo scambio delle coordinate dell'oggetto e delle

// immagini dei rispettivi telescopi

CAB   cid[NCAB];

// vettori per memorizzare le immagini

BYTE videobuf[dim_x*dim_y];

BYTE videobuf1[dim_x*dim_y];

BYTE videobuf2[dim_x*dim_y];

BYTE videobuf3[dim_x*dim_y];

BYTE videobuf4[dim_x*dim_y];

BYTE videobuf5[dim_x*dim_y];

 // Variabili per il disegno delle stelle sullo sfondo

 int num_p;

 int y_p[7];

 int x_p[7];

 int dim_p[7];

 int col_p[7];

TASK    media(void *arg)

{

char *p;

int i=0;

int j=0;

while(1)

{

        p = cab_getmes(cid[1]);

        memcpy(videobuf,p,dim_x*dim_y*sizeof(BYTE));

        cab_unget(cid[1], p);

        p = cab_getmes(cid[2]);

        memcpy(videobuf1,p,dim_x*dim_y*sizeof(BYTE));

        cab_unget(cid[2], p);

        p = cab_getmes(cid[3]);

        memcpy(videobuf2,p,dim_x*dim_y*sizeof(BYTE));

        cab_unget(cid[3], p);

        p = cab_getmes(cid[4]);

        memcpy(videobuf4,p,dim_x*dim_y*sizeof(BYTE));

        cab_unget(cid[4], p);

        p = cab_getmes(cid[5]);

        memcpy(videobuf5,p,dim_x*dim_y*sizeof(BYTE));

        cab_unget(cid[5], p);

        // calcolo media

        for (i=0;i<dim_x;i++)

        {

                for(j=0;j<dim_y;j++)

                {

                videobuf3[i*dim_x+j]=(double)(videobuf[i*dim_x+j]+videobuf1[i*dim_x+j]+videobuf2[i*dim_x+j]+videobuf4[i*dim_x+j]+videobuf5[i*dim_x+j])/numero_telescopi;

                }

        }

        // visualizza immagine mediata

        sem_wait(&mutex);

        grx_putimage(570,10,570+dim_x-1,10+dim_y-1,videobuf3);

        sem_post(&mutex);      

        task_endcycle();

     }

}

TASK    move(void *arg)

{

//int     i = (int)arg;

char *p;

int x_object=300;  // posizione dell'oggetto da rilevare

int y_object=200;

// variabile contatore

int z=0;

char coord_x[dim_msg];

p = cab_reserve(cid[0]);

sprintf(coord_x,"%d %d",x_object,y_object);

memcpy(p,coord_x,dim_msg*sizeof(char));

cab_putmes(cid[0], p);

      //Normalizzazione

      for(z=0;z<900;z++)

        {

        if (luna[z]>0)

                {

                luna[z]=((luna[z])*16.0/255.0)+16;

                }

        }

        task_endcycle();

        while (1)

        {

        // Cancellazione Luna

        sem_wait(&mutex);      

        grx_box(x_object-15,y_object-15,x_object+14,y_object+14,0);

        sem_post(&mutex);

        if (keyb_getcode(&move_tasto,NON_BLOCK))

                {

                       if (move_tasto.ascii=='4')    

                       x_object=x_object-5;      

                       if (move_tasto.ascii=='6')            

                       x_object=x_object+5;

                       if (move_tasto.ascii=='2')

                       y_object=y_object+5;

                       if (move_tasto.ascii=='8')

                       y_object=y_object-5;

                       if (move_tasto.ascii=='9'){    

                       y_object=y_object-5;                

                       x_object=x_object+5; }      

                       if (move_tasto.ascii=='1'){    

                       y_object=y_object+5;                

                       x_object=x_object-5; }

                       if (move_tasto.ascii=='7'){    

                       y_object=y_object-5;                

                       x_object=x_object-5; }

                       if (move_tasto.ascii=='3'){    

                       y_object=y_object+5;

                    x_object=x_object+5;}

                    if (move_tasto.ascii==' '){

                    sem_wait(&mutex_tasto);

                    flag=1;

                            if(numero_telescopi<N_MAX_TELESCOPI)

                            {

                            numero_telescopi++;

                            }

                    sem_post(&mutex_tasto);

                    }

              //CONTROLLI SULLA POSIZIONE

                       if(y_object<(dim_y/2+70)) y_object=dim_y/2+70;

                       if(y_object>(240-dim_y/2)) y_object=(240-dim_y/2);

                       if(x_object<(dim_x/2+20 )) x_object=dim_x/2+20;

                       if(x_object>(630-dim_x/2-10)) x_object=(630-dim_x/2-10);

                }

                p = cab_reserve(cid[0]);

                sprintf(coord_x,"%d %d",x_object,y_object);

                memcpy(p,coord_x,dim_msg*sizeof(char));

                cab_putmes(cid[0], p);

        for(z=0;z<7;z++)

        {

        if (sqrt(pow((x_object-x_p[z]),2)+pow((y_object-y_p[z]),2))<20+dim_p[z]+radius)

                        grx_disc(x_p[z],y_p[z],dim_p[z],col_p[z]);        

        }

        sem_wait(&mutex);

        // Gestione sfondo

        grx_getimage(x_object-15,y_object-15,x_object+14, y_object+14,lunabkg);

        for(z=0;z<900;z++)

        {

         if( luna[z]==0 && lunabkg!=0 )

          {

                lunadest[z]=lunabkg[z];

          }

         else

          {

         lunadest[z]=luna[z];

          }

        }

        grx_putimage(x_object-15,y_object-15,x_object+14, y_object+14,lunadest);

        sem_post(&mutex);

        task_endcycle();

        }

}

TASK    tele(void *arg)

{

int     i = (int)arg;

int     passi;

char    s[100];

int x_object=300;  // posizione dell'oggetto da rilevare

int y_object=200;

int x_start=500;   // posizione iniziale dei telescopi

int y_start=190;

int x_current=x_start; // posizione corrente dei telescopi

int y_current=y_start;

int x_current_d[9];

int y_current_d[9];

int y= Y_TELESCOPI;  // coordinata y dei telescopi

double alpha_new=atan((y-(double)y_current)/((x[i]-(double)x_current)));

double alpha=alpha_new;

double distance=0.0;

double alpha_target=0.0;

double tc=0.1;

double u=0.0;

double u_old=0.0;

double errore=0.0;

double delta_x=0;

double delta_y=0;

char *p;

//int j=0;

// indice matrice per aggiunta di rumore

int k=0;

int q=0;

double polo=poli[i];

int val=0;

BYTE videobuf[dim_x*dim_y];

        passi = 0;

        //srand(i);

        //srand(i+sys_gettime(NULL));

        alpha_target=atan((y-(double)y_object)/((double)x_object-x[i]));      

        sem_wait(&mutex);

        grx_text("targ",4,340,12,0);

        sprintf(s,"%f",poli[i]);

        grx_text(s,x[i],y+60,12,0);

        grx_text("polo",4,y+60,12,0);

        grx_text("new",4,y+50,12,0);

        sprintf(s,"%1.6f",alpha_target);

        grx_text(s, x[i]-25,y+50,12,0);

        grx_rect(x[i]-40,y+25,x[i]+40,y+85,2);

        grx_rect(x[i]-40,y+90,x[i]+40,y+150,2);

        sem_post(&mutex);

        // Disegno telescopio

        grx_disc(x[i],y,19,i+1);

        grx_box( x[i]-19,y,x[i]+19,y+20,i+1);

        task_endcycle();

        while (1) {

                // legge di controllo

                passi++;

                //sprintf(s,"%d",passi);

                //grx_text(s, 50,110,12,0);

                p = cab_getmes(cid[0]);

                sscanf(p,"%d %d",&val,&y_object);

                cab_unget(cid[0], p);

                x_object=val;      

                alpha_target=atan((y-(double)y_object)/((double)x_object-x[i]));

                if (alpha_target <0)

                {

                alpha_target=3.14+alpha_target;

                }

                errore=alpha_target-alpha_new;

                u=u_old+kp*tc*errore;

                //alpha_new=0.13*alpha+0.87*u;

                alpha_new=(exp(polo*tc))*alpha+(1-exp(polo*tc))*u;

                u_old=u;

                alpha=alpha_new;

                // implementazione dei limiti degli attuattori

                if (alpha_new > 3.14)

                        alpha_new=3.14;

                if (alpha_new <0)

                        alpha_new=0;

                distance=sqrt(((y-y_object)*(y-y_object))+((x_object-x[i])*(x_object-x[i])));

                sem_wait(&mutex);

              // Cancello braccio telescopio

              if (passi>1)

              {

                      for (k=0; k<9 ; k++)

                        {

                        grx_line(delta_x,delta_y,x_current_d[k],y_current_d[k],0);

                        }

             }

                x_current=x[i]+distance*cos(alpha_new);

                y_current=300-distance*sin(alpha_new);

                // lettura immagine

                grx_getimage(x_current-(dim_x/2),y_current-(dim_y/2),x_current+(dim_x/2),y_current+(dim_y/2),videobuf);

                //aggiunta rumore

                sem_post(&mutex);

                for (k=0;k<dim_x;k++)

                {

                        for(q=0;q<dim_y;q++)

                        {

                        int num=0;

                                num=rand();

                                num=(num%10)+1;

                                if (num>SOGLIA)

                                {

                                videobuf[k*dim_x+q]=videobuf[k*dim_x+q]+1;

                                }

                        }

                }

                sem_wait(&mutex);

                grx_putimage(x[i]-25,y+92,x[i]-25+dim_x-1,y+92+dim_y-1,videobuf);

                sprintf(s,"%3.4f",(180*(alpha_target/pi)));

                grx_text(s, x[i]-25,y+40,12,0);

                sprintf(s,"%3.4f",180*(alpha_new/pi));

                grx_text(s, x[i]-25,y+50,12,0);

                delta_x=x[i]+20*cos(alpha_new);

                delta_y=y-20*sin(alpha_new);  

                for ( k=0; k<9 ; k++) {

                x_current_d[k]=x[i]+50*cos(alpha_new+(k-4)*0.01);

                y_current_d[k]=300-50*sin(alpha_new+(k-4)*0.01);  }

             // Disegno braccio telescopio

                for(k=0 ; k<9 ; k++)

                 grx_line(delta_x,delta_y,x_current_d[k],y_current_d[k],i+1);

                sem_post(&mutex);

                // scrive immagine nel cab

                p = cab_reserve(cid[i+1]);

                memcpy(p,videobuf,dim_x*dim_y*sizeof(BYTE));

                cab_putmes(cid[i+1], p);

              task_endcycle();

        }

}

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

/* This is the exception handler. It is called when an exception

   is raised.

   It exits from the graphical mode, then it prints a message and

   shutdown the kernel using sys_abort()

*/

void demo_exc_handler(int signo, siginfo_t *info, void *extra)

{

  struct timespec t;

  grx_close();

  /* Default action for an kern exception is  */

  kern_cli();

  ll_gettime(TIME_EXACT, &t),

  kern_printf("\nS.Ha.R.K. Exception raised!!!"

              "\nTime (s:ns)     :%ld:%ld"

              "\nException number:%d (numbers in include/bits/errno.h)"

              "\nPID             :%d\n",

              t.tv_sec, t.tv_nsec, info->si_value.sival_int,

              info->si_task);

  sys_abort(1);

}

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

/* This function is called when Alt-X is pressed.

   It simply shutdown the system using sys_end.

   Note that the byebye() function is called only if we exit from

   the system using sys_end()!!!!

*/

void my_end(KEY_EVT* e)

{

        sys_end();

}

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

/* This function is called when the system exit correctly after Alt-X.

   It exits from the graphic mode and then it prints a small greeting.

   Note that:

   - The function calls grx_exit, so it must be registered using

     RUNLEVEL_BEFORE_EXIT (RUNLEVEL_AFTER_EXIT does not work because

     at that point the kernel is already returned in real mode!!!)

   - When an exception is raised, the exception handler is called.

     Since the exception handler already exits from the graphic mode,

     this funcion has not to be called. For this reason:

     . we registered byebye using the flag NO_AT_ABORT

     . the exception handler exits using sys_abort; in that way byebye is

       NOT called

*/

void byebye(void *arg)

{

  grx_close();

  kern_printf("Bye Bye!\n");

}

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

int main(int argc, char **argv)

{

  PID             pid1, pid4,pid5;

  KEY_EVT         emerg;

  int i=0;  

  int z=0; // contatore per disegno dei pianeti

  HARD_TASK_MODEL m1, m4,m5;

  //FILE *fp;

  struct sigaction action;

  cid[0] = cab_create(cname[0], dim_msg, 5);

  cid[1] = cab_create(cname[1], dim_x*dim_y, 3);

  cid[2] = cab_create(cname[2], dim_x*dim_y, 3);

  cid[3] = cab_create(cname[3], dim_x*dim_y, 3);

  cid[4] = cab_create(cname[4], dim_x*dim_y, 4);

  cid[5] = cab_create(cname[5], dim_x*dim_y, 3);

  cid[6] = cab_create(cname[6], dim_x*dim_y, 3);

  /*fp=fopen("file.txt","r");

  fscanf(fp,"%d",a);

  fclose(fp);

  */

        /* Init the standard S.Ha.R.K. exception handler */

        action.sa_flags = SA_SIGINFO;            /* Set the signal action */

        action.sa_sigaction = demo_exc_handler;

        action.sa_handler = 0;

        sigfillset(&action.sa_mask); /* we block all the other signals... */

        if (sigaction(SIGHEXC, &action, NULL) == -1) {  /* set the signal */

          perror("Error initializing signals...");

          sys_end();

        }

        /* Set the closing function */

        sys_atrunlevel(byebye, NULL, RUNLEVEL_BEFORE_EXIT|NO_AT_ABORT);

        /* Initializes the semaphore */

        sem_init(&mutex,0,1);

        /* graphic card Initialization */

        if (grx_init() < 1) {

           sys_abort(1);

        }

        if (grx_open(640, 480, 8) < 0) {

            kern_printf("GRX Err\n");

            sys_abort(1);

        }

        kern_printf("Video card ok!\n");

        /* set the keyboard handler to exit correctly */

        emerg.ascii = 'x';

        emerg.scan = KEY_X;

        emerg.flag = ALTL_BIT;

        keyb_hook(emerg,my_end);

        srand(i+sys_gettime(NULL));

        emerg.ascii = 'x';

        emerg.scan = KEY_X;

        emerg.flag = ALTR_BIT;

        keyb_hook(emerg,my_end);

        sem_wait(&mutex);

        /* a small banner */

        grx_clear(0);

        grx_text("REALTIME TELESCOPES di Marco Bera e Gabriele Varasio",8,8,WHITE,0);

        grx_text("Premere Spazio per creare un telescopio",8,22,WHITE,0);

        grx_text("Premere Alt-X per uscire",8,32,WHITE,0);

        grx_text("Usare il tastierino numerico per muovere l'oggetto",8,42,WHITE,0);

        // DISEGNARE BORDI

        grx_rect(1,1,638,478,2);

        grx_rect(558,1,638,61,2);

        grx_rect(1,1,638,61,2);

       // Disegno sfondo

        for(z=0;z<7;z++)

        {

        num_p=rand();

        y_p[z]=100+((num_p%130)+1);

        x_p[z]=50+((num_p%540)+1);

        dim_p[z]=2+((num_p%5)+1);

        col_p[z]=((num_p%10)+1);

        grx_disc(x_p[z],y_p[z],dim_p[z],col_p[z]);

        }    

        sem_post(&mutex);

        hard_task_default_model(m4);

        hard_task_def_ctrl_jet (m4);

        hard_task_def_arg      (m4, (void *)3);

        hard_task_def_wcet     (m4, 19000);

        hard_task_def_mit      (m4, PERIOD_T4);

        hard_task_def_group    (m4,1);

        pid4 = task_create("move", move, &m4, NULL);

        if (pid4 == NIL) {

          grx_close();

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

          sys_abort(1);

        }

        task_activate(pid4);

        hard_task_default_model(m5);

        hard_task_def_ctrl_jet (m5);

        hard_task_def_arg      (m5, (void *)3);

        hard_task_def_wcet     (m5, 19000);

        hard_task_def_mit      (m5, PERIOD_T5);

        hard_task_def_group    (m5,1);

        pid5 = task_create("media", media, &m5, NULL);

        if (pid5 == NIL) {

          grx_close();

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

          sys_abort(1);

        }

        task_activate(pid5);

        /* and finally we activate the three threads... */

        do {

        int val_flag=0;

        sem_wait(&mutex_tasto);

        val_flag=flag;

        sem_post(&mutex_tasto);

        if ((val_flag==1) && (i < N_MAX_TELESCOPI))

        {

        flag=0;

        hard_task_default_model(m1);

        hard_task_def_ctrl_jet (m1);

        hard_task_def_arg      (m1, (void *)i);

        hard_task_def_wcet     (m1, WCET_T);

//        hard_task_def_mit      (m1, PERIOD_T1);

        hard_task_def_mit      (m1, period_t1);

        hard_task_def_group    (m1,1);

        pid1 = task_create("tele1", tele, &m1, NULL);

        if (pid1 == NIL)

        {

          grx_close();

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

          sys_abort(1);

        }

            task_activate(pid1);

            i++;

        }

    } while (1);

        /*

           now the task main ends, but the system does not shutdown because

           there are others.

           The demo will finish if a Alt-X key is pressed.

        */

        return 0;

}

/*********** lettura da file ********************/

void read_cfg_file(int argc, char **argv)

{

  int err;

  DOS_FILE      *fp;

  char myfilebuf[1000];

  int myfilebuf_length;

  if (2)

  {

    fp = DOS_fopen("dati.cnf","r");

    if (fp)

    {

      /* read up to 1000 chars */

      myfilebuf_length = DOS_fread(&myfilebuf,1,1000,fp);

      /* check for errors */

      err = DOS_error();

      cprintf("Read %d bytes...\n", myfilebuf_length);

              if (err)

              {

                cprintf("Error %d reading file...Using default values\n", err);

              }

              else

              {

                //geti(myfilebuf, &pos, &NMouses);          // Number of Mouses

              sscanf(myfilebuf,"%lf %lf %lf %lf %lf %lf %d",&poli[0],&poli[1],&poli[2],&poli[3],&poli[4],&kp,&period_t1);

              }

      DOS_fclose(fp);

      return;

    }

    else {

      /* error!! */

      err = DOS_error();

      /* note that if you call DOS_error() here, it return 0!!! */

      cprintf("Error %d opening myfile.txt...Using default values\n", err);

    }

  }

  else {

    cprintf("Wrong number of arguments...\n");

    l1_exit(0);

  }

}

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