37,41 → 37,109 |
|
} |
|
int beta = 30; |
|
void action_stand_up(void) { |
|
struct action_event e; |
int event; |
int event,alfa; |
|
status.power = 1; |
|
kern_gettime(&(e.time)); |
|
for (alfa=0;alfa<=90;alfa+=15) { |
|
ADDUSEC2TIMESPEC(5000000,&(e.time)); |
|
e.type = EVT_SET_MASK_LEG_ANGLE; |
e.mask = 0x3F; |
e.ang.a = 0 * 3600; |
e.ang.b = 70 * 3600; |
e.ang.c = 0; |
e.ang.a = alfa * 3600; |
e.ang.b = beta * 3600; |
e.ang.c = 0 * 3600; |
e.pwm = 7; |
|
event = insert_action_event(&(e)); |
cprintf("Event num = %d\n",event); |
|
ADDUSEC2TIMESPEC(1000000,&(e.time)); |
} |
|
e.type = EVT_SET_MASK_LEG_ANGLE; |
e.mask = 0x3F; |
e.ang.a = 0 * 3600; |
e.ang.b = 0 * 3600; |
e.ang.c = 0; |
e.pwm = 7; |
} |
|
TASK walk_auto_6() |
{ |
short i, n; |
int vd, vs, auto_count = 0; |
int pos_a[6], pos_b[6], pos_c[6]; |
struct action_event e; |
|
float fake_sin[36] = { .00, .50, .80, 1.0, .80, .50, .00, .00, .00, .00, .00, .00, |
.00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, |
.00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00, .00}; |
|
float fake_cos[36] = {-1.0,-.75,-.50, .00, .50, .75, 1.0, .93, .86, .80, .73, .66, |
.60, .53, .46, .40, .33, .26, .20, .13, .06, .00,-.06, .13, |
-.20,-.26,-.33,-.40,-.46,-.53,-.60,-.66,-.73,-.80,-.86,-.93}; |
|
while (1) { |
vd = vel_dx; |
vs = vel_sx; |
|
kern_gettime(&(e.time)); |
ADDUSEC2TIMESPEC(10000,&(e.time)); |
|
for (i = 0; i<6; i++) { |
switch(i) { |
case 0: |
n = 0; |
break; |
case 1: |
n = 3; |
break; |
|
event = insert_action_event(&(e)); |
cprintf("Event num = %d\n",event); |
|
case 2: |
n = 4; |
break; |
|
case 3: |
n = 1; |
break; |
|
case 4: |
n = 2; |
break; |
|
case 5: |
n = 5; |
break; |
} |
if (i%3) |
pos_c[i] = vd * fake_cos[(auto_count+6*n)%36]; |
else |
pos_c[i] = vs * fake_cos[(auto_count+6*n)%36]; |
if ((vs) || (vd)) |
pos_b[i] = POS_B_REF + fake_sin[(auto_count+6*n)%36] * POS_B_UP; |
else |
pos_b[i] = POS_B_REF; |
|
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 > 35) auto_count = 0; |
|
task_testcancel(); |
task_endcycle(); |
} |
return 0; |
} |
|
TASK walk_auto() |
TASK walk_auto_3() |
{ |
short i; |
int vd, vs, auto_count = 0; |
164,7 → 232,7 |
insert_action_event(&(e)); |
} |
|
auto_count++; |
auto_count += 4; |
if (auto_count > 63) auto_count = 0; |
|
task_testcancel(); |
174,20 → 242,20 |
} |
|
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); |
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, 30000); |
hard_task_def_usemath(ms); |
pid_walk = task_create("Walk_Task", walk_auto_3, &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) |
212,6 → 280,11 |
|
init_action_event(100); |
|
if (argc < 2) sys_end(); |
|
beta = atoi(argv[1]); |
cprintf("Beta = %d\n",beta); |
|
//action_stand_up(); |
action_walk(); |
|