Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1330 → Rev 1331

/demos/trunk/chimera/initfile.c
0,0 → 1,81
/*
* Project: HARTIK (HA-rd R-eal TI-me K-ernel)
*
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
* Gerardo Lamastra <gerardo@sssup.it>
*
* Authors : Paolo Gai <pj@hartik.sssup.it>
* (see authors.txt for full list of hartik's authors)
*
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
*
* http://www.sssup.it
* http://retis.sssup.it
* http://hartik.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 "chimera.h"
#include "modules/edf.h"
#include "modules/cbs.h"
#include "modules/rr.h"
#include "modules/dummy.h"
 
/*+ sysyem tick in us +*/
#define TICK 1000
 
/*+ RR tick in us +*/
#define RRTICK 300
 
 
int main(int argc, char **argv);
 
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();
 
return TICK;
}
 
TASK __init__(void *arg)
{
struct multiboot_info *mb = (struct multiboot_info *)arg;
 
KEYB_PARMS kparms = BASE_KEYB;
 
HARTPORT_init();
 
 
//keyb_set_map(itaMap);
//keyb_def_ctrlC(kparms, NULL);
KEYB_init(&kparms);
 
__call_main__(mb);
 
return (void *)0;
}
/demos/trunk/chimera/chimera.c
0,0 → 1,64
 
/*
* Project: S.Ha.R.K.
*
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
*
* Authors : Paolo Gai <pj@hartik.sssup.it>
* (see authors.txt for full list of hartik's authors)
*
* 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 "chimera.h"
 
void my_close(void *arg)
{
int i;
TIME tmp;
 
end_send();
 
for (i=3; i<MAX_PROC; i++){
if (!jet_getstat(i, NULL, &tmp, NULL, NULL))
kern_printf("Task Name : %s - Max Time : %d\n", proc_table[i].name, (int)tmp);
}
}
 
int main(int argc, char **argv)
{
TIME seme;
 
seme = sys_gettime(NULL);
srand(seme);
 
sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT);
 
init_send(); /* Comunication */
init_key(); /* Keyboard */
 
return 0;
}
/demos/trunk/chimera/send.c
0,0 → 1,340
 
/*
* Project: S.Ha.R.K.
*
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
*
* Authors : Paolo Gai <pj@hartik.sssup.it>
* (see authors.txt for full list of hartik's authors)
*
* 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 "chimera.h"
 
//#define DEBUG_SEND /* Print Sent Values */
#define SERIAL_ON /* Send Data using Serial Port */
 
/* Servo Tasks Constants */
#ifdef DUBUG_SEND
#define SEND_TASK_WCET 6000
#else
#define SEND_TASK_WCET 10000
#endif
#define SEND_TASK_MIT 20000
 
#ifdef DUBUG_SEND
#define GET_TASK_WCET 4000
#else
#define GET_TASK_WCET 5000
#endif
#define GET_TASK_MIT 20000
 
#define LEG_A 100.0
#define LEG_B 66.0
#define LEG_C 26.0
#define LEG_D 38.0
#define LEG_CD_2IPO 92.087 /* 2 * sqrt(LEG_C^2 + LEG_D^2) */
#define LEG_CD_ANG 34.380 /* arctg(LEG_C/LEG_D) in gradi */
 
const float c0 = LEG_C * LEG_C;
const float c1 = LEG_B * LEG_B;
const float c2 = LEG_B * LEG_B - LEG_A * LEG_A;
const float todeg = 180.0 / PI;
 
sem_t mx_status, mx_servo;
HEXAPOD_STATE status;
ANGLES_STATE angles[6];
 
void print_status(int n){
int i;
 
if (n>=6) {
for (i=0; i<6;i++)
cprintf("Leg %1d: ( %3d | %3d | %3d ) (%d)\n", i, status.leg[i].x, status.leg[i].y, status.leg[i].z, status.leg[i].pwm );
cprintf("Power: %d", status.power);
cprintf("\n");
} else {
cprintf("Leg %1d: ( %3d | %3d | %3d ) (%1d-%1d)\n", n, status.leg[n].x,status.leg[n].y,status.leg[n].z, status.leg[n].pwm, status.power);
}
}
 
