Rev 1117 |
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 :
* 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.3 2003-01-07 17:10:16 pj Exp $
File: $File$
Revision: $Revision: 1.3 $
Last update: $Date: 2003-01-07 17:10:16 $
------------
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>*******************************/