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