/demos/trunk/mix/mix.c |
---|
18,11 → 18,11 |
/* |
------------ |
CVS : $Id: mix.c,v 1.6 2004-05-23 11:15:29 giacomo Exp $ |
CVS : $Id: mix.c,v 1.5 2004-05-23 10:15:12 giacomo Exp $ |
File: $File$ |
Revision: $Revision: 1.6 $ |
Last update: $Date: 2004-05-23 11:15:29 $ |
Revision: $Revision: 1.5 $ |
Last update: $Date: 2004-05-23 10:15:12 $ |
------------ |
*/ |
56,7 → 56,6 |
#include <semaphore.h> |
#include <stdlib.h> |
#include <string.h> |
#include <math.h> |
#define PIG 3.1415 |
75,7 → 74,7 |
#define YWM 165 /* top Y of MIDDLE window */ |
#define YWL 325 /* top Y of LOW window */ |
int flen = 0; /* file length */ |
int flen; /* file length */ |
int fine = 0; /* ending flag */ |
sem_t mx_mat, mx_grf; /* mutex semaphores */ |
92,17 → 91,17 |
" June 5, 2001 "}; |
char fbuf[1000] = "\ |
TASK NAME PERIOD WCET\n\ |
------------------------------------------\n\ |
task1 watch: 1000000 200\n\ |
task2 tasto: 2000 200\n\ |
task3 palla: 2000 200\n\ |
task4 mosca: 20000 200\n\ |
task5 infor: 20000 300\n\ |
task6 ruota: 5000 400\n\ |
task7 color: 2000 200\n\ |
task8 pendo: 5000 400\n\ |
------------------------------------------\n"; |
TASK NAME PERIOD WCET\ |
------------------------------------------\ |
task1 watch: 1000000 200\ |
task2 tasto: 2000 200\ |
task3 palla: 2000 200\ |
task4 mosca: 20000 200\ |
task5 infor: 20000 300\ |
task6 ruota: 5000 400\ |
task7 color: 2000 200\ |
task8 pendo: 5000 400\ |
------------------------------------------"; |
extern int vga16color[16]; |
138,12 → 137,12 |
int x = 0; |
int i; |
flen = strlen(fbuf); |
for (i=1; i<=8; i++) { |
while ((fbuf[x] != ':') && (x < flen)) x++; |
x++; |
sscanf(&fbuf[x], "%d %d", &period[i], &wcet[i]); |
cprintf("per[%d] = %d, wcet[%d] = %d\n", |
i, period[i], i, wcet[i]); |
} |
} |
682,6 → 681,7 |
srand(seme); |
get_par(); |
keyb_getch(BLOCK); |
grx_rect(XWL,YWH,XWL+LW,YWH+HW,vga16color[14]); |
grx_rect(XWM,YWH,XWM+LW,YWH+HW,vga16color[14]); |
/demos/trunk/servo/ctrl.c |
---|
0,0 → 1,58 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it> |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/* |
* 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 "drivers/keyb.h" |
#include "servo.h" |
void set_leg_position(double px, double py, double pz); |
int main () { |
char ch; |
set_leg_position(10.0,0.0,0.0); |
ch = keyb_getch(BLOCK); |
set_leg_position(10.0,0.0,10.0); |
ch = keyb_getch(BLOCK); |
set_leg_position(10.0,0.0,-10.0); |
ch = keyb_getch(BLOCK); |
return 0; |
} |
/demos/trunk/servo/leg.c |
---|
0,0 → 1,85 |
#include <kernel/kern.h> |
#include <math.h> |
#include "servo.h" |
#define LEG_A 10.0 |
#define LEG_B 10.0 |
#define LEG_C 2.0 |
#define LEG_D 3.0 |
#define LEG_CD_2IPO 2*3.603 // 2*sqrt(LEG_C^2+LEG_D^2) |
#define LEG_CD_ANG 33.690 // arctg(LEG_C/LEG_D) in gradi |
const double c0 = LEG_C * LEG_C; |
const double c1 = LEG_B * LEG_B; |
const double c2 = LEG_B * LEG_B - LEG_A * LEG_A; |
const double todeg = 180.0 / PI; |
int set_leg_position(double px, double py, double pz) |
{ |
double px2 = px * px; |
double py2 = py * py; |
double pz2 = pz * pz; |
double pxz2 = px2 + pz2; |
int alfa,beta,gamma; |
double alfa1,beta1,alfa2,beta2,gamma1; |
double m,dsqrt; |
double delta_xz = pxz2 - c0; |
double s,k,k2,y1,delta_xy; |
if (delta_xz < 0.0) return -1; |
if (pz >= LEG_C) { |
gamma1 = acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg; |
} else { |
gamma1 = -acos((pz * LEG_C + px * sqrt(delta_xz)) / pxz2) * todeg; |
} |
if (gamma1 < -90.0 || gamma1 > 90.0) return -1; |
m = pxz2 - LEG_CD_2IPO * (sin(gamma+LEG_CD_ANG)+cos(gamma+LEG_CD_ANG)); |
s = m + py2; |
k = c2 + s; |
k2 = k * k; |
delta_xy = py2 * k2 - s * (k2 - 4.0 * m * c1); |
if (delta_xy >= 0.0) { |
dsqrt = sqrt(delta_xy); |
y1 = (py * k + dsqrt) / (2.0 * s); |
beta1 = asin(y1/LEG_B) * todeg; |
alfa1 = asin((y1 - py)/LEG_A) * todeg + beta1; |
y1 = (py * k - dsqrt) / (2.0 * s); |
beta2 = asin(y1/LEG_B) * todeg; |
alfa2 = asin((y1 - py)/LEG_A) * todeg + beta2; |
if ((alfa1 >= 0.0 && alfa1 <= 180.0) && (beta1 >= -90.0 && beta1 <= 90.0)) { |
alfa = (int)(alfa1 * 3600.0); |
beta = (int)(beta1 * 3600.0); |
gamma = (int)(gamma1 * 3600.0); |
cprintf("Alfa Sec = %d Beta Sec = %d Gamma Sec = %d\n",alfa,beta,gamma); |
return 0; |
} else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) { |
alfa = (int)(alfa2 * 3600.0); |
beta = (int)(beta2 * 3600.0); |
gamma = (int)(gamma1 * 3600.0); |
cprintf("Alfa Sec = %d Beta Sec = %d Gamma Sec = %d\n",alfa,beta,gamma); |
return 0; |
} else { |
cprintf("No possible !\n"); |
return 1; |
} |
} else |
return 1; |
return 0; |
} |
/demos/trunk/servo/makefile |
---|
7,9 → 7,12 |
endif |
include $(BASE)/config/config.mk |
PROGS = load |
PROGS = ctrl load |
include $(BASE)/config/example.mk |
ctrl: |
make -f $(SUBMAKE) APP=ctrl INIT= OTHEROBJS="initfile.o leg.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __SERVO__" |
load: |
make -f $(SUBMAKE) APP=load INIT= OTHEROBJS="initfile.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __SERVO__" |
/demos/trunk/servo/leg.h |
---|
0,0 → 1,6 |
#ifndef __LEG_H__ |
#define __LEG_H__ |
int set_leg_position(double px, double py, double pz); |
#endif |