Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1418 → Rev 1419

/demos/trunk/rtw/control.c
0,0 → 1,67
#include "kernel/kern.h"
#include "modules/cabs.h"
 
#include "tmwtypes.h"
 
#include "rtw.h"
 
extern CAB input_cid[NINPUTCAB];
 
TASK CONTROL_body(void *arg) {
 
real_T *p;
input_cid[0] = cab_create("INPUT0", sizeof(real_T), 2);
input_cid[1] = cab_create("INPUT1", sizeof(real_T), 2);
while(1) {
/* Reserve a message */
p = (real_T *)cab_reserve(input_cid[0]);
/* Save PAR1 data */
*p = 1000.0;
/* Put CAB message */
cab_putmes(input_cid[0], p);
 
/* Reserve a message */
p = (real_T *)cab_reserve(input_cid[1]);
/* Save PAR2 data */
*p = 1000.0;
/* Put CAB message */
cab_putmes(input_cid[1], p);
task_endcycle();
}
return NULL;
 
}
 
void activate_control() {
 
HARD_TASK_MODEL CONTROL_task;
PID CONTROL_pid;
 
hard_task_default_model(CONTROL_task);
hard_task_def_mit(CONTROL_task,10000);
hard_task_def_wcet(CONTROL_task,500);
hard_task_def_usemath(CONTROL_task);
hard_task_def_ctrl_jet(CONTROL_task);
CONTROL_pid = task_create("CONTROL",CONTROL_body,&CONTROL_task,NULL);
if (CONTROL_pid == NIL) {
cprintf("Error: Cannot create RealTime Workshop [CONTROL] Task\n");
sys_end();
}
 
task_activate(CONTROL_pid);
 
}
 
 
 
/demos/trunk/rtw/rtw.h
5,6 → 5,6
#define NOUTPUTCAB 10
 
void activate_sensors();
void activate_actuators();
void activate_control();
 
#endif
/demos/trunk/rtw/percorso.h
0,0 → 1,48
#ifndef __PERCORSO_H__
#define __PERCORSO_H__
 
#define PATH_ELEM_ARC 1
#define PATH_ELEM_LINE 2
 
#define CLOCKWISE -1
#define COUNTERCLOCK 1
 
typedef struct point {
double x,y;
} POINT;
 
typedef struct path_elem {
int type;
void *data;
struct path_elem *next;
} PATH_ELEM;
 
typedef struct pd_arc {
POINT c;
double r;
double alpha1;
double alpha2;
int dir;
} PATH_DATA_ARC;
 
typedef struct pd_line {
POINT p1;
POINT p2;
double alpha;
int dir;
} PATH_DATA_LINE;
 
 
PATH_ELEM *find_closest_elem(POINT *p, double radius);
double get_distance_from_elem(POINT *p, PATH_ELEM *e);
double get_angle_from_elem(POINT *p, PATH_ELEM *e);
/* ritorna:
1 : curva a sinistra
-1 : curva a destra
0 : dritto
*/
int is_curve(PATH_ELEM *e);
 
void Init_All_Path();
 
#endif
/demos/trunk/rtw/makefile
7,8 → 7,7
endif
include $(BASE)/config/config.mk
 
MATLAB_ROOT = /matlab
MATLAB_RTW_DEMO_DIR = /usr/local/home/giacomo/shark_mat/carrello0_grt_rtw/
MATLAB_RTW_DEMO_DIR = ./carrello0_grt_rtw/
 
DEFINE_MODEL = carrello0
DEFINE_NUMST = 2
22,9 → 21,9
 
PROGS = rtw
 
RTW_CFG = "-I$(MATLAB_ROOT)/simulink/include -I$(MATLAB_ROOT)/extern/include -I$(MATLAB_ROOT)/rtw/c/src -I$(MATLAB_ROOT)/rtw/c/libsrc -I$(MATLAB_RTW_DEMO_DIR) -DUSE_RTMODEL -DRT -DMODEL=$(DEFINE_MODEL) -DNUMST=$(DEFINE_NUMST) -DNCSTATES=$(DEFINE_NCSTATES) -DTID01EQ=$(DEFINE_TID01EQ)"
RTW_CFG = "-I. -I$(MATLAB_RTW_DEMO_DIR) -DUSE_RTMODEL -DRT -DMODEL=$(DEFINE_MODEL) -DNUMST=$(DEFINE_NUMST) -DNCSTATES=$(DEFINE_NCSTATES) -DTID01EQ=$(DEFINE_TID01EQ)"
 
