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,33 → 113,43 |
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 0; |
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) { |
|
struct timespec t; |
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(); |
|
} |