WebSVN - shark - Rev 1655 - /unsupported/branches/unsupported/telesco/tel.c

/*
* Project: S.Ha.R.K.
*
* Coordinators:
* Giorgio Buttazzo <giorgio@sssup.it>
* Paolo Gai <pj@gandalf.sssup.it>
*
* Authors :
* Bera Marco mbera@libero.it
* Varasio Gabriele varasio@odino.unipv.it
*
* Universita' degli studi di Pavia
*
* http://www.sssup.it
* http://retis.sssup.it
* http://shark.sssup.it
*/

/**
------------
CVS : $Id: tel.c,v 1.1.1.1 2004-05-24 18:03:39 giacomo Exp $

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

/*
* Copyright (C) 2000 Paolo Gai and Giorgio Buttazzo
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/

/****************************************************************/
/* PERIODIC PROCESS TEST */
/****************************************************************/

#include <ll/i386/x-dos.h>
#include <kernel/kern.h>
#include <modules/cabs.h>
#include <drivers/glib.h>
#include <drivers/keyb.h>
#include <math.h>
#include <semaphore.h>
#include <stdlib.h>
#include "moon.h"

#define N_MAX_TELESCOPI 5
#define X0 10
#define pi 3.14
#define ex 2.71
#define NCAB N_MAX_TELESCOPI+1
#define dim_msg 8
#define dim_x 49
#define dim_y 49
#define radius 9
#define Y_TELESCOPI 300
/* task periods */
#define PERIOD_T1 80000
#define PERIOD_T4 100000
#define PERIOD_T5 100000
#define WCET_T 5000
int period_t1=0;
#define SOGLIA 8

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

// posizioni dei telescopi e rispettivi poli

int x[5] = {100, 200, 300, 400,500};
//double poli[5]={-50,-30,-1, -20, -5};
double poli[5];
// guadagno proporzionale
double kp=0.0;
// flag per rilevare la pressione del tasto spazio
int numero_telescopi=0;
int flag=0;
KEY_EVT move_tasto;
// Immagini ausiliarie per il disegno della luna
BYTE lunabkg[900];
BYTE lunadest[900];

/* A semaphore used to access Video Cards in mutual exclusion */
sem_t mutex;
sem_t mutex_tasto;

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

// cab necessari allo scambio delle coordinate dell'oggetto e delle
// immagini dei rispettivi telescopi

CAB cid[NCAB];

// vettori per memorizzare le immagini
BYTE videobuf[dim_x*dim_y];
BYTE videobuf1[dim_x*dim_y];
BYTE videobuf2[dim_x*dim_y];
BYTE videobuf3[dim_x*dim_y];
BYTE videobuf4[dim_x*dim_y];
BYTE videobuf5[dim_x*dim_y];

// Variabili per il disegno delle stelle sullo sfondo
int num_p;
int y_p[7];
int x_p[7];
int dim_p[7];
int col_p[7];

TASK media(void *arg)
{
char *p;

int i=0;
int j=0;
while(1)
{
p = cab_getmes(cid[1]);
memcpy(videobuf,p,dim_x*dim_y*sizeof(BYTE));
cab_unget(cid[1], p);
p = cab_getmes(cid[2]);
memcpy(videobuf1,p,dim_x*dim_y*sizeof(BYTE));
cab_unget(cid[2], p);
p = cab_getmes(cid[3]);
memcpy(videobuf2,p,dim_x*dim_y*sizeof(BYTE));
cab_unget(cid[3], p);
p = cab_getmes(cid[4]);
memcpy(videobuf4,p,dim_x*dim_y*sizeof(BYTE));
cab_unget(cid[4], p);

p = cab_getmes(cid[5]);
memcpy(videobuf5,p,dim_x*dim_y*sizeof(BYTE));
cab_unget(cid[5], p);

// calcolo media

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

{
for(j=0;j<dim_y;j++)
{
videobuf3[i*dim_x+j]=(double)(videobuf[i*dim_x+j]+videobuf1[i*dim_x+j]+videobuf2[i*dim_x+j]+videobuf4[i*dim_x+j]+videobuf5[i*dim_x+j])/numero_telescopi;
}
}

// visualizza immagine mediata
sem_wait(&mutex);
grx_putimage(570,10,570+dim_x-1,10+dim_y-1,videobuf3);
sem_post(&mutex);
task_endcycle();
}
}

