/demos/trunk/telesco/initfile.c |
---|
0,0 → 1,133 |
/* |
* 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: initfile.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 $ |
------------ |
System initialization file |
This file contains the 2 functions needed to initialize the system. |
These functions register the following levels: |
an EDF (Earliest Deadline First) level |
a RR (Round Robin) level |
a CBS (Costant Bandwidth Server) level |
a Dummy level |
It can accept these task models: |
HARD_TASK_MODEL (wcet+mit) at level 0 |
SOFT_TASK_MODEL (met, period) at level 1 |
NRT_TASK_MODEL at level 2 |
This file is similar to the configuration of kernel/init/hartik3.c |
TICK is set to 0 (one-shot timer is used) |
*/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* 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 "kernel/kern.h" |
#include "modules/edf.h" |
#include "modules/cbs.h" |
#include "modules/rr.h" |
#include "modules/dummy.h" |
#include "modules/sem.h" |
#include "modules/hartport.h" |
#include "modules/cabs.h" |
#include "drivers/keyb.h" |
/*+ sysyem tick in us +*/ |
#define TICK 0 |
/*+ RR tick in us +*/ |
#define RRTICK 10000 |
int argc; |
char *argv[100]; |
void read_cfg_file(int argc, char **argv); |
int main(int argc, char **argv); |
TIME __kernel_register_levels__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
EDF_register_level(EDF_ENABLE_ALL); |
CBS_register_level(CBS_ENABLE_ALL, 0); |
RR_register_level(RRTICK, RR_MAIN_YES, mb); |
dummy_register_level(); |
SEM_register_module(); |
CABS_register_module(); |
__compute_args__(mb, &argc, argv); |
read_cfg_file(argc, argv); |
//read_cfg_file(argc, argv); |
return TICK; |
} |
TASK __init__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
KEYB_PARMS kparms = BASE_KEYB; |
HARTPORT_init(); |
keyb_def_ctrlC(kparms, NULL); |
keyb_def_map(kparms,itaMap); |
KEYB_init(&kparms); |
//__call_main__(mb); |
return (void *)main(argc,argv); |
//return (void *)0; |
} |
/demos/trunk/telesco/tel.c |
---|
0,0 → 1,709 |
/* |
* 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); |
} |
} |
/****************************************************************/ |
/demos/trunk/telesco/dati.cnf |
---|
0,0 → 1,3 |
-1 -20 -30 -40 -50 0.5 100000 |
# 5 poli kp(guadagno) periodo telescopio |
/demos/trunk/telesco/makefile |
---|
0,0 → 1,16 |
# |
# |
# |
ifndef BASE |
BASE=../.. |
endif |
include $(BASE)/config/config.mk |
PROGS= tel |
include $(BASE)/config/example.mk |
tel: |
make -f $(SUBMAKE) APP=tel INIT= OTHEROBJS="initfile.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __GRX__" |
/demos/trunk/telesco/moon.h |
---|
0,0 → 1,43 |
// Immagine luna |
BYTE luna[900]={ |
0, 0 , 0, 0, 0, 0, 0, 0, 4, 57, 212, 228, 204, 214, 235, 237, 208, 190, 179, 197, 229, 89, 4, 0, |
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 3, 208, 213, 178, 155, 139, 129, 123, 169, 170, 173, 157, |
191, 209, 236, 215, 5, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 36, 103, 146, 116, 93, 143, 119, 96, |
134, 123, 117, 120, 113, 102, 93, 156, 210, 184, 66, 4, 0, 0, 0, 0, 0, 0, 0, 0, 2, 64, 83, 88, |
72, 156, 144, 127, 99, 87, 91, 156, 158, 124, 144, 120, 109, 119, 175, 167, 180, 119, 3, 0, |
0, 0, 0, 0, 0, 5, 56, 59, 63, 58, 138, 104, 71, 46, 55, 68, 79, 89, 172, 146, 141, 152, 110, 126, |
134, 179, 148, 145, 134, 4, 0, 0, 0, 0, 4, 32, 58, 50, 49, 56, 105, 58, 38, 42, 45, 69, 64, 93, |
144, 155, 157, 147, 95, 111, 122, 158, 162, 174, 171, 79, 4, 0, 0, 0, 4, 67, 49, 47, 54, 63, |
51, 55, 43, 38, 48, 62, 55, 86, 89, 145, 92, 60, 99, 72, 97, 130, 184, 169, 195, 171, 3, 0, 0, |
3, 77, 52, 37, 51, 72, 54, 63, 48, 46, 46, 48, 55, 54, 92, 95, 80, 55, 47, 47, 52, 56, 170, 187, |
196, 190, 177, 190, 6, 0, 45, 84, 38, 40, 79, 73, 56, 43, 41, 46, 45, 51, 74, 98, 89, 68, 89, |
46, 40, 48, 45, 38, 171, 176, 152, 165, 133, 157, 107, 3, 154, 42, 34, 42, 68, 66, 47, 52, 62, |
55, 60, 50, 56, 50, 106, 127, 74, 38, 39, 58, 38, 42, 116, 87, 128, 148, 69, 59, 170, 2, 100, |
41, 31, 38, 41, 53, 53, 62, 63, 64, 62, 65, 63, 58, 105, 71, 60, 57, 44, 43, 38, 36, 81, 58, 125, |
205, 61, 43, 130, 37, 88, 39, 31, 33, 43, 52, 76, 66, 97, 116, 101, 96, 90, 56, 76, 47, 43, 135, |
46, 87, 40, 30, 33, 37, 84, 152, 86, 43, 131, 81, 131, 36, 35, 33, 36, 53, 91, 73, 124, 145, |
132, 95, 80, 50, 45, 45, 35, 49, 70, 59, 32, 30, 30, 30, 39, 75, 158, 92, 133, 138, 151, 41, |
39, 38, 39, 82, 125, 59, 89, 98, 84, 64, 68, 40, 104, 107, 85, 78, 115, 75, 34, 43, 39, 36, 47, |
78, 98, 142, 104, 122, 155, 49, 32, 36, 32, 57, 71, 58, 85, 97, 69, 49, 53, 40, 88, 124, 133, |
169, 141, 94, 40, 40, 46, 53, 52, 81, 49, 77, 107, 122, 146, 79, 31, 38, 31, 37, 37, 51, 64, |
59, 84, 72, 64, 96, 88, 132, 153, 158, 154, 149, 140, 109, 77, 125, 120, 54, 51, 59, 123, 86, |
111, 122, 35, 44, 35, 34, 38, 63, 55, 55, 87, 76, 103, 133, 148, 146, 160, 151, 145, 163, 194, |
92, 104, 136, 109, 54, 46, 65, 123, 53, 160, 144, 56, 46, 41, 39, 39, 91, 41, 47, 70, 75, 57, |
112, 116, 140, 168, 179, 161, 164, 188, 123, 100, 145, 162, 87, 51, 111, 140, 5, 172, 148, |
134, 86, 72, 75, 48, 50, 40, 45, 53, 106, 60, 108, 137, 135, 160, 157, 152, 169, 179, 191, |
109, 70, 150, 122, 94, 104, 128, 4, 167, 144, 140, 103, 98, 116, 64, 56, 84, 62, 60, 59, 48, |
80, 150, 143, 151, 187, 161, 147, 178, 219, 79, 69, 163, 132, 87, 108, 128, 0, 83, 195, 195, |
125, 138, 67, 35, 106, 98, 90, 58, 44, 60, 131, 161, 152, 178, 157, 149, 164, 164, 177, 107, |
119, 129, 119, 145, 133, 104, 0, 5, 181, 233, 190, 150, 78, 43, 62, 63, 87, 50, 53, 46, 81, |
157, 187, 155, 181, 189, 145, 166, 211, 170, 137, 166, 189, 188, 164, 5, 0, 0, 5, 167, 175, |
193, 193, 108, 114, 63, 118, 139, 125, 130, 202, 209, 221, 213, 180, 172, 201, 170, 175, |
186, 177, 210, 236, 204, 14, 0, 0, 0, 3, 62, 166, 183, 166, 106, 139, 102, 129, 179, 206, 212, |
240, 232, 224, 210, 208, 196, 173, 179, 177, 186, 191, 227, 185, 100, 2, 0, 0, 0, 0, 5, 125, |
196, 143, 163, 169, 169, 152, 181, 212, 239, 241, 237, 234, 217, 180, 206, 188, 199, 188, |
194, 202, 164, 120, 4, 0, 0, 0, 0, 0, 0, 5, 123, 180, 145, 147, 184, 184, 197, 223, 232, 224, |
227, 227, 220, 190, 179, 192, 192, 197, 204, 165, 120, 4, 0, 0, 0, 0, 0, 0, 0, 0, 5, 74, 162, |
135, 153, 176, 178, 204, 235, 232, 235, 231, 212, 201, 190, 189, 196, 202, 189, 70, 4, 0, |
0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 5, 182, 167, 170, 184, 208, 218, 215, 206, 209, 179, 205, 196, 188, |
184, 193, 12, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 103, 202, 197, 201, 208, 235, 217, 203, |
197, 188, 185, 217, 131, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 5, 89, 130, 218, |
195, 179, 165, 102, 4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
/demos/trunk/autostr/autostr.c |
---|
0,0 → 1,1171 |
/*************************************************************************** |
************************************************************************** |
*** Universita' di Pavia *** |
*** Corso : Informatoca Industriale *** |
************************************************************************** |
*** Progetto : MONITORAGGIO AUTOSTRADA *** |
************************************************************************** |
*** Realizzato da : Nino Verzellesi e Quadrubbi Giacomo *** |
************************************************************************** |
***************************************************************************/ |
/* ----------------------------------------------------------------------------- */ |
#include <kernel/kern.h> |
#include <drivers/glib.h> |
#include <drivers/keyb.h> |
#include <semaphore.h> |
#include <stdlib.h> |
#include <math.h> |
/* ----------------------------------------------------------------------------- */ |
/********************* DEFINIZIONE DELLE COSTANTI **************************/ |
#define ESC 27 /* codice ASCII del tasto ESCAPE */ |
#define MAX_V 35 /* massimo numero di veicoli */ |
#define GRUPPO 1 |
#define LUNGCAMION 40 /* lunghezza del camion */ |
#define LARGCAMION 10 /* larghezza del camion */ |
#define LUNGLENTA 16 /* lunghezza della macchina lenta */ |
#define LARGLENTA 8 /* larghezza della macchina lenta */ |
#define LUNGVELOCE 20 /* lunghezza della macchina veloce */ |
#define LARGVELOCE 8 /* larghezza della macchina veloce */ |
#define DT 0.04 /* passo del campionamento */ |
#define CENTROCORSIA1 477 /* coordinata del centro della corsia 1 */ |
#define CENTROCORSIA2 459 /* coordinata del centro della corsia 2 */ |
#define CENTROCARREGGIATA 469 /* coordinata del centro della carreggiata */ |
/* ----------------------------------------------------------------------------- */ |
/****************** DEFINIZIONE DELLE VARIABILI GLOBALI ********************/ |
double tick = 1.0; /* system tick = 1 ms */ |
int periodo = 40000; /* periodo */ |
int wcet = 1000; /* deadline */ |
PID pid; |
sem_t mutex; /* semaforo per la grafica */ |
sem_t strada; /* semaforo per le variabili corsia1 e corsia2 */ |
int MAX_X; /* dimensione x della modalita' di visualizzazione dello schermo */ |
int MAX_Y; /* dimensione y della modalita' di visualizzazione dello schermo */ |
short corsia1[40000]; /* vettore della corsia1 */ |
short corsia2[40000]; /* vettore della corsia2 */ |
int xelic; /* cordinata dell'elicottero */ |
char c; /* carattere da tastiera */ |
/* ----------------------------------------------------------------------------- */ |
/****************** FUNZIONE CHE DISEGNA I VEICOLI *************************/ |
void draw_veicolo(int x, int y, int colore,int lunghezza,int larghezza) |
{ |
int u; /* indice di ciclo */ |
int v; /* variabile ausiliaria per la ricostruzione della segnaletica */ |
int xreal; /* coordinata di fine veicolo */ |
xreal=x-lunghezza; /* calcola la coordinata di fine veicolo */ |
if (xreal<0) /* la pone a zero nel caso in cui sia minore di zero (veicolo non ancora interamente entrato in autostrada) */ |
xreal=0; |
/* disegna il veicolo nella posizione indicata e con il colore indicato */ |
sem_wait(&mutex); |
grx_box(xreal,(int)(y-larghezza/2),x,(int)(y+larghezza/2),colore); |
sem_post(&mutex); |
/* ricostruisce la segnaletica orrizzontale della strada nel caso in cui si sta' cancellando il veicolo dalla posizione precedente (ridisegna solo quella parte che e' stata cancellata) */ |
if (colore==0) |
for (u=xreal;u<x;u++) |
{ |
v=u%8; |
if ((v==0) || (v==1)) |
{ |
sem_wait(&mutex); |
grx_plot(u,CENTROCARREGGIATA,rgb16(255,255,255)); |
sem_post(&mutex); |
} |
} |
} |
/* ----------------------------------------------------------------------------- */ |
/** FUNZIONE CHE CALCOLA LA DISTANZA DAL VEICOLO CHE PRECEDE SULLA CORSIA **/ |
int dist_ant(int pos_x,int pos_y,int distsic) |
{ |
int libero; /* variabile che indica se l'attuale posizione puntata dal veicolo e' libera */ |
int distant; /* distanza dal veicolo che ci precede */ |
int fine; /* variabile ausiliaria per capire se sto' arrivando alla fine della strada */ |
/* inizializzazione delle variabili */ |
libero=0; |
distant=0; |
/* calcola la distanza da un eventuale veicolo che ci precede */ |
sem_wait(&strada); |
if (pos_y<CENTROCARREGGIATA) /* controlla se il veicolo e' nella prima corsia, altrimenti e' in seconda corsia */ |
while (libero==0 && distant<distsic) /* il calcolo della distanza da un veicolo che ci precede termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */ |
{ |
fine=pos_x+distant; /* nel caso in cui il veicolo si giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia preceduto da nessun veicolo */ |
if (fine>40000) |
distant=distsic; |
else |
if (corsia2[fine]!=0) /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */ |
libero=1; |
else |
distant++; |
} |
else /* il veicolo e' nella seconda corsia */ |
while (libero==0 && distant<distsic) /* il calcolo della distanza da un veicolo che ci precede termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */ |
{ |
fine=pos_x+distant; /* nel caso in cui il veicolo si giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia preceduto da nessun veicolo */ |
if (fine>40000) |
distant=distsic; |
else |
if (corsia1[fine]!=0) /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */ |
libero=1; |
else |
distant++; |
} |
sem_post(&strada); |
return(distant); /* ritorna il valore della distanza misurata dal sensore , se non ha trovato nessun veicolo tale distanza e' pari alla massima lunghezza a cui puo' arrivare il sensore */ |
} |
/* ----------------------------------------------------------------------------- */ |
/** FUNZIONE CHE CALCOLA LA DISTANZA ANTERIORE DESTRA DI UN VEICOLO CHE PRECEDE **/ |
int dist_ant_dx(int pos_x,int pos_y,int distsicdx) |
{ |
int distantdx; /* distanza dal veicolo che ci precede a destra */ |
int libero; /* variabile che indica se l'attuale posizione puntata dal veicolo e' libera */ |
int fine; /* variabile ausiliaria per capire se sto' arrivando alla fine della strada */ |
/* inizializzazione delle variabili */ |
libero=0; |
distantdx=0; |
sem_wait(&strada); |
while (libero==0 && distantdx<distsicdx) /* il calcolo della distanza da un veicolo che ci precede a destra termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */ |
{ |
fine=pos_x+distantdx; /* nel caso in cui il veicolo sia giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia preceduto da nessun veicolo */ |
if (fine>40000) |
distantdx=distsicdx; |
else |
if (corsia1[fine]!=0) /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */ |
libero=1; |
else |
distantdx++; |
} |
sem_post(&strada); |
return(distantdx); /* ritorna il valore della distanza misurata dal sensore , se non ha trovato nessun veicolo tale distanza e' pari alla massima lunghezza a cui puo' arrivare il sensore */ |
} |
/* ----------------------------------------------------------------------------- */ |
/** FUNZIONE CHE CALCOLA LA DISTANZA ANTERIORE SINISTRA DI UN VEICOLO CHE PRECEDE **/ |
int dist_ant_sx(int pos_x,int pos_y,int distsicsx) |
{ |
int distantsx; /* distanza dal veicolo che ci precede a sinistra */ |
int libero; /* variabile che indica se l'attuale posizione puntata dal veicolo e' libera */ |
int fine; /* variabile ausiliaria per capire se sto' arrivando alla fine della strada */ |
/* inizializzazione delle variabili */ |
libero=0; |
distantsx=0; |
sem_wait(&strada); |
while (libero==0 && distantsx<distsicsx) /* il calcolo della distanza da un veicolo che ci precede a sinistra termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */ |
{ |
fine=pos_x+distantsx; /* nel caso in cui il veicolo si giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia preceduto da nessun veicolo */ |
if (fine>40000) |
distantsx=distsicsx; |
else |
if (corsia2[fine]!=0) /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */ |
libero=1; |
else |
distantsx++; |
} |
sem_post(&strada); |
return(distantsx); /* ritorna il valore della distanza misurata dal sensore , se non ha trovato nessun veicolo tale distanza e' pari alla massima lunghezza a cui puo' arrivare il sensore */ |
} |
/* ----------------------------------------------------------------------------- */ |
/** FUNZIONE CHE CALCOLA LA DISTANZA POSTERIORE SINISTRA DI UN VEICOLO CHE INSEGUE **/ |
int dist_postsx (int pos_x,int pos_y,int distsorp) |
{ |
int distpostsx; /* distanza dal veicolo che ci insegue a sinistra */ |
int libero; /* variabile che indica se l'attuale posizione puntata dal veicolo e' libera */ |
int inizio; /* variabile ausiliaria per capire se sono all'inizio della strada */ |
/* inizializzazione delle variabili */ |
distpostsx=0; |
libero=0; |
sem_wait(&strada); |
while (libero==0 && distpostsx<distsorp) /* il calcolo della distanza da un veicolo che ci insegue a sinistra termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */ |
{ |
inizio=pos_x-distpostsx; /* nel caso in cui il veicolo si giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia insuguito da nessun veicolo */ |
if (inizio<0) |
distpostsx=distsorp; |
else |
if (corsia2[inizio]!=0) /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */ |
libero=1; |
else |
distpostsx++; |
} |
sem_post(&strada); |
return(distpostsx); /* ritorna il valore della distanza misurata dal sensore , se non ha trovato nessun veicolo tale distanza e' pari alla massima lunghezza a cui puo' arrivare il sensore */ |
} |
/* ----------------------------------------------------------------------------- */ |
/** FUNZIONE CHE CALCOLA LA DISTANZA POSTERIORE DESTRA DI UN VEICOLO CHE INSEGUE **/ |
int dist_postdx (int pos_x,int pos_y,int distrientro) |
{ |
int distpostdx; /* distanza dal veicolo che ci insegue a destra */ |
int libero; /* variabile che indica se l'attuale posizione puntata dal veicolo e' libera */ |
int inizio; /* variabile ausiliaria per capire se sono all'inizio della strada */ |
/* inizializzazione delle variabili */ |
libero=0; |
distpostdx=0; |
sem_wait(&strada); |
while (libero==0 && distpostdx<distrientro) /* il calcolo della distanza da un veicolo che ci insegue a destra termina quando viene trovato un veicolo o si raggiunge la lunghezza massima di visione del sensore */ |
{ |
inizio=pos_x-distpostdx; /* nel caso in cui il veicolo si giunto alla fine dell'autostrada si assume che il sensore ritorni un valore come nel caso in cui non sia inseguito da nessun veicolo */ |
if (inizio<0) |
distpostdx=distrientro; |
else |
if (corsia1[inizio]!=0) /* se la strada e' occupata da un'altro veicolo pongo la variabile libero ad uno (strada occupata), altrimenti in caso contrario incremento la distanza analizzata dal sensore */ |
libero=1; |
else |
distpostdx++; |
} |
sem_post(&strada); |
return(distpostdx); /* ritorna il valore della distanza misurata dal sensore , se non ha trovato nessun veicolo tale distanza e' pari alla massima lunghezza a cui puo' arrivare il sensore */ |
} |
/* ----------------------------------------------------------------------------- */ |
/**************************** TASK AUTO LENTA ******************************/ |
TASK auto_lenta(void *arg) |
{ |
int x; /* posizione x assunta dal veicolo */ |
int y; /* posizione y assunta dal veicolo */ |
int oxelic; /* posizione vecchia dell'elicottero */ |
int ox; /* posizione vecchia x dall'auto lenta */ |
int oy; /* posizione vecchia y dall'auto lenta */ |
int k; /* indice di ciclo */ |
int estremo; /* distanza massima a cui ci possiamo spostare dall'elicottero (sia a destra che a sinistra) */ |
int sorpasso; /* indica se il veicolo e' in fase di sorpasso */ |
int rientro; /* indica se il veicolo e' in fase di rientro */ |
int precedentedritto; /* distanza anteriore da un veicolo */ |
int od; /* vecchia distanza anteriore da un veicolo */ |
int tot; /* distanza massima raggiungibile del sensore posteriore sinistro */ |
int tot3; /* distanza massima raggiungibile del sensore anteriore destro */ |
int tot2; /* distanza massima raggiungibile del sensore posteriore destro */ |
int i = (int)arg; |
int sensore_ant=200; /* distanza massima raggiungibile del sensore anteriore */ |
char stri[22]; /* vettore di caratteri */ |
char ostri[22]; /* vettore di caratteri */ |
float vcrociera; /* velocita' desiderata */ |
float vmax2=35.0; /* velocita' massima raggiungibile in seconda corsia */ |
float vmax1=25.0; /* velocita' massima raggiungibile in prima corsia */ |
float frenata=-7.0; /* valore massimo della frenata */ |
float amax=10.0; /* valore massimo dell'accellerazione */ |
float v; /* velocita' attuale */ |
float a; /* accellerazione attuale */ |
float ov; /* velocita' precedente */ |
float oa; /* accellerazione precedente */ |
/* inizializzazione delle variabili */ |
v=5.0; |
a=0.0; |
x=LUNGLENTA; |
y=CENTROCORSIA1; |
sorpasso=0; |
rientro=0; |
tot=100; |
tot2=150; |
tot3=100; |
vcrociera=vmax1; |
oa=a; |
ov=v; |
od=0; |
oxelic=xelic; |
estremo=(int)((MAX_X/2)-1); |
while (1) |
{ |
/* cancella il veicolo se era presente sull'autostrada vista dall'elicottero */ |
if (abs(oxelic-ox)<estremo) |
draw_veicolo(ox-oxelic+estremo, oy, 0,LUNGLENTA,LARGLENTA); |
/* salva le vecchie coordinate */ |
ox = x; |
oy = y; |
/* cancella il veicolo dalla vecchia posizione sulla corsia , andando a cancellarlo nel vettore della corsia in cui si trovava */ |
sem_wait(&strada); |
for (k=0;k<LUNGLENTA;k++) |
{ |
if ((oy<CENTROCORSIA1) && (oy>CENTROCORSIA2)) |
{ |
corsia1[ox-k]=0; |
corsia2[ox-k]=0; |
} |
else |
if (oy==CENTROCORSIA1) |
corsia1[ox-k]=0; |
else |
if (oy==CENTROCORSIA2) |
corsia2[ox-k]=0; |
} |
sem_post(&strada); |
/* cancella tutte le vecchie informazioni che compaiono sullo schermo */ |
sprintf(stri,"posizione %d",(int)(ox*0.25)); |
sem_wait(&mutex); |
grx_text(stri,70,50+i*10,rgb16(0,0,0),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"a %nf",oa); |
sem_wait(&mutex); |
grx_text(ostri,300,50+i*10,rgb16(0,0,0),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"v %nf",ov); |
sem_wait(&mutex); |
grx_text(ostri,200,50+i*10,rgb16(0,0,0),rgb16(0,0,0)); |
sem_post(&mutex); |
/* legge la distanza dal sensore anteriore e nel caso in cui sia attaccato alla macchina che precede vengo eliminato */ |
precedentedritto=dist_ant(x,y,sensore_ant); |
if (precedentedritto==1) |
task_abort(i); |
/* disegna il veicolo nella nuova posizione */ |
if (abs(xelic-x)<estremo) |
draw_veicolo(x-xelic+estremo, y, rgb16(255,0,0),LUNGLENTA,LARGLENTA); |
/* varia la massima distanza vista dal sensore posteriore sinistro a seconda della velocita' */ |
tot=(int)(v*4); |
if (tot<50) |
tot=50; |
if (precedentedritto<100) /* ci siamo avvicinando troppo alla macchina che ci precede */ |
if ((sorpasso==0) && (dist_postsx(x,y,200)>tot)&&(dist_ant_sx(x,y,50)>40)) |
{ |
/* c'e' uno davanti e non sopraggiunge nessuno sulla corsia di sorpasso */ |
sorpasso=1; |
rientro=0; |
vcrociera=vmax2; |
} |
else |
a=frenata; /* c'e qualcuno davanti ,ma non possiamo sorpassare */ |
else /* siamo lontani da un veicolo che ci precede o non ci precede nessuno */ |
if (v<vcrociera) /* accelleriamo gradualmente fino a portarci alla velocita' di crociera desiderata */ |
a=amax-(amax/vcrociera)*v; |
else |
if(v>vcrociera) /* freniamo per portarci al valore della velocita'di crociera */ |
a=frenata/3; |
else |
a=0.0; /* siamo alla velocita' di crociera */ |
/* controlla se e' possibile la manovra di rientro dal sorpasso, aggiustando tutti i parametri nel modo corretto */ |
if ((dist_postdx(x,y,200)>tot2) && (dist_ant_dx(x,y,200)>tot3) && (sorpasso==1)) |
{ |
rientro=1; |
sorpasso=0; |
vcrociera=vmax1; |
} |
/* aggiusta la cordinata y per far visualizzare il sorpasso o il rientro della macchina in modo graduale */ |
if(y>CENTROCORSIA2 && sorpasso==1) |
y=y-1; |
if(y<CENTROCORSIA1 && rientro==1) |
y=y+1; |
/* calcola la velocita' e nel caso di v elocita' negative la pone a zero (non sono permesse le retromarcie) */ |
v=v+a*DT; |
if (v<0) |
v=0.0; |
/* calcola la cordinata x a cui si trova la macchina */ |
x=x+(int)((v*DT)/0.25); |
/* scrive a video i nuovi parametri appena calcolati */ |
sprintf(stri,"posizione %d",(int)(x*0.25)); |
sem_wait(&mutex); |
grx_text(stri,70,50+i*10,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"v %nf",v); |
sem_wait(&mutex); |
grx_text(ostri,200,50+i*10,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"a %nf",a); |
sem_wait(&mutex); |
grx_text(ostri,300,50+i*10,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
/* quando il veicolo arriva alla fine dell'autostrada viene eliminato */ |
if (x>=40000) |
task_abort(i); |
/* salva la nuova posizione del veicolo */ |
sem_wait(&strada); |
for (k=0;k<LUNGLENTA;k++) |
{ |
if ((y<CENTROCORSIA1) && (y>CENTROCORSIA2)) |
{ |
corsia2[x-k]=1; |
corsia1[x-k]=1; |
} |
else |
if (y==CENTROCORSIA1) |
corsia1[x-k]=1; |
else |
if (y==CENTROCORSIA2) |
corsia2[x-k]=1; |
} |
sem_post(&strada); |
/* salvo i parametri che occorrono per il prossimo ciclo */ |
oxelic=xelic; |
od=precedentedritto; |
ov=v; |
oa=a; |
task_endcycle(); /* termina le operazioni che il task deve eseguire */ |
} |
} |
/* ----------------------------------------------------------------------------- */ |
/************************** TASK AUTO VELOCE *******************************/ |
TASK auto_veloce(void *arg) |
{ |
int x; /* posizione x assunta dal veicolo */ |
int y; /* posizione y assunta dal veicolo */ |
int oxelic; /* posizione vecchia dell'elicottero */ |
int ox; /* posizione vecchia x dall'auto veloce */ |
int oy; /* posizione vecchia y dall'auto veloce */ |
int k; /* indice di ciclo */ |
int estremo; /* distanza massima a cui ci possiamo spostare dall'elicottero (sia a destra che a sinistra) */ |
int sorpasso; /* indica se il veicolo e' in fase di sorpasso */ |
int rientro; /* indica se il veicolo e' in fase di rientro */ |
int precedentedritto; /* distanza anteriore da un veicolo */ |
int od; /* vecchia distanza anteriore da un veicolo */ |
int tot; /* distanza massima raggiungibile del sensore posteriore sinistro */ |
int tot3; /* distanza massima raggiungibile del sensore anteriore destro */ |
int tot2; /* distanza massima raggiungibile del sensore posteriore destro */ |
int i = (int)arg; |
int sensore_ant=200; /* distanza massima raggiungibile del sensore anteriore */ |
char stri[22]; /* vettore di caratteri */ |
char ostri[22]; /* vettore di caratteri */ |
float vcrociera; /* velocita' desiderata */ |
float vmax1=35.0; /* velocita' massima raggiungibile in prima corsia */ |
float vmax2=40.0; /* velocita' massima raggiungibile in seconda corsia */ |
float frenata=-7.0; /* valore massimo della frenata */ |
float amax=15.0; /* valore massimo del'accellerazione */ |
float v; /* velocita' attuale */ |
float a; /* accellerazione attuale */ |
float ov; /* velocita' precedente */ |
float oa; /* accellerazione precedente */ |
/* inizializzazione delle variabili */ |
v=5.0; |
a=0.0; |
x=LUNGVELOCE; |
y=CENTROCORSIA1; |
sorpasso=0; |
rientro=0; |
tot=100; |
tot2=150; |
tot3=100; |
oa=a; |
vcrociera=vmax1; |
ov=v; |
od=0; |
oxelic=xelic; |
estremo=(int)((MAX_X/2)-1); |
while (1) |
{ |
/* cancella il veicolo se era presente sull'autostrada vista dall'elicottero */ |
if (abs(oxelic-ox)<estremo) |
draw_veicolo(ox-oxelic+estremo, oy, 0,LUNGVELOCE,LARGVELOCE); |
/* salva le vecchie coordinate */ |
ox = x; |
oy = y; |
/* cancella il veicolo dalla vecchia posizione sulla corsia , andando a cancellarlo nel vettore della corsia in cui si trovava */ |
sem_wait(&strada); |
for (k=0;k<LUNGVELOCE;k++) |
{ |
if ((oy<CENTROCORSIA1) && (oy>CENTROCORSIA2)) |
{ |
corsia1[ox-k]=0; |
corsia2[ox-k]=0; |
} |
else |
if (oy==CENTROCORSIA1) |
corsia1[ox-k]=0; |
else |
if (oy==CENTROCORSIA2) |
corsia2[ox-k]=0; |
} |
sem_post(&strada); |
/* cancella tutte le vecchie informazioni che compaiono sullo schermo */ |
sprintf(stri,"posizione %d",(int)(ox*0.25)); |
sem_wait(&mutex); |
grx_text(stri,70,50+i*10,rgb16(0,0,0),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"v %nf",ov); |
sem_wait(&mutex); |
grx_text(ostri,200,50+i*10,rgb16(0,0,0),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"a %nf",oa); |
sem_wait(&mutex); |
grx_text(ostri,300,50+i*10,rgb16(0,0,0),rgb16(0,0,0)); |
sem_post(&mutex); |
/* legge la distanza dal sensore anteriore e nel caso in cui sia attaccato alla macchina che precede vengo eliminato */ |
precedentedritto=dist_ant(x,y,sensore_ant); |
if (precedentedritto==1) |
task_abort(i); |
/* disegna il veicolo nella nuova posizione */ |
if (abs(xelic-x)<estremo) |
draw_veicolo(x-xelic+estremo, y, rgb16(0,255,0),LUNGVELOCE,LARGVELOCE); |
/* varia la massima distanza vista dal sensore posteriore sinistro a seconda della velocita' */ |
tot=(int)(v*4); |
if (tot<50) |
tot=50; |
if (precedentedritto<150) /* ci siamo avvicinando troppo alla macchina che ci precede */ |
if ((sorpasso==0) && (dist_postsx(x,y,200)>tot)&&(dist_ant_sx(x,y,50)>40)) |
{ |
/* c'e' uno davanti e non sopraggiunge nessuno sulla corsia di sorpasso */ |
sorpasso=1; |
rientro=0; |
vcrociera=vmax2; |
} |
else /* c'e qualcuno davanti ,ma non possiamo sorpassare */ |
a=frenata; |
else /* siamo lontani da un veicolo che ci precede o non ci precede nessuno */ |
if (v<vcrociera) /* accelleriamo gradualmente fino a portarci alla velocita' di crociera desiderata */ |
a=amax-(amax/vcrociera)*v; |
else |
if(v>vcrociera) /* freniamo per portarci al valore della velocita'di crociera */ |
a=frenata/3; |
else |
a=0.0; /* siamo alla velocita' di crociera */ |
/* controlla se e' possibile la manovra di rientro dal sorpasso, aggiustando tutti i parametri nel modo corretto */ |
if ((dist_postdx(x,y,200)>tot2) && (dist_ant_dx(x,y,200)>tot3) && (sorpasso==1)) |
{ |
rientro=1; |
sorpasso=0; |
vcrociera=vmax1; |
} |
/* aggiusta la cordinata y per far visualizzare il sorpasso o il rientro della macchina in modo graduale */ |
if(y>CENTROCORSIA2 && sorpasso==1) |
y=y-1; |
if(y<CENTROCORSIA1 && rientro==1) |
y=y+1; |
/* calcola la velocita' e nel caso di v elocita' negative la pone a zero (non sono permesse le retromarcie) */ |
v=v+a*DT; |
if (v<0) |
v=0.0; |
/* calcola la cordinata x a cui si trova la macchina */ |
x=x+(int)((v*DT)/0.25); |
/* scrive a video i nuovi parametri appena calcolati */ |
sprintf(stri,"posizione %d",(int)(x*0.25)); |
sem_wait(&mutex); |
grx_text(stri,70,50+i*10,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"v %nf",v); |
sem_wait(&mutex); |
grx_text(ostri,200,50+i*10,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"a %nf",a); |
sem_wait(&mutex); |
grx_text(ostri,300,50+i*10,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
/* quando il veicolo arriva alla fine dell'autostrada viene eliminato */ |
if (x>=40000) |
task_abort(i); |
/* salva la nuova posizione del veicolo */ |
sem_wait(&strada); |
for (k=0;k<LUNGVELOCE;k++) |
{ |
if ((y<CENTROCORSIA1) && (y>CENTROCORSIA2)) |
{ |
corsia2[x-k]=1; |
corsia1[x-k]=1; |
} |
else |
if (y==CENTROCORSIA1) |
corsia1[x-k]=1; |
else |
if (y==CENTROCORSIA2) |
corsia2[x-k]=1; |
} |
sem_post(&strada); |
/* salvo i parametri che occorrono per il prossimo ciclo */ |
oxelic=xelic; |
od=precedentedritto; |
ov=v; |
oa=a; |
task_endcycle(); /* termina le operazioni che il task deve eseguire */ |
} |
} |
/* ----------------------------------------------------------------------------- */ |
/***************************** TASK AUTOCARRO ******************************/ |
TASK auto_carro(void *arg) |
{ |
int x; /* posizione x assunta dal veicolo */ |
int y; /* posizione y assunta dal veicolo */ |
int oxelic; /* posizione vecchia dell'elicottero */ |
int ox; /* posizione vecchia x assunta dal camion */ |
int oy; /* posizione vecchia y assunta dal camion */ |
int k; /* indice di ciclo */ |
int estremo; /* distanza massima a cui ci possiamo spostare dall'elicottero (sia a destra che a sinistra) */ |
int sorpasso; /* indica se il veicolo e' in fase di sorpasso */ |
int rientro; /* indica se il veicolo e' in fase di rientro */ |
int precedentedritto; /* distanza anteriore da un veicolo */ |
int od; /* vecchia distanza anteriore da un veicolo */ |
int tot; /* distanza massima raggiungibile del sensore posteriore sinistro */ |
int tot3; /* distanza massima raggiungibile del sensore anteriore destro */ |
int tot2; /* distanza massima raggiungibile del sensore posteriore destro */ |
int i = (int)arg; |
int sensore_ant=200; /* distanza massima raggiungibile del sensore anteriore */ |
char stri[22]; /* vettore di caratteri */ |
char ostri[22]; /* vettore di caratteri */ |
float vcrociera; /* velocita' desiderata */ |
float vmax1=15.0; /* velocita' massima raggiungibile in prima corsia */ |
float vmax2=20.0; /* velocita' massima raggiungibile in seconda corsia */ |
float frenata=-5.0; /* valore massimo della frenata */ |
float amax=3.0; /* valore massimo del'accellerazione */ |
float v; /* velocita' attuale */ |
float a; /* accellerazione attuale */ |
float ov; /* velocita' precedente */ |
float oa; /* accellerazione precedente */ |
/* inizializzazione delle variabili */ |
v=5.0; |
a=0.0; |
x=LUNGCAMION; |
y=CENTROCORSIA1; |
sorpasso=0; |
rientro=0; |
tot=100; |
tot2=150; |
tot3=100; |
oa=a; |
vcrociera=vmax1; |
ov=v; |
od=0; |
oxelic=xelic; |
estremo=(int)((MAX_X/2)-1); |
while (1) |
{ |
/* cancella il veicolo se era presente sull'autostrada vista dall'elicottero */ |
if (abs(oxelic-ox)<estremo) |
draw_veicolo(ox-oxelic+estremo, oy, 0,LUNGCAMION,LARGCAMION); |
/* salva le vecchie coordinate */ |
ox = x; |
oy = y; |
/* cancella il veicolo dalla vecchia posizione sulla corsia , andando a cancellarlo nel vettore della corsia in cui si trovava */ |
sem_wait(&strada); |
for (k=0;k<LUNGCAMION;k++) |
{ |
if ((oy<CENTROCORSIA1) && (oy>CENTROCORSIA2)) |
{ |
corsia1[ox-k]=0; |
corsia2[ox-k]=0; |
} |
else |
if (oy==CENTROCORSIA1) |
corsia1[ox-k]=0; |
else |
if (oy==CENTROCORSIA2) |
corsia2[ox-k]=0; |
} |
sem_post(&strada); |
/* cancella tutte le vecchie informazioni che compaiono sullo schermo */ |
sprintf(stri,"posizione %d",(int)(ox*0.25)); |
sem_wait(&mutex); |
grx_text(stri,70,50+i*10,rgb16(0,0,0),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"v %nf",ov); |
sem_wait(&mutex); |
grx_text(ostri,200,50+i*10,rgb16(0,0,0),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"a %nf",oa); |
sem_wait(&mutex); |
grx_text(ostri,300,50+i*10,rgb16(0,0,0),rgb16(0,0,0)); |
sem_post(&mutex); |
/* legge la distanza dal sensore anteriore e nel caso in cui sia attaccato alla macchina che precede vengo eliminato */ |
precedentedritto=dist_ant(x,y,sensore_ant); |
if (precedentedritto==1) |
task_abort(i); |
/* disegna il veicolo nella nuova posizione */ |
if (abs(xelic-x)<estremo) |
draw_veicolo(x-xelic+estremo, y, rgb16(0,0,255),LUNGCAMION,LARGCAMION); |
/* varia la massima distanza vista dal sensore posteriore sinistro a seconda della velocita' */ |
tot=(int)(v*4); |
if (tot<50) |
tot=50; |
if (precedentedritto<100) /* ci siamo avvicinando troppo alla macchina che ci precede */ |
if ((sorpasso==0) && (dist_postsx(x,y,200)>tot)&&(dist_ant_sx(x,y,50)>40)) |
{ |
/* c'e' uno davanti e non sopraggiunge nessuno sulla corsia di sorpasso */ |
sorpasso=1; |
rientro=0; |
vcrociera=vmax2; |
} |
else /* c'e qualcuno davanti ,ma non possiamo sorpassare */ |
a=frenata; |
else /* siamo lontani da un veicolo che ci precede o non ci precede nessuno */ |
if (v<vcrociera) /* accelleriamo gradualmente fino a portarci alla velocita' di crociera desiderata */ |
a=amax-(amax/vcrociera)*v; |
else |
if(v>vcrociera) /* freniamo per portarci al valore della velocita'di crociera */ |
a=frenata/3; |
else |
a=0.0; /* siamo alla velocita' di crociera */ |
/* controlla se e' possibile la manovra di rientro dal sorpasso, aggiustando tutti i parametri nel modo corretto */ |
if ((dist_postdx(x,y,200)>tot2) && (dist_ant_dx(x,y,200)>tot3) && (sorpasso==1)) |
{ |
rientro=1; |
sorpasso=0; |
vcrociera=vmax1; |
} |
/* aggiusta la cordinata y per far visualizzare il sorpasso o il rientro della macchina in modo graduale */ |
if(y>CENTROCORSIA2 && sorpasso==1) |
y=y-1; |
if(y<CENTROCORSIA1 && rientro==1) |
y=y+1; |
/* calcola la velocita' e nel caso di v elocita' negative la pone a zero (non sono permesse le retromarcie) */ |
v=v+a*DT; |
if (v<0) |
v=0.0; |
/* calcola la cordinata x a cui si trova la macchina */ |
x=x+(int)((v*DT)/0.25); |
/* scrive a video i nuovi parametri appena calcolati */ |
sprintf(stri,"posizione %d",(int)(x*0.25)); |
sem_wait(&mutex); |
grx_text(stri,70,50+i*10,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"v %nf",v); |
sem_wait(&mutex); |
grx_text(ostri,200,50+i*10,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(ostri,"a %nf",a); |
sem_wait(&mutex); |
grx_text(ostri,300,50+i*10,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
/* quando il veicolo arriva alla fine dell'autostrada viene eliminato */ |
if (x>=40000) |
task_abort(i); |
/* salva la nuova posizione del veicolo */ |
sem_wait(&strada); |
for (k=0;k<LUNGCAMION;k++) |
{ |
if ((y<CENTROCORSIA1) && (y>CENTROCORSIA2)) |
{ |
corsia2[x-k]=1; |
corsia1[x-k]=1; |
} |
else |
if (y==CENTROCORSIA1) |
corsia1[x-k]=1; |
else |
if (y==CENTROCORSIA2) |
corsia2[x-k]=1; |
} |
sem_post(&strada); |
/* salvo i parametri che occorrono per il prossimo ciclo */ |
oxelic=xelic; |
od=precedentedritto; |
ov=v; |
oa=a; |
task_endcycle(); /* termina le operazioni che il task deve eseguire */ |
} |
} |
/* ----------------------------------------------------------------------------- */ |
/***************************** TASK ELICOTTERO *****************************/ |
TASK eli_cottero(void *arg) |
{ |
int i = (int)arg; |
int oxelic; /* vecchia posizione dell'elicottero */ |
char stri[22]; /* vettore di caratteri */ |
char ostri[22]; /* vettore di caratteri */ |
/* disegna le scritte per l'indicatore a barra e finestra della posizione dell'elicottero */ |
sprintf(stri,"0 Km"); |
sem_wait(&mutex); |
grx_text(stri,100,560,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(stri,"10 Km"); |
sem_wait(&mutex); |
grx_text(stri,700,560,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
while (1) |
{ |
oxelic=xelic; /* salva la posizione precedente dell'elicottero */ |
if (c == '+') /* sposta l'elicottero a destra (verso la fine dell'autostrada */ |
{ |
xelic=xelic+50; |
if (xelic>40000-(int)((MAX_X/2)-1)) |
xelic=40000-(int)((MAX_X/2)-1); |
} |
else |
if (c =='-') /* sposta l'elicottero a sinistra (verso l'inizio dell'autostrada */ |
{ |
xelic=xelic-30; |
if (xelic<(int)((MAX_X/2)-1)) |
xelic=(int)((MAX_X/2)-1); |
} |
/* disegna le scritte innerenti all'elicottero e il valore della sua posizione (mediante scritta ed indicatore a barra con una finestra scorrevole) */ |
sprintf(stri,"- <- xelic %d -> +",(int)(xelic*0.25)); |
sprintf(ostri,"- <- xelic %d -> +",(int)(oxelic*0.25)); |
sem_wait(&mutex); |
grx_text(ostri,MAX_X/2-50,500,rgb16(0,0,0),rgb16(0,0,0)); |
grx_text(stri,MAX_X/2-50,500,rgb16(255,255,255),rgb16(0,0,0)); |
grx_line(150,560,650,560,rgb16(255,255,255)); |
grx_rect((int)((oxelic-(MAX_X/2))/80+150),540,(int)((oxelic+(MAX_X/2))/80+150) ,580 ,rgb16(0,0,0)); |
grx_rect((int)((xelic-(MAX_X/2))/80+150),540,(int)((xelic+(MAX_X/2))/80+150) ,580 ,rgb16(255,255,255)); |
sem_post(&mutex); |
c=' '; /* setta il carattere c */ |
task_endcycle(); /* termina le operazioni che il task deve eseguire */ |
} |
} |
/* -----------------------------------------------------------------------------*/ |
/************* FUNZIONE DI USCITA DAL SISTEMA ******************************/ |
void byebye(void *arg) |
{ /* questa funzione e' chiamata quando il sistema esce */ |
grx_close(); /* chiude la grafica */ |
kern_printf("Ciao Ciao "); /* scrive il messaggio indicato sul terminale */ |
} |
/* -----------------------------------------------------------------------------*/ |
/********************************* MAIN ************************************/ |
int main(int argc, char **argv) |
{ |
int n_task = 0; /* numero di task creati */ |
int u; |
int v; |
char introduzione[100]; /* vettore di caratteri */ |
HARD_TASK_MODEL autolenta; /* task auto lenta */ |
HARD_TASK_MODEL autoveloce; /* task auto veloce */ |
HARD_TASK_MODEL autocarro; /* task camion */ |
HARD_TASK_MODEL elicottero; /* task elicottero */ |
/* inizializza le corsie dell'autostrada */ |
sem_wait(&strada); |
for (u=0;u<=40000;u++) |
{ |
corsia1[u]=0; |
corsia2[u]=0; |
} |
sem_post(&strada); |
/* Set the exception handler */ |
//set_exchandler_grx(); |
/* Set the closing function */ |
sys_atrunlevel(byebye, NULL, RUNLEVEL_BEFORE_EXIT); |
/* inizializzazione grafica */ |
if (grx_init() < 1) |
sys_abort(1); |
/* scelta automatica della risoluzione applicabile con quella scheda video (scegliendola tra 800*600 e 1024*768 ) */ |
if(grx_getmode(1024,768,16)==-1) |
{ |
if (grx_open(800, 600, 16) < 0) |
{ |
kern_printf("GRX Err\n"); |
sys_abort(1); |
} |
MAX_X=800; |
MAX_Y=600; |
} |
else |
{ |
if (grx_open(1024,768, 16) < 0) |
{ |
kern_printf("GRX Err\n"); |
sys_abort(1); |
} |
MAX_X=1024; |
MAX_Y=768; |
} |
kern_printf("La scheda video va'!!\n"); |
/* posizione iniziale elicottero */ |
xelic=(int)((MAX_X/2)-1); |
/* disegna lo scenario della strada ed il menu */ |
sprintf(introduzione,"Monitoraggio dei mezzi che transitano su una autostrada."); |
sem_wait(&mutex); |
grx_text(introduzione,50,10,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(introduzione,"Sviluppato da: Verzellesi Quadrubbi"); |
sem_wait(&mutex); |
grx_text(introduzione,50,20,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(introduzione,"MENU"); |
sem_wait(&mutex); |
grx_text(introduzione,480,70,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(introduzione,"s = macchina sportiva"); |
sem_wait(&mutex); |
grx_text(introduzione,480,80,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(introduzione,"c = mezzo pesante"); |
sem_wait(&mutex); |
grx_text(introduzione,480,90,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(introduzione,"l = macchina lenta"); |
sem_wait(&mutex); |
grx_text(introduzione,480,100,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(introduzione,"+ = sposta l'elicottero verso destra"); |
sem_wait(&mutex); |
grx_text(introduzione,480,110,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(introduzione,"- = sposta l'elicottero verso sinistra"); |
sem_wait(&mutex); |
grx_text(introduzione,480,120,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(introduzione,"esc = uscita"); |
sem_wait(&mutex); |
grx_text(introduzione,480,130,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(introduzione,"NOTA"); |
sem_wait(&mutex); |
grx_text(introduzione,480,140,rgb16(255,0,0),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(introduzione,"Se i veicoli tamponano "); |
sem_wait(&mutex); |
grx_text(introduzione,480,150,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
sprintf(introduzione," vengono eliminati"); |
sem_wait(&mutex); |
grx_text(introduzione,480,160,rgb16(255,255,255),rgb16(0,0,0)); |
sem_post(&mutex); |
grx_line(1,450,MAX_X,450,rgb16(255,255,255)); |
grx_line(1,486,MAX_X,486,rgb16(255,255,255)); |
for (u=0;u<MAX_X;u++) |
{ |
v=u%8; |
if ((v==0) || (v==1)) |
{ |
sem_wait(&mutex); |
grx_plot(u,CENTROCARREGGIATA,rgb16(255,255,255)); |
sem_post(&mutex); |
} |
} |
n_task=0; /* inizializzazione del numero dei task */ |
/* definisce e crea il task elicottero */ |
kern_printf("elicottero"); |
hard_task_default_model (elicottero); |
hard_task_def_ctrl_jet (elicottero); |
hard_task_def_arg (elicottero, (void *)n_task); |
hard_task_def_wcet (elicottero,wcet); |
hard_task_def_mit (elicottero, periodo); |
hard_task_def_group (elicottero, GRUPPO); |
hard_task_def_usemath (elicottero); |
pid = task_create ("elicottero",eli_cottero, &elicottero, NULL); |
if (pid == NIL) |
{ |
grx_close(); |
perror("Non si puo' creare il task"); |
sys_abort(1); |
} |
task_activate(pid); |
n_task=1; /* incremente il numero dei task (ha creato l'elicottero */ |
/*Attesa di un carattere per creare un veicolo */ |
c = keyb_getch(BLOCK); |
do { |
if (((c == 'c')||(c=='s')||(c=='l')) && (n_task < MAX_V)) /* in base al tasto premuto crea il task opportuno */ |
{ |
if (c == 'l') /* definisce e crea il task autolenta */ |
{ |
kern_printf("lenta"); |
hard_task_default_model (autolenta); |
hard_task_def_ctrl_jet (autolenta); |
hard_task_def_arg (autolenta, (void *)n_task); |
hard_task_def_wcet (autolenta, wcet); |
hard_task_def_mit (autolenta, periodo); |
hard_task_def_group (autolenta, GRUPPO); |
hard_task_def_usemath (autolenta); |
pid = task_create ("autolenta",auto_lenta, &autolenta, NULL); |
} |
else |
if (c == 's') /* definisce e crea il task autoveloce */ |
{ |
hard_task_default_model (autoveloce); |
hard_task_def_ctrl_jet (autoveloce); |
hard_task_def_arg (autoveloce, (void *)n_task); |
hard_task_def_wcet (autoveloce, wcet); |
hard_task_def_mit (autoveloce, periodo); |
hard_task_def_group (autoveloce, GRUPPO); |
hard_task_def_usemath (autoveloce); |
pid = task_create ("autoveloce",auto_veloce, &autoveloce, NULL); |
} |
else |
if (c == 'c') /* definisce e crea il task autocarro */ |
{ |
hard_task_default_model (autocarro); |
hard_task_def_ctrl_jet (autocarro); |
hard_task_def_arg (autocarro, (void *)n_task); |
hard_task_def_wcet (autocarro, wcet); |
hard_task_def_mit (autocarro, periodo); |
hard_task_def_group (autocarro, GRUPPO); |
hard_task_def_usemath (autocarro); |
pid = task_create ("camion",auto_carro, &autocarro, NULL); |
} |
if (pid == NIL) /* nel caso in non si possano creare dei task chiude la grafica e con un messaggio segnala l'errore */ |
{ |
grx_close(); |
perror("Non si puo' creare il task"); |
sys_abort(1); |
} |
task_activate(pid); /* attiva i task */ |
n_task++; /* incrementa il numero dei task creati */ |
} |
c = keyb_getch(BLOCK); |
} while (c != ESC); /* termino il tutto solo quando e' stato premuto il tasto esc */ |
sys_end(); /* esco dal sistema */ |
return 0; |
} |
/demos/trunk/autostr/initfile.c |
---|
0,0 → 1,120 |
/* |
* 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: initfile.c,v 1.1 2003-06-04 09:41:15 giacomo Exp $ |
File: $File$ |
Revision: $Revision: 1.1 $ |
Last update: $Date: 2003-06-04 09:41:15 $ |
------------ |
System initialization file |
This file contains the 2 functions needed to initialize the system. |
These functions register the following levels: |
an EDF (Earliest Deadline First) level |
a RR (Round Robin) level |
a CBS (Costant Bandwidth Server) level |
a Dummy level |
It can accept these task models: |
HARD_TASK_MODEL (wcet+mit) at level 0 |
SOFT_TASK_MODEL (met, period) at level 1 |
NRT_TASK_MODEL at level 2 |
This file is similar to the configuration of kernel/init/hartik3.c |
TICK is set to 0 (one-shot timer is used) |
*/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* 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 "kernel/kern.h" |
#include "modules/edf.h" |
#include "modules/cbs.h" |
#include "modules/rr.h" |
#include "modules/dummy.h" |
#include "modules/sem.h" |
#include "modules/hartport.h" |
#include "modules/cabs.h" |
#include "drivers/keyb.h" |
/*+ sysyem tick in us +*/ |
#define TICK 0 |
/*+ RR tick in us +*/ |
#define RRTICK 10000 |
TIME __kernel_register_levels__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
EDF_register_level(EDF_ENABLE_ALL); |
CBS_register_level(CBS_ENABLE_ALL, 0); |
RR_register_level(RRTICK, RR_MAIN_YES, mb); |
dummy_register_level(); |
SEM_register_module(); |
CABS_register_module(); |
return TICK; |
} |
TASK __init__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
KEYB_PARMS kparms = BASE_KEYB; |
HARTPORT_init(); |
keyb_def_ctrlC(kparms, NULL); |
keyb_def_map(kparms,itaMap); |
KEYB_init(&kparms); |
__call_main__(mb); |
return (void *)0; |
} |
/demos/trunk/autostr/makefile |
---|
0,0 → 1,17 |
# |
# |
# |
ifndef BASE |
BASE=../.. |
endif |
include $(BASE)/config/config.mk |
PROGS= autostr |
include $(BASE)/config/example.mk |
autostr: |
make -f $(SUBMAKE) APP=autostr INIT= OTHEROBJS="initfile.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __GRX__" |
/demos/trunk/biliardo/initfil1.c |
---|
0,0 → 1,70 |
#include "kernel/kern.h" |
#include "modules/edf.h" |
#include "modules/cbs.h" |
#include "modules/rr.h" |
#include "modules/dummy.h" |
#include "modules/sem.h" |
#include "modules/hartport.h" |
#include "modules/cabs.h" |
#include "modules/pi.h" |
#include "modules/pc.h" |
#include "modules/srp.h" |
#include "modules/npp.h" |
#include "modules/nop.h" |
#include "drivers/keyb.h" |
/*+ sysyem tick in us +*/ |
#define TICK 0 |
/*+ RR tick in us +*/ |
#define RRTICK 2000 |
TIME __kernel_register_levels__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
EDF_register_level(EDF_ENABLE_ALL); |
CBS_register_level(CBS_ENABLE_ALL, 0); |
RR_register_level(RRTICK, RR_MAIN_YES, mb); |
dummy_register_level(); |
SEM_register_module(); |
CABS_register_module(); |
PI_register_module(); |
NOP_register_module(); |
return TICK; |
} |
TASK __init__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
KEYB_PARMS kparms = BASE_KEYB; |
HARTPORT_init(); |
keyb_def_ctrlC(kparms, NULL); |
KEYB_init(&kparms); |
__call_main__(mb); |
return (void *)0; |
} |
void app_mutex_init(mutex_t *m) |
{ |
PI_mutexattr_t attr; |
PI_mutexattr_default(attr); |
mutex_init(m, &attr); |
} |
/demos/trunk/biliardo/posizion.h |
---|
0,0 → 1,11 |
// definizione della struttura contenente la posizione delle palle |
struct posizione { |
float x; |
float y; |
float v; |
float theta; |
int col; |
} ; |
/demos/trunk/biliardo/biliardo.c |
---|
0,0 → 1,240 |
/* |
* 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: biliardo.c,v 1.1 2003-06-04 09:41:32 giacomo Exp $ |
File: $File$ |
Revision: $Revision: 1.1 $ |
Last update: $Date: 2003-06-04 09:41:32 $ |
------------ |
**/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* 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 "biliardo.h" |
#include <kernel/func.h> |
#include <string.h> |
#include <stdlib.h> |
#include <drivers/keyb.h> |
#include <drivers/glib.h> |
/* graphic mutex... */ |
mutex_t mutex; |
mutex_t palmutex; |
mutex_t delmutex; |
//mutex_t punmutex; |
/* useful colors... */ |
int white; |
int black; |
int red; |
int gray; |
int green; |
int lime; |
int brown; |
void app_mutex_init(mutex_t *m); |
static void version( void ) |
{ |
cprintf( "S.Ha.R.K. San Martino Sicc. Demo 1.0\n" ); |
} |
int myrand(int x) |
{ |
return rand()%x; |
} |
void reverse(char s[]) |
{ |
int c, i, j; |
for (i = 0, j = strlen(s)-1; i<j; i++, j--) |
{ |
c = s[i]; |
s[i] = s[j]; |
s[j] = c; |
} |
} |
char * itoa(int n, char *s) |
{ |
int i, sign; |
if ((sign = n) < 0) |
n = -n; |
i = 0; |
do |
{ |
s[i++] = n % 10 + '0'; |
} while ((n /= 10) > 0); |
if (sign < 0) |
s[i++] = '-'; |
s[i] = 0; |
reverse(s); |
return s; |
} |
void scenario() |
{ |
grx_text("S.Ha.R.K. - Biliardo rivisitato", 322,2, white, black ); |
grx_text("q : uscita",322, 18, gray, black ); |
grx_text("i : inizio partita",322,28, gray, black ); |
grx_text("Space : scocca il colpo" ,322,38, gray, black ); |
grx_text("a,z : calibra la forza" ,322,48, gray, black ); |
grx_text("Freccie : posiziona cursore" ,322,58, gray, black ); |
grx_text("Backspace : nuova partita" ,322,68, gray, black ); |
#ifdef JET_ON |
scenario_jetcontrol(); |
#endif |
#ifdef BALL_ON |
scenario_ball(); |
#endif |
} |
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) :%d:%d" |
"\nException number:%d" |
"\nPID :%d\n", |
t.tv_sec, t.tv_nsec, info->si_value.sival_int, |
info->si_task); |
sys_end(); |
} |
void my_close(void *arg) |
{ |
grx_close(); |
kern_printf("Termine programma\n"); |
} |
void endfun(KEY_EVT *k) |
{ |
cprintf("Tasto q premuto, termine programma\n"); |
sys_end(); |
} |
void zerofun(KEY_EVT *k) |
{ |
int i; |
for (i=0; i<MAX_PROC; i++) jet_delstat(i); |
} |
void printeventqueue(void *arg) |
{ |
struct event *p; |
extern struct event *firstevent; |
kern_cli(); |
grx_close(); |
kern_cli(); |
for (p = firstevent; p != NULL; p = p->next) { |
kern_printf("par:%d time:%d.%d p:%d handler:%d\n", |
p->par, p->time.tv_sec, p->time.tv_nsec/1000, p, p->handler); |
} |
kern_sti(); |
} |
int main(int argc, char **argv) |
{ |
int modenum; |
KEY_EVT k; |
// Evento di termine programma |
k.flag = 0; |
k.scan = KEY_Q; |
k.ascii = 'q'; |
keyb_hook(k,endfun); |
//set_exchandler_grx(); |
sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT); |
grx_init(); |
modenum = grx_getmode(640, 480, 16); |
grx_setmode(modenum); |
/* init the graphic mutex */ |
app_mutex_init(&mutex); |
/* useful colors ... */ |
white = rgb16(255,255,255); |
black = rgb16(0,0,0); |
red = rgb16(255,0,0); |
gray = rgb16(128,128,128); |
green = rgb16(0,128,0); |
lime = rgb16(0,255,0); |
brown = rgb16(128,0,0); |
scenario(); |
init_jetcontrol(); |
app_mutex_init(&palmutex); |
app_mutex_init(&delmutex); |
init_ball(); |
initSched(); |
group_activate(JET_GROUP); |
return 0; |
} |
/demos/trunk/biliardo/jetctrl.c |
---|
0,0 → 1,194 |
// ****************** JetControl ****************** |
#include "biliardo.h" |
#include "kernel/func.h" |
TASK jetdummy_task(void *arg) |
{ |
TIME now_dummy, last_dummy, diff_dummy, slice; |
struct timespec now, last, diff; |
int x = 0; |
int height; |
NULL_TIMESPEC(&last); |
last_dummy = 0; |
for (;;) { |
task_nopreempt(); |
jet_getstat(DUMMY_PID, NULL, NULL, NULL, &now_dummy); |
sys_gettime(&now); |
task_preempt(); |
SUBTIMESPEC(&now, &last, &diff); |
slice = diff.tv_sec * 1000000 + diff.tv_nsec/1000; |
diff_dummy = now_dummy - last_dummy; |
height = (int)(JET_DUMMY_HEIGHT*((float)diff_dummy)/((float)slice)); |
TIMESPEC_ASSIGN(&last, &now); |
last_dummy = now_dummy; |
mutex_lock(&mutex); |
grx_line(JET_DUMMY_X+x,JET_DUMMY_Y, |
JET_DUMMY_X+x,JET_DUMMY_Y+height ,black); |
grx_line(JET_DUMMY_X+x,JET_DUMMY_Y+height, |
JET_DUMMY_X+x,JET_DUMMY_Y+JET_DUMMY_HEIGHT,white); |
grx_line(JET_DUMMY_X+(x+1)%JET_DUMMY_WIDTH,JET_DUMMY_Y, |
JET_DUMMY_X+(x+1)%JET_DUMMY_WIDTH,JET_DUMMY_Y+JET_DUMMY_HEIGHT,white); |
mutex_unlock(&mutex); |
x = (x+1)%JET_DUMMY_WIDTH; |
task_endcycle(); |
} |
} |
TASK jetctrl_task(void *arg) |
{ |
char st[50]; |
TIME sum, max; |
int n; |
PID i; |
int printed = 0; |
for (;;) { |
for (i=2, printed=0; i<MAX_PROC && printed<JET_NTASK; i++) { |
if (jet_getstat(i, &sum, &max, &n, NULL) != -1) { |
if (!n) n=1; |
sprintf(st, "%6d %6d %10s ", (int)sum/n, (int)max, proc_table[i].name); |
mutex_lock(&mutex); |
grx_text(st, JET_DUMMY_X-45, JET_Y_NAME+16+printed*8, gray, black); |
mutex_unlock(&mutex); |
printed++; |
} |
} |
while (printed<JET_NTASK) { |
mutex_lock(&mutex); |
grx_text(" ", |
JET_DUMMY_X-45, JET_Y_NAME+16+printed*8, gray, black); |
mutex_unlock(&mutex); |
printed++; |
} |
task_endcycle(); |
} |
} |
TASK jetslide_task(void *arg) |
{ |
TIME sum, curr, max; |
TIME total[JET_NTASK]; |
int slides[JET_NTASK]; |
PID i; |
int printed = 0; |
for (;;) { |
// Fill the total array in a nonpreemptive section |
task_nopreempt(); |
for (i=2, printed=0; i<MAX_PROC && printed<JET_NTASK; i++) { |
if (jet_getstat(i, &sum, NULL, NULL, &curr) != -1) { |
total[printed] = sum+curr; |
printed++; |
} |
} |
task_preempt(); |
while (printed < JET_NTASK) |
total[printed++] = 0; |
// Compute the Max elapsed time |
max = 0; |
for (i=0; i<JET_NTASK; i++) |
if (total[i] > max) max = total[i]; |
if (!max) max = 1; |
// Compute the slides width |
for (i=0; i<JET_NTASK; i++) |
slides[i] = (int)( (((float)total[i])/max) * JET_SLIDE_WIDTH); |
// print the data |
mutex_lock(&mutex); |
for (i=0; i<JET_NTASK; i++) { |
grx_box(JET_SLIDE_X, JET_Y_NAME+16+i*8, |
JET_SLIDE_X+slides[i], JET_Y_NAME+23+i*8, gray); |
grx_box(JET_SLIDE_X+slides[i], JET_Y_NAME+16+i*8, |
JET_SLIDE_X+JET_SLIDE_WIDTH, JET_Y_NAME+23+i*8, black); |
} |
while (i<JET_NTASK) { |
grx_box(JET_SLIDE_X, JET_Y_NAME+16+i*8, |
JET_SLIDE_X+JET_SLIDE_WIDTH, JET_Y_NAME+20+i*8, black); |
i++; |
} |
mutex_unlock(&mutex); |
task_endcycle(); |
} |
} |
void scenario_jetcontrol(void) |
{ |
grx_text(" Medio Massimo Processo Slide", JET_DUMMY_X-40, JET_Y_NAME, gray, black); |
grx_line(JET_DUMMY_X-40,JET_Y_NAME+10,JET_DUMMY_X+250,JET_Y_NAME+10,lime); |
grx_rect(JET_DUMMY_X-1, JET_DUMMY_Y-1, |
JET_DUMMY_X+JET_DUMMY_WIDTH, JET_DUMMY_Y+JET_DUMMY_HEIGHT+1, lime); |
grx_text("100%", JET_DUMMY_X-40, JET_DUMMY_Y, lime, black); |
grx_text(" 0%", JET_DUMMY_X-40, JET_DUMMY_Y+JET_DUMMY_HEIGHT-8, lime, black); |
} |
void init_jetcontrol(void) |
{ |
SOFT_TASK_MODEL m3, m4, m5; |
PID p3, p4, p5; |
soft_task_default_model(m3); |
soft_task_def_level(m3,1); |
soft_task_def_period(m3, PERIOD_JETCTRL); |
soft_task_def_met(m3, WCET_JETCTRL); |
soft_task_def_ctrl_jet(m3); |
soft_task_def_group(m3, JET_GROUP); |
p3 = task_create("jctrl", jetctrl_task, &m3, NULL); |
if (p3 == -1) { |
grx_close(); |
perror("Could not create task <jetctrl>"); |
sys_end(); |
} |
soft_task_default_model(m4); |
soft_task_def_level(m4,1); |
soft_task_def_period(m4, PERIOD_JETDUMMY); |
soft_task_def_met(m4, WCET_JETDUMMY); |
soft_task_def_group(m4, JET_GROUP); |
soft_task_def_usemath(m4); |
soft_task_def_ctrl_jet(m4); |
p4 = task_create("jdmy", jetdummy_task, &m4, NULL); |
if (p4 == -1) { |
grx_close(); |
perror("Could not create task <jetdummy>"); |
sys_end(); |
} |
soft_task_default_model(m5); |
soft_task_def_level(m5,1); |
soft_task_def_period(m5, PERIOD_JETSLIDE); |
soft_task_def_met(m5, WCET_JETSLIDE); |
soft_task_def_group(m5, JET_GROUP); |
soft_task_def_usemath(m5); |
soft_task_def_ctrl_jet(m5); |
p5 = task_create("jsli", jetslide_task, &m5, NULL); |
if (p5 == -1) { |
grx_close(); |
perror("Could not create task <jetslide>"); |
sys_end(); |
} |
} |
/demos/trunk/biliardo/palla.c |
---|
0,0 → 1,1169 |
/*--------------------------------------------------------------*/ |
/* BILIARDO: FILE CONTENENTE TASK PALLA E SCHEDULAZIONE */ |
/*--------------------------------------------------------------*/ |
#include <kernel/func.h> |
#include <stdlib.h> |
#include <math.h> |
#include "biliardo.h" |
#include "posizion.h" |
#define R 4 // dimensioni della palla |
#define RB 7 // dimensioni della buca |
#define COD_FRECCIA 64 |
static int ballexit[BALL_MAX_P]; // controllo eliminazione task palla |
static int npc = 0; // numero dei task palla in gioco |
struct posizione PosPalla[BALL_MAX_P]; // posizione delle palle in gioco |
int FlagPartita = 0; |
int ch = 0, curs = 0; |
int punteggio[2]; // punteggio relativo al giocatore |
int giocatore = 0; // giocatore |
int bForza; |
TASK palla(int i) { |
float oxf, oyf; // posizione precedente della palla |
int col; |
setPalla (i); |
mutex_lock(&palmutex); |
col = PosPalla[i].col; |
oxf = PosPalla[i].x; |
oyf = BALL_Y - PosPalla[i].y; |
mutex_unlock(&palmutex); |
mutex_lock(&mutex); |
grx_disc(oxf, oyf, R, col); |
grx_text(" ",432,85, black, black); |
mutex_unlock(&mutex); |
while (1) { |
mutex_lock(&mutex); |
grx_disc(oxf, oyf, R, 0); |
// grx_line(oxf, oyf, oxf+vx, oyf-vy, 0); |
mutex_unlock(&mutex); |
mutex_lock(&palmutex); |
oxf = PosPalla[i].x; |
oyf = BALL_Y - PosPalla[i].y; |
mutex_unlock(&palmutex); |
mutex_lock (&delmutex); |
if (ballexit[i]) { |
npc--; |
mutex_unlock (&delmutex); |
return 0; |
} |
mutex_unlock (&delmutex); |
mutex_lock(&mutex); |
grx_disc(oxf, oyf, R, col); |
// grx_line(oxf, oyf, oxf+vx, oyf-vy, col); |
mutex_unlock(&mutex); |
task_endcycle(); |
} |
} |
TASK sched() { |
int i, j; |
int k; |
char strTmp[21]; |
struct posizione posIn[BALL_MAX_P]; |
struct posizione posOut[BALL_MAX_P]; |
struct posizione pos[BALL_MAX_P]; |
int flag, bBevuta = 0; |
float dist = 0; |
float dx, dy; // variazione coordinate |
float dt; /* incremento temporale */ |
float modx, mody; |
// angolo formato dalla retta passante per il baricentro delle palle in collisione |
float thetaB1 = 0, thetaB2 = 0; |
float tratto; //tratto percorso |
float thetaO1, thetaO2; |
float attr = 0.0021; // attrito applicato ad ogni palla |
float acc; |
float g = 9.86; // forza di gravit utilizzata per il calcolo dell'attrito |
float v0 = 0; |
float xCurs, yCurs; |
int bRiposiz = 1; |
int punteggio[2]; |
int giocatore = 0; |
int sign; |
float v1x, v1y, v2x, v2y; |
float phi; |
float thetaV1, thetaV2; |
float thetaBarOld1, thetaBarOld2; |
// evita che venga segnalata pi volte una collisione |
int flag1coll[BALL_MAX_P]; |
// segna nella locazione di una palla con quale altra palla ha colliso |
int flagCollCorr[BALL_MAX_P]; |
// ricorda per ogni palla quale stata la collisione precedente |
int flagCollPrec[BALL_MAX_P]; |
int flagAgg[BALL_MAX_P]; |
int flagAggNum[BALL_MAX_P]; |
for (i=0; i<BALL_MAX_P; i++) { |
flagCollPrec[i] = -1; |
} |
dt = ((float)PERIOD_BALL)/40000; |
acc = attr * g; |
flag = 1; |
k = 0; |
while (1) { |
if (ch == 'i') { |
punteggio[0] = 0, punteggio[1] = 0; |
ch = 0; |
} |
if (FlagPartita) { |
// inizio mutex |
mutex_lock(&palmutex); |
if (PosPalla[i].theta < 0) |
PosPalla[i].theta += 2*PI; |
else |
if (PosPalla[i].theta > 2*PI) |
PosPalla[i].theta -= 2*PI; |
for(i=0; i<BALL_MAX_P; i++) { |
flag1coll[i] = 0; |
flagCollCorr[i] = -1; |
flagAgg[i] = 0; |
posIn[i].x = posOut[i].x = PosPalla[i].x; |
posIn[i].y = posOut[i].y = PosPalla[i].y; |
posIn[i].v = posOut[i].v = PosPalla[i].v; |
posIn[i].theta = posOut[i].theta = PosPalla[i].theta; |
} |
mutex_unlock(&palmutex); |
// fine mutex |
for(i=0; i<BALL_MAX_P; i++) { |
for(j=0; j<BALL_MAX_P; j++) { |
if (i < j && flagCollCorr[i] == -1) { |
if(posIn[i].x-R >= posIn[j].x-R && posIn[i].x-R <= posIn[j].x+R |
&& posIn[i].y-R >= posIn[j].y-R && posIn[i].y-R <= posIn[j].y+R) { |
flagCollCorr[i] = j; |
flagCollCorr[j] = i; |
break; |
} |
else |
if(posIn[i].x-R >= posIn[j].x-R && posIn[i].x-R <= posIn[j].x+R |
&& posIn[i].y+R >= posIn[j].y-R && posIn[i].y+R <= posIn[j].y+R) { |
flagCollCorr[i] = j; |
flagCollCorr[j] = i; |
break; |
} |
else |
if(posIn[i].x+R >= posIn[j].x-R && posIn[i].x+R <= posIn[j].x+R |
&& posIn[i].y+R >= posIn[j].y-R && posIn[i].y+R <= posIn[j].y+R) { |
flagCollCorr[i] = j; |
flagCollCorr[j] = i; |
break; |
} |
else |
if(posIn[i].x+R >= posIn[j].x-R && posIn[i].x+R <= posIn[j].x+R |
&& posIn[i].y-R >= posIn[j].y-R && posIn[i].y-R <= posIn[j].y+R) { |
flagCollCorr[i] = j; |
flagCollCorr[j] = i; |
break; |
} |
else { |
flagCollCorr[i] = -1; |
flagCollPrec[i] = -1; |
} |
} // fine if(i!=j) |
} // fine ciclo for(j) |
if (flagCollCorr[i] != -1 && flagCollCorr[i] != flagCollPrec[i]) { |
dist = sqrt(pow(posIn[i].x - posIn[flagCollCorr[i]].x,2) |
+ pow(posIn[i].y - posIn[flagCollCorr[i]].y,2)); |
if (dist < 2*R) { |
flagAgg[i] = 1; |
flagAgg[flagCollCorr[i]] = 1; |
dist = ceil (2*R-dist); |
if ((int)dist%2 != 0) |
dist += 1; |
flagAggNum[i] = (int)dist/2; |
} |
// permette la sola collisione di due palle contemporaneamente |
flag1coll[i] = 1; |
flagCollPrec[i] = flagCollCorr[i]; |
} |
} // fine ciclo for(i) |
for(i=0; i<BALL_MAX_P; i++) { // inizio ciclo for calcolo collisioni |
if (flag1coll[i]) { |
if (flag1coll[0]) |
bBevuta = 0; |
sign = 1; |
modx = posIn[i].x - posIn[flagCollCorr[i]].x; |
mody = posIn[i].y - posIn[flagCollCorr[i]].y; |
if (modx*mody < 0) sign = 0; |
if (modx < 0) modx = -modx; |
if (mody < 0) mody = -mody; |
// prima palla |
if (posIn[i].x <= posIn[flagCollCorr[i]].x && posIn[i].y <= posIn[flagCollCorr[i]].y) { |
// angolo formato dalla retta passante per il baricentro delle due palle |
thetaB1 = atan (mody / modx); |
if (flagAgg[i]) { |
posOut[i].x -=flagAggNum[i], posOut[i].y -=flagAggNum[i]; |
flagAgg[i] = 0; |
} |
} |
else |
if (posIn[i].x >= posIn[flagCollCorr[i]].x && posIn[i].y <= posIn[flagCollCorr[i]].y) { |
thetaB1 = PI - atan (mody / modx); |
if (flagAgg[i]) { |
posOut[i].x +=flagAggNum[i], posOut[i].y -=flagAggNum[i]; |
flagAgg[i] = 0; |
} |
} |
else |
if (posIn[i].x >= posIn[flagCollCorr[i]].x && posIn[i].y >= posIn[flagCollCorr[i]].y) { |
thetaB1 = PI + atan (mody / modx); |
if (flagAgg[i]) { |
posOut[i].x +=flagAggNum[i], posOut[i].y +=flagAggNum[i]; |
flagAgg[i] = 0; |
} |
} |
else |
if (posIn[i].x <= posIn[flagCollCorr[i]].x && posIn[i].y >= posIn[flagCollCorr[i]].y) { |
thetaB1 = 2*PI - atan (mody / modx); |
if (flagAgg[i]) { |
posOut[i].x -=flagAggNum[i], posOut[i].y +=flagAggNum[i]; |
flagAgg[i] = 0; |
} |
} |
if (thetaB1 < 0) |
thetaB1 += 2*PI; |
else |
if (thetaB1 > 2*PI) |
thetaB1 -= 2*PI; |
// seconda palla |
if (posIn[i].x >= posIn[flagCollCorr[i]].x && posIn[i].y >= posIn[flagCollCorr[i]].y) { |
// angolo formato dalla retta passante per il baricentro delle due palle |
thetaB2 = atan (mody / modx); |
} |
else |
if (posIn[i].x <= posIn[flagCollCorr[i]].x && posIn[i].y >= posIn[flagCollCorr[i]].y) { |
thetaB2 = PI - atan (mody / modx); |
} |
else |
if (posIn[i].x <= posIn[flagCollCorr[i]].x && posIn[i].y <= posIn[flagCollCorr[i]].y) { |
thetaB2 = PI + atan (mody / modx); |
} |
else |
if (posIn[i].x >= posIn[flagCollCorr[i]].x && posIn[i].y <= posIn[flagCollCorr[i]].y) { |
thetaB2 = 2*PI - atan (mody / modx); |
} |
if (thetaB2 < 0) |
thetaB2 += 2*PI; |
else |
if (thetaB2 > 2*PI) |
thetaB2 -= 2*PI; |
// aggiorno gli angoli rispetto al nuovo sistema di riferimento della prima palla |
// con 0 in thetaB1-PI/2 |
posOut[i].theta = posIn[i].theta + PI/2 - thetaB1; |
if (posOut[i].theta < 0) |
posOut[i].theta += 2*PI; |
else |
if (posOut[i].theta > 2*PI) |
posOut[i].theta -= 2*PI; |
thetaO1 = posOut[i].theta; |
// stessa cosa per la seconda palla con 0 in thetaB2-PI/2 |
posOut[flagCollCorr[i]].theta = posIn[flagCollCorr[i]].theta + PI/2 - thetaB2; |
if (posOut[flagCollCorr[i]].theta < 0) |
posOut[flagCollCorr[i]].theta += 2*PI; |
else |
if (posOut[flagCollCorr[i]].theta > 2*PI) |
posOut[flagCollCorr[i]].theta -= 2*PI; |
thetaO2 = posOut[flagCollCorr[i]].theta; |
thetaBarOld1 = thetaB1; |
thetaBarOld2 = thetaB2; |
thetaB1 = PI/2; |
thetaB2 = PI/2; |
// Ho l'angolo formato dalle due palle => aggiorno gli angoli rispetto |
// al nuovo sistema di riferimento |
if (cos(posOut[i].theta) <= 0) |
thetaV1 = PI - posOut[i].theta; |
else |
thetaV1 = -posOut[i].theta; |
if (thetaV1 < 0) |
thetaV1 = -thetaV1; |
if (cos(posOut[flagCollCorr[i]].theta) <= 0) |
thetaV2 = PI - posOut[flagCollCorr[i]].theta; |
else |
thetaV2 = -posOut[flagCollCorr[i]].theta; |
if (thetaV2 < 0) |
thetaV2 = -thetaV2; |
v1x = posIn[i].v * pow(cos(thetaV1), 2); |
v1y = posIn[i].v * pow(sin(thetaV1), 2); |
v2x = posIn[flagCollCorr[i]].v * pow(cos(thetaV2), 2); |
v2y = posIn[flagCollCorr[i]].v * pow(sin(thetaV2), 2); |
// aggiusto i segni secondo il sistema della prima palla |
if (cos(posOut[i].theta) < 0) |
v1x = -v1x; |
if (sin(posOut[i].theta) < 0) |
v1y = -v1y; |
if (cos(posOut[flagCollCorr[i]].theta) > 0) |
v2x = -v2x; |
if (sin(posOut[flagCollCorr[i]].theta) > 0) |
v2y = -v2y; |
if (!v1y && v2y > 0) { |
v2y = 0; |
} |
if (v1y < 0 && v1y < v2y) { |
v2y = v1y; |
} |
// nuovo modulo della velocit della palla 1 |
posOut[i].v = sqrt(pow(v1x, 2) + pow(v2y, 2)); |
if (v1x) { |
phi = atan (v2y / v1x); |
if (phi < 0) |
phi = -phi; |
} |
else |
phi = PI/2; |
if (v1x >= 0 && v2y >= 0) { |
// primo quadrante |
phi = phi; |
} |
else |
if (v1x <= 0 && v2y >= 0) { |
// secondo quadrante |
phi = PI/2 + (PI/2-phi); |
} |
else |
if (v1x <= 0 && v2y <= 0) |
// terzo quadrante |
phi = PI + phi; |
else |
if (v1x >= 0 && v2y <= 0) |
// quarto quadrante |
phi = 3*PI/2 + (PI/2-phi); |
posOut[i].theta = phi; |
if (posOut[i].theta > 2*PI ) |
posOut[i].theta = posOut[i].theta - 2*PI; |
else |
posOut[i].theta = posOut[i].theta; |
// riporto tutto nel sistema di riferimento iniziale |
posOut[i].theta += -PI/2 + thetaBarOld1; |
if (posOut[i].theta < 0) |
posOut[i].theta += 2*PI; |
else |
if (posOut[i].theta > 2*PI) |
posOut[i].theta -= 2*PI; |
posOut[flagCollCorr[i]].theta += (-PI/2 + thetaBarOld2); |
if (posOut[flagCollCorr[i]].theta < 0) |
posOut[flagCollCorr[i]].theta += 2*PI; |
else |
if (posOut[flagCollCorr[i]].theta > 2*PI) |
posOut[flagCollCorr[i]].theta -= 2*PI; |
thetaB1 = thetaBarOld1; |
thetaB2 = thetaBarOld2; |
/* if (flag2 < 4) { |
itoa (i, strTmp); |
grx_text(strTmp,322,185+flag2*22, gray, black ); |
itoa (flagCollCorr[i], strTmp); |
grx_text(strTmp,322,195+flag2*22, gray, black ); |
sprintf (strTmp, "%f", posIn[i].theta*360/(2*PI)); |
grx_text(strTmp,342,185+flag2*22, gray, black ); |
sprintf (strTmp, "%f", posIn[flagCollCorr[i]].theta*360/(2*PI)); |
grx_text(strTmp,342,195+flag2*22, gray, black ); |
sprintf (strTmp, "%f", posOut[i].theta*360/(2*PI)); |
grx_text(strTmp,432,185+flag2*22, gray, black ); |
sprintf (strTmp, "%f", posOut[flagCollCorr[i]].theta*360/(2*PI)); |
grx_text(strTmp,432,195+flag2*22, gray, black ); |
sprintf (strTmp, "%f", thetaB1*360/(2*PI)); |
grx_text(strTmp,522,185+flag2*22, gray, black ); |
sprintf (strTmp, "%f", thetaB2*360/(2*PI)); |
grx_text(strTmp,522,195+flag2*22, gray, black ); |
// ----------------------- |
sprintf (strTmp, "%f", posIn[i].v); |
grx_text(strTmp,322,290+flag2*22, gray, black ); |
sprintf (strTmp, "%f", posIn[flagCollCorr[i]].v); |
grx_text(strTmp,322,300+flag2*22, gray, black ); |
sprintf (strTmp, "%f", v1x); |
grx_text(strTmp,412,290+flag2*22, gray, black ); |
sprintf (strTmp, "%f", v2x); |
grx_text(strTmp,412,300+flag2*22, gray, black ); |
sprintf (strTmp, "%f", v1y); |
grx_text(strTmp,502,290+flag2*22, gray, black ); |
sprintf (strTmp, "%f", v2y); |
grx_text(strTmp,502,300+flag2*22, gray, black ); |
sprintf (strTmp, "%f", posOut[i].v); |
grx_text(strTmp,582,290+flag2*22, gray, black ); |
sprintf (strTmp, "%f", posOut[flagCollCorr[i]].v); |
grx_text(strTmp,582,300+flag2*22, gray, black ); |
// ----------------------- |
sprintf (strTmp, "%f", thetaV1*360/(2*PI)); |
grx_text(strTmp,322,390+flag2*22, gray, black ); |
sprintf (strTmp, "%f", thetaV2*360/(2*PI)); |
grx_text(strTmp,322,400+flag2*22, gray, black ); |
sprintf (strTmp, "%f", thetaO1*360/(2*PI)); |
grx_text(strTmp,402,390+flag2*22, gray, black ); |
sprintf (strTmp, "%f", thetaO2*360/(2*PI)); |
grx_text(strTmp,412,400+flag2*22, gray, black ); |
flag2++; |
} |
*/ |
} // fine if flag1coll |
// questo if fa i conti del movimento di ogni pallina |
if(posOut[i].v > 0) { |
posOut[i].v -= acc * dt; |
tratto = posOut[i].v * dt -0.5*acc*dt*dt; |
dx = (float) (tratto * cos(posOut[i].theta)); |
dy = (float) (tratto * sin(posOut[i].theta)); |
posOut[i].x += dx; |
posOut[i].y += dy; |
if (posOut[i].x > BALL_XMAX) { |
posOut[i].x = BALL_XMAX; |
posOut[i].theta = PI - posOut[i].theta; |
} |
if (posOut[i].x < BALL_XMIN) { |
posOut[i].x = BALL_XMIN; |
posOut[i].theta = PI - posOut[i].theta; |
} |
if (posOut[i].y > BALL_YMAX) { |
posOut[i].y = BALL_YMAX; |
posOut[i].theta = -posOut[i].theta; |
} |
if (posOut[i].y < 0) { |
posOut[i].y = 0; |
posOut[i].theta = -posOut[i].theta; |
} |
} |
pos[i].x = posOut[i].x; |
pos[i].y = posOut[i].y; |
pos[i].v = posOut[i].v; |
pos[i].theta = posOut[i].theta; |
} //fine ciclo for calcolo collisioni |
if (!pos[0].v && !pos[1].v && !pos[2].v && !pos[3].v && !pos[4].v && !pos[5].v && |
!pos[6].v && !pos[7].v && !pos[8].v && !pos[9].v && !pos[10].v) { |
for (i=0; i<BALL_MAX_P; i++) { |
flagCollPrec[i] = -1; |
} |
} |
if (bRiposiz && flag) { |
mutex_lock(&mutex); |
grx_text("Modo riposizionamento palla bianca",322,85, white, black); |
mutex_unlock(&mutex); |
flag = 0; |
} |
// posizionamento palla 0 |
if (bRiposiz && curs) { |
if (curs == 77 && pos[0].x+R/2 <= BALL_XMAX) pos[0].x += 3; |
if (curs == 75 && pos[0].x-R >= BALL_XMIN) pos[0].x -= 3; |
curs = 0; |
} |
// entra in modalit calibrazione forza |
if (ch == ' ' && !bForza && (!pos[0].v && !pos[1].v && !pos[2].v |
&& !pos[3].v && !pos[4].v && !pos[5].v && !pos[6].v && |
!pos[7].v && !pos[8].v && !pos[9].v && !pos[10].v)) { |
bForza = 1; |
ch = 0, v0 = 3.5; |
xCurs = pos[0].x, yCurs = BALL_Y-pos[0].y+10; |
if (yCurs >= BALL_Y) yCurs = BALL_Y-pos[0].y; |
mutex_lock(&mutex); |
grx_text("Modo riposizionamento palla bianca",322,85, black, black); |
grx_text("Modo calibrazione forza",322,85, white, black); |
grx_text("Forza: ",322,95, white, black); |
sprintf (strTmp, "%2f", v0); |
grx_text(strTmp,402,95, red, black); |
grx_line(xCurs-2, yCurs, xCurs+2, yCurs, red); |
grx_line(xCurs, yCurs+2, xCurs, yCurs-2, red); |
mutex_unlock(&mutex); |
bRiposiz = 0; |
} |
// calibrazione forza |
if (ch == 'a' && bForza) { |
if (v0 < 7.5) |
v0 += 0.3; |
if (v0 > 7.5) |
v0 = 7.5; |
ch = 0; |
mutex_lock(&mutex); |
grx_text(" ",402,95, black, black); |
sprintf (strTmp, "%2f", v0); |
grx_text(strTmp,402,95, red, black); |
mutex_unlock(&mutex); |
} |
else |
if (ch == 'z' && bForza) { |
if (v0 > 0) |
v0 -= 0.1; |
if (v0 < 0) |
v0 = 0; |
ch = 0; |
mutex_lock(&mutex); |
grx_text(" ",402,95, black, black); |
sprintf (strTmp, "%2f", v0); |
grx_text(strTmp,402,95, red, black); |
mutex_unlock(&mutex); |
} |
// aggiusta il mirino |
if (bForza && curs) { |
mutex_lock(&mutex); |
grx_line(xCurs-2, yCurs, xCurs+2, yCurs, black); |
grx_line(xCurs, yCurs+2, xCurs, yCurs-2, black); |
if (curs == 72 && yCurs >= BALL_Y-BALL_YMAX-1) yCurs -= 2; |
if (curs == 80 && yCurs <= BALL_Y) yCurs += 2; |
if (curs == 77 && xCurs <= BALL_XMAX) xCurs += 2; |
if (curs == 75 && xCurs >= BALL_XMIN) xCurs -= 2; |
grx_line(xCurs-2, yCurs, xCurs+2, yCurs, red); |
grx_line(xCurs, yCurs+2, xCurs, yCurs-2, red); |
mutex_unlock(&mutex); |
curs = 0; |
bRiposiz = 0; |
} |
// bevuta |
if (!pos[0].v && bBevuta) { |
pos[0].x = BALL_XMIN+100; |
pos[0].y = 50; |
pos[0].v = 0; |
pos[0].theta = 0; |
bRiposiz = 1; |
flag = 1; |
if (giocatore) |
giocatore = 0; |
else |
giocatore = 1; |
bBevuta = 0; |
mutex_lock(&mutex); |
grx_text("Modo riposizionamento palla bianca",322,85, white, black); |
mutex_unlock(&mutex); |
} |
// scocca il colpo di stecca |
if (bForza && ch == ' ') { |
// mutex_lock(&palmutex); |
pos[0].v = v0; |
if (pos[0].x-xCurs) |
pos[0].theta = atan ((BALL_Y-pos[0].y-yCurs)/(pos[0].x-xCurs)); |
else |
pos[0].theta = PI/2; |
if (pos[0].theta <0) |
pos[0].theta = -pos[0].theta; |
if (xCurs <= pos[0].x && yCurs >= BALL_Y-pos[0].y) |
// primo quadrante |
pos[0].theta = pos[0].theta; |
else |
if (xCurs >= pos[0].x && yCurs >= BALL_Y-pos[0].y) |
// secondo quadrante |
pos[0].theta = PI/2 + (PI/2-pos[0].theta); |
else |
if (xCurs >= pos[0].x && yCurs <= BALL_Y-pos[0].y) |
// terzo quadrante |
pos[0].theta = PI + pos[0].theta; |
else |
if (xCurs <= pos[0].x && yCurs <= BALL_Y-pos[0].y) |
// quarto quadrante |
pos[0].theta = 3*PI/2 + (PI/2-pos[0].theta); |
// mutex_unlock(&palmutex); |
mutex_lock(&mutex); |
grx_line(xCurs-2, yCurs, xCurs+2, yCurs, black); |
grx_line(xCurs, yCurs+2, xCurs, yCurs-2, black); |
grx_text("Modo calibrazione forza",322,85, black, black); |
grx_text("Forza: ",322,95, black, black); |
grx_text(" ",402,95, black, black); |
mutex_unlock(&mutex); |
ch = bForza = 0; |
v0 = 0; |
bBevuta = 1; |
} |
if (ch == 'x') { |
for (i=0; i<BALL_MAX_P; i++) { |
flagCollPrec[i] = -1; |
} |
ch = 0; |
} |
itoa (giocatore, strTmp); |
mutex_lock(&mutex); |
grx_text(strTmp,432,110, red, black ); |
itoa (punteggio[giocatore], strTmp); |
grx_text(strTmp,432,120, red, black ); |
mutex_unlock(&mutex); |
// aggiornamento della posizione delle palle |
mutex_lock(&palmutex); |
for (i=0; i<BALL_MAX_P; i++) { |
if (controlloBuche (pos[i].x, pos[i].y, i)) { |
if (i) { |
pos[i].x = -10; |
pos[i].y = 30; |
pos[i].v = 0; |
pos[i].theta = 0; |
punteggio[giocatore]++; |
// itoa (npc, strTmp); |
// grx_text(strTmp,322,285, black, red ); |
if (punteggio[giocatore] >= 5) { |
itoa (punteggio[giocatore], strTmp); |
if (punteggio[giocatore] > 5) { |
mutex_lock(&mutex); |
grx_text(strTmp,432,120, red, black ); |
grx_text("Vittoria giocatore:",432,85, red, black); |
itoa (giocatore, strTmp); |
grx_text(strTmp,592,85, red, black ); |
mutex_unlock(&mutex); |
killball (); |
} |
if (punteggio[giocatore] == 5 && npc == 2) { |
mutex_lock(&mutex); |
grx_text(strTmp,432,120, red, black ); |
grx_text("Pareggio",432,85, red, black); |
mutex_unlock(&mutex); |
killball (); |
} |
} |
} |
else { |
pos[i].x = BALL_XMIN+100; |
pos[i].y = 50; |
pos[i].v = 0; |
pos[i].theta = 0; |
mutex_lock (&delmutex); |
ballexit[i] = 0; |
mutex_unlock (&delmutex); |
bRiposiz = 1; |
flag = 1; |
if (giocatore) |
giocatore = 0; |
else |
giocatore = 1; |
bBevuta = 0; |
} |
} |
PosPalla[i].x = pos[i].x; |
PosPalla[i].y = pos[i].y; |
if (pos[i].v >= 0) { |
PosPalla[i].v = pos[i].v; |
if (pos[i].theta > 2*PI ) |
PosPalla[i].theta = pos[i].theta - 2*PI; |
else |
if (pos[i].theta < 0 ) |
PosPalla[i].theta = pos[i].theta + 2*PI; |
else |
PosPalla[i].theta = pos[i].theta; |
} |
else { |
PosPalla[i].v = 0; |
PosPalla[i].theta = 0; |
} |
} |
mutex_unlock(&palmutex); |
} |
else { |
bRiposiz = 1, flag = 1; |
bBevuta = 0; |
} |
task_endcycle(); |
} // fine ciclo while esterno |
} |
void inizioPartita () { |
if (!FlagPartita) { |
hardball(); |
giocatore = 0; |
punteggio[0] = 0; |
punteggio[1] = 0; |
FlagPartita = 1; |
} |
} |
void killball() { |
int i; |
bForza = 0; |
mutex_lock (&delmutex); |
for (i=0; i<BALL_MAX_P; i++) |
ballexit[i] = 1; |
mutex_unlock (&delmutex); |
FlagPartita = 0; |
} |
void assegnaForza(KEY_EVT *k) { |
ch = k->ascii; |
} |
void movCursore (KEY_EVT *k) { |
curs = k->ascii; |
} |
/* |
void ballfun(KEY_EVT *k) |
{ |
SOFT_TASK_MODEL mp; |
int r,g,b; |
PID pid; |
char palla_str[]="palla "; |
soft_task_default_model(mp); |
soft_task_def_level(mp,1); |
soft_task_def_ctrl_jet(mp); |
soft_task_def_arg(mp); |
soft_task_def_group(mp, BALL_GROUP); |
soft_task_def_met(mp, WCET_BALL); |
soft_task_def_period(mp,PERIOD_BALL); |
soft_task_def_usemath(mp); |
pid = task_create(palla_str, palla, &mp, NULL); |
if (pid != NIL) { |
task_activate(pid); |
} |
} |
*/ |
// avvio dei task palla |
void hardball() |
{ |
HARD_TASK_MODEL mp; |
PID pid; |
char pallaStr[]="palla "; |
int i; |
if (npc == BALL_MAX_P) return; |
for(i=0;i<BALL_MAX_P;i++) { |
ballexit[i] = 0; |
itoa(i,pallaStr+6); |
hard_task_default_model(mp); |
hard_task_def_ctrl_jet(mp); |
hard_task_def_arg(mp, (void *)i); |
hard_task_def_wcet(mp, WCET_BALL); |
hard_task_def_mit(mp,PERIOD_BALL); |
hard_task_def_group(mp, BALL_GROUP); |
hard_task_def_usemath(mp); |
pid = task_create(pallaStr, palla, &mp, NULL); |
if (pid == NIL) { |
grx_close(); |
perror("Could not create task <pallaEDF>"); |
sys_end(); |
} |
else { |
npc++; |
} |
} |
group_activate (BALL_GROUP); |
} |
// avvio del task di contollo |
void hardSched() |
{ |
HARD_TASK_MODEL mp; |
PID pid; |
hard_task_default_model(mp); |
hard_task_def_ctrl_jet(mp); |
// hard_task_def_arg(mp, (void *)1); |
hard_task_def_wcet(mp, WCET_SCHED); |
hard_task_def_mit(mp, PERIOD_SCHED); |
hard_task_def_usemath(mp); |
pid = task_create("Schedular", sched, &mp, NULL); |
if (pid == NIL) { |
grx_close(); |
perror("Could not create task <Schedulatore>"); |
sys_end(); |
} |
else |
task_activate(pid); |
} |
/*--------------------------------------------------------------*/ |
/* MAIN process */ |
/*--------------------------------------------------------------*/ |
void scenario_ball() |
{ |
int i; |
// Margine dello schermo |
grx_rect(0, 0, 639, 479, red); |
grx_line(320,78,639,78,red); |
grx_line(320,0,320,479,red); |
// Tavolo da biliardo |
grx_rect(60,30,260,430,lime); |
for (i=1; i<27; i++) { |
grx_rect(60-i,30-i,260+i,430+i,brown); |
} |
// buche del tavolo |
grx_disc(62, 32, RB, black); |
grx_disc(258, 32, RB, black); |
grx_disc(62, 230, RB, black); |
grx_disc(258, 230, RB, black); |
grx_disc(62, 428, RB, black); |
grx_disc(258, 428, RB, black); |
grx_line(320,105,639,105,red); |
grx_line(320,130,639,130,red); |
grx_text("Giocatore:" ,322,110, red, black ); |
grx_text("Punteggio:" ,322,120, red, black ); |
} |
void init_ball(void) |
{ |
KEY_EVT k; |
k.flag = 0; |
k.scan = KEY_I; |
k.ascii = 'i'; |
keyb_hook(k,assegnaForza); |
keyb_hook(k,inizioPartita); |
/* intercetta il tasto 'spazio' per passare in modo posizionamento |
e modo forza, mi serve per dare un colpo di stecca */ |
k.flag = 0; |
k.scan = KEY_SPC; |
k.ascii = ' '; |
keyb_hook(k,assegnaForza); |
// inizia una nuova partita |
k.flag = 0; |
k.scan = KEY_BKS; |
k.ascii = ' '; |
keyb_hook(k,killball); |
k.flag = 0; |
k.scan = KEY_X; |
k.ascii = 'x'; |
keyb_hook(k,assegnaForza); |
k.flag = 0; |
k.scan = KEY_A; |
k.ascii = 'a'; |
keyb_hook(k,assegnaForza); |
k.flag = 0; |
k.scan = KEY_Z; |
k.ascii = 'z'; |
keyb_hook(k,assegnaForza); |
k.flag = COD_FRECCIA; |
k.scan = 72; |
k.ascii = 72; |
keyb_hook(k,movCursore); |
k.flag = COD_FRECCIA; |
k.scan = 75; |
k.ascii = 75; |
keyb_hook(k,movCursore); |
k.flag = COD_FRECCIA; |
k.scan = 77; |
k.ascii = 77; |
keyb_hook(k,movCursore); |
k.flag = COD_FRECCIA; |
k.scan = 80; |
k.ascii = 80; |
keyb_hook(k,movCursore); |
} |
// inizializzzazione task di schedulazione |
void initSched(void) { |
hardSched(); |
} |
// inizializzazione delle posizioni delle palle |
void setPalla (int num) { |
int nPalla = num; |
nPalla++; |
switch(nPalla) { |
case 1: |
PosPalla[nPalla-1].x = BALL_XMIN + 100; |
PosPalla[nPalla-1].y = 50; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = white; |
break; |
case 2: |
PosPalla[nPalla-1].x = BALL_XMIN + 100; |
PosPalla[nPalla-1].y = 300; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = green; |
break; |
case 3: |
PosPalla[nPalla-1].x = BALL_XMIN + 90; |
PosPalla[nPalla-1].y = 310; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = green; |
break; |
case 4: |
PosPalla[nPalla-1].x = BALL_XMIN + 110; |
PosPalla[nPalla-1].y = 310; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = green; |
break; |
case 5: |
PosPalla[nPalla-1].x = BALL_XMIN + 80; |
PosPalla[nPalla-1].y = 320; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = green; |
break; |
case 6: |
PosPalla[nPalla-1].x = BALL_XMIN + 100; |
PosPalla[nPalla-1].y = 320; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = green; |
break; |
case 7: |
PosPalla[nPalla-1].x = BALL_XMIN + 120; |
PosPalla[nPalla-1].y = 320; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = green; |
break; |
case 8: |
PosPalla[nPalla-1].x = BALL_XMIN + 70; |
PosPalla[nPalla-1].y = 330; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = green; |
break; |
case 9: |
PosPalla[nPalla-1].x = BALL_XMIN + 90; |
PosPalla[nPalla-1].y = 330; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = green; |
break; |
case 10: |
PosPalla[nPalla-1].x = BALL_XMIN + 110; |
PosPalla[nPalla-1].y = 330; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = green; |
break; |
case 11: |
PosPalla[nPalla-1].x = BALL_XMIN + 130; |
PosPalla[nPalla-1].y = 330; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = green; |
break; |
default: |
PosPalla[nPalla-1].x = BALL_XMIN; |
PosPalla[nPalla-1].y = 0; |
PosPalla[nPalla-1].v = 0; |
PosPalla[nPalla-1].theta = 0; |
PosPalla[nPalla-1].col = green; |
break; |
} |
} |
// controllo quando una palla finisce in buca |
int controlloBuche (float x, float yRel, int i) { |
float y; |
y = BALL_Y - yRel; |
if ((x <= 62+RB && y <= 32+RB) || (x >= 258-RB && y <= 32+RB) || |
(x <= 62+RB && (y >= 230-RB && y <= 230+RB)) || (x >= 258-RB && (y >= 230-RB && y <= 230+RB)) || |
(x <= 62+RB && y >= 428-RB) || (x >= 258-RB && y >= 428-RB)){ |
// palla in buca |
mutex_lock (&delmutex); |
ballexit[i] = 1; |
mutex_unlock (&delmutex); |
return (1); |
} |
return (0); |
} |
/demos/trunk/biliardo/demo.h |
---|
0,0 → 1,170 |
/* |
* 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: demo.h,v 1.1 2003-06-04 09:41:32 giacomo Exp $ |
File: $File$ |
Revision: $Revision: 1.1 $ |
Last update: $Date: 2003-06-04 09:41:32 $ |
------------ |
**/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* 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 <ll/ll.h> |
#include <kernel/types.h> |
#include <kernel/descr.h> |
#include <math.h> |
#include <drivers/glib.h> |
#include <drivers/keyb.h> |
#include <modules/hartport.h> |
#include <modules/sem.h> |
#define JET_ON |
#define BALL_ON |
/* |
* |
* WCET, Periods and Models |
* |
*/ |
/* define if you want NRT or SOFT... */ |
#define TASK_TYPE SOFT |
//#define TASK_TYPE NRT |
#define WCET_JETCTRL 7500 |
#define WCET_JETDUMMY 200 |
#define WCET_JETSLIDE 2100 |
#define PERIOD_JETCTRL 100000 |
#define PERIOD_JETDUMMY 100000 |
#define PERIOD_JETSLIDE 100000 |
//#define WCET_BALL 100 |
#define WCET_BALL 500 |
#define PERIOD_BALL 25000 |
#define WCET_SCHED 450 |
#define PERIOD_SCHED 15000 |
/* |
* |
* Global Stuffs |
* |
*/ |
/* graphic mutex... */ |
extern mutex_t mutex; |
/* sincronizzazione */ |
//extern sem_t pmutex; |
extern mutex_t palmutex; |
/* useful colors... */ |
extern int white; |
extern int black; |
extern int red; |
extern int gray; |
extern int green; |
void init_jetcontrol(); |
void init_ball(void); |
void scenario_jetcontrol(); |
void scenario_ball(); |
char *itoa(int n, char *s); |
int myrand(int x); |
void initSched(void); |
void setPalla (int); |
void inizioPartita (void); |
void collisioneRilevata(int); |
int controlloBuche (float, float, int); |
void hardSched (void); |
void hardball (void); |
/* |
* |
* JETCONTROL stuffs |
* |
*/ |
#define JET_NTASK 15 |
#define JET_Y_NAME 170 |
#define DUMMY_PID 1 |
#define JET_DUMMY_WIDTH 210 |
#define JET_DUMMY_HEIGHT 80 |
/* the point (x, y) is the top left corner */ |
#define JET_DUMMY_X 428 |
#define JET_DUMMY_Y 65 |
#define JET_SLIDE_WIDTH 50 |
#define JET_SLIDE_X 576 |
/* |
* |
* BALL stuffs |
* |
*/ |
// x and y corners are specified whithout consider a border of 3 pixels |
#define BALL_YMAX 390 /* position of the ceil */ |
#define BALL_Y 425 /* position of the floor */ |
#define BALL_VELY .1 /* vertical ball velocity */ |
#define BALL_HEIGHT 5 /* initial height of the ball */ |
#define BALL_XMIN 65 /* min position X of the ball */ |
#define BALL_XMAX 255 /* max position X of the ball */ |
#define BALL_VELX 0. /* horizontal ball velocity */ |
#define BALL_VYMIN 11. /* min ground speed */ |
#define BALL_MAX_P 11 /* max number of balls */ |
#define BALL_GROUP 2 /* task group of the balls */ |
#define COLLISION 1 |
#define NO_COLLISION 0 |
/demos/trunk/biliardo/biliardo.h |
---|
0,0 → 1,102 |
#include <ll/ll.h> |
#include <kernel/types.h> |
#include <kernel/descr.h> |
#include <math.h> |
#include <drivers/glib.h> |
#include <drivers/keyb.h> |
#include <modules/hartport.h> |
#include <modules/sem.h> |
#define JET_ON |
#define BALL_ON |
/* |
* |
* WCET, Periods and Models |
* |
*/ |
/* define if you want NRT or SOFT... */ |
#define TASK_TYPE SOFT |
//#define TASK_TYPE NRT |
#define WCET_JETCTRL 7500 |
#define WCET_JETDUMMY 200 |
#define WCET_JETSLIDE 2100 |
#define PERIOD_JETCTRL 100000 |
#define PERIOD_JETDUMMY 100000 |
#define PERIOD_JETSLIDE 100000 |
#define JET_GROUP 1 |
#define WCET_BALL 600 |
#define PERIOD_BALL 20000 |
#define WCET_SCHED 1200 |
#define PERIOD_SCHED 15000 |
/* graphic mutex... */ |
extern mutex_t mutex; |
extern mutex_t palmutex; |
extern mutex_t delmutex; |
/* useful colors... */ |
extern int white; |
extern int black; |
extern int red; |
extern int gray; |
extern int green; |
extern int lime; |
extern int brown; |
void init_jetcontrol(); |
void init_ball(void); |
void scenario_jetcontrol(); |
void scenario_ball(); |
char *itoa(int n, char *s); |
int myrand(int x); |
void initSched(void); |
void setPalla (int); |
void inizioPartita (void); |
void collisioneRilevata(int); |
int controlloBuche (float, float, int); |
void hardSched (void); |
void hardball (void); |
void killball (void); |
void assegnaForza (KEY_EVT *); |
#define JET_NTASK 15 |
#define JET_Y_NAME 260 |
#define DUMMY_PID 1 |
#define JET_DUMMY_WIDTH 250 |
#define JET_DUMMY_HEIGHT 80 |
#define JET_DUMMY_X 370 |
#define JET_DUMMY_Y 150 |
#define JET_SLIDE_WIDTH 50 |
#define JET_SLIDE_X 566 |
#define BALL_YMAX 390 /* position of the ceil */ |
#define BALL_Y 425 /* position of the floor */ |
#define BALL_XMIN 65 /* min position X of the ball */ |
#define BALL_XMAX 255 /* max position X of the ball */ |
#define BALL_MAX_P 11 // numero di palle |
#define BALL_GROUP 2 // gruppo palla |
/demos/trunk/biliardo/makefile |
---|
0,0 → 1,16 |
# |
# |
# |
ifndef BASE |
BASE=../.. |
endif |
include $(BASE)/config/config.mk |
PROGS=biliardo |
include $(BASE)/config/example.mk |
biliardo: |
make -f $(SUBMAKE) APP=biliardo INIT= OTHEROBJS="initfil1.o jetctrl.o palla.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __GRX__" |