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;

}