Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1124 → Rev 1125

/demos/trunk/kalm3d/initfile.c
0,0 → 1,120
/*
* Project: S.Ha.R.K.
*
* Coordinators:
* Giorgio Buttazzo <giorgio@sssup.it>
* Paolo Gai <pj@gandalf.sssup.it>
*
* Authors :
* Paolo Gai <pj@gandalf.sssup.it>
* (see the web pages for full authors list)
*
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
*
* http://www.sssup.it
* http://retis.sssup.it
* http://shark.sssup.it
*/
 
/*
------------
CVS : $Id: initfile.c,v 1.1 2003-03-13 13:11:40 giacomo Exp $
 
File: $File$
Revision: $Revision: 1.1 $
Last update: $Date: 2003-03-13 13:11:40 $
------------
 
System initialization file
 
This file contains the 2 functions needed to initialize the system.
 
These functions register the following levels:
 
an EDF (Earliest Deadline First) level
a RR (Round Robin) level
a CBS (Costant Bandwidth Server) level
a Dummy level
 
It can accept these task models:
 
HARD_TASK_MODEL (wcet+mit) at level 0
SOFT_TASK_MODEL (met, period) at level 1
NRT_TASK_MODEL at level 2
 
This file is similar to the configuration of kernel/init/hartik3.c
 
TICK is set to 0 (one-shot timer is used)
*/
 
/*
* Copyright (C) 2000 Paolo Gai
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
 
#include "kernel/kern.h"
#include "modules/edf.h"
#include "modules/cbs.h"
#include "modules/rr.h"
#include "modules/dummy.h"
 
#include "modules/sem.h"
#include "modules/hartport.h"
#include "modules/cabs.h"
 
#include "drivers/keyb.h"
 
 
/*+ sysyem tick in us +*/
#define TICK 0
 
/*+ RR tick in us +*/
#define RRTICK 10000
 
TIME __kernel_register_levels__(void *arg)
{
struct multiboot_info *mb = (struct multiboot_info *)arg;
 
EDF_register_level(EDF_ENABLE_ALL);
CBS_register_level(CBS_ENABLE_ALL, 0);
RR_register_level(RRTICK, RR_MAIN_YES, mb);
dummy_register_level();
 
SEM_register_module();
 
CABS_register_module();
 
return TICK;
}
 
TASK __init__(void *arg)
{
struct multiboot_info *mb = (struct multiboot_info *)arg;
 
KEYB_PARMS kparms = BASE_KEYB;
 
HARTPORT_init();
 
keyb_def_ctrlC(kparms, NULL);
keyb_def_map(kparms,itaMap);
KEYB_init(&kparms);
 
__call_main__(mb);
 
return (void *)0;
}
 
/demos/trunk/kalm3d/carrello.c
0,0 → 1,198
#include <math.h>
 
#include <modules/hartport.h>
#include <kernel/kern.h>
#include <kernel/func.h>
#include <ll/i386/x-dos.h>
 
#include "drivers/../../drivers/pcl812/pclab.h"
 
#include "const.h"
 
float vmax = 0.0, vmin = 0.0, vmaxth = 0.0, vminth = 0.0;
 
int da_motor(float v)
{
 
da_conv( 2.5 , 2 );
da_conv( v + 2.5 , 1 );
 
return(0);
 
}
 
float v2x(float v)
{
float x;
 
x= LUNGH * (v-(vmax+vmin)/2 ) / (vmax-vmin);
 
return x;
 
}
 
float v2theta(float v)
{
float theta;
theta=2.0*(THETAMAX/FCA) * (v-(vmaxth+vminth)/2 ) / (vmaxth-vminth);
return theta;
 
}
 
float bass1(float u)
{
float y;
static float oldy=0;
 
y=(oldy + prm.WCUT * DEADSECX(PERIOD_CARRELLO) *u)/(1+ prm.WCUT * DEADSECX(PERIOD_CARRELLO));
oldy=y;
 
return y;
 
}
 
