Subversion Repositories shark

Rev

Rev 1655 | 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     :
 *   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: eli.c,v 1.1.1.1 2004-05-24 18:03:47 giacomo Exp $

 File:        $File$
 Revision:    $Revision: 1.1.1.1 $
 Last update: $Date: 2004-05-24 18:03:47 $
 ------------

 This file is similar to the configuration of Hartik 3.3.1

**/


/*
 * Copyright (C) 2000 Paolo Gai and ALLEN-DESTRO
 *
 * 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
 *
 */


#include        "eli.h"

#include <time.h>

//########################################
// Definizione variabili globali
//########################################
char tastiera;
long double Forza_x,Forza_y,Velocita_x,Velocita_y,Acc_x,Acc_y,Pos_x,Pos_y;
long double Pos_pe_y[5];
long double Pos_pe_x[5];
long double T1=0;
long double r;
int n_peso=0;
int peso_agganciato[5]={0,0,0,0,0};
int pre_peso[5]={0,0,0,0,0};
int libero_peso[5]={0,0,0,0,0};
PID pid_peso[5];
PID pid_ruspa[5];
int x_r=200;
int n_ruspa=5;
int indietro;
int eli_occ=0;
int kill_p;

//PID pid_r0,pid_r1,pid_r2,pid_r3,pid_r4;
PID pid_kill_p,pid_pulisci_p,pid_kill_e,pid_pulisci_e;


// Definizione del semaforo per l'utilizzo della modalita' grafica
sem_t           mutex;
sem_t           pu;




//#######################################################################
//############### DEFINIZIONE TASK ######################################
//#######################################################################

TASK kill_task(int i)
{ int x,y,dim;
 while (1)
   {

     if(i==1)
       {
         x=Pos_pe_x[kill_p];
         y=Pos_pe_y[kill_p];
         dim=25;
       }
     else
       {
         x=Pos_x;
         y=Pos_y;
         dim=35;
       }

     PULISCI(x,y,dim);
     ESPLOSIONE(x,y);

     sem_wait(&pu);

     if(i==1)    task_activate(pid_pulisci_p);
     else     task_activate(pid_pulisci_e);

     task_endcycle();

   }


}

TASK pulisci(int i)
{
  int x,y,dim;
  struct timespec delay;

  delay.tv_sec=0;
  delay.tv_nsec=3000000;

  while (1)
   {

     if(i==1)
       {
         x=Pos_pe_x[kill_p];
         y=Pos_pe_y[kill_p];
         dim=25;
       }
     else
       {
         x=Pos_x;
         y=Pos_y;
         dim=35;
       }


     nanosleep(&delay, NULL);
     PULISCI(x,y,dim);

     sem_post(&pu);
   
     task_endcycle();
   }
}




