/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__" |