/demos/trunk/chimera/initfile.c |
---|
25,7 → 25,7 |
#include <drivers/keyb.h> |
/*+ sysyem tick in us +*/ |
#define TICK 500 |
#define TICK 0 |
/*+ RR tick in us +*/ |
#define RRTICK 10000 |
/demos/trunk/chimera/chimera.c |
---|
12,6 → 12,12 |
#include "chimera.h" |
#define POS_B_REF 0 |
#define POS_B_UP 50 |
int vel_dx = 15; |
int vel_sx = 15; |
void program_key_end(KEY_EVT* e) |
{ |
65,6 → 71,125 |
} |
TASK walk_auto() |
{ |
short i; |
int vd, vs, auto_count = 0; |
float cos_a; |
int pos_a[6], pos_b[6], pos_c[6]; |
struct action_event e; |
float fake_cos[32] = { 1.0000, .99518, .98079, .95694, .92388, .88192, .83147, .77301, |
.70711, .63439, .55557, .47140, .38268, .29028, .19509, .09802, |
.00000,-.09802,-.19509,-.29028,-.38268,-.47140,-.55557,-.63439, |
-.70711,-.77301,-.83147,-.88192,-.92388,-.95694,-.98079,-.99518}; |
float fake_sin[64] = { -0.50, -0.45, -0.40, -0.30, -0.20, -0.15, -0.10, -0.05, |
0.00, 0.00, 0.40, 0.40, 0.70, 0.70, 0.90, 0.90, |
1.00, 1.00, 0.90, 0.90, 0.70, 0.70, 0.40, 0.40, |
0.00, 0.00, -0.05, -0.10, -0.15, -0.20, -0.30, -0.40, |
-0.45, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, |
-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, |
-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, |
-0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50, -0.50 }; |
while (1) { |
cos_a = (auto_count < 32) ? fake_cos[auto_count] : -fake_cos[auto_count-32]; |
vd = vel_dx; |
vs = vel_sx; |
kern_gettime(&(e.time)); |
ADDUSEC2TIMESPEC(10000,&(e.time)); |
for (i = 0; i<6; i++) { |
switch (i) { |
case 0: |
/* Leg 0 */ |
pos_c[i] = vs * cos_a; |
if ((vs) || (vd)) |
pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP; |
else |
pos_b[i] = POS_B_REF; |
break; |
case 1: |
/* Leg 1 */ |
pos_c[i] = -vd * cos_a; |
if ((vs) || (vd)) |
pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP; |
else |
pos_b[i] = POS_B_REF; |
break; |
case 2: |
/* Leg 2 */ |
pos_c[i] = -vs * cos_a; |
if ((vs) || (vd)) |
pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP; |
else |
pos_b[i] = POS_B_REF; |
break; |
case 3: |
/* Leg 3 */ |
pos_c[i] = vd * cos_a; |
if ((vs) || (vd)) |
pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP; |
else |
pos_b[i] = POS_B_REF; |
break; |
case 4: |
/* Leg 4 */ |
pos_c[i] = vs * cos_a; |
if ((vs) || (vd)) |
pos_b[i] = POS_B_REF + fake_sin[(auto_count+32)%64] * POS_B_UP; |
else |
pos_b[i] = POS_B_REF; |
break; |
case 5: |
/* Leg 5 */ |
pos_c[i] = -vd * cos_a; |
if ((vs) || (vd)) |
pos_b[i] = POS_B_REF + fake_sin[auto_count] * POS_B_UP; |
else |
pos_b[i] = POS_B_REF; |
break; |
} |
e.type = EVT_SET_MASK_LEG_ANGLE; |
e.mask = 1 << i; |
e.ang.a = (80) * 3600; |
e.ang.b = (pos_b[i]) * 3600; |
e.ang.c = (pos_c[i]) * 3600; |
e.pwm = 7; |
insert_action_event(&(e)); |
} |
auto_count++; |
if (auto_count > 63) auto_count = 0; |
task_testcancel(); |
task_endcycle(); |
} |
return 0; |
} |
void action_walk(void) { |
HARD_TASK_MODEL ms; |
PID pid_walk; |
hard_task_default_model(ms); |
hard_task_def_ctrl_jet(ms); |
hard_task_def_wcet(ms, 1000); |
hard_task_def_mit(ms, 25000); |
hard_task_def_usemath(ms); |
pid_walk = task_create("Walk_Task", walk_auto, &ms, NULL); |
if (pid_walk == NIL) { |
perror("Could not create task <Walk_Auto>"); |
sys_end(); |
} else |
task_activate(pid_walk); |
} |
int main(int argc, char **argv) |
{ |
TIME seme; |
87,7 → 212,8 |
init_action_event(100); |
action_stand_up(); |
//action_stand_up(); |
action_walk(); |
return 0; |
/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(); |
} |