TASK elicottero(int i)
{  long double x_old,x_new,y_old,y_new;
 TIME us_old,us_new;
 long double delta_T;
 long double Fx,Fy,Ax,Ay,Vx,Vy,m_el;
 int Destra_old,Destra_new;
 //   int uccidi,colore;
 //   int prova;

 m_el=1;
 Vx=0;Vy=0;
 Ax=0;Ay=0;
 Fx=0;Fy=0-m_el*G; // Fy=0;
 x_old=150;
 y_old=150;
 Destra_old=1;
 Destra_new=1;

 us_old=sys_gettime(NULL);
   
 while (1)
   {

     //  prova=2;


     us_new=sys_gettime(NULL);
     delta_T=(us_new-us_old)/100000;
     //    if(delta_T<0.9) prova=1;
     //    if(delta_T<0) prova=4;
     //    if(delta_T>1.1) prova=3;


     delta_T=1;
     Ax=(Fx-(r*Vx))/m_el;
     Ay=(Fy-(r*Vy)+m_el*G+T1)/m_el;
     x_new=x_old+Vx*delta_T+0.5*Ax*delta_T*delta_T;
     y_new=y_old+Vy*delta_T+0.5*Ay*delta_T*delta_T;

     //    if(prova==1) {x_new=300; y_new=150;}
     //    if(prova==3) {x_new=150; y_new=300;}
     //    if(prova==2) {x_new=300; y_new=300;}
     //    if(prova==4) {x_new=400; y_new=400;}

     Vx=Vx+Ax*delta_T;
     Vy=Vy+Ay*delta_T;
     if ((Vx>0)|| ((Vx==0) && (Destra_old==1))) Destra_new=1;
     else Destra_new=0;

     if ( ((y_new>=(Y0max-DIM_EL/2-2)) || ( (y_old<=400-DIM_EL/2-2) &&   (y_new>=400-DIM_EL/2-2) && (x_new>=X0min+151) && (x_new<=X0min+280) )) )
       {
         if ((x_new>=X0min+151) && (x_new<=X0min+280)) y_new=400-DIM_EL/2-2;
         else  y_new=Y0max-DIM_EL/2-2;
         Vy=0;
         Vx=0;
         x_new=x_old;
         Ay=0;
         Ax=0;
       }




     sem_wait(&mutex);

     /*
       uccidi=0;

       colore=grx_getpixel(x_new-2*DIM_EL,y_new-DIM_EL);
       if ( (colore!=COL_SFONDO) && (colore!=YELLOW) && (colore!=RED)) uccidi=1;
       colore=grx_getpixel(x_new+2*DIM_EL,y_new-DIM_EL);
       if ( (colore!=COL_SFONDO) && (colore!=YELLOW) && (colore!=RED)) uccidi=1;
       colore=grx_getpixel(x_new-2*DIM_EL,y_new+DIM_EL/2);
       if ( (colore!=COL_SFONDO) && (colore!=YELLOW) && (colore!=RED)) uccidi=1;
       colore=grx_getpixel(x_new+2*DIM_EL,y_new+DIM_EL/2);
       if ( (colore!=COL_SFONDO) && (colore!=YELLOW) && (colore!=RED)) uccidi=1;
       colore=grx_getpixel(x_new,y_new);
       if ( (colore!=COL_SFONDO) && (colore!=YELLOW) && (colore!=RED)) uccidi=1;
     */


     if ((x_old>100) && (y_old>100) && (x_old<X0max) )
       {
         if (Destra_old==0) {ELICOTTERO_S(x_old,y_old,DIM_EL,COL_SFONDO,COL_SFONDO);}
         else {ELICOTTERO_D(x_old,y_old,DIM_EL,COL_SFONDO,COL_SFONDO);}
       }

     if ((x_new>100) && (y_new>100) && (x_new<X0max) )
       {
         /*
           if ( uccidi==1)
           {
           //  grx_box(10,10,100,100,RED);
           //  task_activate(pid_kill_e);
           //  sem_post(&mutex);
           //   eli_occ=0;
           sem_post(&mutex);
           task_abort();
           }
         */



         if (Destra_new==0) {ELICOTTERO_S(x_new,y_new,DIM_EL,COL_EL,COL_SFONDO);}
         else {ELICOTTERO_D(x_new,y_new,DIM_EL,COL_EL,COL_SFONDO);}
       }
     sem_post(&mutex);

     if (tastiera=='c') Fx=Fx+0.25;
     if (tastiera=='z') Fx=Fx-0.25;
     if (tastiera=='x') Fy=Fy+0.25;
     if (tastiera=='s') Fy=Fy-0.25;

     if (Fx>2) Fx=2;
     if (Fx<-2) Fx=-2;
     if (Fy>0) Fy=0;
     if (Fy<-17.5) Fy=-17.5;

     Forza_x=Fx;
     Forza_y=Fy;
     Acc_x=Ax;
     Acc_y=Ay;
     Velocita_x=Vx;
     Velocita_y=Vy;
     Pos_x=x_new;
     Pos_y=y_new;


     if ( (tastiera=='z') || (tastiera=='x') || (tastiera=='c') || (tastiera=='s') )
       tastiera='q';


     us_old=us_new;
     x_old=x_new;
     y_old=y_new;
     Destra_old=Destra_new;

     task_endcycle();
   }
}



