Subversion Repositories shark

Rev

Rev 1313 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1125 giacomo 1
/*
2
 * Project: S.Ha.R.K.
3
 *
4
 * Coordinators:
5
 *   Giorgio Buttazzo    <giorgio@sssup.it>
6
 *   Paolo Gai           <pj@gandalf.sssup.it>
7
 *
8
 * Authors     :
9
 *   Giacomo Guidi       <giacomo@gandalf.sssup.it>
10
 *
11
 *
12
 * ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
13
 *
14
 * http://www.sssup.it
15
 * http://retis.sssup.it
16
 * http://shark.sssup.it
17
 */
18
 
19
#include <ll/i386/defs.h>
20
 
1313 giacomo 21
#include <drivers/glib.h>
1125 giacomo 22
#include <drivers/pclab.h>
23
 
24
#include <math.h>
25
#include <stdlib.h>
26
#include <kernel/log.h>
27
#include <GL/osmesa.h>
28
#include <GL/glut.h>
29
 
30
#include <modules/hartport.h>
31
#include <kernel/kern.h>
32
#include <kernel/func.h>
33
#include <ll/i386/x-dos.h>
34
#include <drivers/keyb.h>
35
 
36
#include "const.h"
37
 
38
#define WIDTH 640
39
#define HEIGHT 480
40
#define BYTES_PP 2
41
 
42
unsigned long int PERIOD_CARRELLO = 10000;
43
unsigned long int PERIOD_DISEGNA = 80000;
44
struct Parametri prm=PARAM;
45
 
46
unsigned long int WCET_CARRELLO, WCET_DISEGNA;
47
 
48
TASK carrello(void *);
49
 
50
PID carrello_PID, disegna_PID;
51
 
52
unsigned char *buffers = NULL;
53
unsigned char *vbuf = NULL;
54
OSMesaContext ctx;
55
 
56
static GLfloat view_rotx = 170.0, view_roty = -200.0, view_rotz = 0.0;
57
static GLfloat angle;
58
 
59
GLUquadricObj *quadratic;
60
 
61
static GLfloat SUP_W = 110.0;
62
static GLfloat SUP_P = 30.0;
63
 
64
extern void da_motor(float v);
65
 
66
#ifndef M_PI
67
#define M_PI 3.14159265
68
#endif
69
 
70
void program_end(void)
71
{
72
 
73
  da_motor(0.0);
74
 
75
  OSMesaDestroyContext(ctx);
76
  free(buffers);
77
 
1313 giacomo 78
  grx_close();
1125 giacomo 79
 
80
  sys_end();
81
 
82
}
83
 
84
void program_key_end(KEY_EVT *k)
85
{
86
 
1148 giacomo 87
  sys_end();
1125 giacomo 88
 
89
}
90
 
91
static void draw_box(GLfloat p1x, GLfloat p1y ,GLfloat p1z, GLfloat p2x, GLfloat p2y, GLfloat p2z) {
92
 
93
        glBegin(GL_QUADS);     
94
 
95
                // Front Face
96
                glVertex3f(p1x, p1y,  p1z);    
97
                glVertex3f(p2x, p1y,  p1z);
98
                glVertex3f(p2x, p2y,  p1z);
99
                glVertex3f(p1x, p2y,  p1z);    
100
 
101
                // Back Face
102
                glVertex3f(p1x, p1y,  p2z);    
103
                glVertex3f(p1x, p2y,  p2z);
104
                glVertex3f(p2x, p2y,  p2z);
105
                glVertex3f(p2x, p1y,  p2z);    
106
 
107
                // Top Face
108
                glVertex3f(p1x, p2y,  p2z);    
109
                glVertex3f(p1x, p2y,  p1z);
110
                glVertex3f(p2x, p2y,  p1z);
111
                glVertex3f(p2x, p2y,  p2z);            
112
 
113
                // Bottom Face
114
                glVertex3f(p1x, p1y,  p2z);    
115
                glVertex3f(p2x, p1y,  p2z);
116
                glVertex3f(p2x, p1y,  p1z);
117
                glVertex3f(p1x, p1y,  p1z);            
118
 
119
                // Right face
120
                glVertex3f(p2x, p1y,  p2z);    
121
                glVertex3f(p2x, p2y,  p2z);
122
                glVertex3f(p2x, p2y,  p1z);
123
                glVertex3f(p2x, p1y,  p1z);                    
124
 
125
                // Left Face
126
                glVertex3f(p1x, p1y,  p2z);    
127
                glVertex3f(p1x, p1y,  p1z);
128
                glVertex3f(p1x, p2y,  p1z);
129
                glVertex3f(p1x, p2y,  p2z);            
130
 
131
        glEnd();
132
 
133
}
134
 
