17,11 → 17,11 |
|
/* Servo Tasks Constants */ |
#ifdef DUBUG_SEND |
#define SEND_TASK_WCET 6000 |
#define SEND_TASK_WCET 1000 |
#else |
#define SEND_TASK_WCET 6000 |
#define SEND_TASK_WCET 1000 |
#endif |
#define SEND_TASK_MIT 20000 |
#define SEND_TASK_MIT 30000 |
|
#define LEG_A 100.0 |
#define LEG_B 66.0 |
178,17 → 178,12 |
|
if (status.power != old_status.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(); |
old_status.power = status.power; |
|
task_endcycle(); |
|
#endif |
} |
|
211,27 → 206,14 |
|
if (new_pos && (1<<n)) { |
#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"); |
task_preempt(); |
|
task_endcycle(); |
|
task_nopreempt(); |
res = servo_set_angle_sec(com(n*3+1), pin(n*3+1), adjust(status.ang[n].b,n,1)); |
if (res != 0) cprintf("Error send data\n"); |
task_preempt(); |
|
task_endcycle(); |
|
task_nopreempt(); |
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(); |
|
task_endcycle(); |
|
#endif |
|
} |
238,38 → 220,17 |
|
if (new_pwm && (1<<n)) { |
#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 )); |
task_preempt(); |
|
task_endcycle(); |
|
task_nopreempt(); |
(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)); |
task_preempt(); |
|
task_endcycle(); |
|
task_nopreempt(); |
(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(); |
|
task_endcycle(); |
#endif |
|
} |
|
} |
|
#ifdef SERIAL_ON |
task_nopreempt(); |
status.cfg[0].adc_in = servo_get_analog(COM_PORT, 0, 8); |
printf_xy(1,1,WHITE,"[%d : %d]",0,status.cfg[0].adc_in); |
task_preempt(); |
//#ifdef DEBUG_SEND |
//cprintf("Analog in %d: %d\n", 0, status.cfg[0].adc_in); |
//#endif |
#endif |
|
task_endcycle(); |
task_testcancel(); |
} |