TASK move(void *arg)
{
//int i = (int)arg;
char *p;
int x_object=300; // posizione dell'oggetto da rilevare
int y_object=200;
// variabile contatore
int z=0;
char coord_x[dim_msg];
p = cab_reserve(cid[0]);
sprintf(coord_x,"%d %d",x_object,y_object);
memcpy(p,coord_x,dim_msg*sizeof(char));
cab_putmes(cid[0], p);

//Normalizzazione
for(z=0;z<900;z++)
{
if (luna[z]>0)
{
luna[z]=((luna[z])*16.0/255.0)+16;
}

}

task_endcycle();
while (1)
{

// Cancellazione Luna
sem_wait(&mutex);
grx_box(x_object-15,y_object-15,x_object+14,y_object+14,0);
sem_post(&mutex);
if (keyb_getcode(&move_tasto,NON_BLOCK))
{
if (move_tasto.ascii=='4')
x_object=x_object-5;
if (move_tasto.ascii=='6')
x_object=x_object+5;
if (move_tasto.ascii=='2')
y_object=y_object+5;
if (move_tasto.ascii=='8')
y_object=y_object-5;
if (move_tasto.ascii=='9'){
y_object=y_object-5;
x_object=x_object+5; }
if (move_tasto.ascii=='1'){
y_object=y_object+5;
x_object=x_object-5; }
if (move_tasto.ascii=='7'){
y_object=y_object-5;
x_object=x_object-5; }
if (move_tasto.ascii=='3'){
y_object=y_object+5;
x_object=x_object+5;}
if (move_tasto.ascii==' '){
sem_wait(&mutex_tasto);
flag=1;
if(numero_telescopi<N_MAX_TELESCOPI)
{
numero_telescopi++;
}
sem_post(&mutex_tasto);
}
//CONTROLLI SULLA POSIZIONE
if(y_object<(dim_y/2+70)) y_object=dim_y/2+70;
if(y_object>(240-dim_y/2)) y_object=(240-dim_y/2);
if(x_object<(dim_x/2+20 )) x_object=dim_x/2+20;
if(x_object>(630-dim_x/2-10)) x_object=(630-dim_x/2-10);
}
p = cab_reserve(cid[0]);
sprintf(coord_x,"%d %d",x_object,y_object);
memcpy(p,coord_x,dim_msg*sizeof(char));
cab_putmes(cid[0], p);

for(z=0;z<7;z++)
{
if (sqrt(pow((x_object-x_p[z]),2)+pow((y_object-y_p[z]),2))<20+dim_p[z]+radius)
grx_disc(x_p[z],y_p[z],dim_p[z],col_p[z]);
}

sem_wait(&mutex);
// Gestione sfondo
grx_getimage(x_object-15,y_object-15,x_object+14, y_object+14,lunabkg);
for(z=0;z<900;z++)
{
if( luna[z]==0 && lunabkg!=0 )
{
lunadest[z]=lunabkg[z];
}
else
{
lunadest[z]=luna[z];
}
}
grx_putimage(x_object-15,y_object-15,x_object+14, y_object+14,lunadest);
sem_post(&mutex);
task_endcycle();
}
}

