Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
#include <drivers/glib.h>
#include <stdlib.h>
#include <math.h>
#include "include/simcity.h"
#include "include/misc.h"
#include "include/car.h"
#include "include/draw.h"
char collision_sensor
(car_data
*cd
) {
float i
,dx
,dx1
,dy
,dy1
,deltax
,deltay
;
int colore
,colore1
;
int speed_done
=0;
char col
=FREE
;
dx
=cd
->xpos
+2.0*cosine
[normalize
(cd
->angle
+90)];
dy
=cd
->ypos
-2.0*sine
[normalize
(cd
->angle
+90)];
dx1
=cd
->xpos
-8.0*cosine
[normalize
(cd
->angle
+90)];
dy1
=cd
->ypos
+8.0*sine
[normalize
(cd
->angle
+90)];
for (i
=0.0;i
<MAX_LOOK
;i
+=0.1) {
deltax
=i
*cosine
[normalize
(cd
->angle
)];
deltay
=i
*sine
[normalize
(cd
->angle
)];
sem_wait
(&mutex
);
colore
=grx_getpixel
(my_rint
(dx
+deltax
),my_rint
(dy
-deltay
));
colore1
=grx_getpixel
(my_rint
(dx1
+deltax
),my_rint
(dy1
-deltay
));
sem_post
(&mutex
);
if (((colore
==white
)||(colore1
==white
) || ((colore
==red
)||(colore1
==red
)))) {
if((colore
==white
)||(colore1
==white
))
col
=STOP
;
else
col
=CAR
;
cd
->dist_obs
=i
;
cd
->collision
=1;
speed_done
=1;
}
if (!speed_done
) {
if (sens
) {
sem_wait
(&mutex
);
grx_plot
(my_rint
(dx
+deltax
),my_rint
(dy
-deltay
),gray
);
grx_plot
(my_rint
(dx1
+deltax
),my_rint
(dy1
-deltay
),gray
);
sem_post
(&mutex
);
}
}
}
if (!speed_done
) {
cd
->collision
=0;
cd
->dist_obs
=MAX_LOOK
;
}
return col
;
}
int sensore
(car_data
*cd
) {
int col_front
,col_rear
,col_middle
;
int done_front
=0,done_rear
=0,done_middle
=0;
float front
,rear
,middle
,store_middle
;
int angolo
;
float i
;
float x1
,y1
,x05
,y05
;
float xs
,ys
,xs1
,ys1
;
float coordx
,coordy
,distx
,disty
;
cd
->front
=cd
->rear
=cd
->middle
=front
=rear
=middle
=store_middle
=NORM_DIST
;
x1
=cd
->xpos
-10.0*cosine
[normalize
(cd
->angle
)]; // coordinata sensore dietro
y1
=cd
->ypos
+10.0*sine
[normalize
(cd
->angle
)];
x05
=cd
->xpos
-5.0*cosine
[normalize
(cd
->angle
)]; // coordinata sensore mezzo
y05
=cd
->ypos
+5.0*sine
[normalize
(cd
->angle
)];
for (i
=0.0;i
<MAX_DIST
;i
+=0.1) {
coordx
=i
*cosine
[normalize
(cd
->angle
-LOOK_ANGLE
)];
coordy
=i
*sine
[normalize
(cd
->angle
-LOOK_ANGLE
)];
distx
=i
*cosine
[normalize
(cd
->angle
-DIST_ANGLE
)];
disty
=i
*sine
[normalize
(cd
->angle
-DIST_ANGLE
)];
col_front
=grx_getpixel
(my_rint
(cd
->xpos
+coordx
),my_rint
(cd
->ypos
-coordy
));
col_rear
=grx_getpixel
(my_rint
(x1
+coordx
),my_rint
(y1
-coordy
));
col_middle
=grx_getpixel
(my_rint
(x05
+distx
),my_rint
(y05
-disty
));
if ((col_front
==border
) && !done_front
) {
done_front
=1;
cd
->front
=front
=i
;
}
if ((col_middle
==border
) && !done_middle
) {
done_middle
=1;
middle
=cd
->middle
=i
;
}
if ((col_rear
==border
) && !done_rear
) {
done_rear
=1;
cd
->rear
=rear
=i
;
}
if (sens
) {
sem_wait
(&mutex
);
if (!done_front
) {
grx_plot
(my_rint
(cd
->xpos
+coordx
),my_rint
(cd
->ypos
-coordy
),gray
);
}
if (!done_rear
) {
grx_plot
(my_rint
(x1
+coordx
),my_rint
(y1
-coordy
),blue
);
}
if (!done_middle
) {
grx_plot
(my_rint
(x05
+distx
),my_rint
(y05
-disty
),green
);
}
sem_post
(&mutex
);
}
}
if (!done_front
) {
front
=cd
->front
=MAX_DIST
;
}
if (!done_rear
) {
rear
=cd
->rear
=MAX_DIST
;
}
if (!done_middle
) {
cd
->middle
=middle
=MAX_DIST
;
}
xs
=cd
->xpos
+front
*cosine
[normalize
(cd
->angle
-LOOK_ANGLE
)];
xs1
=x1
+rear
*cosine
[normalize
(cd
->angle
-LOOK_ANGLE
)];
ys
=cd
->ypos
-front
*sine
[normalize
(cd
->angle
-LOOK_ANGLE
)];
ys1
=y1
-rear
*sine
[normalize
(cd
->angle
-LOOK_ANGLE
)];
if (xs
==xs1
) {
angolo
=90;
} else {
angolo
=my_rint
(RAD_TO_DEG
(atan((-ys1
+ys
)/(xs1
-xs
))));
}
if (angolo
<0) angolo
+=360;
if (angolo
>=360) angolo
-=360;
if ((ys
-ys1
<0) && (xs1
-xs
>0)) {
angolo
-=180;
}
if ((ys
-ys1
>=0) && (xs1
-xs
>0)) {
angolo
+=180;
}
if ((xs1
-xs
==0) && (ys
-ys1
>0)) {
angolo
=270;
}
if (abs(middle
-NORM_DIST
)>FRANTIC
) {
cd
->correggi
=1;
} else {
cd
->somma
=0;
cd
->correggi
=0;
}
if (cd
->correggi
) {
if (middle
>NORM_DIST
) {
cd
->somma
-=CORR_FACT
;
}
if (middle
<NORM_DIST
) {
cd
->somma
+=CORR_FACT
;
}
}
angolo
+=cd
->somma
;
return angolo
;
}
void ch_spd
(car_data
*cd
,char ob_type
) {
float set_spd
=0.0;
float bf
;
if(cd
->collision
) {
if(ob_type
==STOP
)
bf
=0.001;
else
bf
=0.7;
set_spd
=cd
->speed
-(MAX_LOOK
-cd
->dist_obs
)*bf
;
if(set_spd
<0)
set_spd
=0;
}
else {
set_spd
=cd
->speed
+0.4;
if(set_spd
>MAX_SPEED
)
set_spd
=MAX_SPEED
;
}
cd
->speed
=set_spd
;
}