float bass2(float u)
{
float y;
static float oldy=0;
 
y=(oldy + prm.WCUT1 * DEADSECX(PERIOD_CARRELLO) *u)/(1+ prm.WCUT1 * DEADSECX(PERIOD_CARRELLO));
oldy=y;
 
return y;
 
}
 
float bass3(float u)
{
float y;
static float oldy=0;
 
y=(oldy + prm.WCUT * DEADSECTH(PERIOD_CARRELLO) *u)/(1+ prm.WCUT * DEADSECTH(PERIOD_CARRELLO));
oldy=y;
 
return y;
 
}
 
float bass4(float u)
{
float y;
static float oldy=0;
 
y=(oldy + prm.WCUT1 * DEADSECTH(PERIOD_CARRELLO) *u)/(1+ prm.WCUT1 * DEADSECTH(PERIOD_CARRELLO));
oldy=y;
 
return y;
 
}
 
float dx(float u)
{
static float oldu=0;
float y;
 
y=(u-oldu)/DEADSECX(PERIOD_CARRELLO);
oldu=u;
 
return y;
 
}
 
float dth(float u)
{
static float oldu=0;
float y;
 
y=(u-oldu)/DEADSECTH(PERIOD_CARRELLO);
oldu=u;
 
return y;
 
}
 
TASK carrello(void)
{
float y[2]={0,0}, yp[2]={0,0};
PORT px, pth;
float th_input = 0.0,arr_th_input[AVR],th_eff,av_th_input;
float x_input = 0.0,arr_x_input[AVR],x_eff,av_x_input;
 
int flag_th=1,flag_x=1,index_th=0,index_x=0;
int i;
float offset,vout,vout_total;
px = port_create("porta1",sizeof(float),1,STICK,WRITE);
pth = port_create("porta2",sizeof(float),1,STICK,WRITE);
 
while (1) {
 
task_nopreempt();
th_input=ad_conv(10);
task_preempt();
if(flag_th==1) {
for(i=0; i<AVR;++i) arr_th_input[i] = th_input;
flag_th=0;
}
av_th_input=0;
for(i=0;i<AVR;++i) av_th_input += arr_th_input[i];
av_th_input /= AVR ;
 
if(fabs(th_input-av_th_input)>=prm.NOISE) th_input=av_th_input;
 
arr_th_input[index_th]=th_input;
index_th = (index_th+1) % AVR ;
 
th_eff = v2theta(th_input);
y[1] = bass3(th_eff);
yp[1] = bass4(dth(y[1]));
 
task_nopreempt();
x_input=ad_conv(11);
task_preempt();
 
if(flag_x==1) {
for(i=0; i<AVR;++i) arr_x_input[i] = x_input;
flag_x=0;
}
av_x_input=0;
for(i=0;i<AVR;++i) av_x_input += arr_x_input[i];
av_x_input /= AVR;
if(fabs(x_input-av_x_input)>=prm.NOISE) x_input = av_x_input;
 
arr_x_input[index_x] = x_input;
index_x = (index_x+1) % AVR ;
x_eff = v2x(x_input);
y[0] = bass1(x_eff);
yp[0] = bass2(dx(y[0]));
vout = prm.COST * (prm.GUAD[0] * (y[0]-v2x((vmax+vmin)/2)) + prm.GUAD[1] * th_eff\
+ prm.GUAD[2] * yp[0] + prm.GUAD[3] * yp[1]);
 
if(vout >= 0) offset=prm.OFFSVAL;
else offset=-prm.OFFSVAL;
 
vout_total = vout + offset;
 
if(vout_total >= VDANG ) vout_total = VDANG ;
if(vout_total <= -VDANG ) vout_total = -VDANG ;
da_motor(vout_total);
 
port_send(px, &y[0], NON_BLOCK);
port_send(pth, &y[1], NON_BLOCK);
 
task_endcycle();
}
port_delete(px);
port_delete(pth);
 
} /* FINE DEL TASK CARRELLO */
/demos/trunk/kalm3d/const.h
0,0 → 1,36
#ifndef __CONST__
#define __CONST__
 