135
static void draw_carrello(GLfloat x_scene, GLfloat angle_scene) {
136
 
137
        static GLfloat gl_white[3] = {1.0f, 1.0f, 1.0f};
138
        static GLfloat gl_dark_gray[3] = {0.3f, 0.3f, 0.3f};
139
        static GLfloat gl_gray[3] = {0.7f, 0.7f, 0.7f};
140
 
141
        glPushMatrix();
142
 
143
                glColor3fv(gl_dark_gray);
144
 
145
                glShadeModel(GL_FLAT);
146
 
147
                glTranslatef(x_scene, 0.0f, 0.0f);
148
 
149
                draw_box(-6.0f, -6.0f, 9.0f, 6.0f, -2.0f, -9.0f);
150
                draw_box(-6.0f, -2.0f, 9.0f, 6.0f, -1.0f, 4.0f);
151
 
152
                glColor3fv(gl_white);
153
 
154
                draw_box(-8.0f, -2.0f, -3.0f, 8.0f, -1.0f, -10.0f);
155
 
156
                glColor3fv(gl_dark_gray);
157
 
158
                draw_box(-5.0f, 1.0f, 8.0f, 5.0f, 9.0f, 6.0f);
159
                draw_box(-5.0f,1.0f,-6.0f,5.0f,9.0f,-8.0f);
160
                draw_box(-5.0f,-1.0f,8.0f,5.0f,1.0f,-8.0f);
161
 
162
                glShadeModel(GL_SMOOTH);
163
 
164
                glColor3fv(gl_white);
165
 
166
                glTranslatef(0.0f, 4.5f, -6.0f);
167
                gluCylinder(quadratic, 2.2f, 2.2f, 12.0f, 16, 16);
168
 
169
                glColor3fv(gl_gray);
170
 
171
                glTranslatef(0.0f, 0.0f, 6.0f);
172
                glRotatef(-90.0f, 1.0f, 0.0f, 0.0f);
173
                glRotatef(angle_scene, 0.0f, 1.0f, 0.0f);
174
                gluCylinder(quadratic, 1.3f, 1.3f, 160.0f, 16, 16);
175
                glRotatef(-angle_scene, 0.0f, 1.0f, 0.0f);
176
                glRotatef(-90.0f, 1.0f, 0.0f, 0.0f);
177
 
178
                glTranslatef(-4.0f, -2.5f, -6.0f);
179
                gluCylinder(quadratic, 0.5f, 0.5f, 12.0f, 16, 16);
180
 
181
                glTranslatef(8.0f, 0.0f, 0.0f);
182
                gluCylinder(quadratic, 0.5f, 0.5f, 12.0f, 16, 16);
183
 
184
        glPopMatrix();
185
 
186
}
187
 
188
static void draw_support() {
189
 
190
        static GLfloat gl_dark_gray[3] = {0.3f, 0.3f, 0.3f};
191
        static GLfloat gl_gray[3] = {0.7f, 0.7f, 0.7f};
192
 
193
        glPushMatrix();
194
 
195
                glColor3fv(gl_gray);
196
 
197
                glShadeModel(GL_FLAT);
198
 
199
                draw_box(-SUP_W/2, -0.1f, SUP_P/2, SUP_W/2, 0.1f, -SUP_P/2);
200
 
201
                glColor3fv(gl_dark_gray);
202
 
203
                draw_box(-SUP_W/2-3.0f, -0.1f, SUP_P/2, -SUP_W/2, 14.0f, -SUP_P/2-0.2f);
204
                draw_box(SUP_W/2, -0.1f, SUP_P/2, SUP_W/2+3.0f, 14.0f, -SUP_P/2-0.2f);
205
 
206
                glShadeModel(GL_SMOOTH);
207
 
208
                glColor3fv(gl_gray);
209
 
210
                glTranslatef(-SUP_W/2, 6.0f, 0.0f);
211
                glRotatef(90.0f, 0.0f, 1.0f, 0.0f);
212
                gluCylinder(quadratic, 1.5f, 1.5f, SUP_W, 16, 16);
213
 
214
        glPopMatrix();
215
 
216
}
217
 