int set_leg_position(int n, float px, float py, float pz)
{
float px2 = px * px;
float py2 = py * py;
float pz2 = pz * pz;
 
float pxz2 = px2 + pz2;
 
float alfa1,beta1,alfa2,beta2,gamma1;
float m,dsqrt;
 
float delta_xz = pxz2 - c0;
float 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(gamma1+LEG_CD_ANG)+cos(gamma1+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)) {
angles[n].a = (int)(alfa1 * 3600.0);
angles[n].b = (int)(beta1 * 3600.0);
angles[n].c = (int)(gamma1 * 3600.0);
#ifdef DEBUG_SEND
cprintf("Leg %d: ( %3d | %3d | %3d )\n", n, angles[n].a, angles[n].b, angles[n].c);
#endif
return 0;
} else if ((alfa2 >= 0.0 && alfa2 <= 180.0) && (beta2 >= -90.0 && beta2 <= 90.0)) {
angles[n].a = (int)(alfa2 * 3600.0);
angles[n].b = (int)(beta2 * 3600.0);
angles[n].c = (int)(gamma1 * 3600.0);
#ifdef DEBUG_SEND
cprintf("Leg %d: ( %3d | %3d | %3d )\n", n, angles[n].a, angles[n].b, angles[n].c);
#endif
return 0;
} else {
#ifdef DEBUG_SEND
cprintf("No possible !\n");
#endif
return 1;
}
} else
return 1;
 
return 0;
}
 
TASK servo_send()
{
HEXAPOD_STATE old_status;
register char changes, new_pos, new_pwm, new_power;
int n;
 
for (n=0; n<6;n++) {
old_status.leg[n].x = 150;
old_status.leg[n].y = 0;
old_status.leg[n].z = 0;
old_status.leg[n].pwm = 0;
}
old_status.power = 0;
 
while (1) {
changes = 0;
for (n=0; n<6; n++){
new_pos = 0;
new_pwm = 0;
new_power = 0;
//sem_wait(&mx_status);
if ((status.leg[n].x != old_status.leg[n].x) || (status.leg[n].y != old_status.leg[n].y) || (status.leg[n].z != old_status.leg[n].z)) {
if (set_leg_position(n, status.leg[n].x, status.leg[n].y, status.leg[n].z)==0) {
old_status.leg[n].x = status.leg[n].x;
old_status.leg[n].y = status.leg[n].y;
old_status.leg[n].z = status.leg[n].z;
new_pos++;
} else {
#ifdef DEBUG_SEND
cprintf("Position not reachable.\n");
#endif
status.leg[n].x = old_status.leg[n].x;
status.leg[n].y = old_status.leg[n].y;
status.leg[n].z = old_status.leg[n].z;
}
}
if (status.leg[n].pwm != old_status.leg[n].pwm) {
old_status.leg[n].pwm = status.leg[n].pwm;
new_pwm++;
}
if (status.power != old_status.power) {
old_status.power = status.power;
new_power++;
}
//sem_post(&mx_status);
if (new_pos) {
#ifdef SERIAL_ON
sem_wait(&mx_servo);
servo_set_angle_sec(COM_PORT, n*3+2, angles[n].a);
servo_set_angle_sec(COM_PORT, n*3+1, angles[n].b);
servo_set_angle_sec(COM_PORT, n*3 , angles[n].c);
sem_post(&mx_servo);
#endif
}
if (new_pwm) {
#ifdef SERIAL_ON
sem_wait(&mx_servo);
(old_status.leg[n].pwm & 1) ? servo_turn_on(COM_PORT, n*3+2) : servo_turn_off(COM_PORT, n*3+2);
(old_status.leg[n].pwm & 2) ? servo_turn_on(COM_PORT, n*3+1) : servo_turn_off(COM_PORT, n*3+1);
(old_status.leg[n].pwm & 4) ? servo_turn_on(COM_PORT, n*3 ) : servo_turn_off(COM_PORT, n*3 );
sem_post(&mx_servo);
#endif
}
if (new_power) {
#ifdef SERIAL_ON
if (old_status.power) {
sem_wait(&mx_servo);
servo_set_RC5_switch(COM_PORT, 1);
sem_post(&mx_servo);
} else {
sem_wait(&mx_servo);
servo_set_RC5_switch(COM_PORT, 0);
sem_post(&mx_servo);
}
#endif
}
if (new_pos || new_pwm || new_power) {
changes++;
#ifdef DEBUG_SEND
print_status(n);
#endif
}
}
task_endcycle();
}
return 0;
}
 
