21,15 → 21,8 |
#else |
#define SEND_TASK_WCET 20000 |
#endif |
#define SEND_TASK_MIT 40000 |
#define SEND_TASK_MIT 30000 |
|
#ifdef DUBUG_SEND |
#define GET_TASK_WCET 5000 |
#else |
#define GET_TASK_WCET 5000 |
#endif |
#define GET_TASK_MIT 40000 |
|
#define LEG_A 100.0 |
#define LEG_B 66.0 |
#define LEG_C 26.0 |
171,8 → 164,8 |
register char changes, new_pos, new_pwm, new_power; |
int res,n; |
struct timespec t; |
int actual_leg = 0; |
|
|
for (n=0; n<6;n++) { |
old_status.ang[n].a = 0; |
old_status.ang[n].b = 0; |
212,6 → 205,7 |
|
if (new_pos) { |
#ifdef SERIAL_ON |
task_nopreempt(); |
res = servo_set_angle_sec(com(n*3 ), pin(n*3 ), adjust(status.ang[n].a,n,0)); |
if (res != 0) cprintf("Error send data\n"); |
res = servo_set_angle_sec(com(n*3+1), pin(n*3+1), adjust(status.ang[n].b,n,1)); |
218,6 → 212,7 |
if (res != 0) cprintf("Error send data\n"); |
res = servo_set_angle_sec(com(n*3+2), pin(n*3+2), adjust(status.ang[n].c,n,2)); |
if (res != 0) cprintf("Error send data\n"); |
task_preempt(); |
#endif |
|
} |
224,18 → 219,22 |
|
if (new_pwm) { |
#ifdef SERIAL_ON |
task_nopreempt(); |
(old_status.cfg[n].pwm & 1) ? servo_turn_on(com(n*3 ), pin(n*3 )) : servo_turn_off(com(n*3 ), pin(n*3 )); |
(old_status.cfg[n].pwm & 2) ? servo_turn_on(com(n*3+1), pin(n*3+1)) : servo_turn_off(com(n*3+1), pin(n*3+1)); |
(old_status.cfg[n].pwm & 4) ? servo_turn_on(com(n*3+2), pin(n*3+2)) : servo_turn_off(com(n*3+2), pin(n*3+2)); |
task_preempt(); |
#endif |
} |
if (new_power) { |
#ifdef SERIAL_ON |
task_nopreempt(); |
if (old_status.power) { |
servo_set_RC5_switch(COM2, 1); |
} else { |
servo_set_RC5_switch(COM2, 0); |
} |
task_preempt(); |
#endif |
} |
|
246,27 → 245,25 |
#endif |
} |
} |
sys_gettime(&t); |
printf_xy(1,20,WHITE,"%d:CHANGE = %d",(int)t.tv_sec,changes); |
task_endcycle(); |
} |
|
return 0; |
} |
|
TASK servo_get() |
{ |
int i = 0; |
if (!new_pos) { |
|
while (1) { |
#ifdef SERIAL_ON |
status.cfg[i ].adc_in = servo_get_analog(COM1, i ); |
status.cfg[i+1].adc_in = servo_get_analog(COM1, i+1); |
#ifdef DEBUG_SEND |
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; |
#endif |
task_nopreempt(); |
status.cfg[actual_leg].adc_in = servo_get_analog(COM1, actual_leg); |
task_preempt(); |
|
sys_gettime(&t); |
printf_xy(1,20,WHITE,"(%d) (%d) (%d) (%d) (%d) (%d) ", |
status.cfg[0].adc_in, |
status.cfg[1].adc_in, |
status.cfg[2].adc_in, |
status.cfg[3].adc_in, |
status.cfg[4].adc_in, |
status.cfg[5].adc_in); |
actual_leg = (actual_leg+1)%6; |
|
} |
|
task_endcycle(); |
} |
|
299,7 → 296,7 |
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); |
pid = task_create("Servo_Task", servo_send, &ms, NULL); |
if (pid == NIL) { |
perror("Could not create task <Send_Task>"); |
sys_end(); |
306,17 → 303,6 |
} 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); |
pid = task_create("Get_Task", servo_get, &ms, NULL); |
if (pid == NIL) { |
perror("Could not create task <Get_Task>"); |
sys_end(); |
} else |
task_activate(pid); |
} |
|
void init_send() |