0,0 → 1,453 |
/* |
* 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/vga.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/keyb.h> |
|
#include "const.h" |
|
#define WIDTH 640 |
#define HEIGHT 480 |
#define BYTES_PP 2 |
#define INITSTR G640x480x64K |
#define CARD SAVAGE |
|
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); |
|
#ifndef M_PI |
#define M_PI 3.14159265 |
#endif |
|
void program_end(void) |
{ |
|
da_motor(0.0); |
|
OSMesaDestroyContext(ctx); |
free(buffers); |
|
vga_setmode(TEXT,CARD); |
|
sys_end(); |
|
} |
|
void program_key_end(KEY_EVT *k) |
{ |
|
program_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; |
|
static unsigned long int count = WIDTH * HEIGHT / 2; |
|
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)); |
|
copy_videomem_16to16(buffers,vbuf,count); |
|
task_endcycle(); |
|
} |
|
port_disconnect(pr_x); |
port_disconnect(pr_th); |
|
sys_end(); |
|
} |
|
static void screen(int mode) |
{ |
vga_modeinfo *minf; |
int mem; |
|
vga_setmode(mode,CARD); |
minf = vga_getmodeinfo(mode); |
if(! (minf->flags & CAPABLE_LINEAR)){ |
vga_setmode(TEXT,CARD); |
printk(KERN_INFO "The mode %d is not capable of linear\n",mode); |
return; |
} |
vga_setpage(0); |
if(vga_setlinearaddressing() == -1) { |
vga_setmode(TEXT,CARD); |
printk(KERN_INFO "Could not set linear addressing for mode %d\n",mode); |
return; |
} |
|
mem = minf->linewidth*minf->height; |
|
vbuf = vga_getgraphmem(); |
printk(KERN_INFO "Memory mapped to %08x. Mode = %d.\n",(int) vbuf,mode); |
memset(vbuf,0,mem); |
|
} |
|
void waitenter() { |
|
KEY_EVT k; |
char esc = FALSE; |
|
while(!esc) { |
keyb_getcode(&k,BLOCK); |
if (k.ascii == 13) esc = TRUE; |
} |
|
} |
|
|
void init_motor(void) |
{ |
|
da_motor(0.0); |
|
cprintf("Calibrazione pendolo inverso...\n"); |
|
cprintf("Carr a sx e premi enter\n"); |
waitenter(); |
vmin=ad_conv(11); |
|
cprintf("Carr a dx e premi enter\n"); |
waitenter(); |
vmax=ad_conv(11); |
|
cprintf("Asta a sx e premi enter\n"); |
waitenter(); |
vminth=ad_conv(10); |
|
cprintf("Asta a dx e premi enter\n"); |
waitenter(); |
vmaxth=ad_conv(10); |
|
cprintf("Vxmax:%f Vxmin:%f Vthmax:%f Vthmin:%f\n",vmax,vmin,vmaxth,vminth); |
waitenter(); |
|
} |
|
int main (int argc, char *argv[]) |
{ |
HARD_TASK_MODEL ht_carrello, ht_disegna; |
|
clear(); |
|
WCET_CARRELLO =((long int) PERIOD_CARRELLO * (0.05)); |
WCET_DISEGNA =((long int) PERIOD_DISEGNA * (0.875)); |
|
sys_atrunlevel((void *) program_end,NULL, RUNLEVEL_BEFORE_EXIT); |
|
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(); |
exit(4); |
} |
|
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); |
|
disegna_PID = task_create("disegna", disegna, &ht_disegna, NULL); |
if (disegna_PID == -1) { |
sys_end(); |
exit(4); |
} |
|
{ |
KEY_EVT k; |
k.flag = ALTL_BIT; |
k.scan = KEY_C; |
k.ascii = 'c'; |
keyb_hook(k,program_key_end); |
} |
|
init_motor(); |
|
screen(INITSTR); |
|
ctx = OSMesaCreateContext(OSMESA_RGB_565, NULL ); |
buffers = malloc(WIDTH * HEIGHT * 2); |
|
grx_setbuffer(buffers, WIDTH, HEIGHT); |
OSMesaMakeCurrent(ctx, buffers, GL_UNSIGNED_SHORT_5_6_5, WIDTH, HEIGHT); |
|
init_gl(); |
|
group_activate(1); |
|
return 0; |
|
} |