Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1421 → Rev 1422

/demos/trunk/chimera/send.c
19,9 → 19,9
 
/* Servo Tasks Constants */
#ifdef DUBUG_SEND
#define SEND_TASK_WCET 20000
#define SEND_TASK_WCET 26000
#else
#define SEND_TASK_WCET 20000
#define SEND_TASK_WCET 26000
#endif
#define SEND_TASK_MIT 30000
 
230,8 → 230,8
int socket;
IP_ADDR bindlist[5];
strcpy(local_ip, "192.168.1.10");
strcpy(target_ip, "192.168.1.1");
strcpy(local_ip, "192.168.0.99");
strcpy(target_ip, "192.168.0.104");
if (init_network(local_ip)) sys_end();
264,11 → 264,11
 
actual += 16;
 
if (actual > 1000) {
if (actual > 800) {
pkg_buffer[actual] = 0;
cprintf("X");
udp_sendto(socket, pkg_buffer, actual+1, &target);
usleep(10000);
udp_sendto(socket, pkg_buffer, actual, &target);
usleep(100000);
actual = 0;
}
 
291,7 → 291,7
TASK servo_send()
{
HEXAPOD_STATE old_status;
register char changes, new_pos, new_pwm, new_power;
register char new_pos, new_pwm, new_power;
int res,n;
struct timespec t;
int actual_leg = 0;
305,15 → 305,42
old_status.power = 0;
 
while (1) {
changes = 0;
new_pos = 0;
new_pwm = 0;
new_power = 0;
 
update_event_action();
 
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;
#endif
}
 
for (n=0; n<6; n++){
new_pos = 0;
new_pwm = 0;
new_power = 0;
#ifdef SERIAL_ON
task_nopreempt();
status.cfg[actual_leg].adc_in = servo_get_analog(COM1, actual_leg);
trace_consumption(actual_leg,status.cfg[actual_leg].adc_in);
task_preempt();
#endif
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;
 
if ((status.ang[n].a != old_status.ang[n].a) ||
(status.ang[n].b != old_status.ang[n].b) ||
(status.ang[n].c != old_status.ang[n].c)) {
321,19 → 348,15
old_status.ang[n].a = status.ang[n].a;
old_status.ang[n].b = status.ang[n].b;
old_status.ang[n].c = status.ang[n].c;
new_pos++;
new_pos += 1 << n;
}
 
if (status.cfg[n].pwm != old_status.cfg[n].pwm) {
old_status.cfg[n].pwm = status.cfg[n].pwm;
new_pwm++;
new_pwm += 1 << n;
}
if (status.power != old_status.power) {
old_status.power = status.power;
new_power++;
}
if (new_pos) {
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));
347,7 → 370,7
 
}
if (new_pwm) {
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 ));
356,45 → 379,9
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
}
if (new_pos || new_pwm || new_power) {
changes++;
}
}
 
if (!new_pos) {
 
 
#ifdef SERIAL_ON
task_nopreempt();
status.cfg[actual_leg].adc_in = servo_get_analog(COM1, actual_leg);
trace_consumption(actual_leg,status.cfg[actual_leg].adc_in);
task_preempt();
#endif
 
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_testcancel();
task_endcycle();
}
417,10 → 404,11
servo_close(COM2);
}
 
PID servo_pid;
 
void init_send_task()
{
HARD_TASK_MODEL ms;
PID pid;
 
hard_task_default_model(ms);
hard_task_def_ctrl_jet(ms);
427,12 → 415,12
hard_task_def_wcet(ms, SEND_TASK_WCET);
hard_task_def_mit(ms, SEND_TASK_MIT);
hard_task_def_usemath(ms);
pid = task_create("Servo_Task", servo_send, &ms, NULL);
if (pid == NIL) {
servo_pid = task_create("Servo_Task", servo_send, &ms, NULL);
if (servo_pid == NIL) {
perror("Could not create task <Send_Task>");
sys_end();
} else
task_activate(pid);
task_activate(servo_pid);
 
}
 
459,5 → 447,7
 
void end_send()
{
task_kill(servo_pid);
 
end_serial();
}