Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1332 → Rev 1333

/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;
 
}