TASK peso(int i)
{
  long double delta_T;
  long double Ay,Vy;
  long double x_new,x_old,y_old,y_new;
  int scelta=0;
  int rit,colore,uccidi;
  double m_peso[5]={0.5,0.4,0.3,0.1,0.6};
  int delta_y1,delta_y2;


  //   PID pid;
  //   MODEL           m = BASE_MODEL;

  Vy=0;
  Ay=0;

  x_old=700-DIM_PESO-1;
  y_old=Y0max-DIM_PESO*2-1;
  delta_T=1;
  x_new=700-DIM_PESO-1;

  libero_peso[i]=0;

  while (1 )
    {
      uccidi=0;

      if (scelta==0 )
        {

          y_new=y_old;
          if (x_new+DIM_PESO==x_r-1) x_new=x_new-1;
          else x_new=x_new;
          if (indietro==1) {scelta=1; libero_peso[i]=1;}
        }

      if (scelta==1 )
        {
          y_new=y_old;
          x_new=x_old;
          if (peso_agganciato[i]==1) scelta=2;
        }
      if (scelta==2)
        {
          eli_occ=1;
          Ay=(m_peso[i]*G-(r*Vy)-T1)/m_peso[i];
          if(peso_agganciato[i]==1) x_new=Pos_x;
          else x_new=x_old;
          y_new=y_old+Vy*delta_T+0.5*Ay*delta_T*delta_T;
          Vy=Vy+Ay*delta_T;

          delta_y1=Y0max-y_new;
          delta_y2=400-y_new;

          if ( (y_new>=(Y0max-2*DIM_PESO-1)) || ( (y_old<400-2*DIM_PESO-1) && (y_new>=400-2*DIM_PESO-1) && (x_new>=X0min+151-DIM_PESO) && (x_new<=X0min+280+DIM_PESO) ) )
            {
              if ((x_new>=X0min+151-DIM_PESO) && (x_new<=X0min+280+DIM_PESO))
                {
                  if (delta_y2<6) uccidi=1;
                  y_new=400-2*DIM_PESO-2;
                }
              else {
                if (delta_y1<6) uccidi=1;
                y_new=Y0max-2*DIM_PESO-2;
              }
              Vy=0;
              if(peso_agganciato[i]==0) {scelta=3; rit=0;}
            }
        }
      if (scelta==3)
        {
          x_new=x_new;
          y_new=y_new;
          eli_occ=0;
          if (y_new==400-2*DIM_PESO-2) rit++;


     
          if (rit==100) {
            peso_agganciato[i]=0;
            pre_peso[i]=0;
            sem_wait(&mutex);
            PESO(x_old,y_old,DIM_PESO,COL_SFONDO);
            sem_post(&mutex);

            return NULL;
          }

          if(peso_agganciato[i]==1) scelta=2;
        }

      sem_wait(&mutex);


      colore=grx_getpixel(x_new-DIM_PESO,y_new);
      if ( (colore!=COL_SFONDO) && (colore!=DELTA_COL+i*2) && (colore!=RED)) uccidi=1;

      colore=grx_getpixel(x_new+DIM_PESO,y_new);
      if ( (colore!=COL_SFONDO) && (colore!=DELTA_COL+i*2) && (colore!=RED)) uccidi=1;

      colore=grx_getpixel(x_new-DIM_PESO,y_new+2*DIM_PESO);
      if ( (colore!=COL_SFONDO) && (colore!=DELTA_COL+i*2) && (colore!=RED)) uccidi=1;

      colore=grx_getpixel(x_new+DIM_PESO,y_new+2*DIM_PESO);
      if ( (colore!=COL_SFONDO) && (colore!=DELTA_COL+i*2) && (colore!=RED)) uccidi=1;

      colore=grx_getpixel(x_new,y_new+DIM_PESO);
      if ( (colore!=COL_SFONDO) && (colore!=DELTA_COL+i*2) && (colore!=RED)) uccidi=1;



      if ((x_old>100) && (y_old>100) && (x_old<X0max) )
        {
          PESO(x_old,y_old,DIM_PESO,COL_SFONDO);
        }

      y_old=y_new;
      x_old=x_new;
      Pos_pe_y[i]=y_new;
      Pos_pe_x[i]=x_new;


      if ((x_new>100) && (y_new>100) && (x_new<X0max) )
        {
          if ( uccidi==1)
            {
              kill_p=i;
              task_activate(pid_kill_p);
              sem_post(&mutex);
              eli_occ=0;
              pre_peso[i]=0;
              peso_agganciato[i]=0;
              return NULL;
            }

          PESO(x_new,y_new,DIM_PESO,DELTA_COL+i*2);
        }
      sem_post(&mutex);


      task_endcycle();
    }


}

