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 | } |