Subversion Repositories shark

Compare Revisions

Regard whitespace Rev 1335 → Rev 1336

/demos/trunk/chimera/calibrate.c
76,6 → 76,8
 
for (i=0;i<20;i++) test_angle[i] = 0;
clear();
sem_wait(&mx_servo);
servo_turn_off(COM_PORT, active_leg*3+2);
servo_turn_off(COM_PORT, active_leg*3+1);
/demos/trunk/chimera/chimera.c
4,9 → 4,6
*
* 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
62,10 → 59,10
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;
*(unsigned char *)(e[i].data + 12) = 7;
e[i].ang.a = 45 * 3600;
e[i].ang.b = 0;
e[i].ang.c = 0;
e[i].pwm = 7;
insert_action_event(&(e[i]));
80,27 → 77,16
e[6+i].type = EVT_SET_MASK_LEG_ANGLE;
e[6+i].mask = 1 << i;
*(int *)(e[6+i].data) = 135 * 3600;
*(int *)(e[6+i].data + 4) = 0;
*(int *)(e[6+i].data + 8) = 0;
*(unsigned char *)(e[6+i].data + 12) = 7;
e[6+i].ang.a = 135 * 3600;
e[6+i].ang.b = 0;
e[6+i].ang.c = 0;
e[6+i].pwm = 7;
insert_action_event(&(e[6+i]));
}
 
kern_gettime(&(e[15].time));
ADDUSEC2TIMESPEC(30000000,&(e[15].time));
e[15].type = EVT_SET_MASK_LEG_ANGLE;
e[15].mask = 1 << 3;
*(int *)(e[15].data) = 0;
*(int *)(e[15].data + 4) = 0;
*(int *)(e[15].data + 8) = -45 * 3600;
*(unsigned char *)(e[15].data + 12) = 4;
insert_action_event(&(e[15]));
}
 
int main(int argc, char **argv)
/demos/trunk/chimera/send.c
4,9 → 4,6
*
* 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
64,27 → 61,17
const float c1 = LEG_B * LEG_B;
const float c2 = LEG_B * LEG_B - LEG_A * LEG_A;
const float todeg = 180.0 / PI;
const float torad = PI / 180.0;
 
sem_t mx_status, mx_servo;
sem_t mx_status, mx_adc_status, mx_servo;
HEXAPOD_STATE status;
ANGLES_STATE angles[6];
 
extern struct leg_calibration calibration_table[];
 
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.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.cfg[n].pwm, status.power);
}
}
 
int set_leg_position(int n, float px, float py, float pz)
int leg_to_ang(float px, float py, float pz, int *alfa, int *beta, int *gamma)
{
float px2 = px * px;
float py2 = py * py;
126,31 → 113,41
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
*alfa = (int)(alfa1 * 3600.0);
*beta = (int)(beta1 * 3600.0);
*gamma = (int)(gamma1 * 3600.0);
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
*alfa = (int)(alfa2 * 3600.0);
*beta = (int)(beta2 * 3600.0);
*gamma = (int)(gamma1 * 3600.0);
return 0;
} else {
#ifdef DEBUG_SEND
cprintf("No possible !\n");
#endif
return 1;
return -1;
}
} else
return 1;
return -1;
 
return -1;
 
}
 
int ang_to_leg(int alfa, int beta, int gamma, float *px, float *py, float *pz) {
 
float alfa1 = (float)(alfa)/3600.0 * torad;
float beta1 = (float)(beta)/3600.0 * torad;
float sin_gamma = sin((float)(gamma)/3600.0 * torad);
float cos_gamma = cos((float)(gamma)/3600.0 * torad);
float m;
m = LEG_B * cos(beta1) + LEG_A * cos(alfa1 - beta1);
*py = LEG_B * sin(beta1) - LEG_A * sin(alfa1 - beta1);
 
*pz = (LEG_D + m) * sin_gamma + LEG_C * cos_gamma;
*px = (LEG_D + m) * cos_gamma - LEG_C * sin_gamma;
 
return 0;
 
}
 
void update_event_action(void) {
163,22 → 160,21
 
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+12);
sem_wait(&mx_status);
 
status.ang[i].a = e->ang.a;
status.ang[i].b = e->ang.b;
status.ang[i].c = e->ang.c;
 