TASK tele(void *arg)
{
int i = (int)arg;
int passi;
char s[100];
int x_object=300; // posizione dell'oggetto da rilevare
int y_object=200;
int x_start=500; // posizione iniziale dei telescopi
int y_start=190;
int x_current=x_start; // posizione corrente dei telescopi
int y_current=y_start;
int x_current_d[9];
int y_current_d[9];
int y= Y_TELESCOPI; // coordinata y dei telescopi
double alpha_new=atan((y-(double)y_current)/((x[i]-(double)x_current)));
double alpha=alpha_new;
double distance=0.0;
double alpha_target=0.0;
double tc=0.1;
double u=0.0;
double u_old=0.0;
double errore=0.0;
double delta_x=0;
double delta_y=0;

char *p;
//int j=0;
// indice matrice per aggiunta di rumore
int k=0;
int q=0;
double polo=poli[i];
int val=0;

BYTE videobuf[dim_x*dim_y];

passi = 0;
//srand(i);
//srand(i+sys_gettime(NULL));
alpha_target=atan((y-(double)y_object)/((double)x_object-x[i]));

sem_wait(&mutex);
grx_text("targ",4,340,12,0);
sprintf(s,"%f",poli[i]);
grx_text(s,x[i],y+60,12,0);
grx_text("polo",4,y+60,12,0);
grx_text("new",4,y+50,12,0);
sprintf(s,"%1.6f",alpha_target);
grx_text(s, x[i]-25,y+50,12,0);
grx_rect(x[i]-40,y+25,x[i]+40,y+85,2);
grx_rect(x[i]-40,y+90,x[i]+40,y+150,2);
sem_post(&mutex);

// Disegno telescopio
grx_disc(x[i],y,19,i+1);
grx_box( x[i]-19,y,x[i]+19,y+20,i+1);

task_endcycle();

while (1) {
// legge di controllo
passi++;
//sprintf(s,"%d",passi);
//grx_text(s, 50,110,12,0);
p = cab_getmes(cid[0]);
sscanf(p,"%d %d",&val,&y_object);
cab_unget(cid[0], p);
x_object=val;
alpha_target=atan((y-(double)y_object)/((double)x_object-x[i]));
if (alpha_target <0)
{
alpha_target=3.14+alpha_target;
}

errore=alpha_target-alpha_new;
u=u_old+kp*tc*errore;
//alpha_new=0.13*alpha+0.87*u;
alpha_new=(exp(polo*tc))*alpha+(1-exp(polo*tc))*u;
u_old=u;
alpha=alpha_new;

// implementazione dei limiti degli attuattori
if (alpha_new > 3.14)
alpha_new=3.14;
if (alpha_new <0)
alpha_new=0;

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

sem_wait(&mutex);
// Cancello braccio telescopio
if (passi>1)
{
for (k=0; k<9 ; k++)
{
grx_line(delta_x,delta_y,x_current_d[k],y_current_d[k],0);
}
}
x_current=x[i]+distance*cos(alpha_new);
y_current=300-distance*sin(alpha_new);

// lettura immagine

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

//aggiunta rumore

sem_post(&mutex);
for (k=0;k<dim_x;k++)
{
for(q=0;q<dim_y;q++)
{
int num=0;
num=rand();
num=(num%10)+1;
if (num>SOGLIA)
{
videobuf[k*dim_x+q]=videobuf[k*dim_x+q]+1;
}
}
}
sem_wait(&mutex);

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

sprintf(s,"%3.4f",(180*(alpha_target/pi)));
grx_text(s, x[i]-25,y+40,12,0);
sprintf(s,"%3.4f",180*(alpha_new/pi));
grx_text(s, x[i]-25,y+50,12,0);

delta_x=x[i]+20*cos(alpha_new);
delta_y=y-20*sin(alpha_new);

for ( k=0; k<9 ; k++) {
x_current_d[k]=x[i]+50*cos(alpha_new+(k-4)*0.01);
y_current_d[k]=300-50*sin(alpha_new+(k-4)*0.01); }

// Disegno braccio telescopio
for(k=0 ; k<9 ; k++)
grx_line(delta_x,delta_y,x_current_d[k],y_current_d[k],i+1);

sem_post(&mutex);

// scrive immagine nel cab
p = cab_reserve(cid[i+1]);
memcpy(p,videobuf,dim_x*dim_y*sizeof(BYTE));
cab_putmes(cid[i+1], p);

task_endcycle();

}

}

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