RTW_OBJS = $(MATLAB_RTW_DEMO_DIR)/$(FILE_CODE) $(MATLAB_ROOT)/rtw/c/src/$(DEFINE_SOLVER) $(MODULES_rtwlib)
RTW_OBJS = $(MATLAB_RTW_DEMO_DIR)/$(FILE_CODE) $(DEFINE_SOLVER) $(MODULES_rtwlib)
 
ifeq ($(FILE_DATA),$(DEFINE_MODEL)_data.o)
RTW_OBJS += $(MATLAB_RTW_DEMO_DIR)/$(FILE_DATA)
33,10 → 32,5
include $(BASE)/config/example.mk
 
rtw:
make -f $(SUBMAKE) OTHERLIBS=$(DEFINE_RTWLIB) OTHERINCL=$(RTW_CFG) APP=rtw INIT= OTHEROBJS="initfile.o sensors.o actuators.o rt_sim_shark.o $(RTW_OBJS)" SHARKOPT="__OLDCHAR__"
make -f $(SUBMAKE) OTHERLIBS=$(DEFINE_RTWLIB) OTHERINCL=$(RTW_CFG) APP=rtw INIT= OTHEROBJS="initfile.o control.o sensors.o percorso.o rt_sim_shark.o $(RTW_OBJS)" SHARKOPT="__OLDCHAR__"
 
clean_total:
rm -rf *.o
rm -rf $(PROGS)
rm -rf $(RTW_OBJS)
 
/demos/trunk/rtw/rtw.c
9,6 → 9,7
 
#include "kernel/kern.h"
#include "modules/cabs.h"
#include "percorso.h"
 
#include "rtw.h"
 
368,6 → 369,9
 
TASK RTW_body(void *arg) {
real_T *input, *p;
 
input = (real_T *)rtmGetU(S);
simulation_run = 1;
 
while (!GBLbuf.stopExecutionFlag &&
378,6 → 382,21
(boolean_T *)&rtmGetStopRequested(S));
if (rtmGetStopRequested(S)) break;
/* Get PAR0 */
if (input_cid[0] != -1) {
p = (real_T *)cab_getmes(input_cid[0]);
input[0] = *p;
cab_unget(input_cid[0], p);
}
 
/* Get PAR1 */
if (input_cid[1] != -1) {
p = (real_T *)cab_getmes(input_cid[1]);
input[1] = *p;
cab_unget(input_cid[1], p);
}
rt_OneStep(S);
 
task_endcycle();
395,124 → 414,94
 
}
 
