Rev 1656 |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
/*
* Project: S.Ha.R.K.
*
* Coordinators:
* Giorgio Buttazzo <giorgio@sssup.it>
* Paolo Gai <pj@gandalf.sssup.it>
*
* Authors :
* Giacomo Guidi <giacomo@gandalf.sssup.it>
*
*
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
*
* http://www.sssup.it
* http://retis.sssup.it
* http://shark.sssup.it
*/
#include <ll/i386/defs.h>
#include <drivers/pclab.h>
#include <math.h>
#include <stdlib.h>
#include <kernel/log.h>
#include <GL/osmesa.h>
#include <GL/glut.h>
#include <modules/hartport.h>
#include <kernel/kern.h>
#include <kernel/func.h>
#include <ll/i386/x-dos.h>
#include <drivers/shark_keyb26.h>
#include <drivers/shark_fb26.h>
#include "const.h"
#define WIDTH 640
#define HEIGHT 480
#define BYTES_PP 2
unsigned long int PERIOD_CARRELLO
= 10000;
unsigned long int PERIOD_DISEGNA
= 80000;
struct Parametri prm
=PARAM
;
unsigned long int WCET_CARRELLO
, WCET_DISEGNA
;
TASK carrello
(void *);
PID carrello_PID
, disegna_PID
;
unsigned char *buffers
= NULL
;
unsigned char *vbuf
= NULL
;
OSMesaContext ctx
;
static GLfloat view_rotx
= 170.0, view_roty
= -200.0, view_rotz
= 0.0;
static GLfloat angle
;
GLUquadricObj
*quadratic
;
static GLfloat SUP_W
= 110.0;
static GLfloat SUP_P
= 30.0;
extern void da_motor
(float v
);
extern void *video_memory
;
#ifndef M_PI
#define M_PI 3.14159265
#endif
void program_key_end
(KEY_EVT
*k
)
{
da_motor
(0.0);
OSMesaDestroyContext
(ctx
);
free(buffers
);
sys_end
();
}
static void draw_box
(GLfloat p1x
, GLfloat p1y
,GLfloat p1z
, GLfloat p2x
, GLfloat p2y
, GLfloat p2z
) {
glBegin
(GL_QUADS
);
// Front Face
glVertex3f
(p1x
, p1y
, p1z
);
glVertex3f
(p2x
, p1y
, p1z
);
glVertex3f
(p2x
, p2y
, p1z
);
glVertex3f
(p1x
, p2y
, p1z
);
// Back Face
glVertex3f
(p1x
, p1y
, p2z
);
glVertex3f
(p1x
, p2y
, p2z
);
glVertex3f
(p2x
, p2y
, p2z
);
glVertex3f
(p2x
, p1y
, p2z
);
// Top Face
glVertex3f
(p1x
, p2y
, p2z
);
glVertex3f
(p1x
, p2y
, p1z
);
glVertex3f
(p2x
, p2y
, p1z
);
glVertex3f
(p2x
, p2y
, p2z
);
// Bottom Face
glVertex3f
(p1x
, p1y
, p2z
);
glVertex3f
(p2x
, p1y
, p2z
);
glVertex3f
(p2x
, p1y
, p1z
);
glVertex3f
(p1x
, p1y
, p1z
);
// Right face
glVertex3f
(p2x
, p1y
, p2z
);
glVertex3f
(p2x
, p2y
, p2z
);
glVertex3f
(p2x
, p2y
, p1z
);
glVertex3f
(p2x
, p1y
, p1z
);
// Left Face
glVertex3f
(p1x
, p1y
, p2z
);
glVertex3f
(p1x
, p1y
, p1z
);
glVertex3f
(p1x
, p2y
, p1z
);
glVertex3f
(p1x
, p2y
, p2z
);
glEnd
();
}
static void draw_carrello
(GLfloat x_scene
, GLfloat angle_scene
) {
static GLfloat gl_white
[3] = {1.0f, 1.0f, 1.0f};
static GLfloat gl_dark_gray
[3] = {0.3f, 0.3f, 0.3f};
static GLfloat gl_gray
[3] = {0.7f, 0.7f, 0.7f};
glPushMatrix
();
glColor3fv
(gl_dark_gray
);
glShadeModel
(GL_FLAT
);
glTranslatef
(x_scene
, 0.0f, 0.0f);
draw_box
(-6.0f, -6.0f, 9.0f, 6.0f, -2.0f, -9.0f);
draw_box
(-6.0f, -2.0f, 9.0f, 6.0f, -1.0f, 4.0f);
glColor3fv
(gl_white
);
draw_box
(-8.0f, -2.0f, -3.0f, 8.0f, -1.0f, -10.0f);
glColor3fv
(gl_dark_gray
);
draw_box
(-5.0f, 1.0f, 8.0f, 5.0f, 9.0f, 6.0f);
draw_box
(-5.0f,1.0f,-6.0f,5.0f,9.0f,-8.0f);
draw_box
(-5.0f,-1.0f,8.0f,5.0f,1.0f,-8.0f);
glShadeModel
(GL_SMOOTH
);
glColor3fv
(gl_white
);
glTranslatef
(0.0f, 4.5f, -6.0f);
gluCylinder
(quadratic
, 2.2f, 2.2f, 12.0f, 16, 16);
glColor3fv
(gl_gray
);
glTranslatef
(0.0f, 0.0f, 6.0f);
glRotatef
(-90.0f, 1.0f, 0.0f, 0.0f);
glRotatef
(angle_scene
, 0.0f, 1.0f, 0.0f);
gluCylinder
(quadratic
, 1.3f, 1.3f, 160.0f, 16, 16);
glRotatef
(-angle_scene
, 0.0f, 1.0f, 0.0f);
glRotatef
(-90.0f, 1.0f, 0.0f, 0.0f);
glTranslatef
(-4.0f, -2.5f, -6.0f);
gluCylinder
(quadratic
, 0.5f, 0.5f, 12.0f, 16, 16);
glTranslatef
(8.0f, 0.0f, 0.0f);
gluCylinder
(quadratic
, 0.5f, 0.5f, 12.0f, 16, 16);
glPopMatrix
();
}
static void draw_support
() {
static GLfloat gl_dark_gray
[3] = {0.3f, 0.3f, 0.3f};
static GLfloat gl_gray
[3] = {0.7f, 0.7f, 0.7f};
glPushMatrix
();
glColor3fv
(gl_gray
);
glShadeModel
(GL_FLAT
);
draw_box
(-SUP_W
/2, -0.1f, SUP_P
/2, SUP_W
/2, 0.1f, -SUP_P
/2);
glColor3fv
(gl_dark_gray
);
draw_box
(-SUP_W
/2-3.0f, -0.1f, SUP_P
/2, -SUP_W
/2, 14.0f, -SUP_P
/2-0.2f);
draw_box
(SUP_W
/2, -0.1f, SUP_P
/2, SUP_W
/2+3.0f, 14.0f, -SUP_P
/2-0.2f);
glShadeModel
(GL_SMOOTH
);
glColor3fv
(gl_gray
);
glTranslatef
(-SUP_W
/2, 6.0f, 0.0f);
glRotatef
(90.0f, 0.0f, 1.0f, 0.0f);
gluCylinder
(quadratic
, 1.5f, 1.5f, SUP_W
, 16, 16);
glPopMatrix
();
}
static void draw
(GLfloat prm_x
, GLfloat prm_th
)
{
prm_x
= -prm_x
/ LUNGH
* (SUP_W
-20.0f);
prm_th
= -prm_th
* FCA
;
glPushMatrix
();
glRotatef
(view_rotx
, 1.0f, 0.0f, 0.0f);
glRotatef
(view_roty
, 0.0f, 1.0f, 0.0f);
glRotatef
(view_rotz
, 0.0f, 0.0f, 1.0f);
glClear
(GL_COLOR_BUFFER_BIT
| GL_DEPTH_BUFFER_BIT
);
glRotatef
(angle
, 0.0f, 1.0f, 0.0f);
draw_support
();
glTranslatef
(0.0f, 10.0f, -4.0f);
draw_carrello
(prm_x
,prm_th
);
glPopMatrix
();
glFinish
();
}
static void init_gl
()
{
static GLfloat pos0
[4] = {40.0f, -70.0f, 180.0f, 1.0f};
glClearColor
(0.8f, 0.8f, 0.8f, 1.0f);
glLightfv
(GL_LIGHT0
, GL_POSITION
, pos0
);
glEnable
(GL_LIGHTING
);
glEnable
(GL_LIGHT0
);
glEnable
(GL_DEPTH_TEST
);
glEnable
(GL_CULL_FACE
);
glViewport
(0, 0, (GLint
) WIDTH
, (GLint
) HEIGHT
);
glMatrixMode
(GL_PROJECTION
);
glLoadIdentity
();
gluPerspective
(45.0f,(GLfloat
)WIDTH
/(GLfloat
)HEIGHT
,0.1f,250.0f);
glMatrixMode
(GL_MODELVIEW
);
glLoadIdentity
();
glTranslatef
(0.0f, 30.0f, -120.0f);
glColorMaterial
(GL_FRONT_AND_BACK
, GL_AMBIENT_AND_DIFFUSE
);
quadratic
=gluNewQuadric
();
glEnable
(GL_AUTO_NORMAL
);
glEnable
(GL_COLOR_MATERIAL
);
glEnable
(GL_NORMALIZE
);
glDepthFunc
(GL_LEQUAL
);
}
TASK disegna
(void)
{
PORT pr_x
, pr_th
;
float prm_x
, prm_th
;
char text
[100];
TIME disegna_TIME
, carrello_TIME
;
pr_x
= port_connect
("porta1",sizeof(float),STICK
,READ
);
pr_th
= port_connect
("porta2",sizeof(float),STICK
,READ
);
//vbuf = malloc(WIDTH * HEIGHT * 2); //Debug line
while(1) {
port_receive
(pr_x
, &prm_x
, BLOCK
);
port_receive
(pr_th
, &prm_th
, BLOCK
);
angle
+= 0.2;
draw
(prm_x
,prm_th
);
jet_gettable
(carrello_PID
, &carrello_TIME
, 1);
jet_gettable
(disegna_PID
, &disegna_TIME
, 1);
sprintf(text
,"Hard Task Control PER:%6d us EX:%6d us",(int)PERIOD_CARRELLO
,(int)carrello_TIME
);
grx_text
(text
,10,5,rgb16
(0,0,255),rgb16
(255,255,255));
sprintf(text
,"Hard Task Draw PER:%6d us EX:%6d us",(int)PERIOD_DISEGNA
,(int)disegna_TIME
);
grx_text
(text
,10,15,rgb16
(0,0,255),rgb16
(255,255,255));
memcpy(buffers
,vbuf
,WIDTH
*HEIGHT
*BYTES_PP
);
task_endcycle
();
}
port_disconnect
(pr_x
);
port_disconnect
(pr_th
);
sys_end
();
}
void waitenter
() {
KEY_EVT k
;
char esc
= FALSE
;
while(!esc
) {
keyb_getcode
(&k
,BLOCK
);
if (k.
ascii == 13) esc
= TRUE
;
}
}
void init_motor
(void)
{
char txt
[100];
da_motor
(0.0);
sprintf(txt
,"Calibrazione pendolo inverso...\n");
grx_text
(txt
,10,10,rgb16
(255,255,255),0);
sprintf(txt
,"Carr a sx e premi enter\n");
grx_text
(txt
,10,20,rgb16
(255,255,255),0);
waitenter
();
vmin
=ad_conv
(11);
sprintf(txt
,"Carr a dx e premi enter\n");
grx_text
(txt
,10,30,rgb16
(255,255,255),0);
waitenter
();
vmax
=ad_conv
(11);
sprintf(txt
,"Asta a sx e premi enter\n");
grx_text
(txt
,10,40,rgb16
(255,255,255),0);
waitenter
();
vminth
=ad_conv
(10);
sprintf(txt
,"Asta a dx e premi enter\n");
grx_text
(txt
,10,50,rgb16
(255,255,255),0);
waitenter
();
vmaxth
=ad_conv
(10);
sprintf(txt
,"Vxmax:%f Vxmin:%f Vthmax:%f Vthmin:%f\n",vmax
,vmin
,vmaxth
,vminth
);
grx_text
(txt
,10,60,rgb16
(255,255,255),0);
waitenter
();
}
int main
(int argc
, char *argv
[])
{
HARD_TASK_MODEL ht_carrello
, ht_disegna
;
vbuf
= video_memory
;
WCET_CARRELLO
=((long int) PERIOD_CARRELLO
* (0.05));
WCET_DISEGNA
=((long int) PERIOD_DISEGNA
* (0.875));
hard_task_default_model
(ht_carrello
);
hard_task_def_wcet
(ht_carrello
,WCET_CARRELLO
);
hard_task_def_mit
(ht_carrello
,PERIOD_CARRELLO
);
hard_task_def_usemath
(ht_carrello
);
hard_task_def_group
(ht_carrello
,1);
hard_task_def_ctrl_jet
(ht_carrello
);
carrello_PID
= task_create
("carrello", carrello
, &ht_carrello
, NULL
);
if (carrello_PID
== -1) {
sys_end
();
}
hard_task_default_model
(ht_disegna
);
hard_task_def_mit
(ht_disegna
,PERIOD_DISEGNA
);
hard_task_def_wcet
(ht_disegna
,WCET_DISEGNA
);
hard_task_def_group
(ht_disegna
,1);
hard_task_def_ctrl_jet
(ht_disegna
);
hard_task_def_usemath
(ht_disegna
);
hard_task_def_stack
(ht_disegna
,60000);
disegna_PID
= task_create
("disegna", disegna
, &ht_disegna
, NULL
);
if (disegna_PID
== -1) {
sys_end
();
}
{
KEY_EVT k
;
k.
flag = ALTL_BIT
;
k.
scan = KEY_C
;
k.
ascii = 'c';
k.
status = KEY_PRESSED
;
keyb_hook
(k
,program_key_end
,FALSE
);
}
init_motor
();
ctx
= OSMesaCreateContext
(OSMESA_RGB_565
, NULL
);
buffers
= malloc(WIDTH
*HEIGHT
*BYTES_PP
);
OSMesaMakeCurrent
(ctx
, buffers
, GL_UNSIGNED_SHORT_5_6_5
, WIDTH
, HEIGHT
);
init_gl
();
group_activate
(1);
return 0;
}