47,45 → 47,70 |
void action_stand_up(void) { |
|
static struct action_event e[20]; |
int i,temp; |
int i = 0; |
|
for (i=0;i<3;i++) { |
status.power = 1; |
|
kern_gettime(&(e[i].time)); |
ADDUSEC2TIMESPEC(10000000,&(e[i].time)); |
temp = i*1000000; |
ADDUSEC2TIMESPEC(temp,&(e[i].time)); |
|
e[i].type = EVT_SET_MASK_LEG_ANGLE; |
e[i].mask = 1 << i; |
e[i].ang.a = 45 * 3600; |
e[i].ang.b = 0; |
e[i].mask = 31; |
e[i].ang.a = 0 * 3600; |
e[i].ang.b = 80 * 3600; |
e[i].ang.c = 0; |
e[i].pwm = 7; |
|
insert_action_event(&(e[i])); |
|
} |
kern_gettime(&(e[i+1].time)); |
ADDUSEC2TIMESPEC(11000000,&(e[i+1].time)); |
|
for (i=0;i<3;i++) { |
e[i+1].type = EVT_SET_MASK_LEG_ANGLE; |
e[i+1].mask = 31; |
e[i+1].ang.a = 90 * 3600; |
e[i+1].ang.b = 80 * 3600; |
e[i+1].ang.c = 0; |
e[i+1].pwm = 7; |
|
kern_gettime(&(e[6+i].time)); |
ADDUSEC2TIMESPEC(20000000,&(e[6+i].time)); |
temp = i*1000000; |
ADDUSEC2TIMESPEC(temp,&(e[6+i].time)); |
insert_action_event(&(e[i+1])); |
|
e[6+i].type = EVT_SET_MASK_LEG_ANGLE; |
e[6+i].mask = 1 << i; |
e[6+i].ang.a = 135 * 3600; |
e[6+i].ang.b = 0; |
e[6+i].ang.c = 0; |
e[6+i].pwm = 7; |
kern_gettime(&(e[i+2].time)); |
ADDUSEC2TIMESPEC(12000000,&(e[i+2].time)); |
|
e[i+2].type = EVT_SET_MASK_LEG_ANGLE; |
e[i+2].mask = 31; |
e[i+2].ang.a = 90 * 3600; |
e[i+2].ang.b = -50 * 3600; |
e[i+2].ang.c = 0; |
e[i+2].pwm = 7; |
|
insert_action_event(&(e[6+i])); |
insert_action_event(&(e[i+2])); |
|
} |
kern_gettime(&(e[i+3].time)); |
ADDUSEC2TIMESPEC(13000000,&(e[i+3].time)); |
|
e[i+3].type = EVT_SET_MASK_LEG_ANGLE; |
e[i+3].mask = 31; |
e[i+3].ang.a = 90 * 3600; |
e[i+3].ang.b = 0 * 3600; |
e[i+3].ang.c = 0; |
e[i+3].pwm = 7; |
|
insert_action_event(&(e[i+3])); |
|
kern_gettime(&(e[i+4].time)); |
ADDUSEC2TIMESPEC(15000000,&(e[i+4].time)); |
|
e[i+4].type = EVT_SET_MASK_LEG_ANGLE; |
e[i+4].mask = 31; |
e[i+4].ang.a = 90 * 3600; |
e[i+4].ang.b = 0 * 3600; |
e[i+4].ang.c = 0; |
e[i+4].pwm = 7; |
|
insert_action_event(&(e[i+4])); |
|
} |
|
int main(int argc, char **argv) |