Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1331 → Rev 1332

/demos/trunk/chimera/initfile.c
35,7 → 35,7
 
#include "chimera.h"
#include "modules/edf.h"
#include "modules/cbs.h"
#include "modules/hardcbs.h"
#include "modules/rr.h"
#include "modules/dummy.h"
 
43,7 → 43,7
#define TICK 1000
 
/*+ RR tick in us +*/
#define RRTICK 300
#define RRTICK 10000
 
 
int main(int argc, char **argv);
53,7 → 53,7
struct multiboot_info *mb = (struct multiboot_info *)arg;
 
EDF_register_level(EDF_ENABLE_ALL);
CBS_register_level(CBS_ENABLE_ALL, 0);
HCBS_register_level(HCBS_ENABLE_ALL, 0);
RR_register_level(RRTICK, RR_MAIN_YES, mb);
dummy_register_level();
 
70,9 → 70,6
 
HARTPORT_init();
 
 
//keyb_set_map(itaMap);
//keyb_def_ctrlC(kparms, NULL);
KEYB_init(&kparms);
 
__call_main__(mb);
/demos/trunk/chimera/calibrate.c
0,0 → 1,214
 
/*
* 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 "chimera.h"
#include <ll/i386/64bit.h>
 
volatile int calibrate_status = 0;
volatile int calibrate_exec_step = 0;
 
extern HEXAPOD_STATE status;
extern unsigned char active_leg;
 
extern sem_t mx_servo;
 
struct leg_calibration {
int pos[6];
int delta_90[3];
int zero[3];
};
 
struct leg_calibration calibration_table[] = {
{{0,90,-45,45,-90,0},{0,0,0},{0,0,0}},
{{0,90,-45,45,-45,45},{0,0,0},{0,0,0}},
{{0,90,-45,45,0,90},{417601,403201,424801},{-169200,36000,-144000}},
{{0,90,-45,45,-90,0},{0,0,0},{0,0,0}},
{{0,90,-45,45,-45,45},{0,0,0},{0,0,0}},
{{0,90,-45,45,0,90},{0,0,0},{0,0,0}},
};
 
int adjust(int angle_sec, int leg, int num) {
 
int temp;
 
smul32div32to32(angle_sec,calibration_table[leg].delta_90[num],324000,temp);
 
return temp + calibration_table[leg].zero[num];
}
 
TASK calibrate(void *arg) {
 
int i, num = 0, angsec = 0, angsec_temp[6];
static int test_angle[20],set,turn_on,servo_count;
char servo_name[10];
 
for (i=0;i<20;i++) test_angle[i] = 0;
sem_wait(&mx_servo);
servo_turn_off(COM_PORT, active_leg*3+2);
servo_turn_off(COM_PORT, active_leg*3+1);
servo_turn_off(COM_PORT, active_leg*3);
 
servo_count = 0;
servo_turn_on(COM_PORT, active_leg*3+2);
turn_on = 0;
sem_post(&mx_servo);
 
while (calibrate_status == 1) {
 
sem_wait(&mx_servo);
 
if (calibrate_exec_step == 100000) {
angsec_temp[servo_count] = test_angle[3*active_leg+num];
printf_xy(0,10+servo_count,WHITE,"%08d",test_angle[3*active_leg+num]);
servo_turn_off(COM_PORT,3*active_leg+num);
servo_count++;
 
if (servo_count == 6) {
 
for (i=0;i<3;i++) {
calibration_table[active_leg].delta_90[i] = abs(angsec_temp[2*i+1] - angsec_temp[2*i] + 1);
calibration_table[active_leg].zero[i] = abs(calibration_table[active_leg].pos[2*i] * calibration_table[active_leg].delta_90[i] / 90) + angsec_temp[2*i];
 
printf_xy(20*i,22,RED,"D%d %7d Z%d %7d",i,calibration_table[active_leg].delta_90[i],
i,calibration_table[active_leg].zero[i]);
 
}
 
calibrate_status = 0;
calibrate_exec_step = 0;
 
sem_post(&mx_servo);
 
task_kill(exec_shadow);
task_testcancel();
 
}
 
turn_on = 1;
 
calibrate_exec_step = 0;
 
}
 
switch (servo_count) {
case 0:
case 1:
sprintf(servo_name,"ALFA ");
num = 2;
break;
case 2:
case 3:
sprintf(servo_name,"BETA ");
num = 1;
break;
case 4:
case 5:
sprintf(servo_name,"GAMMA");
num = 0;
break;
}
 
if (turn_on == 1) {
servo_turn_on(COM_PORT,3*active_leg+num);
turn_on = 0;
}
 
set = calibration_table[active_leg].pos[servo_count];
printf_xy(10,10+servo_count,WHITE,"Set servo %s to position %d",servo_name,set);
servo_turn_on(COM_PORT, active_leg*3+num);
 
if (calibrate_exec_step != 0) {
test_angle[3*active_leg+num] += calibrate_exec_step;
printf_xy(10,20,WHITE,"Set %08d to servo %03d",test_angle[3*active_leg+num],3*active_leg+num);
servo_set_angle_sec(COM_PORT, 3*active_leg+num, test_angle[3*active_leg+num]);
 
calibrate_exec_step = 0;
 
}
 
angsec = servo_get_angle_sec(COM_PORT, 3*active_leg+num);
sem_post(&mx_servo);
 
printf_xy(10,21,WHITE,"Angle Seconds = %08d",angsec);
 
task_endcycle();
 
}
 
return 0;
 
}
 
void calibrate_init() {
 
SOFT_TASK_MODEL st;
PID st_pid;
 
if (calibrate_status == 0) {
calibrate_status = 1;
 
soft_task_default_model(st);
soft_task_def_period(st,300000);
soft_task_def_met(st,30000);
soft_task_def_usemath(st);
soft_task_def_ctrl_jet(st);
 
st_pid = task_create("Calibration task",calibrate,&st,NULL);
if (st_pid == NIL) {
cprintf("Error creating calibration task\n");
sys_end();
}
 
task_activate(st_pid);
 
} else {
return;
}
}
 
void calibrate_step(int step) {
 
if (calibrate_status != 0 && calibrate_exec_step == 0) {
 
calibrate_exec_step = step;
 
}
 
}
/demos/trunk/chimera/send.c
42,14 → 42,14
#ifdef DUBUG_SEND
#define SEND_TASK_WCET 6000
#else
#define SEND_TASK_WCET 10000
#define SEND_TASK_WCET 6000
#endif
#define SEND_TASK_MIT 20000
 
#ifdef DUBUG_SEND
#define GET_TASK_WCET 4000
#define GET_TASK_WCET 6000
#else
#define GET_TASK_WCET 5000
#define GET_TASK_WCET 6000
#endif
#define GET_TASK_MIT 20000
 
69,6 → 69,8
HEXAPOD_STATE status;
ANGLES_STATE angles[6];
 
extern struct leg_calibration calibration_table[];
 
void print_status(int n){
int i;
 
202,10 → 204,21
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);
servo_set_angle_sec(COM_PORT, n*3+2, adjust(angles[n].a,n,0));
servo_set_angle_sec(COM_PORT, n*3+1, adjust(angles[n].b,n,1));
servo_set_angle_sec(COM_PORT, n*3 , adjust(angles[n].c,n,2));
sem_post(&mx_servo);
printf_xy(3,3,WHITE,"PX = %05d PY = %05d PZ = %05d",(int)(status.leg[n].x)
,(int)(status.leg[n].y)
,(int)(status.leg[n].z));
 
printf_xy(3,4,WHITE,"ALFA = %07d BETA = %07d GAMMA = %07d",(int)(angles[n].a)
,(int)(angles[n].b)
,(int)(angles[n].c));
 
printf_xy(3,5,WHITE,"ADJ_ALFA = %07d ADJ_BETA = %07d ADJ_GAMMA = %07d",adjust(angles[n].a,n,0),adjust(angles[n].b,n,1),adjust(angles[n].c,n,2));
 
#endif
}
271,7 → 284,7
int init_serial()
{
int err;
err = servo_open(COM_PORT, COM_SPEED);
err = servo_open(COM_PORT, 19200);
 
return err;
}
283,14 → 296,14
 
void init_send_task()
{
HARD_TASK_MODEL ms;
SOFT_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);
soft_task_default_model(ms);
soft_task_def_ctrl_jet(ms);
soft_task_def_met(ms, SEND_TASK_WCET);
soft_task_def_period(ms, SEND_TASK_MIT);
soft_task_def_usemath(ms);
pid = task_create("Send_Task", servo_send, &ms, NULL);
if (pid == NIL) {
perror("Could not create task <Send_Task>");
298,11 → 311,11
} 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);
soft_task_default_model(ms);
soft_task_def_ctrl_jet(ms);
soft_task_def_met(ms, GET_TASK_WCET);
soft_task_def_period(ms, GET_TASK_MIT);
soft_task_def_usemath(ms);
pid = task_create("Get_Task", servo_get, &ms, NULL);
if (pid == NIL) {
perror("Could not create task <Get_Task>");
/demos/trunk/chimera/keys.c
118,8 → 118,33
active_leg = 4;
break;
case KEY_6:
active_leg = 5;
//active_leg = 5;
calibrate_step(100000);
break;
/*
case KEY_C:
calibrate_init();
break;
 
case KEY_A:
calibrate_step(-3600);
break;
case KEY_S:
calibrate_step(-60);
break;
case KEY_D:
calibrate_step(-1);
break;
case KEY_F:
calibrate_step(+1);
break;
case KEY_G:
calibrate_step(+60);
break;
case KEY_H:
calibrate_step(+3600);
break;
*/
}
}
 