TASK ruspa(int i)
{
  int x,y,ii;
  int end_peso[5]={370,420,470,520,570};

  PID pid;
  HARD_TASK_MODEL m_hard;

  pre_peso[i]=1;

  hard_task_default_model(m_hard);
  hard_task_def_wcet(m_hard,1);  // wcet ignored!!!
  hard_task_def_mit(m_hard,PER_DISEGNA);
  hard_task_def_usemath(m_hard);
  hard_task_def_arg(m_hard,(void *)i);
  pid=task_create("peso2",peso,&m_hard,NULL);
  pid_peso[i]=pid;
  task_activate(pid_peso[i]);


  x=720;
  y=Y0max;
 
  indietro=0;
  while (x!=end_peso[i]+DIM_PESO)
    {
      sem_wait(&mutex);
      RUSPA_S(x+1,y,COL_SFONDO,COL_SFONDO);
      RUSPA_S(x,y,YELLOW-1,LIGHTBLUE);
      sem_post(&mutex);
      x_r=x;
      x--;
      task_endcycle();
    }
  sem_wait(&mutex);
  RUSPA_S(x+1,y,COL_SFONDO,COL_SFONDO);
  sem_post(&mutex);
  indietro=1;
  while (x!=730 )
    {
      sem_wait(&mutex);
      RUSPA_D(x-1+40,y,COL_SFONDO,COL_SFONDO);
      RUSPA_D(x+40,y,YELLOW-1,LIGHTBLUE);
      sem_post(&mutex);
      x++;
      task_endcycle();
    }

  ii=i+1;

  while(ii>4)
    {
      n_ruspa=5;
      if(pre_peso[4]==0){
        ii=4;
        if(pre_peso[3]==0){
          ii=3;
          if(pre_peso[2]==0){
            ii=2;
            if(pre_peso[1]==0){
              ii=1;
              if(pre_peso[0]==0){
                ii=0;
              }
            }
          }
        }
      }
      task_endcycle();

    }

  hard_task_def_arg(m_hard,(void *)ii);
  pid_ruspa[ii]=task_create("ruspa",ruspa,&m_hard,NULL);
  task_activate(pid_ruspa[ii]);
  n_ruspa=ii;

  return NULL;
}




TASK molla(int i)
{
  double k=0.05;
  double x,x_old,y1_old,y2_old;
  double T1_old; //,T2_old;

  x_old=Pos_x;
  y1_old=Pos_y;
  y2_old=Pos_pe_y[n_peso];
   
  while (1)
    {

      sem_wait(&mutex);
      if ((x_old>100-DIM_EL) && (y1_old>100-DIM_EL) && (x_old<X0max) )
        {
          MOLLA(x_old,y1_old+DIM_EL,y2_old,COL_SFONDO)
            }
      else if( (y2_old>100) && (x_old<X0max) && (x_old>100) )
        {
          MOLLA(x_old,X0min+100,y2_old,COL_SFONDO)
            }

      x_old=Pos_x;
      y1_old=Pos_y;
 
      if(peso_agganciato[n_peso]==0) y2_old=y1_old+20;
      else                y2_old=Pos_pe_y[n_peso];

      if ((x_old>100) && (y1_old>100-DIM_EL) && (x_old<X0max)  )
        {
          MOLLA(x_old,y1_old+DIM_EL,y2_old,RED)
            }
      else if( (y2_old>100) && (x_old<X0max) && (x_old>100))
        {
          MOLLA(x_old,X0min+100,y2_old,RED)
            }
      sem_post(&mutex);

      x=y2_old-y1_old;
      x=x-20;
      T1=x*k;
      T1_old=T1;

      task_endcycle();

    }
}

TASK seleziona(int i)
{ int w,deltax,deltay,disegna;

 eli_occ=0;
 disegna=0;

 while (1)
   {
     disegna=0;
     for (w=0;w<5;w++)
       {
         deltax=Pos_pe_x[w]-Pos_x;
         deltay=Pos_pe_y[w]-Pos_y-DIM_PESO-20+DIM_EL;
         if ( (pre_peso[w]==1) && (eli_occ==0) && (deltax<4) && (deltax>-4) && (deltay<3) && (deltay>-3) )
           {
             disegna=1;
             if ( (tastiera=='p') && (libero_peso[w]==1) )
               {
                 eli_occ=1;
                 peso_agganciato[w]=1;
                 n_peso=w;

               }
           }  
       }

     if(disegna==1)
       {
         sem_wait(&mutex);
         grx_box(X0max-90,Y0min+5,X0max-5,Y0min+95,RED);
         //        grx_text("Ok", X0max-40, Y0min+50, COL_TESTO, RED);
         sem_post(&mutex);
       } else
         {
           sem_wait(&mutex);
           grx_box(X0max-95,Y0min+5,X0max-5,Y0min+95,COL_SFONDO);
           sem_post(&mutex);
         }

     if(eli_occ==1)
       {
         sem_wait(&mutex);
         grx_box(X0max-90-100,Y0min+5,X0max-5-100,Y0min+95,GREEN);
         //      grx_text("Go", X0max-35-100, Y0min+50, COL_TESTO, GREEN);
         sem_post(&mutex);
       } else
         {
           sem_wait(&mutex);
           grx_box(X0max-90-100,Y0min+5,X0max-5-100,Y0min+95,COL_SFONDO);
           sem_post(&mutex);
         }


     if (tastiera=='l') peso_agganciato[n_peso]=0;

     task_endcycle();

   }
}

                                                                 
//###############################################################
//                                                              ##
//        Funzione di uscita dal programma                      ##
//                                                              ##
//################################################################
void my_end(KEY_EVT* e)
{
  //####################################
  // Sezione esecutiva della funzione ##
  //####################################

  grx_close();
  cprintf("Ctrl-brk pressed!\n");
  sys_end();
}       // Fine della funzionemy_end
                                                                 