/* This is the exception handler. It is called when an exception
is raised.
It exits from the graphical mode, then it prints a message and
shutdown the kernel using sys_abort()
*/

void demo_exc_handler(int signo, siginfo_t *info, void *extra)
{
struct timespec t;

grx_close();

/* Default action for an kern exception is */
kern_cli();
ll_gettime(TIME_EXACT, &t),
kern_printf("\nS.Ha.R.K. Exception raised!!!"
"\nTime (s:ns) :%ld:%ld"
"\nException number:%d (numbers in include/bits/errno.h)"
"\nPID :%d\n",
t.tv_sec, t.tv_nsec, info->si_value.sival_int,
info->si_task);
sys_abort(1);
}

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

/* This function is called when Alt-X is pressed.
It simply shutdown the system using sys_end.
Note that the byebye() function is called only if we exit from
the system using sys_end()!!!!
*/
void my_end(KEY_EVT* e)
{
sys_end();
}

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

/* This function is called when the system exit correctly after Alt-X.
It exits from the graphic mode and then it prints a small greeting.
Note that:
- The function calls grx_exit, so it must be registered using
RUNLEVEL_BEFORE_EXIT (RUNLEVEL_AFTER_EXIT does not work because
at that point the kernel is already returned in real mode!!!)
- When an exception is raised, the exception handler is called.
Since the exception handler already exits from the graphic mode,
this funcion has not to be called. For this reason:
. we registered byebye using the flag NO_AT_ABORT
. the exception handler exits using sys_abort; in that way byebye is
NOT called
*/

void byebye(void *arg)
{
grx_close();
kern_printf("Bye Bye!\n");
}

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