TASK INPUT_body(void *input_port) {
TASK OUTPUT_body(void *arg) {
 
real_T *input, *p;
int in = (int)(input_port);
real_T *output, *p;
real_T th;
 
if (in >= NINPUTCAB) return NULL;
POINT pos;
PATH_ELEM *e;
real_T distance = 0;
real_T angle = 0;
real_T curve = 0;
 
input = (real_T *)rtmGetU(S);
output_cid[0] = cab_create("DISTANCE", sizeof(real_T), 2);
output_cid[1] = cab_create("ANGLE", sizeof(real_T), 2);
output_cid[2] = cab_create("CURVE", sizeof(real_T), 2);
 
while(1) {
output = (real_T *)rtmGetY(S);
 
/* Get CAB message */
p = (real_T *)cab_getmes(input_cid[in]);
while(1) {
 
/* Set INPUT port */
input[in] = *p;
pos.x = output[0];
pos.y = output[1];
th = output[2];
e = find_closest_elem(&pos, 1.0);
if (e != 0) {
distance = get_distance_from_elem(&pos, e);
angle = th - get_angle_from_elem(&pos,e);
curve = is_curve(e);
}
/* Send Distance */
p = (real_T *)cab_reserve(output_cid[0]);
*p = distance;
cab_putmes(output_cid[0], p);
 
/* Release CAB message */
cab_unget(input_cid[in], p);
/* Send Angle */
p = (real_T *)cab_reserve(output_cid[1]);
*p = angle;
cab_putmes(output_cid[1], p);
 
task_endcycle();
/* Send Curve */
p = (real_T *)cab_reserve(output_cid[2]);
*p = curve;
cab_putmes(output_cid[2], p);
 
}
 
return NULL;
 
task_endcycle();
}
return NULL;
}
 
TASK OUTPUT_body(void *output_port) {
void Init_Rtw() {
 
real_T *output, *p;
int out = (int)(output_port);
char cname[20];
 
if (out >= NOUTPUTCAB) return NULL;
sprintf(cname,"OUTPUT%d",out);
output_cid[out] = cab_create(cname, sizeof(real_T), 2);
HARD_TASK_MODEL RTW_task,OUTPUT_task;
PID RTW_pid,OUTPUT_pid;
output = (real_T *)rtmGetY(S);
int i;
while(1) {
/* Reserve a message */
p = (real_T *)cab_reserve(output_cid[out]);
 
/* Save data */
*p = output[out];
 
/* Put CAB message */
cab_putmes(output_cid[out], p);
task_endcycle();
}
return NULL;
}
 
int main() {
 
HARD_TASK_MODEL RTW_task,INPUT_task,OUTPUT_task;
PID RTW_pid,INPUT_pid,OUTPUT_pid;
 
int i;
 
int RTW_period;
int RTW_wcet;
 
int INPUT_period;
int INPUT_wcet;
 
int OUTPUT_period;
int OUTPUT_wcet;
 
Init_RealTime_Workshop();
Init_All_Path();
 
for (i=0;i<NINPUTCAB;i++) input_cid[i] = -1;
for (i=0;i<NOUTPUTCAB;i++) output_cid[i] = -1;
 
RTW_period =(int)(rtmGetStepSize(S) * 1000000.0);
RTW_wcet = (int)(rtmGetStepSize(S) * 1000000.0 * 0.45);
 
INPUT_period = (int)(10000.0);
INPUT_wcet = (int)(10000.0 * 0.10);
 
OUTPUT_period = (int)(10000.0);
OUTPUT_wcet = (int)(10000.0 * 0.10);
 
cprintf("Task Setup\n");
cprintf("RTW (P %8d W %8d)\n",RTW_period,RTW_wcet);
cprintf("INPUT (P %8d W %8d)\n",INPUT_period,INPUT_wcet);
cprintf("OUTPUT (P %8d W %8d)\n",OUTPUT_period,OUTPUT_wcet);
hard_task_default_model(RTW_task);
hard_task_def_mit(RTW_task,RTW_period);
hard_task_def_wcet(RTW_task,RTW_wcet);
hard_task_def_usemath(RTW_task);
hard_task_def_ctrl_jet(RTW_task);
 
RTW_pid = task_create("RTW",RTW_body,&RTW_task,NULL);
if (RTW_pid == NIL) {
cprintf("Error: Cannot create RealTime Workshop [RTW] Task\n");
sys_end();
}
 
hard_task_default_model(INPUT_task);
hard_task_def_mit(INPUT_task,INPUT_period);
hard_task_def_wcet(INPUT_task,INPUT_wcet);
/* Set input port number */
hard_task_def_arg(INPUT_task,(void *)(0));
hard_task_def_usemath(INPUT_task);
hard_task_def_ctrl_jet(INPUT_task);
INPUT_pid = task_create("INPUT0",INPUT_body,&INPUT_task,NULL);
if (INPUT_pid == NIL) {
cprintf("Error: Cannot create RealTime Workshop [INPUT0] Task\n");
cprintf("Error: Cannot create RealTime Workshop [RTW] Task\n");
sys_end();
}
 
519,23 → 508,26
hard_task_default_model(OUTPUT_task);
hard_task_def_mit(OUTPUT_task,OUTPUT_period);
hard_task_def_wcet(OUTPUT_task,OUTPUT_wcet);
/* Set output port number */
hard_task_def_arg(OUTPUT_task,(void *)(0));
hard_task_def_usemath(OUTPUT_task);
hard_task_def_ctrl_jet(OUTPUT_task);
OUTPUT_pid = task_create("OUTPUT0",OUTPUT_body,&OUTPUT_task,NULL);
OUTPUT_pid = task_create("OUTPUT",OUTPUT_body,&OUTPUT_task,NULL);
if (OUTPUT_pid == NIL) {
cprintf("Error: Cannot create RealTime Workshop [OUTPUT0] Task\n");
cprintf("Error: Cannot create RealTime Workshop [OUTPUT] Task\n");
sys_end();
}
 
task_activate(INPUT_pid);
task_activate(RTW_pid);
task_activate(OUTPUT_pid);
task_activate(OUTPUT_pid);
 
}
 
int main() {
 
Init_Rtw();
 
activate_sensors();
activate_actuators();
activate_control();
 
while(simulation_run);
/demos/trunk/rtw/sensors.c
5,30 → 5,28
 
#include "rtw.h"
 
extern CAB input_cid[NINPUTCAB];
extern CAB output_cid[NOUTPUTCAB];
 
TASK SENSOR_body(void *input_port) {
TASK SENSOR_body(void *output_port) {
 
real_T *p;
int in = (int)(input_port);
char cname[20];
real_T *p, value;
int out = (int)(output_port);
if (in >= NINPUTCAB) return NULL;
if (out >= NOUTPUTCAB) return NULL;
sprintf(cname,"INPUT%d",in);
input_cid[in] = cab_create(cname, sizeof(real_T), 2);
while(1) {
/* Get CAB message */
p = (real_T *)cab_getmes(output_cid[out]);
/* Reserve a message */
p = (real_T *)cab_reserve(input_cid[in]);
/* Set value */
value = *p;
/* Save data */
*p = 1000.0;
/* Put CAB message */
cab_putmes(input_cid[in], p);
/* Release CAB message */
cab_unget(output_cid[out], p);
 
cprintf("Value %d = %lf\n",out,value);
task_endcycle();
}
40,24 → 38,39
void activate_sensors() {
 
HARD_TASK_MODEL SENSOR_task;
PID SENSOR_pid;
PID DISTANCE_pid,ANGLE_pid,CURVE_pid;
 
hard_task_default_model(SENSOR_task);
hard_task_def_mit(SENSOR_task,10000);
hard_task_def_wcet(SENSOR_task,1000);
/* Set sensor port number */
hard_task_def_arg(SENSOR_task,(void *)(0));
hard_task_def_mit(SENSOR_task,60000);
hard_task_def_wcet(SENSOR_task,4000);
hard_task_def_usemath(SENSOR_task);
hard_task_def_ctrl_jet(SENSOR_task);
SENSOR_pid = task_create("SENSOR0",SENSOR_body,&SENSOR_task,NULL);
if (SENSOR_pid == NIL) {
cprintf("Error: Cannot create RealTime Workshop [SENSOR0] Task\n");
hard_task_def_arg(SENSOR_task,(void *)(0));
DISTANCE_pid = task_create("DISTANCE",SENSOR_body,&SENSOR_task,NULL);
if (DISTANCE_pid == NIL) {
cprintf("Error: Cannot create RealTime Workshop [DISTACE] Task\n");
sys_end();
}
 
task_activate(SENSOR_pid);
hard_task_def_arg(SENSOR_task,(void *)(1));
ANGLE_pid = task_create("ANGLE",SENSOR_body,&SENSOR_task,NULL);
if (DISTANCE_pid == NIL) {
cprintf("Error: Cannot create RealTime Workshop [DISTACE] Task\n");
sys_end();
}
 
hard_task_def_arg(SENSOR_task,(void *)(2));
CURVE_pid = task_create("CURVE",SENSOR_body,&SENSOR_task,NULL);
if (CURVE_pid == NIL) {
cprintf("Error: Cannot create RealTime Workshop [CURVE] Task\n");
sys_end();
}
task_activate(DISTANCE_pid);
task_activate(ANGLE_pid);
task_activate(CURVE_pid);
 
}
 
 
/demos/trunk/rtw/percorso.c
0,0 → 1,288
#include <stdio.h>
#include <math.h>
#include "percorso.h"
 
int nelem;
PATH_ELEM * head;
 
/* void print_elem(PATH_ELEM *elem) */
/* { */
/* if (elem->type == PATH_ELEM_ARC) { */
/* PATH_DATA_ARC *arc= (PATH_DATA_ARC *) elem->data; */
/* printf("Center: x =%3.3lf y =%3.3lf\n", arc->c.x, arc->c.y); */
/* printf("angles: a1=%3.3lf a2=%3.3lf\n", arc->alpha1, arc->alpha2); */
/* printf("Radius: r =%3.3lf\n", arc->r); */
/* } */
/* else if (elem->type == PATH_ELEM_LINE) { */
/* PATH_DATA_LINE *line = (PATH_DATA_LINE *) elem->data; */
/* printf("x1 = %3.3lf y1 = %3.3lf x2 = %3.3lf y2 = %3.3lf\n", */
/* line->p1.x, line->p1.y, line->p2.x, line->p2.y); */
/* } */
/* } */
 
/* void print_point(POINT *p) */
/* { */
/* printf("p.x = %3.3lf p.y = %3.3lf\n", p->x, p->y); */
/* } */
 
 
/*-----------------------------------------------------------------*/
 
inline double point_dist(POINT *p1, POINT *p2)
{
return sqrt((p1->x - p2->x)*(p1->x - p2->x) +
(p1->y - p2->y)*(p1->y - p2->y));
}
 
double get_angle_from_arc(POINT *p1, PATH_DATA_ARC *arc)
{
return acos((p1->x - arc->c.x) / point_dist(p1, &(arc->c)));
}
 
int inside_arc(PATH_DATA_ARC *arc, POINT *p)
{
double alpha = get_angle_from_arc(p, arc);
if (p->y < arc->c.y) alpha = 2*M_PI - alpha;
 
if (alpha <= arc->alpha2 && alpha >= arc->alpha1) return 1;
else return 0;
}
 
double get_distance_from_arc(POINT *p1, PATH_DATA_ARC *arc)
{
return point_dist (p1,&(arc->c)) - arc->r;
}
 
 
double get_distance_from_line(POINT *p, PATH_DATA_LINE *l)
{
return ((l->p1.y - l->p2.y) * p->x +
(l->p2.x - l->p1.x) * p->y +
(l->p1.x * l->p2.y - l->p2.x * l->p1.y)) /
point_dist(&(l->p1), &(l->p2));
}
 
 
inline double get_t(POINT *p, PATH_DATA_LINE *line)
{
double t = ( (line->p2.x - line->p1.x) * (p->x - line->p1.x) +
(line->p2.y - line->p1.y) * (p->y - line->p1.y)) /
(point_dist(&(line->p1), &(line->p2)) *
point_dist(&(line->p1), &(line->p2)));
return t;
}
 
 
double get_angle_from_line(POINT *p, PATH_DATA_LINE *l)
{
return atan2(l->p1.y - l->p2.y, l->p1.x - l->p2.x);
}
 
int includes_elem(POINT *p, PATH_ELEM *e, double radius)
{
if (e->type == PATH_ELEM_ARC) {
PATH_DATA_ARC *arc = (PATH_DATA_ARC *) e->data;
if (get_distance_from_arc(p, arc) <= radius && inside_arc(arc,p))
return 1;
else return 0;
}
else if (e->type==PATH_ELEM_LINE) {
PATH_DATA_LINE *line = (PATH_DATA_LINE *) e->data;
double t = get_t(p, line);
double d = get_distance_from_line(p, line);
if (t >= 0 && t <= 1 && d < radius) return 1;
else return 0;
}
return 0;
}
 
void init_arc(PATH_DATA_ARC *arc, double cx, double cy, double r,
double a1, double a2, int dir)
{
arc->c.x = cx;
arc->c.y = cy;
arc->r = r;
arc->alpha1 = a1;
arc->alpha2 = a2;
arc->dir = dir;
}
 
void init_line(PATH_DATA_LINE *l, double x1, double y1, double x2,
double y2, int dir)
{
//printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", x1,y1,x2,y2);
l->p1.x = x1;
l->p2.x = x2;
l->p1.y = y1;
l->p2.y = y2;
l->dir = dir;
//printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", l->p1.x,l->p1.y,l->p2.x,l->p2.y);
}
 
/*---------------------------------------------------------------*/
 
PATH_ELEM *find_closest_elem(POINT *p, double radius)
{
PATH_ELEM *cur = head;
int i = 0;
for (i=0; i<nelem; i++) {
//printf("Searching element %d \n", i);
//print_elem(cur);
if (includes_elem(p, cur, radius)) return cur;
else cur = cur->next;
}
return 0;
}
 
double get_distance_from_elem(POINT *p, PATH_ELEM *e)
{
double d = 0;
if (e->type == PATH_ELEM_ARC) {
PATH_DATA_ARC *arc = e->data;
d = get_distance_from_arc(p, arc);
}
else if (e->type == PATH_ELEM_LINE) {
PATH_DATA_LINE *line = e->data;
d = get_distance_from_line(p, line);
}
return d;
}
 
double get_angle_from_elem(POINT *p, PATH_ELEM *e)
{
double alpha = 0;
if (e->type == PATH_ELEM_ARC) {
PATH_DATA_ARC *arc = e->data;
alpha = get_angle_from_arc(p, arc) - M_PI/2;
}
else if (e->type == PATH_ELEM_LINE) {
PATH_DATA_LINE *line = e->data;
alpha = get_angle_from_line(p, line);
}
return alpha;
}
 
 
/* ritorna:
1 : curva a sinistra
-1 : curva a destra
0 : dritto */
int is_curve(PATH_ELEM *e)
{
if (e->type == PATH_ELEM_ARC) {
PATH_DATA_ARC *arc = e->data;
return arc->dir;
}
else return 0;
}
 
/*---------------------------------------------------------------*/
 
PATH_ELEM elems[11];
PATH_DATA_ARC arcs[6];
PATH_DATA_LINE lines[5];
 
void Init_All_Path()
{
init_line(&lines[0], 0,0,3,0, -1);
elems[0].type = PATH_ELEM_LINE;
elems[0].data = &lines[0];
elems[0].next = &elems[1];
 
init_arc(&arcs[0], 3,1, 1, -M_PI/2,0, 1);
elems[1].type = PATH_ELEM_ARC;
elems[1].data = &arcs[0];
elems[1].next = &elems[2];
 
init_line(&lines[1], 4,1,4,7, -1);
elems[2].type = PATH_ELEM_LINE;
elems[2].data = &lines[1];
elems[2].next = &elems[3];
 
init_arc(&arcs[1], 2,7, 2, 0, M_PI/2, 1);
elems[3].type = PATH_ELEM_ARC;
elems[3].data = &arcs[1];
elems[3].next = &elems[4];
 
init_line(&lines[2], 2,9,-1,9, 1);
elems[4].type = PATH_ELEM_LINE;
elems[4].data = &lines[2];
elems[4].next = &elems[5];
 
init_arc(&arcs[2], -2,8, 1, M_PI/2, M_PI, 1);
elems[5].type = PATH_ELEM_ARC;
elems[5].data = &arcs[2];
elems[5].next = &elems[6];
 
init_line(&lines[3], -3,8,-3,5, 1);
elems[6].type = PATH_ELEM_LINE;
elems[6].data = &lines[3];
elems[6].next = &elems[7];
 
init_arc(&arcs[3], -2,5, 1, M_PI, 3*M_PI/2, 1);
elems[7].type = PATH_ELEM_ARC;
elems[7].data = &arcs[3];
elems[7].next = &elems[8];
 
init_arc(&arcs[4], -2,3, 1, 0, M_PI/2, -1);
elems[8].type = PATH_ELEM_ARC;
elems[8].data = &arcs[4];
elems[8].next = &elems[9];
 
init_line(&lines[4], -1,3,-1,1, 1);
elems[9].type = PATH_ELEM_LINE;
elems[9].data = &lines[4];
elems[9].next = &elems[10];
 
init_arc(&arcs[5], 0,1, 1, M_PI, 3*M_PI/2, -1);
elems[10].type = PATH_ELEM_ARC;
elems[10].data = &arcs[5];
elems[10].next = &elems[0];
 
head = &elems[0];
nelem = 11;
}
 
 
/* int main(void) */
/* { */
/* double d; */
/* double alpha; */
/* POINT p; */
 
/* init_all(); */
 
/* p.x = -1.75; */
/* p.y = 4.25; */
 
/* PATH_ELEM *e = find_closest_elem(&p, head, 1); */
 
/* if (e != 0) { */
/* d = get_distance_from_elem(&p, e); */
/* alpha = get_angle_from_elem(&p, e); */
/* print_elem(e); */
/* printf("distance : %3.3lf alpha %3.3lf\n", d, alpha); */
/* printf("direzione %d\n", is_curve(e)); */
/* } */
/* else */
/* printf("Not found!\n"); */
 
 
/* p.x = 4.75; */
/* p.y = 7.1; */
/* e = find_closest_elem(&p, head, 1); */
 
/* if (e != 0) { */
/* d = get_distance_from_elem(&p, e); */
/* alpha = get_angle_from_elem(&p, e); */
/* print_elem(e); */
/* printf("distance : %3.3lf alpha %3.3lf\n", d, alpha); */
/* printf("direzione %d\n", is_curve(e)); */
/* } */
/* else */
/* printf("Not found!\n"); */
 
/* } */