Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1332 → Rev 1331

/demos/trunk/chimera/send.c
42,14 → 42,14
#ifdef DUBUG_SEND
#define SEND_TASK_WCET 6000
#else
#define SEND_TASK_WCET 6000
#define SEND_TASK_WCET 10000
#endif
#define SEND_TASK_MIT 20000
 
#ifdef DUBUG_SEND
#define GET_TASK_WCET 6000
#define GET_TASK_WCET 4000
#else
#define GET_TASK_WCET 6000
#define GET_TASK_WCET 5000
#endif
#define GET_TASK_MIT 20000
 
69,8 → 69,6
HEXAPOD_STATE status;
ANGLES_STATE angles[6];
 
extern struct leg_calibration calibration_table[];
 
void print_status(int n){
int i;
 
204,21 → 202,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, 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);
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
}
284,7 → 271,7
int init_serial()
{
int err;
err = servo_open(COM_PORT, 19200);
err = servo_open(COM_PORT, COM_SPEED);
 
return err;
}
296,14 → 283,14
 
void init_send_task()
{
SOFT_TASK_MODEL ms;
HARD_TASK_MODEL ms;
PID pid;
 
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);
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);
pid = task_create("Send_Task", servo_send, &ms, NULL);
if (pid == NIL) {
perror("Could not create task <Send_Task>");
311,11 → 298,11
} else
task_activate(pid);
 
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);
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);
pid = task_create("Get_Task", servo_get, &ms, NULL);
if (pid == NIL) {
perror("Could not create task <Get_Task>");