int main(int argc, char **argv)
{
PID pid1, pid4,pid5;
KEY_EVT emerg;
int i=0;
int z=0; // contatore per disegno dei pianeti
HARD_TASK_MODEL m1, m4,m5;
//FILE *fp;

struct sigaction action;

cid[0] = cab_create(cname[0], dim_msg, 5);
cid[1] = cab_create(cname[1], dim_x*dim_y, 3);
cid[2] = cab_create(cname[2], dim_x*dim_y, 3);
cid[3] = cab_create(cname[3], dim_x*dim_y, 3);
cid[4] = cab_create(cname[4], dim_x*dim_y, 4);
cid[5] = cab_create(cname[5], dim_x*dim_y, 3);
cid[6] = cab_create(cname[6], dim_x*dim_y, 3);

/*fp=fopen("file.txt","r");
fscanf(fp,"%d",a);
fclose(fp);
*/

/* Init the standard S.Ha.R.K. exception handler */
action.sa_flags = SA_SIGINFO; /* Set the signal action */
action.sa_sigaction = demo_exc_handler;
action.sa_handler = 0;
sigfillset(&action.sa_mask); /* we block all the other signals... */

if (sigaction(SIGHEXC, &action, NULL) == -1) { /* set the signal */
perror("Error initializing signals...");
sys_end();
}

/* Set the closing function */
sys_atrunlevel(byebye, NULL, RUNLEVEL_BEFORE_EXIT|NO_AT_ABORT);

/* Initializes the semaphore */
sem_init(&mutex,0,1);

/* graphic card Initialization */
if (grx_init() < 1) {
sys_abort(1);
}

if (grx_open(640, 480, 8) < 0) {
kern_printf("GRX Err\n");
sys_abort(1);
}
kern_printf("Video card ok!\n");

/* set the keyboard handler to exit correctly */
emerg.ascii = 'x';
emerg.scan = KEY_X;
emerg.flag = ALTL_BIT;
keyb_hook(emerg,my_end);

srand(i+sys_gettime(NULL));

emerg.ascii = 'x';
emerg.scan = KEY_X;
emerg.flag = ALTR_BIT;
keyb_hook(emerg,my_end);
sem_wait(&mutex);
/* a small banner */
grx_clear(0);

grx_text("REALTIME TELESCOPES di Marco Bera e Gabriele Varasio",8,8,WHITE,0);
grx_text("Premere Spazio per creare un telescopio",8,22,WHITE,0);
grx_text("Premere Alt-X per uscire",8,32,WHITE,0);
grx_text("Usare il tastierino numerico per muovere l'oggetto",8,42,WHITE,0);

// DISEGNARE BORDI
grx_rect(1,1,638,478,2);
grx_rect(558,1,638,61,2);
grx_rect(1,1,638,61,2);

// Disegno sfondo
for(z=0;z<7;z++)
{
num_p=rand();
y_p[z]=100+((num_p%130)+1);
x_p[z]=50+((num_p%540)+1);
dim_p[z]=2+((num_p%5)+1);
col_p[z]=((num_p%10)+1);
grx_disc(x_p[z],y_p[z],dim_p[z],col_p[z]);
}
sem_post(&mutex);

hard_task_default_model(m4);
hard_task_def_ctrl_jet (m4);
hard_task_def_arg (m4, (void *)3);
hard_task_def_wcet (m4, 19000);
hard_task_def_mit (m4, PERIOD_T4);
hard_task_def_group (m4,1);
pid4 = task_create("move", move, &m4, NULL);
if (pid4 == NIL) {
grx_close();
perror("Could not create task <move>");
sys_abort(1);
}
task_activate(pid4);
hard_task_default_model(m5);
hard_task_def_ctrl_jet (m5);
hard_task_def_arg (m5, (void *)3);
hard_task_def_wcet (m5, 19000);
hard_task_def_mit (m5, PERIOD_T5);
hard_task_def_group (m5,1);
pid5 = task_create("media", media, &m5, NULL);

if (pid5 == NIL) {
grx_close();
perror("Could not create task <move>");
sys_abort(1);
}
task_activate(pid5);
/* and finally we activate the three threads... */

do {
int val_flag=0;
sem_wait(&mutex_tasto);
val_flag=flag;
sem_post(&mutex_tasto);

if ((val_flag==1) && (i < N_MAX_TELESCOPI))
{
flag=0;
hard_task_default_model(m1);
hard_task_def_ctrl_jet (m1);
hard_task_def_arg (m1, (void *)i);
hard_task_def_wcet (m1, WCET_T);
// hard_task_def_mit (m1, PERIOD_T1);
hard_task_def_mit (m1, period_t1);

hard_task_def_group (m1,1);
pid1 = task_create("tele1", tele, &m1, NULL);
if (pid1 == NIL)
{
grx_close();
perror("Could not create task <tele1>");
sys_abort(1);
}
task_activate(pid1);
i++;
}
} while (1);

/*
now the task main ends, but the system does not shutdown because
there are others.

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

return 0;
}
/*********** lettura da file ********************/
void read_cfg_file(int argc, char **argv)
{
int err;
DOS_FILE *fp;
char myfilebuf[1000];
int myfilebuf_length;

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

if (fp)
{
/* read up to 1000 chars */
myfilebuf_length = DOS_fread(&myfilebuf,1,1000,fp);

/* check for errors */
err = DOS_error();

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

if (err)
{
cprintf("Error %d reading file...Using default values\n", err);
}
else
{
//geti(myfilebuf, &pos, &NMouses); // Number of Mouses
sscanf(myfilebuf,"%lf %lf %lf %lf %lf %lf %d",&poli[0],&poli[1],&poli[2],&poli[3],&poli[4],&kp,&period_t1);
}

DOS_fclose(fp);
return;

}
else {
/* error!! */
err = DOS_error();
/* note that if you call DOS_error() here, it return 0!!! */
cprintf("Error %d opening myfile.txt...Using default values\n", err);
}

}
else {
cprintf("Wrong number of arguments...\n");
l1_exit(0);
}
}

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

Subversion Repositories shark

(root)/unsupported/branches/unsupported/telesco/tel.c @ 1690 - Rev 1655