Subversion Repositories shark

/*

 * 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/glib.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

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);

  grx_close();

  sys_end();

}

void program_key_end(KEY_EVT *k)

{

  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();

}

static void screen()

{

  extern DWORD flbaddr;

  /* graphic card Initialization */

  if (grx_init() < 1) {

     sys_abort(1);

  }

  if (grx_open(640, 480, 16) < 0) {

     cprintf("GRX Err\n");

     sys_abort(1);

  }

  vbuf = (unsigned char *)flbaddr;

}

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;

    WCET_CARRELLO =((long int) PERIOD_CARRELLO * (0.05));

    WCET_DISEGNA =((long int) PERIOD_DISEGNA * (0.875));

    clear();

    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);

    hard_task_def_stack(ht_disegna,60000);

    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();

    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;

}