191,21 → 216,58
 
k.flag = 0;
k.scan = KEY_Q;
k.ascii = 'o';
k.ascii = 'q';
keyb_hook(k,pad);
k.flag = 0;
k.scan = KEY_W;
k.ascii = 'o';
k.ascii = 'w';
keyb_hook(k,pad);
k.flag = 0;
k.scan = KEY_E;
k.ascii = 'o';
k.ascii = 'e';
keyb_hook(k,pad);
k.flag = 0;
k.scan = KEY_Z;
k.ascii = 'o';
k.ascii = 'z';
keyb_hook(k,pad);
 
 
k.flag = 0;
k.scan = KEY_C;
k.ascii = 'c';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_A;
k.ascii = 'a';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_S;
k.ascii = 's';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_D;
k.ascii = 'd';
keyb_hook(k,pad);
k.flag = 0;
k.scan = KEY_F;
k.ascii = 'f';
keyb_hook(k,pad);
 
k.flag = 0;
k.scan = KEY_G;
k.ascii = 'g';
keyb_hook(k,pad);
k.flag = 0;
k.scan = KEY_H;
k.ascii = 'h';
keyb_hook(k,pad);
 
}
/demos/trunk/chimera/makefile
12,5 → 12,5
include $(BASE)/config/example.mk
 
chimera:
make -f $(SUBMAKE) APP=chimera INIT= OTHEROBJS="initfile.o send.o keys.o" SHARKOPT="__OLDCHAR__ __SERVO__"
make -f $(SUBMAKE) APP=chimera INIT= OTHEROBJS="initfile.o calibrate.o send.o keys.o" SHARKOPT="__OLDCHAR__ __SERVO____LINUXC26__"