218
static void draw(GLfloat prm_x, GLfloat prm_th)
219
{
220
 
221
        prm_x = -prm_x / LUNGH * (SUP_W-20.0f);
222
        prm_th = -prm_th * FCA;
223
 
224
        glPushMatrix();
225
 
226
                glRotatef(view_rotx, 1.0f, 0.0f, 0.0f);
227
                glRotatef(view_roty, 0.0f, 1.0f, 0.0f);
228
                glRotatef(view_rotz, 0.0f, 0.0f, 1.0f);
229
 
230
                glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
231
 
232
                glRotatef(angle, 0.0f, 1.0f, 0.0f);
233
 
234
                draw_support();
235
                glTranslatef(0.0f, 10.0f, -4.0f);
236
                draw_carrello(prm_x,prm_th);   
237
 
238
        glPopMatrix();
239
 
240
        glFinish();
241
 
242
}
243
 
244
static void init_gl()
245
{
246
 
247
  static GLfloat pos0[4] = {40.0f, -70.0f, 180.0f, 1.0f};
248
 
249
  glClearColor(0.8f, 0.8f, 0.8f, 1.0f);
250
 
251
  glLightfv(GL_LIGHT0, GL_POSITION, pos0);
252
  glEnable(GL_LIGHTING);
253
  glEnable(GL_LIGHT0);
254
  glEnable(GL_DEPTH_TEST);
255
  glEnable(GL_CULL_FACE);
256
 
257
  glViewport(0, 0, (GLint) WIDTH, (GLint) HEIGHT);
258
  glMatrixMode(GL_PROJECTION);
259
  glLoadIdentity();
260
  gluPerspective(45.0f,(GLfloat)WIDTH/(GLfloat)HEIGHT,0.1f,250.0f);
261
  glMatrixMode(GL_MODELVIEW);
262
  glLoadIdentity();
263
  glTranslatef(0.0f, 30.0f, -120.0f);
264
 
265
  glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
266
 
267
  quadratic=gluNewQuadric();
268
 
269
  glEnable(GL_AUTO_NORMAL);
270
  glEnable(GL_COLOR_MATERIAL);
271
  glEnable(GL_NORMALIZE);
272
  glDepthFunc(GL_LEQUAL);
273
 
274
}
275
 
276
TASK disegna(void)
277
{
278
 
279
  PORT pr_x, pr_th;
280
  float prm_x, prm_th;
281
 
282
  char text[100];
283
  TIME disegna_TIME, carrello_TIME;
284
 
285
  pr_x = port_connect("porta1",sizeof(float),STICK,READ);
286
  pr_th = port_connect("porta2",sizeof(float),STICK,READ);
287
 
288
  //vbuf = malloc(WIDTH * HEIGHT * 2); //Debug line
289
 
290
  while(1) {
291
 
292
    port_receive(pr_x, &prm_x, BLOCK);
293
    port_receive(pr_th, &prm_th, BLOCK);
294
 
295
    angle += 0.2;
296
 
297
    draw(prm_x,prm_th);
298
 
299
    jet_gettable(carrello_PID, &carrello_TIME, 1);
300
    jet_gettable(disegna_PID, &disegna_TIME, 1);
301
 
302
    sprintf(text,"Hard Task Control PER:%6d us EX:%6d us",(int)PERIOD_CARRELLO,(int)carrello_TIME);
303
    grx_text(text,10,5,rgb16(0,0,255),rgb16(255,255,255));    
304
    sprintf(text,"Hard Task Draw    PER:%6d us EX:%6d us",(int)PERIOD_DISEGNA,(int)disegna_TIME);
305
    grx_text(text,10,15,rgb16(0,0,255),rgb16(255,255,255));
306
 
1315 giacomo 307
    memcpy(buffers,vbuf,WIDTH*HEIGHT*BYTES_PP);
1125 giacomo 308
 
309
    task_endcycle();
310
 
311
  }
312
 
313
  port_disconnect(pr_x);
314
  port_disconnect(pr_th);
315
 
316
  sys_end();
317
 
318
}
319
 
