Subversion Repositories shark

Rev

Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

/*
 * 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.1.1 2004-05-24 18:03:39 giacomo Exp $

 File:        $File$
 Revision:    $Revision: 1.1.1.1 $
 Last update: $Date: 2004-05-24 18:03:39 $
 ------------
**/


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


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