Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1390 → Rev 1391

/demos/trunk/chimera/initfile.c
25,7 → 25,7
#include <drivers/keyb.h>
 
/*+ sysyem tick in us +*/
#define TICK 1000
#define TICK 500
 
/*+ RR tick in us +*/
#define RRTICK 10000
/demos/trunk/chimera/send.c
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()