status.cfg[i].pwm = e->pwm;
 
sem_post(&mx_status);
 
//#ifdef DEBUG_SEND
printf_xy(3,2,WHITE,"%8d: Update leg %2d angle",(int)kern_gettime(NULL),i);
//#endif
201,9 → 197,6
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.ang[n].a = 0;
old_status.ang[n].b = 0;
old_status.ang[n].c = 0;
222,21 → 215,6
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.ang[n].a != old_status.ang[n].a) ||
(status.ang[n].b != old_status.ang[n].b) ||
261,21 → 239,10
if (new_pos) {
#ifdef SERIAL_ON
sem_wait(&mx_servo);
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));
servo_set_angle_sec(COM_PORT, n*3+2, adjust(status.ang[n].a,n,0));
servo_set_angle_sec(COM_PORT, n*3+1, adjust(status.ang[n].b,n,1));
servo_set_angle_sec(COM_PORT, n*3 , adjust(status.ang[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/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)/3600,adjust(angles[n].b,n,1)/3600,adjust(angles[n].c,n,2)/3600);
 
#endif
}
321,7 → 288,7
 
while (1) {
#ifdef SERIAL_ON
//sem_wait(&mx_status);
sem_wait(&mx_adc_status);
sem_wait(&mx_servo);
status.cfg[i ].adc_in = servo_get_analog(COM_PORT, i );
status.cfg[i+1].adc_in = servo_get_analog(COM_PORT, i+1);
330,7 → 297,7
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);
sem_post(&mx_adc_status);
#endif
task_endcycle();
}
377,8 → 344,8
if (pid == NIL) {
perror("Could not create task <Get_Task>");
sys_end();
} //else
//task_activate(pid);
} else
task_activate(pid);
}
 
void init_send()
391,6 → 358,7
}
 
sem_init(&mx_status,0,1);
sem_init(&mx_adc_status,0,1);
sem_init(&mx_servo,0,1);
 
for (i=0; i<16;i++) {
404,17 → 372,17
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.cfg[i].pwm = 0;
}
status.power = 0;
 
init_send_task();
 
}
 
void end_send()
{
 
end_serial();
 
}
/demos/trunk/chimera/keys.c
35,11 → 35,10
 
#include "chimera.h"
 
/* Manual step increment */
#define INC_LEG 2
 
unsigned char active_leg;
 
extern volatile int calibrate_status;
 
void pad(KEY_EVT *k)
{
switch (k->scan) {
65,46 → 64,6
//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;
119,11 → 78,11
break;
case KEY_6:
active_leg = 5;
//calibrate_step(100000);
break;
/*
case KEY_C:
calibrate_init();
if (calibrate_init != 0) calibrate_step(100000);
break;
 
case KEY_A:
144,7 → 103,6
case KEY_H:
calibrate_step(+108000);
break;
*/
}
}
 
/demos/trunk/chimera/chimera.h
65,27 → 65,20
unsigned char pwm;
} LEG_CFG_STATE;
 
typedef struct { /*describe the position and adc value of a leg*/
typedef struct { /*describe the position of leg*/
int x;
int y;
int z;
} LEG_POS_STATE;
 
typedef struct { /*describe the position and adc value of a leg*/
typedef struct { /*describe the servo angles*/
int a;
int b;
int c;
} LEG_ANG_STATE;
 
typedef struct { /*describe the position of a leg in servo angles*/
int a;
int b;
int c;
} ANGLES_STATE;
 
typedef struct {
LEG_CFG_STATE cfg[6];
LEG_POS_STATE leg[6];
LEG_ANG_STATE ang[6];
char power;
} HEXAPOD_STATE;
93,7 → 86,6
/*****************************************/
 
#define EVT_SET_MASK_LEG_ANGLE 0x01
#define EVT_SET_MASK_LEG_POS 0x02
 
#define EVT_STATUS_WAIT 0x01
#define EVT_STATUS_EXEC 0x02
105,7 → 97,8
unsigned char status;
struct timespec time;
unsigned char mask;
unsigned char data[20]; //Generic data
LEG_ANG_STATE ang; //Servo angle data
unsigned char pwm;
struct action_event *next;
 
};
119,3 → 112,6
void end_send(void);
 
void init_key(void);
 
int adjust(int angle_sec, int leg, int num);