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>"); |