Subversion Repositories shark

Compare Revisions

Regard whitespace Rev 1421 → Rev 1422

/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,14 → 305,41
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) ||
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();
}