/demos/trunk/chimera/calibrate.c |
---|
43,6 → 43,7 |
extern sem_t mx_servo; |
struct leg_calibration { |
int side; |
int pos[6]; |
int delta_90[3]; |
int zero[3]; |
49,12 → 50,12 |
}; |
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}}, |
{1,{0,90,-45,45,-90,0},{450001,392401,435601},{-208800,41400,234001}}, |
{1,{0,90,-45,45,-45,45},{403201,374401,349201},{-201600,-32400,63000}}, |
{1,{0,90,-45,45,0,90},{417601,403201,424801},{-169200,36000,-144000}}, |
{-1,{0,90,-45,45,-90,0},{413949,453599,431999},{216000,50401,-215599}}, |
{-1,{0,90,-45,45,-45,45},{421199,421199,443799},{165600,-8999,30600}}, |
{-1,{0,90,-45,45,0,90},{0,0,0},{0,0,0}}, |
}; |
int adjust(int angle_sec, int leg, int num) { |
63,7 → 64,7 |
smul32div32to32(angle_sec,calibration_table[leg].delta_90[num],324000,temp); |
return temp + calibration_table[leg].zero[num]; |
return calibration_table[leg].side * temp + calibration_table[leg].zero[num]; |
} |
100,10 → 101,11 |
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]; |
calibration_table[active_leg].zero[i] = calibration_table[active_leg].side * 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]); |
printf_xy(22*i,22,WHITE,"D%d %7d Z%d %7d", |
i,calibration_table[active_leg].delta_90[i], |
i,calibration_table[active_leg].zero[i]); |
} |
/demos/trunk/chimera/chimera.c |
---|
48,6 → 48,32 |
} |
} |
void action_stand_up(void) { |
static struct action_event e[6]; |
int i,temp; |
for (i=0;i<5;i++) { |
kern_gettime(&(e[i].time)); |
ADDUSEC2TIMESPEC(10000000,&(e[i].time)); |
temp = i*1000000; |
ADDUSEC2TIMESPEC(temp,&(e[i].time)); |
e[i].type = EVT_SET_MASK_LEG_ANGLE; |
e[i].mask = 1 << i; |
*(int *)(e[i].data) = 45 * 3600; |
*(int *)(e[i].data + 4) = 0; |
*(int *)(e[i].data + 8) = 0; |
*(int *)(e[i].data + 12) = 0; |
*(unsigned char *)(e[i].data + 16) = 7; |
insert_action_event(&(e[i])); |
} |
} |
int main(int argc, char **argv) |
{ |
TIME seme; |
60,5 → 86,7 |
init_send(); /* Comunication */ |
init_key(); /* Keyboard */ |
action_stand_up(); |
return 0; |
} |
/demos/trunk/chimera/send.c |
---|
76,11 → 76,11 |
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("Leg %1d: ( %3d | %3d | %3d ) (%d)\n", i, status.leg[i].x, status.leg[i].y, status.leg[i].z, status.cfg[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); |
cprintf("Leg %1d: ( %3d | %3d | %3d ) (%1d-%1d)\n", n, status.leg[n].x,status.leg[n].y,status.leg[n].z, status.cfg[n].pwm, status.power); |
} |
} |
153,6 → 153,47 |
return 0; |
} |
void update_event_action(void) { |
struct timespec t; |
struct action_event *e; |
int i; |
kern_gettime(&t); |
while ((e = get_first_old_event(&t)) != NULL) { |
if (e->type == EVT_SET_MASK_LEG_POS) { |
e->status = EVT_STATUS_DONE; |
} |
if (e->type == EVT_SET_MASK_LEG_ANGLE) { |
for (i=0;i<6;i++) |
if ((1 << i) & e->mask) { |
status.ang[i].a = angles[i].a = *(int *)(e->data); |
status.ang[i].b = angles[i].b = *(int *)(e->data+4); |
status.ang[i].c = angles[i].c = *(int *)(e->data+8); |
status.cfg[i].pwm = *(unsigned char *)(e->data+16); |
//#ifdef DEBUG_SEND |
cprintf("Update leg %d angle\n",i); |
//#endif |
} |
e->status = EVT_STATUS_DONE; |
} |
} |
} |
TASK servo_send() |
{ |
HEXAPOD_STATE old_status; |
161,14 → 202,19 |
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.leg[n].y = 0; |
old_status.leg[n].z = 0; |
old_status.ang[n].a = 0; |
old_status.ang[n].b = 0; |
old_status.ang[n].c = 0; |
old_status.cfg[n].pwm = 0; |
} |
old_status.power = 0; |
while (1) { |
changes = 0; |
update_event_action(); |
for (n=0; n<6; n++){ |
new_pos = 0; |
175,7 → 221,7 |
new_pwm = 0; |
new_power = 0; |
//sem_wait(&mx_status); |
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; |
191,8 → 237,19 |
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; |
if ((status.ang[n].a != old_status.ang[n].a) || |
(status.ang[n].b != old_status.ang[n].b) || |
(status.ang[n].c != old_status.ang[n].c)) { |
old_status.ang[n].a = status.ang[n].a; |
old_status.ang[n].b = status.ang[n].b; |
old_status.ang[n].c = status.ang[n].c; |
new_pos++; |
} |
if (status.cfg[n].pwm != old_status.cfg[n].pwm) { |
old_status.cfg[n].pwm = status.cfg[n].pwm; |
new_pwm++; |
} |
if (status.power != old_status.power) { |
199,7 → 256,7 |
old_status.power = status.power; |
new_power++; |
} |
//sem_post(&mx_status); |
sem_post(&mx_status); |
if (new_pos) { |
#ifdef SERIAL_ON |
213,11 → 270,11 |
,(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,4,WHITE,"ALFA = %07d BETA = %07d GAMMA = %07d",(int)(angles[n].a/3600) |
,(int)(angles[n].b/3600) |
,(int)(angles[n].c/3600)); |
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)); |
printf_xy(3,5,WHITE,"ADJ_ALFA = %07d ADJ_BETA = %07d ADJ_GAMMA = %07d",adjust(angles[n].a,n,0)/3600,adjust(angles[n].b,n,1)/3600,adjust(angles[n].c,n,2)/3600); |
#endif |
} |
225,9 → 282,9 |
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 ); |
(old_status.cfg[n].pwm & 1) ? servo_turn_on(COM_PORT, n*3+2) : servo_turn_off(COM_PORT, n*3+2); |
(old_status.cfg[n].pwm & 2) ? servo_turn_on(COM_PORT, n*3+1) : servo_turn_off(COM_PORT, n*3+1); |
(old_status.cfg[n].pwm & 4) ? servo_turn_on(COM_PORT, n*3 ) : servo_turn_off(COM_PORT, n*3 ); |
sem_post(&mx_servo); |
#endif |
} |
266,8 → 323,8 |
#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); |
status.cfg[i ].adc_in = servo_get_analog(COM_PORT, i ); |
status.cfg[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); |
333,14 → 390,17 |
sys_end(); |
} |
//sem_init(&mx_status,0,1); |
sem_init(&mx_status,0,1); |
sem_init(&mx_servo,0,1); |
for (i=0; i<6;i++) { |
status.ang[i].a = 0; |
status.ang[i].b = 0; |
status.ang[i].c = 0; |
status.leg[i].x = 150; |
status.leg[i].y = 0; |
status.leg[i].z = 0; |
status.leg[i].pwm = 0; |
status.cfg[i].pwm = 0; |
} |
status.power = 0; |
/demos/trunk/chimera/keys.c |
---|
127,22 → 127,22 |
break; |
case KEY_A: |
calibrate_step(-3600); |
calibrate_step(-108000); |
break; |
case KEY_S: |
calibrate_step(-60); |
calibrate_step(-3600); |
break; |
case KEY_D: |
calibrate_step(-1); |
calibrate_step(-60); |
break; |
case KEY_F: |
calibrate_step(+1); |
calibrate_step(+60); |
break; |
case KEY_G: |
calibrate_step(+60); |
calibrate_step(+3600); |
break; |
case KEY_H: |
calibrate_step(+3600); |
calibrate_step(+108000); |
break; |
*/ |
} |
/demos/trunk/chimera/makefile |
---|
12,5 → 12,5 |
include $(BASE)/config/example.mk |
chimera: |
make -f $(SUBMAKE) APP=chimera INIT= OTHEROBJS="initfile.o calibrate.o send.o keys.o" SHARKOPT="__OLDCHAR__ __SERVO____LINUXC26__" |
make -f $(SUBMAKE) APP=chimera INIT= OTHEROBJS="initfile.o calibrate.o send.o action.o keys.o" SHARKOPT="__OLDCHAR__ __SERVO____LINUXC26__" |
/demos/trunk/chimera/chimera.h |
---|
60,14 → 60,23 |
#define POS_Z_MIN -150 |
#define POS_Z_MAX 150 |
typedef struct { |
int adc_in; |
unsigned char pwm; |
} LEG_CFG_STATE; |
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; |
} LEG_POS_STATE; |
typedef struct { /*describe the position and adc value of a leg*/ |
int a; |
int b; |
int c; |
} LEG_ANG_STATE; |
typedef struct { /*describe the position of a leg in servo angles*/ |
int a; |
int b; |
75,12 → 84,34 |
} ANGLES_STATE; |
typedef struct { |
LEG_STATE leg[6]; |
LEG_CFG_STATE cfg[6]; |
LEG_POS_STATE leg[6]; |
LEG_ANG_STATE ang[6]; |
char power; |
} HEXAPOD_STATE; |
/*****************************************/ |
#define EVT_SET_MASK_LEG_ANGLE 0x01 |
#define EVT_SET_MASK_LEG_POS 0x02 |
#define EVT_STATUS_WAIT 0x01 |
#define EVT_STATUS_EXEC 0x02 |
#define EVT_STATUS_DONE 0x03 |
struct action_event { |
unsigned char type; |
unsigned char status; |
struct timespec time; |
unsigned char mask; |
unsigned char data[20]; //Generic data |
struct action_event *next; |
}; |
struct action_event *get_first_old_event(struct timespec *time); |
extern sem_t mx_status; |
extern HEXAPOD_STATE status; |
/demos/trunk/chimera/action.c |
---|
0,0 → 1,133 |
/* |
* 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" |
struct action_event *first_action_event = NULL; |
int insert_action_event(struct action_event *e) { |
struct action_event *t = first_action_event, *k = NULL; |
SYS_FLAGS f; |
f = kern_fsave(); |
if (!t) { |
first_action_event = e; |
e->next = NULL; |
kern_frestore(f); |
return 0; |
} |
while(t) { |
if (TIMESPEC_A_LT_B(&e->time,&t->time)) |
break; |
k = t; |
t = t->next; |
} |
t = k->next; |
k->next = e; |
e->next = t; |
t = first_action_event; |
while(t) { |
cprintf("Time %d:%d\n",t->time.tv_sec,t->time.tv_nsec); |
t = t->next; |
} |
kern_frestore(f); |
return 0; |
} |
int delete_action_event(struct action_event *e) { |
struct action_event *t = first_action_event; |
SYS_FLAGS f; |
if (!t) return -1; |
f = kern_fsave(); |
if (t == e) { |
first_action_event = t->next; |
kern_frestore(f); |
return 0; |
} |
while(t) { |
if (t->next == e) |
break; |
t = t->next; |
} |
if (t) { |
t->next = e->next; |
kern_frestore(f); |
return 0; |
} |
kern_frestore(f); |
return -1; |
} |
struct action_event * get_first_old_event(struct timespec *time) { |
struct action_event *t = first_action_event; |
SYS_FLAGS f; |
if (!t) return NULL; |
f = kern_fsave(); |
if (TIMESPEC_A_GT_B(time,&(t->time))) { |
first_action_event = t->next; |
kern_frestore(f); |
return t; |
} |
kern_frestore(f); |
return NULL; |
} |