TASK servo_get()
{
int i = 0;
 
while (1) {
#ifdef SERIAL_ON
//sem_wait(&mx_status);
sem_wait(&mx_servo);
status.leg[i ].adc_in = servo_get_analog(COM_PORT, i );
status.leg[i+1].adc_in = servo_get_analog(COM_PORT, i+1);
sem_post(&mx_servo);
#ifdef DEBUG_SEND
cprintf("Leg %1d-%1d: (%4d) (%4d)\n", i, i+1, status.leg[i].adc_in, status.leg[i+1].adc_in);
#endif
i = (i+2)%6;
//sem_post(&mx_status);
#endif
task_endcycle();
}
return 0;
}
 
int init_serial()
{
int err;
err = servo_open(COM_PORT, COM_SPEED);
 
return err;
}
 
void end_serial()
{
servo_close(COM_PORT);
}
 
void init_send_task()
{
HARD_TASK_MODEL ms;
PID pid;
 
hard_task_default_model(ms);
hard_task_def_ctrl_jet(ms);
hard_task_def_wcet(ms, SEND_TASK_WCET);
hard_task_def_mit(ms, SEND_TASK_MIT);
hard_task_def_usemath(ms);
pid = task_create("Send_Task", servo_send, &ms, NULL);
if (pid == NIL) {
perror("Could not create task <Send_Task>");
sys_end();
} else
task_activate(pid);
 
hard_task_default_model(ms);
hard_task_def_ctrl_jet(ms);
hard_task_def_wcet(ms, GET_TASK_WCET);
hard_task_def_mit(ms, GET_TASK_MIT);
hard_task_def_usemath(ms);
pid = task_create("Get_Task", servo_get, &ms, NULL);
if (pid == NIL) {
perror("Could not create task <Get_Task>");
sys_end();
} else
task_activate(pid);
}
 
void init_send()
{
int i;
 
if (init_serial()) {
perror("Could not initialize serial port.");
sys_end();
}
 
//sem_init(&mx_status,0,1);
sem_init(&mx_servo,0,1);
for (i=0; i<6;i++) {
status.leg[i].x = 150;
status.leg[i].y = 0;
status.leg[i].z = 0;
status.leg[i].pwm = 0;
}
status.power = 0;
 
init_send_task();
}
 
void end_send()
{
end_serial();
}
/demos/trunk/chimera/keys.c
0,0 → 1,211
 
/*
* Project: S.Ha.R.K.
*
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
*
* Authors : Paolo Gai <pj@hartik.sssup.it>
* (see authors.txt for full list of hartik's authors)
*
* 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 "chimera.h"
 
/* Manual step increment */
#define INC_LEG 2
 
unsigned char active_leg;
 
void pad(KEY_EVT *k)
{
switch (k->scan) {
case KEY_Z:
//sem_wait(&mx_status);
status.power ^= 1;
//sem_post(&mx_status);
break;
case KEY_Q:
//sem_wait(&mx_status);
status.leg[active_leg].pwm ^= 0x1;
//sem_post(&mx_status);
break;
case KEY_W:
//sem_wait(&mx_status);
status.leg[active_leg].pwm ^= 0x2;
//sem_post(&mx_status);
break;
case KEY_E:
//sem_wait(&mx_status);
status.leg[active_leg].pwm ^= 0x4;
//sem_post(&mx_status);
break;
case KEY_U:
//sem_wait(&mx_status);
status.leg[active_leg].x += INC_LEG;
if (status.leg[active_leg].x > POS_X_MAX) status.leg[active_leg].x = POS_X_MAX;
//sem_post(&mx_status);
break;
case KEY_J:
//sem_wait(&mx_status);
status.leg[active_leg].x -= INC_LEG;
if (status.leg[active_leg].x < POS_X_MIN) status.leg[active_leg].x = POS_X_MIN;
//sem_post(&mx_status);
break;
case KEY_I:
//sem_wait(&mx_status);
status.leg[active_leg].y += INC_LEG;
if (status.leg[active_leg].y > POS_Y_MAX) status.leg[active_leg].y = POS_Y_MAX;
//sem_post(&mx_status);
break;
case KEY_K:
//sem_wait(&mx_status);
status.leg[active_leg].y -= INC_LEG;
if (status.leg[active_leg].y < POS_Y_MIN) status.leg[active_leg].y = POS_Y_MIN;
//sem_post(&mx_status);
break;
case KEY_O:
//sem_wait(&mx_status);
status.leg[active_leg].z += INC_LEG;
if (status.leg[active_leg].z > POS_Z_MAX) status.leg[active_leg].z = POS_Z_MAX;
//sem_post(&mx_status);
break;
case KEY_L:
//sem_wait(&mx_status);
status.leg[active_leg].z -= INC_LEG;
if (status.leg[active_leg].z < POS_Z_MIN) status.leg[active_leg].z = POS_Z_MIN;
//sem_post(&mx_status);
break;
case KEY_1:
active_leg = 0;
break;
case KEY_2:
active_leg = 1;
break;
case KEY_3:
active_leg = 2;
break;
case KEY_4:
active_leg = 3;
break;
case KEY_5:
active_leg = 4;
break;
case KEY_6:
active_leg = 5;
break;
}
}
 