1313 giacomo 320
static void screen()
1125 giacomo 321
{
1313 giacomo 322
  extern DWORD flbaddr;
323
 
324
  /* graphic card Initialization */
325
  if (grx_init() < 1) {
326
     sys_abort(1);
1125 giacomo 327
  }
1313 giacomo 328
 
329
  if (grx_open(640, 480, 16) < 0) {
330
     cprintf("GRX Err\n");
331
     sys_abort(1);
1125 giacomo 332
  }
1313 giacomo 333
 
334
  vbuf = (unsigned char *)flbaddr;
1125 giacomo 335
 
336
}
337
 
338
void waitenter() {
339
 
340
        KEY_EVT k;
341
        char esc = FALSE;
342
 
343
        while(!esc) {
344
                keyb_getcode(&k,BLOCK);
345
                if (k.ascii == 13) esc = TRUE;
346
        }
347
 
348
}
349
 
350
 
351
void init_motor(void)
352
{
353
 
354
        da_motor(0.0);
355
 
356
        cprintf("Calibrazione pendolo inverso...\n");
357
 
358
        cprintf("Carr a sx e premi enter\n");
359
        waitenter();
360
        vmin=ad_conv(11);
361
 
362
        cprintf("Carr a dx e premi enter\n");
363
        waitenter();
364
        vmax=ad_conv(11);
365
 
366
        cprintf("Asta a sx e premi enter\n");
367
        waitenter();
368
        vminth=ad_conv(10);
369
 
370
        cprintf("Asta a dx e premi enter\n");
371
        waitenter();
372
        vmaxth=ad_conv(10);
373
 
374
        cprintf("Vxmax:%f Vxmin:%f Vthmax:%f Vthmin:%f\n",vmax,vmin,vmaxth,vminth);
375
        waitenter();
376
 
377
}
378
 
379
int main (int argc, char *argv[])
380
{
381
    HARD_TASK_MODEL ht_carrello, ht_disegna;
382
 
383
    WCET_CARRELLO =((long int) PERIOD_CARRELLO * (0.05));
384
    WCET_DISEGNA =((long int) PERIOD_DISEGNA * (0.875));
385
 
1149 giacomo 386
    clear();
387
 
1125 giacomo 388
    sys_atrunlevel((void *) program_end,NULL, RUNLEVEL_BEFORE_EXIT);
389
 
390
    hard_task_default_model(ht_carrello);
391
    hard_task_def_wcet(ht_carrello,WCET_CARRELLO);
392
    hard_task_def_mit(ht_carrello,PERIOD_CARRELLO);
393
    hard_task_def_usemath(ht_carrello);
394
    hard_task_def_group(ht_carrello,1);
395
    hard_task_def_ctrl_jet(ht_carrello);
396
 
397
    carrello_PID = task_create("carrello", carrello, &ht_carrello, NULL);
398
    if (carrello_PID == -1) {
399
      sys_end();
400
      exit(4);
401
    }
402
 
403
    hard_task_default_model(ht_disegna);
404
    hard_task_def_mit(ht_disegna,PERIOD_DISEGNA);
405
    hard_task_def_wcet(ht_disegna,WCET_DISEGNA);
406
    hard_task_def_group(ht_disegna,1);
407
    hard_task_def_ctrl_jet(ht_disegna);
408
    hard_task_def_usemath(ht_disegna);
1148 giacomo 409
    hard_task_def_stack(ht_disegna,60000);
410
 
1125 giacomo 411
    disegna_PID = task_create("disegna", disegna, &ht_disegna, NULL);
412
    if (disegna_PID == -1) {
413
      sys_end();
414
      exit(4);
415
    }
416
 
417
    {
418
      KEY_EVT k;
419
      k.flag = ALTL_BIT;
420
      k.scan = KEY_C;
421
      k.ascii = 'c';
422
      keyb_hook(k,program_key_end);
423
    }
424
 
425
    init_motor();
426
 
1313 giacomo 427
    screen();
1125 giacomo 428
 
429
    ctx = OSMesaCreateContext(OSMESA_RGB_565, NULL );
430
 
1313 giacomo 431
    buffers = malloc(WIDTH*HEIGHT*BYTES_PP);
432
 
1125 giacomo 433
    OSMesaMakeCurrent(ctx, buffers, GL_UNSIGNED_SHORT_5_6_5, WIDTH, HEIGHT);
434
 
435
    init_gl();
436
 
437
    group_activate(1);
438
 
439
    return 0;
440
 
441
}