extern unsigned long int PERIOD_CARRELLO;
extern unsigned long int PERIOD_DISEGNA;
 
#define DEADSECX(x) (x * 1.0e-6 )
#define DEADSECTH(x) (x * 1.0e-6 )
 
#define PARAM {{ 1. , 300 , 0.79 , 23 },0.70 ,2.5 ,20.,20., 0.1, 1 , 1, 0.}
 
#define LUNGH 35.0
#define THETAMAX 45.0
#define FCA 57.29578
#define AVR 10
#define VDANG 2.4
#define NMAX 300
#define SEC_ID 7.0
#define MAXRUN_ID 9 //16
 
struct Parametri{
float GUAD[4];
float COST;
float NOISE;
float WCUT;
float WCUT1;
float OFFSVAL;
int DEADLINE;
int SCAN;
float XTRASL;
};
 
extern struct Parametri prm;
extern float vmax, vmin, vmaxth, vminth;
 
#endif
/demos/trunk/kalm3d/kalm3d.c
0,0 → 1,453
/*
* 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/vga.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
#define INITSTR G640x480x64K
#define CARD SAVAGE
 
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);
 
vga_setmode(TEXT,CARD);
 
sys_end();
 
}
 
void program_key_end(KEY_EVT *k)
{
 
program_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;
static unsigned long int count = WIDTH * HEIGHT / 2;
 
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));
copy_videomem_16to16(buffers,vbuf,count);
 
task_endcycle();
 
}
 
port_disconnect(pr_x);
port_disconnect(pr_th);
 
sys_end();
 
}
 
static void screen(int mode)
{
vga_modeinfo *minf;
int mem;
 
vga_setmode(mode,CARD);
minf = vga_getmodeinfo(mode);
if(! (minf->flags & CAPABLE_LINEAR)){
vga_setmode(TEXT,CARD);
printk(KERN_INFO "The mode %d is not capable of linear\n",mode);
return;
}
vga_setpage(0);
if(vga_setlinearaddressing() == -1) {
vga_setmode(TEXT,CARD);
printk(KERN_INFO "Could not set linear addressing for mode %d\n",mode);
return;
}
 
mem = minf->linewidth*minf->height;
 
vbuf = vga_getgraphmem();
printk(KERN_INFO "Memory mapped to %08x. Mode = %d.\n",(int) vbuf,mode);
memset(vbuf,0,mem);
 
}
 
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;
 
clear();
WCET_CARRELLO =((long int) PERIOD_CARRELLO * (0.05));
WCET_DISEGNA =((long int) PERIOD_DISEGNA * (0.875));
 
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);
 
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(INITSTR);
 
ctx = OSMesaCreateContext(OSMESA_RGB_565, NULL );
buffers = malloc(WIDTH * HEIGHT * 2);
 
grx_setbuffer(buffers, WIDTH, HEIGHT);
OSMesaMakeCurrent(ctx, buffers, GL_UNSIGNED_SHORT_5_6_5, WIDTH, HEIGHT);
init_gl();
group_activate(1);
 
return 0;
 
}
/demos/trunk/kalm3d/makefile
0,0 → 1,19
#
#
#
 
ifndef BASE
BASE=../..
endif
include $(BASE)/config/config.mk
 
PROGS = kalm3d
GRAPH_DRV = svga
 
include $(BASE)/config/example.mk
 
kalm3d:
make -f $(SUBMAKE) GRAPH_DRV=svga APP=kalm3d INIT= OTHEROBJS="initfile.o carrello.o" OTHERINCL=
cd $(BASE)/demos/kalm3d/
zip -9 kalm3d.zip kalm3d