void init_key()
{
KEY_EVT k;
 
/* Leg Selector */
k.flag = 0;
k.scan = KEY_1;
k.ascii = '1';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_2;
k.ascii = '2';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_3;
k.ascii = '3';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_4;
k.ascii = '4';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_5;
k.ascii = '5';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_6;
k.ascii = '6';
keyb_hook(k,pad);
 
/* Leg Position */
k.flag = 0;
k.scan = KEY_J;
k.ascii = 'j';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_K;
k.ascii = 'k';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_L;
k.ascii = 'l';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_U;
k.ascii = 'u';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_I;
k.ascii = 'i';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_O;
k.ascii = 'o';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_Q;
k.ascii = 'o';
keyb_hook(k,pad);
k.flag = 0;
k.scan = KEY_W;
k.ascii = 'o';
keyb_hook(k,pad);
k.flag = 0;
k.scan = KEY_E;
k.ascii = 'o';
keyb_hook(k,pad);
k.flag = 0;
k.scan = KEY_Z;
k.ascii = 'o';
keyb_hook(k,pad);
}
/demos/trunk/chimera/makefile
0,0 → 1,16
#
#
#
 
ifndef BASE
BASE=../..
endif
include $(BASE)/config/config.mk
 
PROGS= chimera
 
include $(BASE)/config/example.mk
 
chimera:
make -f $(SUBMAKE) APP=chimera INIT= OTHEROBJS="initfile.o send.o keys.o" SHARKOPT="__OLDCHAR__ __SERVO__"
 
/demos/trunk/chimera/chimera.h
0,0 → 1,90
/*
* Project: HARTIK (HA-rd R-eal TI-me K-ernel)
*
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
* Gerardo Lamastra <gerardo@sssup.it>
*
* Authors : Paolo Gai <pj@hartik.sssup.it>
* (see authors.txt for full list of hartik's authors)
*
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
*
* http://www.sssup.it
* http://retis.sssup.it
* http://hartik.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 <stdlib.h>
#include <string.h>
#include <math.h>
#include <kernel/kern.h>
#include <kernel/func.h>
#include <semaphore.h>
#include "modules/sem.h"
#include "modules/hartport.h"
#include <drivers/keyb.h>
 
#include <servo.h>
 
/* COM Port Constants */
#define COM_PORT COM2
#define COM_SPEED 115200
 
#define com(i) ((i) / 12) ? COM2 : COM1
#define pin(i) (i) % 12
 
/* Angle bounds */
#define POS_X_MIN 0
#define POS_X_MAX 200
#define POS_Y_MIN -200
#define POS_Y_MAX 200
#define POS_Z_MIN -150
#define POS_Z_MAX 150
 
typedef struct { /*describe the position and adc value of a leg*/
int x;
int y;
int z;
int adc_in;
unsigned char pwm;
} LEG_STATE;
 
typedef struct { /*describe the position of a leg in servo angles*/
int a;
int b;
int c;
} ANGLES_STATE;
 
typedef struct {
LEG_STATE leg[6];
char power;
} HEXAPOD_STATE;
 
/*****************************************/
 
extern sem_t mx_status;
extern HEXAPOD_STATE status;
 
void init_send(void);
void end_send(void);
 
void init_key(void);