//################################################################
//                                                              ##
//        Funzione di uscita dal programma                      ##
//                                                              ##
//################################################################
void end()
{
  //####################################
  // Sezione esecutiva della funzione ##
  //####################################

  grx_close();
}       // Fine della funzione my_end



//##############################################################
//                                                            ##
//                            Main                            ##
//                                                            ##
//##############################################################
int main()
{
  //########################################
  // Sezione dichiarativa delle variabili ##
  //########################################
  // Identficativi di task generici
  PID             pid0, pid1, pid3, pid5; //, pid2,pid4,pid6;

  HARD_TASK_MODEL m_hard;
  NRT_TASK_MODEL m_nrt;

  // Ascoltatore di eventi
  KEY_EVT         emerg;
  // Contatori
  int              j;


  //####################################
  //    Sezione esecutiva del main    ##
  //####################################

  // Inizializzazione dei parametri hartik
  sem_init(&mutex,0,1);
  sem_init(&pu,0,1);

  emerg.ascii = 'x';
  emerg.scan = KEY_X;
  emerg.flag = ALTL_BIT;
  keyb_hook(emerg, my_end);
  sys_atrunlevel(end, NULL, RUNLEVEL_BEFORE_EXIT);

  if (grx_open(800, 600, 8) < 0)
    {
      cprintf("GRX Err\n");
      sys_abort(300);
    }
  cprintf("scheda ok\n");

  r=0.1;

  /* Disposizione degli elementi grafici statici */
  sem_wait(&mutex);
  draw_static();
  sem_post(&mutex);

  // Attivazione dei task
  hard_task_default_model(m_hard);
  hard_task_def_wcet(m_hard,1);  // wcet ignored!!!
  hard_task_def_usemath(m_hard);
     
  hard_task_def_mit(m_hard,PER_DISEGNA);
  pid0=task_create("elicottero",elicottero,&m_hard,NULL);
  task_activate(pid0);

  hard_task_def_mit(m_hard,PER_DISEGNA);
  pid1=task_create("indicometro",disegna_stato,&m_hard,NULL);
  task_activate(pid1);

  hard_task_def_mit(m_hard,PER_MOLLA);
  pid3=task_create("molla",molla,&m_hard,NULL);
  task_activate(pid3);

  hard_task_def_mit(m_hard,PER_DISEGNA);
  pid5=task_create("seleziona",seleziona,&m_hard,NULL);
  task_activate(pid5);


  hard_task_def_mit(m_hard,PER_DISEGNA);
  hard_task_def_arg(m_hard,0);
  pid_ruspa[0]=task_create("ruspa",ruspa,&m_hard,NULL);
  task_activate(pid_ruspa[0]);
  n_ruspa=0;

  nrt_task_default_model(m_nrt);
  nrt_task_def_usemath(m_nrt);
  nrt_task_def_arg(m_nrt,0);
  pid_kill_e = task_create("kill_task",kill_task, &m_nrt, NULL);
  pid_pulisci_e = task_create("pulisci",pulisci, &m_nrt, NULL);

  nrt_task_def_arg(m_nrt,(void *)1);
  pid_kill_p = task_create("kill_task",kill_task, &m_nrt, NULL);
  pid_pulisci_p = task_create("pulisci",pulisci, &m_nrt, NULL);




  do {
    tastiera = keyb_getch(BLOCK);

  } while(tastiera!=ESC);

  task_kill(pid5);
  task_kill(pid3);
  task_kill(pid1);
  task_kill(pid0);

  task_kill(pid_kill_p);
  task_kill(pid_kill_e);

  if (n_ruspa!=5) task_kill(pid_ruspa[n_ruspa]);
  for(j=0;j<5;j++) { if (pre_peso[j]==1) task_kill(pid_peso[j]); }

  grx_close();
  sys_end();

  return 0;
}       // Fine del main

/***************************<Fine del file>*******************************/